/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <iostream>
#include <cmath>

#include <boost/make_shared.hpp>
#include <boost/lambda/lambda.hpp>

#include "tudat/math/basic/coordinateConversions.h"
#include "tudat/astro/basic_astro/physicalConstants.h"
#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/math/basic/mathematicalConstants.h"
#include "tudat/astro/basic_astro/celestialBodyConstants.h"

#include "tudat/simulation/environment_setup/createBodies.h"
#include "tudat/astro/basic_astro/sphericalBodyShapeModel.h"
#include "tudat/astro/ephemerides/simpleRotationalEphemeris.h"
//#include "tudat/astro/reference_frames/referenceFrameTransformations.h"
#include "tudat/interface/spice/spiceRotationalEphemeris.h"

namespace tudat
{

namespace simulation_setup
{

using namespace ephemerides;
using namespace gravitation;
using namespace basic_astrodynamics;

void addAerodynamicCoefficientInterface(
        const SystemOfBodies& bodies, const std::string bodyName,
        const std::shared_ptr< AerodynamicCoefficientSettings > aerodynamicCoefficientSettings )
{
    if( bodies.count( bodyName ) == 0 )
    {
        throw std::runtime_error( "Error when setting aerodynamic coefficients for body "+ bodyName + ", body is not found in system of bodies" );
    }
    bodies.at( bodyName )->setAerodynamicCoefficientInterface(
                createAerodynamicCoefficientInterface( aerodynamicCoefficientSettings, bodyName) );
}

void addRadiationPressureInterface(
        const SystemOfBodies& bodies, const std::string bodyName,
        const std::shared_ptr< RadiationPressureInterfaceSettings > radiationPressureSettings )
{
    if( bodies.count( bodyName ) == 0 )
    {
        throw std::runtime_error( "Error when setting radiation pressure interface for body "+ bodyName + ", body is not found in system of bodies" );
    }
    bodies.at( bodyName )->setRadiationPressureInterface(
                radiationPressureSettings->getSourceBody( ), createRadiationPressureInterface(
                    radiationPressureSettings, bodyName, bodies ) );
}

void setSimpleRotationSettingsFromSpice(
        const BodyListSettings& bodySettings, const std::string& bodyName, const double spiceEvaluationTime )
{
    if( bodySettings.count( bodyName ) == 0 )
    {
        throw std::runtime_error( "Error when setting simple rotation model settings for body " +
                                  bodyName + ", no settings found for this body." );
    }

    Eigen::Quaterniond rotationAtReferenceTime =
            spice_interface::computeRotationQuaternionBetweenFrames(
                bodySettings.getFrameOrientation( ), "IAU_" + bodyName, spiceEvaluationTime );
    double rotationRateAtReferenceTime =
            spice_interface::getAngularVelocityVectorOfFrameInOriginalFrame(
                bodySettings.getFrameOrientation( ), "IAU_" + bodyName, spiceEvaluationTime ).norm( );

    bodySettings.at( bodyName )->rotationModelSettings =
            std::make_shared< SimpleRotationModelSettings >(
                bodySettings.getFrameOrientation( ), "IAU_" + bodyName, rotationAtReferenceTime,
                spiceEvaluationTime, rotationRateAtReferenceTime );
}

void addEmptyTabulatedEphemeris(
        const SystemOfBodies& bodies, const std::string& bodyName, const std::string& ephemerisOrigin )
{
    if( bodies.count( bodyName ) ==  0 )
    {
        throw std::runtime_error( "Error when setting empty tabulated ephemeris for body " + bodyName + ", no such body found" );
    }
    std::string ephemerisOriginToUse = ( ephemerisOrigin == "" ) ? bodies.getFrameOrigin( ) : ephemerisOrigin;
    bodies.at( bodyName )->setEphemeris( std::make_shared< ephemerides::TabulatedCartesianEphemeris< > >(
                                            std::shared_ptr< interpolators::OneDimensionalInterpolator
                                            < double, Eigen::Vector6d > >( ), ephemerisOriginToUse, bodies.getFrameOrientation( ) ) );

    bodies.processBodyFrameDefinitions( );
}

void addEmptyTabulatedRotationalEphemeris(
        const SystemOfBodies& bodies, const std::string& bodyName, const std::string& bodyFixedFrameName )
{
    if( bodies.count( bodyName ) ==  0 )
    {
        throw std::runtime_error( "Error when setting empty tabulated rotational ephemeris for body " + bodyName + ", no such body found" );
    }
    std::string bodyFixedFrameNameToUse = ( bodyFixedFrameName == "" ) ? ( bodyName + "_fixed" ) : bodyFixedFrameName;

    bodies.at( bodyName )->setRotationalEphemeris( std::make_shared< ephemerides::TabulatedRotationalEphemeris< > >(
                                            std::shared_ptr< interpolators::OneDimensionalInterpolator
                                            < double, Eigen::Vector7d > >( ), bodies.getFrameOrientation( ), bodyFixedFrameNameToUse ) );
}

//! Function that determines the order in which bodies are to be created
std::vector< std::pair< std::string, std::shared_ptr< BodySettings > > > determineBodyCreationOrder(
        const std::map< std::string, std::shared_ptr< BodySettings > >& bodySettings )
{
    std::vector< std::pair< std::string, std::shared_ptr< BodySettings > > > outputVector;

    // Create vector of pairs (body name and body settings) that is to be created.
    for( std::map< std::string, std::shared_ptr< BodySettings > >::const_iterator bodyIterator
         = bodySettings.begin( );
         bodyIterator != bodySettings.end( ); bodyIterator ++ )
    {
        outputVector.push_back( std::make_pair( bodyIterator->first, bodyIterator->second ) );
    }

    return outputVector;
}

//! Function to create a map of bodies objects.
SystemOfBodies createSystemOfBodies(
        const BodyListSettings& bodySettings )
{
    std::vector< std::pair< std::string, std::shared_ptr< BodySettings > > > orderedBodySettings
            = determineBodyCreationOrder( bodySettings.getMap( ) );

    // Declare map of bodies that is to be returned.
    SystemOfBodies bodyList = SystemOfBodies(
                bodySettings.getFrameOrigin( ), bodySettings.getFrameOrientation( ) );

    // Create empty body objects.
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        bodyList.createEmptyBody( orderedBodySettings.at( i ).first, false );
    }

    // Define constant mass for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        const double constantMass = orderedBodySettings.at( i ).second->constantMass;
        if ( constantMass == constantMass )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setConstantBodyMass( constantMass );
        }
    }

    // Create ephemeris objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->ephemerisSettings != nullptr )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setEphemeris(
                        createBodyEphemeris( orderedBodySettings.at( i ).second->ephemerisSettings,
                                             orderedBodySettings.at( i ).first ) );
        }
    }

    // Create atmosphere model objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->atmosphereSettings != nullptr )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setAtmosphereModel(
                        createAtmosphereModel( orderedBodySettings.at( i ).second->atmosphereSettings,
                                               orderedBodySettings.at( i ).first ) );
        }
    }

    // Create body shape model objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->shapeModelSettings != nullptr )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setShapeModel(
                        createBodyShapeModel( orderedBodySettings.at( i ).second->shapeModelSettings,
                                              orderedBodySettings.at( i ).first ) );
        }
    }

    // Create rotation model objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->rotationModelSettings != nullptr )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setRotationalEphemeris(
                        createRotationModel( orderedBodySettings.at( i ).second->rotationModelSettings,
                                             orderedBodySettings.at( i ).first, bodyList ) );
        }
    }

    // Create gravity field model objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->gravityFieldSettings != nullptr )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setGravityFieldModel(
                        createGravityFieldModel( orderedBodySettings.at( i ).second->gravityFieldSettings,
                                                 orderedBodySettings.at( i ).first, bodyList,
                                                 orderedBodySettings.at( i ).second->gravityFieldVariationSettings ) );
        }
    }

    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->gravityFieldVariationSettings.size( ) > 0 )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setGravityFieldVariationSet(
                        createGravityFieldModelVariationsSet(
                            orderedBodySettings.at( i ).first, bodyList,
                            orderedBodySettings.at( i ).second->gravityFieldVariationSettings ) );
        }
    }

    // Create aerodynamic coefficient interface objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->aerodynamicCoefficientSettings != nullptr )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setAerodynamicCoefficientInterface(
                        createAerodynamicCoefficientInterface(
                            orderedBodySettings.at( i ).second->aerodynamicCoefficientSettings,
                            orderedBodySettings.at( i ).first ) );
        }
    }

    // Create radiation pressure coefficient objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        std::map< std::string, std::shared_ptr< RadiationPressureInterfaceSettings > >
                radiationPressureSettings
                = orderedBodySettings.at( i ).second->radiationPressureSettings;
        for( std::map< std::string, std::shared_ptr< RadiationPressureInterfaceSettings > >::iterator
             radiationPressureSettingsIterator = radiationPressureSettings.begin( );
             radiationPressureSettingsIterator != radiationPressureSettings.end( );
             radiationPressureSettingsIterator++ )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setRadiationPressureInterface(
                        radiationPressureSettingsIterator->first,
                        createRadiationPressureInterface(
                            radiationPressureSettingsIterator->second,
                            orderedBodySettings.at( i ).first, bodyList ) );
        }

    }

    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        for( unsigned int j = 0; j < orderedBodySettings.at( i ).second->groundStationSettings.size( ); j++ )
        {
            createGroundStation( bodyList.at( orderedBodySettings.at( i ).first ), orderedBodySettings.at( i ).first,
                     orderedBodySettings.at( i ).second->groundStationSettings.at( j ) );
        }
    }

    bodyList.processBodyFrameDefinitions( );

    return bodyList;

}

//! Function to create a simplified system of bodies
simulation_setup::SystemOfBodies createSimplifiedSystemOfBodies(const double secondsSinceJ2000)
{
    using namespace ephemerides;
    using namespace gravitation;

    // Creation of bodies
    SystemOfBodies bodies("SSB","ECLIPJ2000");
    bodies.createEmptyBody( "Sun" );
    bodies.createEmptyBody( "Mercury" );
    bodies.createEmptyBody( "Venus" );
    bodies.createEmptyBody( "Earth" );
    bodies.createEmptyBody( "Mars" );
    bodies.createEmptyBody( "Jupiter" );
    bodies.createEmptyBody( "Saturn" );
    bodies.createEmptyBody( "Uranus" );
    bodies.createEmptyBody( "Neptune" );
    bodies.createEmptyBody( "Pluto" );

    // Ephemerides
    bodies.getBody( "Sun" )->setEphemeris( std::make_shared< ConstantEphemeris >( Eigen::Vector6d::Zero( )) );
    bodies.getBody( "Mercury" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Mercury") );
    bodies.getBody( "Venus" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Venus" ) );
    bodies.getBody( "Earth" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Earth" ) );
    bodies.getBody( "Mars" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Mars" ) );
    bodies.getBody( "Jupiter" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Jupiter" ) );
    bodies.getBody( "Saturn" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Saturn" ) );
    bodies.getBody( "Uranus" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Uranus" ) );
    bodies.getBody( "Neptune" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Neptune" ) );
    bodies.getBody( "Pluto" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Pluto" ) );

    // Gravity field
    bodies.getBody( "Sun" )->setGravityFieldModel( std::make_shared< GravityFieldModel >(celestial_body_constants::SUN_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Mercury" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::MERCURY_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Venus" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::VENUS_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Earth" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::EARTH_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Mars" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::MARS_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Jupiter" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::JUPITER_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Saturn" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::SATURN_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Uranus" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::URANUS_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Neptune" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::NEPTUNE_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Pluto" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::PLUTO_GRAVITATIONAL_PARAMETER ) );

    // Earth's shape model
    bodies.getBody( "Earth" )->setShapeModel( std::make_shared< SphericalBodyShapeModel >(celestial_body_constants::EARTH_EQUATORIAL_RADIUS ) );

    // Calculate position of rotation axis at initialTime, with respect to J2000 frame. Values from:
    // "Report of the IAU Working Group on Cartographic Coordinates and Rotational Elements: 2009", B.A. Archinal et al.(2011)
    const double daysSinceJ2000 = secondsSinceJ2000 / 86400;
    const double centuriesSinceJ2000 = daysSinceJ2000 / 36525;
    const double poleRightAscension = (0 - 0.641 * centuriesSinceJ2000) * mathematical_constants::PI/180;
    const double poleDeclination = (90 - 0.557 * centuriesSinceJ2000) * mathematical_constants::PI/180;
    const double initialRotationAngle = 190.147 * mathematical_constants::PI/180;
    const double rotationRate = 360.985235 * mathematical_constants::PI/180 / physical_constants::JULIAN_DAY;

    Eigen::Matrix3d J2000toPlanetocentricMatrix = reference_frames::getInertialToPlanetocentricFrameTransformationMatrix (
            poleDeclination, poleRightAscension, initialRotationAngle);
    Eigen::Matrix3d ECLIPJ2000toJ2000Matrix = reference_frames::getECLIPJ2000toJ2000TransformationMatrix();
    Eigen::Matrix3d ECLIPJ2000toPlanetocentricMatrix =  J2000toPlanetocentricMatrix * ECLIPJ2000toJ2000Matrix;

    bodies.getBody( "Earth" )->setRotationalEphemeris(std::make_shared< SimpleRotationalEphemeris >(
                    Eigen::Quaterniond(ECLIPJ2000toPlanetocentricMatrix),
                    rotationRate, secondsSinceJ2000, "ECLIPJ2000", "Earth_Fixed" ) );

    return bodies;
}

} // namespace simulation_setup

} // namespace tudat


#pragma once
#include "MpmSimulationBase.h"
#include <Ziran/Math/Geometry/SourceCollisionObject.h>

#include <math.h>

#include <math.h>

#include <tbb/concurrent_unordered_map.h>
#include <tbb/tbb.h>

#include <Eigen/Core>
#include <Eigen/Dense>
#undef B2

#include <Ziran/CS/Util/Timer.h>
#include <Ziran/Math/Geometry/Particles.h>
#include <Ziran/Math/Geometry/AnalyticLevelSet.h>
#include <Ziran/Math/Geometry/CollisionObject.h>
#include <Ziran/Math/Geometry/PartioIO.h>
#include <Ziran/Math/Splines/BSplines.h>
#include <Ziran/Physics/LagrangianForce/Inertia.h>
#include <Ziran/Physics/LagrangianForce/LagrangianForce.h>
#include <Ziran/Sim/DiffTest.h>
#include <Ziran/Sim/Scene.h>

#include <MPM/Force/MpmForceBase.h>
#include "MpmSimulationDataAnalysis.h"

namespace ZIRAN {

template <class T, int dim>
MpmSimulationBase<T, dim>::MpmSimulationBase()
    : transfer_scheme(APIC_blend_RPIC)
    , apic_rpic_ratio((T)1)
    , flip_pic_ratio((T)0)
    , flip_rpic_ratio((T)0)
    , print_stats(false)
    , write_partio(true)
    , write_meshes(true)
    , use_particle_collision(false)
    , autorestart(true)
    , gravity(TV::Unit(1) * (0))
    , cfl((T).6)
    , dx(1)
    , dt((T)1.0 / 24)
    , element_partitions(0)
    , particles(scene.particles)
    , element_measure(particles.add(element_measure_name<T>()))
    , objective(*this)
    , newton(objective, (T)1e-5, 3)
    , force((forces.emplace_back(std::make_unique<MpmForceBase<T, dim>>(mls_mpm, dx, dt, particles, scene, scratch_xp, scratch_vp, scratch_fp, scratch_gradV, scratch_stress, dv, vn, grid, particle_base_offset, particle_order, particle_group, block_offset, num_nodes, plasticity_appliers, full_implicit)), forces.back()))
{
    symplectic = true;
    quasistatic = false;
    openvdb::initialize();
    inertia = std::make_unique<MassLumpedInertia<T, dim>>(mass_matrix, dv, dt);
    explicit_velocity_field = nullptr;
    ignoreCollisionObject = false;
    mls_mpm = false;
    useTrialCollision = true; //default to true
}

template <class T, int dim>
MpmSimulationBase<T, dim>::~MpmSimulationBase()
{
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::setDx(const T new_dx)
{
    dx = new_dx;
}

template <class T, int dim>
auto MpmSimulationBase<T, dim>::getMpmForce() -> MpmForceBase<T, dim>*
{
    return force.get();
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::addCollisionObject(AnalyticCollisionObject<T, dim>&& object)
{
    collision_objects.emplace_back(std::make_unique<AnalyticCollisionObject<T, dim>>(std::move(object)));
}

template <class T, int dim>
int MpmSimulationBase<T, dim>::addSourceCollisionObject(SourceCollisionObject<T, dim>&& object)
{
    collision_objects.emplace_back(std::make_unique<SourceCollisionObject<T, dim>>(std::move(object)));
    return collision_objects.size() - 1;
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::initialize()
{
    ZIRAN_TIMER();
    Base::initialize();
    ZIRAN_INFO("Initialize called");
    ZIRAN_INFO("end_frame = ", end_frame);

    if (mls_mpm) {
        ZIRAN_ASSERT(transfer_scheme == APIC_blend_RPIC && interpolation_degree != 1 && symplectic == true,
            "mls_mpm requires apic quadratic/cubic and only supports explicit for now");
        ZIRAN_INFO("Using mls_mpm.");
    }

    if ((transfer_scheme == APIC_blend_RPIC)
        && interpolation_degree != 1) {

        // use datamanager to store C
        particles.add(C_name<TM>());
        // compute D inverse
        T dx2 = dx * dx;
        if constexpr (interpolation_degree == 2)
            D_inverse = 4 / dx2;
        else if (interpolation_degree == 3)
            D_inverse = 3 / dx2;
        else
            ZIRAN_ASSERT(false);

        ZIRAN_ASSERT(apic_rpic_ratio >= 0 && apic_rpic_ratio <= 1);

        // resize C array
        const DisjointRanges& x_range = particles.X.ranges;
        particles.DataManager::get(C_name<TM>()).lazyResize(x_range, TM::Zero());
    }

    if (!symplectic) {
        objective.initialize(
            [&](TVStack& dv) {
                for (auto iter = collision_nodes.begin(); iter != collision_nodes.end(); ++iter) {
                    int node_id = (*iter).node_id;
                    (*iter).project(dv.col(node_id));
                }
            });
    }

    for (auto& em : scene.element_managers) {
        em->registerParticleReorderingCallback(particles);
    }

    writeSimulationInformation();

    ZIRAN_INFO("Registering restart callback for collider state update.");
    restart_callbacks.emplace_back([this](int frame) {
        for (size_t k = 0; k < collision_objects.size(); ++k) {
            if (collision_objects[k]->updateState) {
                collision_objects[k]->updateState(step.time, *collision_objects[k]);
            }
        }
    });
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::reinitialize()
{
    ZIRAN_TIMER();
    Base::reinitialize();

    if (!restarting) {
        if (element_partitions) {
            int count = 0;
            bool repartitioned = false;
            for (auto& em : scene.element_managers) {
                if (em && em->needRepartitioning()) {
                    repartitioned = true;
                    em->partitionAndReindexElements(element_partitions);

                    if (count++ == 0) {

                        StdVector<int> old_to_new;
                        StdVector<int> new_to_old;
                        if (em->renumberParticles(particles.count, old_to_new, new_to_old)) {
                            particles.reorder(new_to_old);
                        }
                    }
                }
            }

            if (repartitioned) {

                scene.segmesh_to_write.indices.clear();
                AttributeName<int> no_write("no_write");

                for (auto& em : scene.element_managers) {

                    // TODO: rebuild boundary for tet mesh

                    if (SimplexElements<T, 2, dim>* tris = dynamic_cast<SimplexElements<T, 2, dim>*>(em.get()))
                        scene.trimesh_to_write.indices = tris->indices.array;

                    if (SimplexElements<T, 1, dim>* segs = dynamic_cast<SimplexElements<T, 1, dim>*>(em.get())) {
                        if (!segs->exist(no_write)) {
                            ZIRAN_INFO("segmesh_to_write did not find no_write tag");
                            scene.segmesh_to_write.indices = segs->indices.array;
                        }
                        else {
                            ZIRAN_INFO("segmesh_to_write found no_write tag");
                            DataArray<int>& no_write_array = segs->get(no_write);
                            for (size_t z = 0; z < segs->indices.array.size(); ++z) {
                                if (no_write_array.valueId(z) == -1) // that means z is not labled no write
                                    scene.segmesh_to_write.indices.push_back(segs->indices.array[z]);
                            }
                        }
                    }
                }
            }
        }
    }

    // TODO: Fix the restarting flag
    ZIRAN_INFO("calling mpmsimulationbase reinitialize");
    ZIRAN_INFO("collision object refresh to time = ", step.time);
    ZIRAN_INFO("restarting = ", restarting); // TODO: SimulationBase already sets this to false (see simulate()).
    for (size_t k = 0; k < collision_objects.size(); ++k) {
        if (step.time == 0 || restarting) {
            if (collision_objects[k]->updateState) {
                // ZIRAN_INFO("Time=0 or restarting, in reinitialize, updating collision object to time ", step.time);
                collision_objects[k]->updateState(step.time, *collision_objects[k]);
            }
        }
        else {
            if (collision_objects[k]->updateState) {
                // ZIRAN_INFO("Calling in reinitialize anyway, to update collision object to time ", step.time);
                collision_objects[k]->updateState(step.time, *collision_objects[k]);
            }
        }
    }

    collision_nodes.clear();

    force->reinitialize();
    inertia->reinitialize();

    sortParticlesAndPolluteGrid();

    // TV
    scratch_xp.resize(particles.count, TV::Constant(0));
    scratch_vp.resize(dim, particles.count);
    scratch_fp.resize(dim, particles.count);
    // TM
    scratch_gradV.resize(particles.count, TM::Constant(0));
    scratch_stress.resize(particles.count, TM::Constant(0));
    // resize C array
    if ((transfer_scheme == APIC_blend_RPIC)
        && interpolation_degree != 1) {
        const DisjointRanges& x_range = particles.X.ranges;
        particles.DataManager::get(C_name<TM>()).lazyResize(x_range, TM::Zero());
    }
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::beginTimeStep(int frame, int substep, double time, double dt)
{
    this->dt = dt;
    for (auto f : begin_time_step_callbacks) {
        f(frame, substep, time, dt);
    }
}

template <class T, int dim>
T MpmSimulationBase<T, dim>::totalEnergy(std::string info)
{
    T pe = 0;
    for (auto& lf : forces) {
        pe += lf->totalEnergy();
    }

    MpmSimulationDataAnalysis<T, dim> mpm_data(*this);
    T ke = mpm_data.evalTotalKineticEnergyGrid();

    ZIRAN_INFO(info, pe, " ", ke, " ", ke + pe);
    return ke + pe;
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::advanceOneTimeStep(double dt)
{
    ZIRAN_TIMER();
    ZIRAN_INFO("Advance one time step from time ", std::setw(7), step.time, " with                     dt = ", dt);

    reinitialize();

    if (symplectic) {
        forcesUpdateParticleState();
        for (auto f : before_p2g_callbacks) {
            f();
        }
        particlesToGrid();
        for (auto f : before_euler_callbacks) {
            f();
        }
        gridVelocityExplicitUpdate(dt);
        for (size_t k = 0; k < collision_objects.size(); ++k)
            if (collision_objects[k]->updateState)
                collision_objects[k]->updateState(step.time + dt, *collision_objects[k]);
        if (useTrialCollision) {
            trialCollision();
        }
        gridToParticles(dt);
    }
    else {
        for (auto f : before_p2g_callbacks) {
            f();
        }
        particlesToGrid();
        for (auto f : before_euler_callbacks) {
            f();
        }
        backwardEulerStep();
        ZIRAN_INFO("After BE (but before G2P), updating collision object to time ", step.time + dt);
        for (size_t k = 0; k < collision_objects.size(); ++k)
            if (collision_objects[k]->updateState) {
                collision_objects[k]->updateState(step.time + dt, *collision_objects[k]);
            }
        gridToParticles(dt);
    }
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::trialCollision()
{
    ZIRAN_TIMER();

    ZIRAN_ASSERT(symplectic);
    grid.iterateGrid([&](IV node, GridState<T, dim>& g) {
        if (explicit_collision_nodes[g.idx])
            return;
        TV vi = g.new_v;
        TV xi = node.template cast<T>() * dx;
        AnalyticCollisionObject<T, dim>::trialCollision(collision_objects, xi, dt, vi);
        g.new_v = vi;
    });
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::buildAndPassGradVnToMpmForceHelpers()
{
    bool need_to_compute = false;
    for (auto& h : force->helpers) {
        if (h->needGradVn()) {
            need_to_compute = true;
            break;
        }
    }
    if (need_to_compute) {
        buildGradVn(scratch_gradV);
        for (auto& h : force->helpers)
            if (h->needGradVn())
                h->getGradVn(scratch_gradV);
    }
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::buildGradVn(StdVector<TM>& grad_vn)
{
    // TODO: use real rasterized grad_vn instead of grad_v from previous time step!
    auto& Xarray = particles.X.array;
    for (int i = 0; i < particles.count; ++i) {
        TM& gradVp = grad_vn[i];
        if (gradVp != gradVp) //check for NAN/unitialized data
            continue;
        gradVp = TM::Zero();
        TV Xp = Xarray[i];
        BSplineWeights<T, dim> spline(Xp, dx);
        grid.iterateKernel(spline, particle_base_offset[i], [&](IV node, T w, TV dw, GridState<T, dim>& g) {
            gradVp.noalias() += g.v * dw.transpose();
        });
    }
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::performRpicVelocityFilteringAtBeginningOfTimeStep()
{
    ZIRAN_ASSERT(!mls_mpm);
    ZIRAN_ASSERT(FLIP_blend_PIC == transfer_scheme);
    ZIRAN_ASSERT(0 == flip_pic_ratio);
    ZIRAN_ASSERT(1 != ZIRAN_MPM_DEGREE);

    reinitialize();
    forcesUpdateParticleState();

    particlesToGridWithForceHelper<false, false, true>();
    num_nodes = grid.getNumNodes();
    grid.iterateGrid([&](IV node, GridState<T, dim>& g) {
        if (g.m != 0) {
            g.v /= g.m;
        }
        else {
            g.v = TV::Zero();
        }
    });

    grid.iterateGrid([&](IV node, GridState<T, dim>& g) {
        g.new_v = g.v;
    });

    auto* C = ((transfer_scheme == APIC_blend_RPIC) && interpolation_degree != 1)
        ? &particles.DataManager::get(C_name<TM>())
        : nullptr;
    static constexpr bool USE_APIC_BLEND_RPIC = false;
    static constexpr bool USE_MPM_DEGREE_ONE = false;
    static constexpr bool USE_MLS_MPM = false;
    {
        tbb::parallel_for(0, (int)particle_group.size(), [&](int group_idx) {
            for (int idx = particle_group[group_idx].first; idx <= particle_group[group_idx].second; ++idx) {

                int i = particle_order[idx];
                TV& Xp = particles.X[i];

                TV picV = TV::Zero();
                BSplineWeights<T, dim> spline(Xp, dx);
                if constexpr (!USE_APIC_BLEND_RPIC) {
                    TV oldV = TV::Zero();
                    TM& gradVp = scratch_gradV[i];
                    gradVp = TM::Zero();
                    grid.iterateKernel(spline, particle_base_offset[i], [&](IV node, T w, TV dw, GridState<T, dim>& g) {
                        picV += w * g.new_v;
                        oldV += w * g.v;
                        gradVp.noalias() += g.new_v * dw.transpose();
                    });
                    particles.V[i] *= flip_pic_ratio;
                    particles.V[i] += picV - flip_pic_ratio * oldV;
                }
                else if constexpr (!USE_MPM_DEGREE_ONE) {
                    TM Bp = TM::Zero();
                    TM& gradVp = scratch_gradV[i];
                    gradVp = TM::Zero();
                    grid.iterateKernel(spline, particle_base_offset[i], [&](IV node, T w, TV dw, GridState<T, dim>& g) {
                        picV += w * g.new_v;
                        TV xi_minus_xp = node.template cast<T>() * dx - Xp;
                        Bp.noalias() += w * g.new_v * xi_minus_xp.transpose();
                        // !mls_mpm
                        if constexpr (!USE_MLS_MPM) {
                            gradVp.noalias() += g.new_v * dw.transpose();
                        }
                    });
                    particles.V[i] = picV;
                    TM CC = Bp * D_inverse;
                    (*C)[i] = ((apic_rpic_ratio + 1) * (T)0.5) * CC + ((apic_rpic_ratio - 1) * (T)0.5) * CC.transpose();
                    // no check for NAN/unitialized data
                    if constexpr (USE_MLS_MPM) {
                        gradVp = (*C)[i];
                    }
                }
                else {
                    TM& gradVp = scratch_gradV[i];
                    gradVp = TM::Zero();
                    grid.iterateKernel(spline, particle_base_offset[i], [&](IV node, T w, TV dw, GridState<T, dim>& g) {
                        picV += w * g.new_v;
                        gradVp.noalias() += g.new_v * dw.transpose();
                    });
                    particles.V[i] = picV;
                }
            }
        });
    }
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::particlesToGrid()
{
#if 0
    {
        ZIRAN_TIMER();
        ZIRAN_INFO("particle group size ", particle_group.size());
        for (uint64_t color = 0; color < (1 << dim); ++color) {
            tbb::parallel_for(0, (int)particle_group.size(), [&](int group_idx) {
                if ((block_offset[group_idx] & ((1 << dim) - 1)) != color)
                    return;
                for (int idx = particle_group[group_idx].first; idx <= particle_group[group_idx].second; ++idx) {
                    int i = particle_order[idx];
                    TV& Xp = particles.X.array[i];
                    T mass = particles.mass.array[i];
                    BSplineWeights<T, dim> spline(Xp, dx);
                    grid.iterateKernel(spline, particle_base_offset[i], [&](const IV& node, T w, const TV& dw, GridState<T, dim>& g) {
                        g.m += w * mass;
                    });
                }
            });
        }
        ZIRAN_WARN("Just finish normal grid mass p2g");
        //getchar();
    }
#endif
    if (symplectic) {
        ZIRAN_TIMER();
        if (mls_mpm) {
            if (transfer_scheme == FLIP_blend_PIC && ZIRAN_MPM_DEGREE == 1)
                particlesToGridWithForceHelper<false, true, true>();
            else if (transfer_scheme == FLIP_blend_PIC && ZIRAN_MPM_DEGREE != 1)
                particlesToGridWithForceHelper<false, false, true>();
            else if (transfer_scheme == APIC_blend_RPIC && ZIRAN_MPM_DEGREE == 1)
                particlesToGridWithForceHelper<true, true, true>();
            else if (transfer_scheme == APIC_blend_RPIC && ZIRAN_MPM_DEGREE != 1)
                particlesToGridWithForceHelper<true, false, true>();
        }
        else {
            if (transfer_scheme == FLIP_blend_PIC && ZIRAN_MPM_DEGREE == 1)
                particlesToGridWithForceHelper<false, true, false>();
            else if (transfer_scheme == FLIP_blend_PIC && ZIRAN_MPM_DEGREE != 1)
                particlesToGridWithForceHelper<false, false, false>();
            else if (transfer_scheme == APIC_blend_RPIC && ZIRAN_MPM_DEGREE == 1)
                particlesToGridWithForceHelper<true, true, false>();
            else if (transfer_scheme == APIC_blend_RPIC && ZIRAN_MPM_DEGREE != 1)
                particlesToGridWithForceHelper<true, false, false>();
        }
    }
    else {
        ZIRAN_TIMER();
        if (transfer_scheme == FLIP_blend_PIC && ZIRAN_MPM_DEGREE == 1)
            particlesToGridHelper<false, true>();
        else if (transfer_scheme == FLIP_blend_PIC && ZIRAN_MPM_DEGREE != 1)
            particlesToGridHelper<false, false>();
        else if (transfer_scheme == APIC_blend_RPIC && ZIRAN_MPM_DEGREE == 1)
            particlesToGridHelper<true, true>();
        else if (transfer_scheme == APIC_blend_RPIC && ZIRAN_MPM_DEGREE != 1)
            particlesToGridHelper<true, false>();
    }
    {
        ZIRAN_TIMER();
        num_nodes = grid.getNumNodes();
        grid.iterateGrid([&](IV node, GridState<T, dim>& g) {
            if (g.m != 0) {
                g.v /= g.m;
            }
            else {
                g.v = TV::Zero();
            }
        });
    }
}

template <class T, int dim>
template <bool USE_APIC_BLEND_RPIC, bool USE_MPM_DEGREE_ONE, bool USE_MLS_MPM>
void MpmSimulationBase<T, dim>::particlesToGridWithForceHelper()
{
    auto& Xarray = particles.X.array;
    auto& Varray = particles.V.array;
    auto& marray = particles.mass.array;
    const StdVector<Matrix<T, dim, dim>>* Carray_pointer;
    Carray_pointer = ((transfer_scheme == APIC_blend_RPIC) && interpolation_degree != 1) ? (&(particles.DataManager::get(C_name<TM>()).array)) : NULL;

    {
        for (uint64_t color = 0; color < (1 << dim); ++color) {
            tbb::parallel_for(0, (int)particle_group.size(), [&](int group_idx) {
                if ((block_offset[group_idx] & ((1 << dim) - 1)) != color)
                    return;
                for (int idx = particle_group[group_idx].first; idx <= particle_group[group_idx].second; ++idx) {
                    int i = particle_order[idx];
                    TV& Xp = Xarray[i];
                    T mass = marray[i];
                    TV momentum = marray[i] * Varray[i];

                    TM C = TM::Zero();
                    if constexpr (USE_APIC_BLEND_RPIC) {
                        if constexpr (!USE_MPM_DEGREE_ONE) {
                            C = mass * (*Carray_pointer)[i];
                        }
                        else {
                            const TM& gradVp = scratch_gradV[i];
                            C = mass * (((apic_rpic_ratio + 1) * (T)0.5) * gradVp + ((apic_rpic_ratio - 1) * (T)0.5) * gradVp.transpose());
                        }
                    }
                    TM4 stress_density = TM4::Zero();
                    TM& stress = scratch_stress[i];
                    stress_density.template topLeftCorner<dim, dim>() = stress; // stress density
                    const TV& fp = scratch_fp.col(i);
                    stress_density.template topRightCorner<dim, 1>() = -fp; // fp (for meshed forces).
                    TM4 velocity_density = TM4::Zero();
                    velocity_density.template topLeftCorner<dim, dim>() = C;
                    velocity_density.template topRightCorner<dim, 1>() = momentum;
                    velocity_density(3, 3) = mass;
                    BSplineWeights<T, dim> spline(Xp, dx);
                    if constexpr (USE_MLS_MPM) {
                        grid.iterateKernel(spline, particle_base_offset[i],
                            [&](IV node, T w, TV dw, GridState<T, dim>& g) {
                                TV4 xi_minus_xp = TV4::Zero();
                                xi_minus_xp.template topLeftCorner<dim, 1>() = node.template cast<T>() * dx - Xp; // top dim entries non-zero
                                xi_minus_xp(3) = 1;
                                TV4 force_delta = ((stress_density * xi_minus_xp) * (w * D_inverse));
                                TV4 velocity_delta = velocity_density * xi_minus_xp * w;
                                g.m += velocity_delta(3);
                                g.v += velocity_delta.template topLeftCorner<dim, 1>();
                                g.new_v -= force_delta.template topLeftCorner<dim, 1>(); // fi -= \sum_p (Ap (xi-xp)  - fp )w_ip Dp_inv
                            });
                    }
                    else {
                        grid.iterateKernel(spline, particle_base_offset[i],
                            [&](IV node, T w, TV dw, GridState<T, dim>& g) {
                                TV4 weight = TV4::Zero();
                                weight.template topLeftCorner<dim, 1>() = dw;
                                weight(3) = w;
                                TV4 xi_minus_xp = TV4::Zero();
                                xi_minus_xp.template topLeftCorner<dim, 1>() = node.template cast<T>() * dx - Xp; // top dim entries non-zero
                                xi_minus_xp(3) = 1;
                                TV4 force_delta = (stress_density * weight);
                                TV4 velocity_delta = velocity_density * xi_minus_xp * w;
                                g.m += velocity_delta(3);
                                g.v += velocity_delta.template topLeftCorner<dim, 1>();
                                g.new_v -= force_delta.template topLeftCorner<dim, 1>();
                            });
                    }
                }
            });
        }
    }
}

template <class T, int dim>
template <bool USE_APIC_BLEND_RPIC, bool USE_MPM_DEGREE_ONE>
void MpmSimulationBase<T, dim>::particlesToGridHelper()
{
    auto& Xarray = particles.X.array;
    auto& Varray = particles.V.array;
    auto& marray = particles.mass.array;
    const StdVector<Matrix<T, dim, dim>>* Carray_pointer;
    Carray_pointer = ((transfer_scheme == APIC_blend_RPIC) && interpolation_degree != 1) ? (&(particles.DataManager::get(C_name<TM>()).array)) : NULL;

    for (uint64_t color = 0; color < (1 << dim); ++color) {
        tbb::parallel_for(0, (int)particle_group.size(), [&](int group_idx) {
            if ((block_offset[group_idx] & ((1 << dim) - 1)) != color)
                return;
            for (int idx = particle_group[group_idx].first; idx <= particle_group[group_idx].second; ++idx) {
                int i = particle_order[idx];
                TV& Xp = Xarray[i];
                T mass = marray[i];
                TV momentum = marray[i] * Varray[i];
                TM C = TM::Zero();
                if constexpr (USE_APIC_BLEND_RPIC) {
                    if constexpr (!USE_MPM_DEGREE_ONE) {
                        C = mass * (*Carray_pointer)[i];
                    }
                    else {
                        const TM& gradVp = scratch_gradV[i];
                        C = mass * (((apic_rpic_ratio + 1) * (T)0.5) * gradVp + ((apic_rpic_ratio - 1) * (T)0.5) * gradVp.transpose());
                    }
                }
                TM4 velocity_density = TM4::Zero();
                velocity_density.template topLeftCorner<dim, dim>() = C;
                velocity_density.template topRightCorner<dim, 1>() = momentum;
                velocity_density(3, 3) = mass;
                BSplineWeights<T, dim> spline(Xp, dx);
                grid.iterateKernel(spline, particle_base_offset[i], [&](const IV& node, T w, const TV& dw, GridState<T, dim>& g) {
                    TV4 xi_minus_xp = TV4::Zero();
                    xi_minus_xp.template topLeftCorner<dim, 1>() = node.template cast<T>() * dx - Xp; // top dim entries non-zero
                    xi_minus_xp(3) = 1;
                    TV4 velocity_delta = velocity_density * xi_minus_xp * w;
                    g.m += velocity_delta(3);
                    g.v += velocity_delta.template topLeftCorner<dim, 1>();
                });
            }
        });
    }
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::startBackwardEuler()
{
    ZIRAN_TIMER();
    ZIRAN_ASSERT(objective.matrix_free, "MPM only supports matrix free");

    buildMassMatrix();
    objective.setPreconditioner(([&](const TVStack& in, TVStack& out) {
        for (int i = 0; i < num_nodes; i++) {
            for (int d = 0; d < dim; d++) {
                out(d, i) = in(d, i) / mass_matrix(i);
            }
        }
    }));

    dv.resize(dim, num_nodes);
    vn.resize(dim, num_nodes);

    buildInitialDvAndVnForNewton(); // This also builds collision_nodes
    // TODO: this should probably be done in objective reinitialize.
    // Which should be called at the beginning of newton.
    force->backupStrain();
    objective.reinitialize(); // Reinitialize matrix sparsity pattern
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::backwardEulerStep()
{
    ZIRAN_TIMER();
    startBackwardEuler();
    for (auto f : general_callbacks) {
        f(frame, BeforeNewtonSolve);
    }
    // run diff_test if we specify it
    if (diff_test)
        runDiffTest<T, dim, Objective>(num_nodes, dv, objective, diff_test_perturbation_scale);
    newton.solve(dv, verbose);
    for (auto f : general_callbacks) {
        f(frame, AfterNewtonSolve);
    }
    // run diff_test if we specify it
    if (diff_test)
        runDiffTest<T, dim, Objective>(num_nodes, dv, objective, diff_test_perturbation_scale);

    force->restoreStrain();

    constructNewVelocityFromNewtonResult();
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::forcesUpdateParticleState()
{
    ZIRAN_TIMER();

    // zero out scratch_fp and scratch_stress, prepare for adding lagrangian force
    tbb::parallel_for(tbb::blocked_range<size_t>(0, particles.count),
        [&](const tbb::blocked_range<size_t>& range) {
                          for (size_t b = range.begin(), b_end = range.end(); b < b_end; ++b) {
                              scratch_fp.col(b) = TV::Zero();
                              scratch_stress[b] = TM::Zero(); } });
    // Lagrangian
    for (auto& lf : scene.forces)
        lf->updatePositionBasedState();

    auto& vtau = scratch_stress;
    auto& fp = scratch_fp;
    auto ranges = particles.X.ranges;
    tbb::parallel_for(ranges,
        [&](DisjointRanges& subrange) {
            for (auto& h : force->helpers)
                h->updateState(subrange, vtau, fp); // this adds to vtau and fp
        });

    // Lagrangian
    scene.addScaledForces(1, fp); // add forces to fp
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::moveNodes(const TVStack& dv_in)
{
    ZIRAN_QUIET_TIMER();
    if (dv.data() == dv_in.data())
        return;
    tbb::parallel_for(tbb::blocked_range<size_t>(0, num_nodes),
        [&](const tbb::blocked_range<size_t>& range) {
            for (size_t i = range.begin(), i_end = range.end(); i < i_end; ++i) {
                dv.col(i) = dv_in.col(i);
            }
        });
}

/**
       Callback to write the simulation state

       Should be overridden to write the simulation state
    */
template <class T, int dim>
void MpmSimulationBase<T, dim>::writeState(std::ostream& out)
{
    ZIRAN_TIMER();
    ZIRAN_INFO("Write state called");

    if (write_partio) {
        std::string obj_filename = output_dir.absolutePath(outputFileName("partio", ".bgeo"));
        writePartio(obj_filename, particles);
    }

    scene.writeState(out, outputFileName("boundary", ".obj"), outputFileName("segmesh", ".poly"), output_dir, binary_ver, write_meshes);

    if (transfer_scheme == APIC_blend_RPIC && interpolation_degree == 1)
        writeSTDVector(out, scratch_gradV); // apic C matrix
}

/**
       Callback to read the simulation state

       Should be overridden to read the output of writeState
    */
template <class T, int dim>
void MpmSimulationBase<T, dim>::readState(std::istream& in)
{
    ZIRAN_QUIET_TIMER();
    ZIRAN_INFO("Read state called");
    scene.readState(in, binary_ver);

    if (transfer_scheme == APIC_blend_RPIC && interpolation_degree == 1)
        readSTDVector(in, scratch_gradV); // apic C matrix
}

/**
       Callback to calculate dt based on CFL
    */
template <class T, int dim>
double MpmSimulationBase<T, dim>::calculateDt()
{
    ZIRAN_QUIET_TIMER();
    TV p_min_corner, p_max_corner;
    Eigen::Array<T, 2, 1> max_speed = evalMaxParticleSpeed(p_min_corner, p_max_corner);
    ZIRAN_INFO("Particle Min Corner: ", p_min_corner.transpose());
    ZIRAN_INFO("Particle Max Corner: ", p_max_corner.transpose());
    p_min_corner.array() -= (interpolation_degree + 2) * dx; // expand by influenced region and buffer
    p_max_corner.array() += (interpolation_degree + 2) * dx;
    ZIRAN_INFO("Maximum particle speed: ", max_speed(0));
    ZIRAN_ASSERT(max_speed(0) == max_speed(1));
    T max_collision_object_speed = 0;
    for (size_t k = 0; k < collision_objects.size(); ++k)
        max_collision_object_speed = std::max(collision_objects[k]->evalMaxSpeed(p_min_corner, p_max_corner), max_collision_object_speed);
    ZIRAN_INFO("Maximum collision object speed: ", max_collision_object_speed);
    max_speed(1) = std::max(max_speed(1), max_collision_object_speed);
    double dt_compute = step.max_dt;
    if (max_speed(1))
        dt_compute = cfl * dx / max_speed(1);
    ZIRAN_INFO("Computed next dt (based on CFL): ", dt_compute);

    //if (step.time == 0) return std::min(dt_compute, (double)1e-4);
    return dt_compute;
}

// Build and mass_matrix (for implicit)
template <class T, int dim>
void MpmSimulationBase<T, dim>::buildMassMatrix()
{
    ZIRAN_QUIET_TIMER();
    mass_matrix.resize(num_nodes, 1);

    grid.iterateGrid([&](IV node, GridState<T, dim>& g) {
        mass_matrix(g.idx) = g.m;
    });
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::addScaledForces(const T scale, TVStack& f) // called BE Newton objective (for computing residual)
{
    for (auto& lf : forces)
        lf->addScaledForces(scale, f);
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::addScaledForceDifferentials(const T scale, const TVStack& x, TVStack& f) // called BE Newton objective (for computing residual)
{
    for (auto& lf : forces)
        lf->addScaledForceDifferential(scale, x, f);
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::gridVelocityExplicitUpdate(double dt)
{
    ZIRAN_TIMER();
    assert(symplectic);

    explicit_collision_nodes.resize((int)num_nodes, 0);

    TV dv_gravity = gravity * dt;
    if (explicit_velocity_field) {
        grid.iterateGrid([&](IV node, GridState<T, dim>& g) {
            T mi = g.m;
            TV xi = node.template cast<T>() * dx;
            TV vi = g.v;
            explicit_velocity_field(mi, xi, vi);
            g.new_v = vi;
        });
    }
    else {
        grid.iterateGrid([&](IV node, GridState<T, dim>& g) {
            T mi = g.m;
            T dt_over_mi = (T)dt / mi;
            TV xi = node.template cast<T>() * dx;
            TV old_v = g.v;
            TV force_value = g.new_v;
            TV dvijk = force_value * dt_over_mi;
            TV vi = old_v + dv_gravity + dvijk;
            TM normal_basis;

            // apply external body force field: fext(x,t)
            for (auto ff : fext) {
                TV external_force = ff(xi, (T)(step.time));
                vi += dt * external_force;
            }
            auto old_vi = vi;

            bool collided = AnalyticCollisionObject<T, dim>::
                multiObjectCollision(collision_objects, xi, vi, normal_basis);

            explicit_collision_nodes[g.idx] = (int)collided;

            if (ignoreCollisionObject)
                vi = old_vi;

            g.new_v = vi;
        });
    }
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::constructNewVelocityFromNewtonResult()
{
    ZIRAN_QUIET_TIMER();
    grid.iterateTouchedGrid([&](IV node, GridState<T, dim>& g) {
        g.new_v = TV::Zero();
    });
    grid.iterateGrid([&](IV node, GridState<T, dim>& g) {
        g.new_v = g.v + dv.col(g.idx);
    });
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::gridToParticles(double dt)
{
    if (mls_mpm) {
        ZIRAN_TIMER();
        if (transfer_scheme == FLIP_blend_PIC && ZIRAN_MPM_DEGREE == 1)
            gridToParticlesHelper<false, true, true>(dt);
        else if (transfer_scheme == FLIP_blend_PIC && ZIRAN_MPM_DEGREE != 1)
            gridToParticlesHelper<false, false, true>(dt);
        else if (transfer_scheme == APIC_blend_RPIC && ZIRAN_MPM_DEGREE == 1)
            gridToParticlesHelper<true, true, true>(dt);
        else if (transfer_scheme == APIC_blend_RPIC && ZIRAN_MPM_DEGREE != 1)
            gridToParticlesHelper<true, false, true>(dt);
    }
    else {
        ZIRAN_TIMER();
        if (transfer_scheme == FLIP_blend_PIC && ZIRAN_MPM_DEGREE == 1)
            gridToParticlesHelper<false, true, false>(dt);
        else if (transfer_scheme == FLIP_blend_PIC && ZIRAN_MPM_DEGREE != 1)
            gridToParticlesHelper<false, false, false>(dt);
        else if (transfer_scheme == APIC_blend_RPIC && ZIRAN_MPM_DEGREE == 1)
            gridToParticlesHelper<true, true, false>(dt);
        else if (transfer_scheme == APIC_blend_RPIC && ZIRAN_MPM_DEGREE != 1)
            gridToParticlesHelper<true, false, false>(dt);
    }
}

template <class T, int dim>
template <bool USE_APIC_BLEND_RPIC, bool USE_MPM_DEGREE_ONE, bool USE_MLS_MPM>
void MpmSimulationBase<T, dim>::gridToParticlesHelper(double dt)
{
    std::atomic<bool> faster_than_grid_cell(false);
    std::atomic<bool> faster_than_half_grid_cell(false);
    auto* C = ((transfer_scheme == APIC_blend_RPIC) && interpolation_degree != 1)
        ? &particles.DataManager::get(C_name<TM>())
        : nullptr;

    //    if constexpr (false) {
    {
        tbb::parallel_for(0, (int)particle_group.size(), [&](int group_idx) {
            for (int idx = particle_group[group_idx].first; idx <= particle_group[group_idx].second; ++idx) {
                bool local_faster_than_grid_cell = false;
                bool local_faster_than_half_grid_cell = false;

                int i = particle_order[idx];
                TV& Xp = particles.X[i];

                TV picV = TV::Zero();
                BSplineWeights<T, dim> spline(Xp, dx);
                if constexpr (!USE_APIC_BLEND_RPIC) {
                    TV oldV = TV::Zero();
                    TM& gradVp = scratch_gradV[i];
                    gradVp = TM::Zero();
                    grid.iterateKernel(spline, particle_base_offset[i], [&](IV node, T w, TV dw, GridState<T, dim>& g) {
                        picV += w * g.new_v;
                        oldV += w * g.v;
                        gradVp.noalias() += g.new_v * dw.transpose();
                    });
                    particles.V[i] *= flip_pic_ratio;
                    particles.V[i] += picV - flip_pic_ratio * oldV;
                }
                else if constexpr (!USE_MPM_DEGREE_ONE) {
                    TM Bp = TM::Zero();
                    TM& gradVp = scratch_gradV[i];
                    gradVp = TM::Zero();
                    grid.iterateKernel(spline, particle_base_offset[i], [&](IV node, T w, TV dw, GridState<T, dim>& g) {
                        picV += w * g.new_v;
                        TV xi_minus_xp = node.template cast<T>() * dx - Xp;
                        Bp.noalias() += w * g.new_v * xi_minus_xp.transpose();
                        // !mls_mpm
                        if constexpr (!USE_MLS_MPM) {
                            gradVp.noalias() += g.new_v * dw.transpose();
                        }
                    });
                    particles.V[i] = picV;
                    TM CC = Bp * D_inverse;
                    (*C)[i] = ((apic_rpic_ratio + 1) * (T)0.5) * CC + ((apic_rpic_ratio - 1) * (T)0.5) * CC.transpose();
                    // no check for NAN/unitialized data
                    if constexpr (USE_MLS_MPM) {
                        gradVp = (*C)[i];
                    }
                }
                else {
                    TM& gradVp = scratch_gradV[i];
                    gradVp = TM::Zero();
                    grid.iterateKernel(spline, particle_base_offset[i], [&](IV node, T w, TV dw, GridState<T, dim>& g) {
                        picV += w * g.new_v;
                        gradVp.noalias() += g.new_v * dw.transpose();
                    });
                    particles.V[i] = picV;
                }

                TV increment = dt * picV;
                particles.X[i] += increment; // particle position update is the same for all transfer schemes
                T inc = increment.squaredNorm();
                T dx2 = dx * dx;
                local_faster_than_grid_cell = local_faster_than_grid_cell + (inc > dx2);
                local_faster_than_half_grid_cell = local_faster_than_half_grid_cell + (inc > dx2 * (T)0.25 * (cfl * cfl));
                if (local_faster_than_half_grid_cell)
                    faster_than_half_grid_cell.store(true, std::memory_order_relaxed);
                if (local_faster_than_grid_cell)
                    faster_than_grid_cell.store(true, std::memory_order_relaxed);
            }
        });
    }

    static int latest_frame_restarted = 0;
    if (faster_than_grid_cell && autorestart) {
        ZIRAN_WARN("Particle traveling more than a grid cell detected");
        int frame_to_restart = frame - 1;
        if (step.max_dt <= step.min_dt) {
            ZIRAN_WARN("Unable to shrink dt further");
            frame_to_restart--;
            ZIRAN_ASSERT(frame_to_restart >= start_frame, "Unstable intial conditions detected, shrink min_dt or change other parameters");
        }
        else {
            double new_max_dt = std::max(step.max_dt / 2, step.min_dt);
            ZIRAN_WARN("Shrinking max dt to ", new_max_dt);

            restart_callbacks.emplace_back([new_max_dt, this](int frame) {
                step.max_dt = new_max_dt;
            });
        }
        latest_frame_restarted = std::max(frame_to_restart, latest_frame_restarted);
        throw RestartException(frame_to_restart);
    }

    if (!faster_than_half_grid_cell && frame > latest_frame_restarted + 1) {
        double new_max_dt = std::min(step.max_dt_original, step.max_dt * (double)1);
        if (new_max_dt != step.max_dt) {
            ZIRAN_WARN("All particles traveled less than half a grid cell");
            step.max_dt = new_max_dt;
            ZIRAN_WARN("Increasing max dt to ", step.max_dt);
        }
    }

    force->evolveStrain(dt);

    applyPlasticity();
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::applyPlasticity()
{
    // Split all particles to subranges.
    // In parallel, each subrange goes through all plasticity appliers in serial.
    ZIRAN_QUIET_TIMER();
    auto ranges = particles.X.ranges;
    tbb::parallel_for(ranges,
        [&](DisjointRanges& subrange) {
            for (auto& p : plasticity_appliers)
                p->applyPlasticity(subrange, particles);
        });

    if (use_particle_collision) {
        particles.parallel_for([&](const auto& X_local, auto& V_local) {
            for (size_t k = 0; k < collision_objects.size(); ++k)
                collision_objects[k]->particleCollision(dx, X_local, V_local);
        },
            particles.X_name(), particles.V_name());
    }
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::sortParticlesAndPolluteGrid()
{
    auto& Xarray = particles.X.array;

    constexpr int index_bits = (32 - SparseMask::block_bits);
    ZIRAN_ASSERT(particles.count < (1 << index_bits));

    if (particles.count != (int)particle_sorter.size()) {
        particle_base_offset.resize(particles.count);
        particle_sorter.resize(particles.count);
        particle_order.resize(particles.count);
    }

    T one_over_dx = (T)1 / dx;
    tbb::parallel_for(0, particles.count, [&](int i) {
        uint64_t offset = SparseMask::Linear_Offset(to_std_array(
            baseNode<interpolation_degree, T, dim>(Xarray[i] * one_over_dx)));
        particle_sorter[i] = ((offset >> SparseMask::data_bits) << index_bits) + i;
    });

    tbb::parallel_sort(particle_sorter.begin(), particle_sorter.end());

    particle_group.clear();
    block_offset.clear();
    int last_index = 0;
    for (int i = 0; i < particles.count; ++i)
        if (i == particles.count - 1 || (particle_sorter[i] >> 32) != (particle_sorter[i + 1] >> 32)) {
            particle_group.push_back(std::make_pair(last_index, i));
            block_offset.push_back(particle_sorter[i] >> 32);
            last_index = i + 1;
        }

    grid.page_map->Clear();
    for (int i = 0; i < particles.count; ++i) {
        particle_order[i] = (int)(particle_sorter[i] & ((1ll << index_bits) - 1));
        uint64_t offset = (particle_sorter[i] >> index_bits) << SparseMask::data_bits;
        particle_base_offset[particle_order[i]] = offset;
        if (i == particles.count - 1 || (particle_sorter[i] >> 32) != (particle_sorter[i + 1] >> 32)) {
            grid.page_map->Set_Page(offset);
            if constexpr (dim == 2) {
                auto x = 1 << SparseMask::block_xbits;
                auto y = 1 << SparseMask::block_ybits;
                for (int i = 0; i < 2; ++i)
                    for (int j = 0; j < 2; ++j)
                        grid.page_map->Set_Page(SparseMask::Packed_Add(
                            offset, SparseMask::Linear_Offset(x * i, y * j)));
            }
            else {
                auto x = 1 << SparseMask::block_xbits;
                auto y = 1 << SparseMask::block_ybits;
                auto z = 1 << SparseMask::block_zbits;
                for (int i = 0; i < 2; ++i)
                    for (int j = 0; j < 2; ++j)
                        for (int k = 0; k < 2; ++k)
                            grid.page_map->Set_Page(SparseMask::Packed_Add(
                                offset, SparseMask::Linear_Offset(x * i, y * j, z * k)));
            }
        }
    }
    grid.page_map->Update_Block_Offsets();

    auto grid_array = grid.grid->Get_Array();
    auto blocks = grid.page_map->Get_Blocks();
    for (int b = 0; b < (int)blocks.second; ++b) {
        auto base_offset = blocks.first[b];
        std::memset(&grid_array(base_offset), 0, (size_t)(1 << MpmGrid<T, dim>::log2_page));
        GridState<T, dim>* g = reinterpret_cast<GridState<T, dim>*>(&grid_array(base_offset));
        for (int i = 0; i < (int)SparseMask::elements_per_block; ++i)
            g[i].idx = -1;
    }
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::buildInitialDvAndVnForNewton()
{
    ZIRAN_QUIET_TIMER();
    assert(!symplectic);

    grid.iterateGrid([&](IV node, GridState<T, dim>& g) {
        TV old_v = g.v;
        TV vi = old_v;
        TV wn;

        const TV xi = node.template cast<T>() * dx;
        TM normal_basis, R;

        bool any_collision = AnalyticCollisionObject<T, dim>::
            multiObjectCollision(collision_objects, xi, vi, normal_basis, wn);

        int node_id = g.idx;
        assert(node_id < num_nodes);
        if (any_collision) {
#if 1
            bool isSlip = wn != TV::Zero();
            if (isSlip) {
                R = RotationExtractor<T, dim>::rotate(wn);
                //ZIRAN_INFO(isSlip ? "SLIP!\n" : "STICKY!\n");
                //printf("normal: %e\t%e\t%e\n\n%e\t%e\t%e\n%e\t%e\t%e\n%e\t%e\t%e\n\n", wn(0), wn(1), wn(2), R(0, 0), R(0, 1), R(0, 2), R(1, 0), R(1, 1), R(1, 2), R(2, 0), R(2, 1), R(2, 2));
                //getchar();
            }
            else
                R = TM::Identity();
            CollisionNode<T, dim> Z{ node_id, TM::Identity() - normal_basis * normal_basis.transpose(), /* wn, */ R, R.inverse(), isSlip };
#else
            CollisionNode<T, dim> Z{ node_id, TM::Identity() - normal_basis * normal_basis.transpose() };
#endif
            collision_nodes.push_back(Z);

            // Newton initial guess for collided nodes.
            // Setting it this way so that the new velocity constructed after the solve agrees with collision.
            dv.col(node_id) = vi - old_v;
        }
        else
            dv.col(node_id) = gravity * dt; // Newton initial guess for non-collided nodes

        vn.col(node_id) = old_v;
    });
}

/**
       result(0) is purely particle speed maximum.
       result(1) is APIC enhanced 'speed' maximum.
    */
template <class T, int dim>
Eigen::Array<T, 2, 1> MpmSimulationBase<T, dim>::evalMaxParticleSpeed(TV& min_corner, TV& max_corner)
{
    ZIRAN_TIMER();
    using TArray = Eigen::Array<T, 2 * dim + 2, 1>;
    TArray init = TArray::Zero();
    T Tmin = std::numeric_limits<float>::lowest();
    TV TVmin = TV::Zero();
    TVmin.array() = Tmin;
    init << (T)0, (T)0, TVmin, TVmin;
    TArray result;
    result = particles.map_reduce(
        init,
        [](const TV& position, const TV& velocity) {
            T velocity_norm = velocity.norm();
            TArray r;
            r << velocity_norm, velocity_norm, position, -position;
            return r;
        },
        [](const TArray& a, const TArray& b) {
            return a.max(b);
        },
        particles.X_name(), particles.V_name());

    max_corner = result.template segment<dim>(2);
    min_corner = -result.template tail<dim>();
    return result.template head<2>();
}

template <class T, int dim>
void MpmSimulationBase<T, dim>::writeSimulationInformation()
{
    ZIRAN_INFO("Particle count:      ", particles.count);
    ZIRAN_INFO("Grid dx:             ", dx);
    ZIRAN_INFO("Interp degree:       ", interpolation_degree);
    ZIRAN_INFO("Gravity:             ", gravity.transpose());
    ZIRAN_INFO("");
    ZIRAN_INFO("CFL:                 ", cfl);
    ZIRAN_INFO("Transfer scheme:     ", transfer_scheme);
    ZIRAN_INFO("Print stats:         ", print_stats);
    ZIRAN_INFO("Using mls_mpm:       ", mls_mpm);
    writeParticlePerCellHistogram();
}

// Called by writeSimulationInformation
template <class T, int dim>
void MpmSimulationBase<T, dim>::writeParticlePerCellHistogram()
{
    typedef tbb::concurrent_unordered_map<uint64_t, size_t> ConcurrentHash;
    ConcurrentHash cellParticleCount; // mapping IV to size_t
    AttributeName<Vector<T, dim>> x_name = Particles<T, dim>::X_name();
    for (auto iter = particles.iter(x_name); iter; ++iter) {
        Vector<T, dim>& X = iter.template get<0>();
        Vector<int, dim> IX = (X / dx).template cast<int>();
        uint64_t index = SparseMask::Linear_Offset(to_std_array(IX));
        auto search = cellParticleCount.find(index);
        if (search == cellParticleCount.end())
            cellParticleCount[index] = 1;
        else
            cellParticleCount[index]++;
    }
    StdVector<size_t> number_of_cells_with_particles(101, 0);
    for (auto& iter : cellParticleCount)
        number_of_cells_with_particles[std::min(iter.second, (size_t)100)]++;

    for (int i = 0; i < 101; i++) {
        int count = number_of_cells_with_particles[i];
        if (count > 0 && i != 100)
            ZIRAN_INFO("Cells with ", std::setw(2), i, " particles：", count);
        else if (count > 0 && i == 100)
            ZIRAN_INFO("Cells with 100 or more particles: ", count);
    }
}

template <class T, int dim>
constexpr int MpmSimulationBase<T, dim>::interpolation_degree;
} // namespace ZIRAN


/*******************************************************************************
 *         Copyright 2003-2012 LASMEA UMR 6602 CNRS/U.B.P
 *         Copyright 2011-2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_TOOLBOX_INTERPOL_FUNCTIONS_GENERIC_IDXY_BILINEAR_HPP_INCLUDED
#define NT2_TOOLBOX_INTERPOL_FUNCTIONS_GENERIC_IDXY_BILINEAR_HPP_INCLUDED

#include <nt2/toolbox/interpol/functions/idxy_bilinear.hpp>
#include <nt2/include/functions/is_nge.hpp>
#include <nt2/include/functions/is_nle.hpp>
#include <nt2/include/functions/if_else.hpp>
#include <nt2/include/functions/sx.hpp>
#include <nt2/include/functions/logical_or.hpp>
#include <nt2/include/functions/oneminus.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/min.hpp>
#include <nt2/include/functions/idx_linear.hpp>
#include <nt2/include/functions/minusone.hpp>
#include <nt2/include/functions/reshape.hpp>
#include <nt2/include/functions/first_index.hpp>
#include <nt2/include/functions/last_index.hpp>
#include <nt2/include/functions/firstnonsingleton.hpp>
#include <nt2/include/functions/linear_interp.hpp>
#include <nt2/include/functions/toint.hpp>
#include <nt2/include/functions/oneplus.hpp>
#include <nt2/include/functions/floor.hpp>
#include <nt2/include/functions/sx.hpp>
#include <nt2/include/functions/expand_to.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/simd/logical.hpp>
#include <boost/fusion/include/make_vector.hpp>
// #include <boost/assert.hpp>

namespace nt2 { namespace ext
{

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::run_assign_, tag::cpu_
                            , (A0)(A1)(N1)
                            , ((ast_<A0, nt2::container::domain>))
                              ((node_<A1,nt2::tag::idxy_bilinear_,N1,nt2::container::domain>))
                            )
  {
    typedef typename boost::proto::result_of::child_c<A1&,0>::value_type value_t;
    typedef typename boost::proto::result_of::child_c<A1&,1>::value_type   idx_t;
    typedef typename boost::proto::result_of::child_c<A1&,2>::value_type   idy_t;
    typedef typename value_t::value_type                              value_type;
    typedef typename meta::as_integer<value_type>::type               index_type;
    typedef typename A0::value_type                                    sale_type;
    typedef A0&                                                      result_type;
    typedef typename A0::extent_type                                       ext_t;

    typedef boost::fusion::vector<value_type, value_type>                 f_type;

    result_type operator()(A0& r, A1& inputs) const
    {
      //      BOOST_ASSERT_MSG(are_sx_compatible(xi, y), "Inputs dimensions are not compatible");
      const idx_t & xi   =  boost::proto::child_c<1>(inputs);
      const idx_t & yi   =  boost::proto::child_c<2>(inputs);
      const value_t & y     =  boost::proto::child_c<0>(inputs);
      bool extrap = false;
      std::size_t dim1 = 2;
      std::size_t dim2 = 1;
      value_type extrapval1x = Nan<value_type>();
      value_type extrapval2x = extrapval1x;
      value_type extrapval1y = extrapval1x;
      value_type extrapval2y = extrapval1x;
      choices(inputs, extrap, extrapval1x, extrapval2x, extrapval1y, extrapval2y, dim1, dim2, N1());
      r.resize(inputs.extent());
      r = idx_linear(idx_linear(y,xi,true,nt2::_,dim1),yi,true,nt2::_,dim2);
      if (!extrap) {
         ext_t sizee; sizee[0] = 1;
         sizee[dim1-1] = numel(xi);
         value_type fx = value_type(nt2::first_index(y, dim1));
         value_type lx = value_type(nt2::last_index(y, dim1));
         BOOST_AUTO_TPL(z1, nt2::expand_to(nt2::reshape(xi, sizee), size(r)));
         r = nt2::if_else(boost::simd::is_nge(z1, fx), extrapval1x, r);
         r = nt2::if_else(boost::simd::is_nle(z1, lx), extrapval2x, r);
         sizee[dim1-1] = 1;
         sizee[dim2-1] = numel(yi);
         value_type fy = value_type(nt2::first_index(y, dim2));
         value_type ly = value_type(nt2::last_index(y, dim2));
         BOOST_AUTO_TPL(z2, nt2::expand_to(nt2::reshape(yi, sizee), size(r)));
         r = nt2::if_else(boost::simd::is_nge(z2, fy), extrapval1x, r);
         r = nt2::if_else(boost::simd::is_nle(z2, ly), extrapval2x, r);
      }
      return r;
    }
  private :
    // three inputs y and xi and yi
    // idxy_bilinear(y, xi, yi)
    static void choices(const A1&, bool &,  value_type&, value_type&, value_type&, value_type&,
                        std::size_t&, std::size_t&, boost::mpl::long_<3> const &)  //nothing to get
      { }
    // Four inputs y and xi yi and a bool or a floating
    // idxy_bilinear(y, xi, yi, true)
    // idxy_bilinear(y, xi, yi, extrapval)
    static void choices(const A1& inputs,
                        bool & extrap,
                        value_type& extrapval1x, value_type& extrapval2x,
                        value_type& extrapval1y, value_type& extrapval2y,
                        std::size_t& dim1, std::size_t& dim2,
                        boost::mpl::long_<4> const &)
      {
        typedef typename boost::proto::result_of::child_c<A1&,3>::type             child3;
        typedef typename meta::scalar_of<child3>::type                    cref_param_type;
        typedef typename meta::strip<cref_param_type>::type                    param_type;
        get(inputs, extrap, extrapval1x, extrapval2x, extrapval1y, extrapval2y, nt2::meta::as_<param_type>());
      }
    static void get(const A1& inputs, bool & extrap,
                    value_type&, value_type&,
                    value_type&, value_type&,
                    const nt2::meta::as_<bool> &) //get the bool
      {
        extrap =  boost::proto::child_c<3>(inputs);
      }
    static void get(const A1& inputs, bool &,
                    value_type& extrapval1x, value_type& extrapval2x,
                    value_type& extrapval1y, value_type& extrapval2y,
                    const nt2::meta::as_<value_type> &)                     //get extrapval/1x/2x/1y/2y common value (default Nan)
      {
        extrapval1x =  extrapval2x = extrapval1y = extrapval2y = boost::proto::child_c<3>(inputs);
      }

    // Five inputs y and xi yi and two floatings
    static void choices(const A1& inputs,
                        bool & extrap,
                        value_type& extrapval1x, value_type& extrapval2x,
                        value_type& extrapval1y, value_type& extrapval2y,
                        std::size_t& dim1, std::size_t& dim2,
                        boost::mpl::long_<5> const &)                //get extrapval/1x/2x and extrapval/1y/2y commons values
      {
        extrapval1x = extrapval1y = boost::proto::child_c<3>(inputs);
        extrapval2x = extrapval2y = boost::proto::child_c<4>(inputs);
      }

    // Six inputs y and xi yi,  _ or bool and 2 integers
    static void choices(const A1& inputs,
                        bool & extrap,
                        value_type&, value_type&,
                        value_type&, value_type&,
                        std::size_t& dim1, std::size_t& dim2,
                        boost::mpl::long_<6> const &)                //get extrapval/1x/2x and extrapval/1y/2y commons values
      {
        typedef typename boost::proto::result_of::child_c<A1&,3>::type             child3;
        typedef typename meta::scalar_of<child3>::type                    cref_param_type;
        typedef typename meta::strip<cref_param_type>::type                    param_type;
        get(inputs, extrap, nt2::meta::as_<param_type>());
        dim1 = boost::proto::child_c<4>(inputs);
        dim2 = boost::proto::child_c<5>(inputs);
      }
    static void get(const A1& inputs, bool & extrap,
                    const nt2::meta::as_<bool> &) //get the bool
      {
        extrap =  boost::proto::child_c<2>(inputs);
      }
    static void get(const A1& inputs, bool & extrap,
                    const nt2::meta::as_<nt2::container::colon_> &) //nothing to get
      { }

    // Seven inputs y and xi yi and 4 floating
    // idxy_bilinear(y, xi, yi, extrapval1x, extrapval2x, extrapval1y, extrapval2y)

    static void choices(const A1& inputs,
                        bool &,
                        value_type& extrapval1x, value_type& extrapval2x,
                        value_type& extrapval1y, value_type& extrapval2y,
                        std::size_t&, std::size_t&,
                        boost::mpl::long_<7> const &)
      {
        extrapval1x = boost::proto::child_c<3>(inputs);
        extrapval2x = boost::proto::child_c<4>(inputs);
        extrapval1y = boost::proto::child_c<5>(inputs);
        extrapval2y = boost::proto::child_c<6>(inputs);
      }

    // eight inputs y and xi, yi, 4 floatings,  _, and 2 integer
    static void choices(const A1& inputs,
                        bool &,
                        value_type& extrapval1x, value_type& extrapval2x,
                        value_type& extrapval1y, value_type& extrapval2y,
                        std::size_t& dim1, std::size_t& dim2,
                        boost::mpl::long_<8> const &) //get extrapval1 and extrapval2 and dimension
      {
        dim1 =  boost::proto::child_c<7>(inputs);
        dim2 =  boost::proto::child_c<8>(inputs);
        extrapval1x = extrapval1y = boost::proto::child_c<3>(inputs);
        extrapval2x = extrapval2y = boost::proto::child_c<4>(inputs);
      }

    // ten inputs y and xi, yi, two flotings,  _, and 2 integer
    static void choices(const A1& inputs,
                        bool & extrap,
                        value_type& extrapval1x, value_type& extrapval2x,
                        value_type& extrapval1y, value_type& extrapval2y,
                        std::size_t& dim1, std::size_t& dim2,
                        boost::mpl::long_<10> const &) //get extrapval1 and extrapval2 and dimension
      {
        dim1 =  boost::proto::child_c<8>(inputs);
        dim2 =  boost::proto::child_c<9>(inputs);
        extrapval1x = boost::proto::child_c<3>(inputs);
        extrapval2x = boost::proto::child_c<4>(inputs);
        extrapval1y = boost::proto::child_c<5>(inputs);
        extrapval2y = boost::proto::child_c<6>(inputs);
      }
  };
} }

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2014-2021.
// Modifications copyright (c) 2014-2021 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_CONVEX_HULL_INTERFACE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_CONVEX_HULL_INTERFACE_HPP

#include <array>

#include <boost/geometry/algorithms/detail/assign_box_corners.hpp>
#include <boost/geometry/algorithms/detail/convex_hull/graham_andrew.hpp>
#include <boost/geometry/algorithms/detail/equals/point_point.hpp>
#include <boost/geometry/algorithms/detail/for_each_range.hpp>
#include <boost/geometry/algorithms/detail/select_geometry_type.hpp>
#include <boost/geometry/algorithms/detail/visit.hpp>
#include <boost/geometry/algorithms/is_empty.hpp>

#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/geometry_types.hpp>
#include <boost/geometry/core/point_order.hpp>
#include <boost/geometry/core/ring_type.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/visit.hpp>

#include <boost/geometry/geometries/adapted/boost_variant.hpp> // For backward compatibility
#include <boost/geometry/geometries/concepts/check.hpp>
#include <boost/geometry/geometries/ring.hpp>

#include <boost/geometry/strategies/convex_hull/cartesian.hpp>
#include <boost/geometry/strategies/convex_hull/geographic.hpp>
#include <boost/geometry/strategies/convex_hull/spherical.hpp>
#include <boost/geometry/strategies/default_strategy.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/range.hpp>
#include <boost/geometry/util/sequence.hpp>
#include <boost/geometry/util/type_traits.hpp>

namespace boost { namespace geometry
{

// TODO: This file is named interface.hpp but the code below is not the interface.
//       It's the implementation of the algorithm.

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace convex_hull
{

// Abstraction representing ranges/rings of a geometry
template <typename Geometry>
struct input_geometry_proxy
{
    input_geometry_proxy(Geometry const& geometry)
        : m_geometry(geometry)
    {}

    template <typename UnaryFunction>
    inline void for_each_range(UnaryFunction fun) const
    {
        geometry::detail::for_each_range(m_geometry, fun);
    }

    Geometry const& m_geometry;
};

// Abstraction representing ranges/rings of subgeometries of geometry collection
// with boxes converted to rings
template <typename Geometry, typename BoxRings>
struct input_geometry_collection_proxy
{
    input_geometry_collection_proxy(Geometry const& geometry, BoxRings const& box_rings)
        : m_geometry(geometry)
        , m_box_rings(box_rings)
    {}

    template <typename UnaryFunction>
    inline void for_each_range(UnaryFunction fun) const
    {
        detail::visit_breadth_first([&](auto const& g)
        {
            input_geometry_collection_proxy::call_for_non_boxes(g, fun);
            return true;
        }, m_geometry);

        for (auto const& r : m_box_rings)
        {
            geometry::detail::for_each_range(r, fun);
        }
    }

private:
    template <typename G, typename F, std::enable_if_t<! util::is_box<G>::value, int> = 0>
    static inline void call_for_non_boxes(G const& g, F & f)
    {
        geometry::detail::for_each_range(g, f);
    }
    template <typename G, typename F, std::enable_if_t<util::is_box<G>::value, int> = 0>
    static inline void call_for_non_boxes(G const&, F &)
    {}

    Geometry const& m_geometry;
    BoxRings const& m_box_rings;
};

// TODO: Or just implement point_type<> for GeometryCollection
//   and enforce the same point_type used in the whole sequence in check().
template <typename Geometry, typename Tag = typename tag<Geometry>::type>
struct default_strategy
{
    using type = typename strategies::convex_hull::services::default_strategy
        <
            Geometry
        >::type;
};

template <typename Geometry>
struct default_strategy<Geometry, geometry_collection_tag>
    : default_strategy<typename detail::first_geometry_type<Geometry>::type>
{};

// Utilities for output GC and DG
template <typename G1, typename G2>
struct output_polygonal_less
{
    template <typename G>
    using priority = std::integral_constant
        <
            int,
            (util::is_ring<G>::value ? 0 :
             util::is_polygon<G>::value ? 1 :
             util::is_multi_polygon<G>::value ? 2 : 3)
        >;

    static const bool value = priority<G1>::value < priority<G2>::value;
};

template <typename G1, typename G2>
struct output_linear_less
{
    template <typename G>
    using priority = std::integral_constant
        <
            int,
            (util::is_segment<G>::value ? 0 :
             util::is_linestring<G>::value ? 1 :
             util::is_multi_linestring<G>::value ? 2 : 3)
        >;

    static const bool value = priority<G1>::value < priority<G2>::value;
};

template <typename G1, typename G2>
struct output_pointlike_less
{
    template <typename G>
    using priority = std::integral_constant
        <
            int,
            (util::is_point<G>::value ? 0 :
             util::is_multi_point<G>::value ? 1 : 2)
        >;

    static const bool value = priority<G1>::value < priority<G2>::value;
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct convex_hull
{
    template <typename OutputGeometry, typename Strategy>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategy const& strategy)
    {
        detail::convex_hull::input_geometry_proxy<Geometry> in_proxy(geometry);
        detail::convex_hull::graham_andrew
            <
                typename point_type<Geometry>::type
            >::apply(in_proxy, out, strategy);
    }
};

// A hull for boxes is trivial. Any strategy is (currently) skipped.
// TODO: This is not correct in spherical and geographic CS.
template <typename Box>
struct convex_hull<Box, box_tag>
{
    template <typename OutputGeometry, typename Strategy>
    static inline void apply(Box const& box,
                             OutputGeometry& out,
                             Strategy const& strategy)
    {
        static bool const Close
            = geometry::closure<OutputGeometry>::value == closed;
        static bool const Reverse
            = geometry::point_order<OutputGeometry>::value == counterclockwise;

        std::array<typename point_type<OutputGeometry>::type, 4> arr;
        // TODO: This assigns only 2d cooridnates!
        //       And it is also used in box_view<>!
        geometry::detail::assign_box_corners_oriented<Reverse>(box, arr);

        std::move(arr.begin(), arr.end(), range::back_inserter(out));
        if (BOOST_GEOMETRY_CONDITION(Close))
        {
            range::push_back(out, range::front(out));
        }
    }
};

template <typename GeometryCollection>
struct convex_hull<GeometryCollection, geometry_collection_tag>
{
    template <typename OutputGeometry, typename Strategy>
    static inline void apply(GeometryCollection const& geometry,
                             OutputGeometry& out,
                             Strategy const& strategy)
    {
        // Assuming that single point_type is used by the GeometryCollection
        using subgeometry_type = typename detail::first_geometry_type<GeometryCollection>::type;
        using point_type = typename geometry::point_type<subgeometry_type>::type;
        using ring_type = model::ring<point_type, true, false>;

        // Calculate box rings once
        std::vector<ring_type> box_rings;
        detail::visit_breadth_first([&](auto const& g)
        {
            convex_hull::add_ring_for_box(box_rings, g, strategy);
            return true;
        }, geometry);

        detail::convex_hull::input_geometry_collection_proxy
            <
                GeometryCollection, std::vector<ring_type>
            > in_proxy(geometry, box_rings);

        detail::convex_hull::graham_andrew
            <
                point_type
            >::apply(in_proxy, out, strategy);
    }

private:
    template
    <
        typename Ring, typename SubGeometry, typename Strategy,
        std::enable_if_t<util::is_box<SubGeometry>::value, int> = 0
    >
    static inline void add_ring_for_box(std::vector<Ring> & rings, SubGeometry const& box,
                                        Strategy const& strategy)
    {
        Ring ring;
        convex_hull<SubGeometry>::apply(box, ring, strategy);
        rings.push_back(std::move(ring));
    }
    template
    <
        typename Ring, typename SubGeometry, typename Strategy,
        std::enable_if_t<! util::is_box<SubGeometry>::value, int> = 0
    >
    static inline void add_ring_for_box(std::vector<Ring> & , SubGeometry const& ,
                                        Strategy const& )
    {}
};

template <typename OutputGeometry, typename Tag = typename tag<OutputGeometry>::type>
struct convex_hull_out
{
    BOOST_GEOMETRY_STATIC_ASSERT_FALSE("This OutputGeometry is not supported.", OutputGeometry, Tag);
};

template <typename OutputGeometry>
struct convex_hull_out<OutputGeometry, ring_tag>
{
    template <typename Geometry, typename Strategies>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategies const& strategies)
    {
        dispatch::convex_hull<Geometry>::apply(geometry, out, strategies);
    }
};

template <typename OutputGeometry>
struct convex_hull_out<OutputGeometry, polygon_tag>
{
    template <typename Geometry, typename Strategies>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategies const& strategies)
    {
        auto&& ring = exterior_ring(out);
        dispatch::convex_hull<Geometry>::apply(geometry, ring, strategies);
    }
};

template <typename OutputGeometry>
struct convex_hull_out<OutputGeometry, multi_polygon_tag>
{
    template <typename Geometry, typename Strategies>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategies const& strategies)
    {
        typename boost::range_value<OutputGeometry>::type polygon;
        auto&& ring = exterior_ring(polygon);
        dispatch::convex_hull<Geometry>::apply(geometry, ring, strategies);
        // Empty input is checked so the output shouldn't be empty
        range::push_back(out, std::move(polygon));
    }
};

template <typename OutputGeometry>
struct convex_hull_out<OutputGeometry, geometry_collection_tag>
{
    using polygonal_t = typename util::sequence_min_element
        <
            typename traits::geometry_types<OutputGeometry>::type,
            detail::convex_hull::output_polygonal_less
        >::type;
    using linear_t = typename util::sequence_min_element
        <
            typename traits::geometry_types<OutputGeometry>::type,
            detail::convex_hull::output_linear_less
        >::type;
    using pointlike_t = typename util::sequence_min_element
        <
            typename traits::geometry_types<OutputGeometry>::type,
            detail::convex_hull::output_pointlike_less
        >::type;

    // select_element may define different kind of geometry than the one that is desired
    BOOST_GEOMETRY_STATIC_ASSERT(util::is_polygonal<polygonal_t>::value,
        "It must be possible to store polygonal geometry in OutputGeometry.", polygonal_t);
    BOOST_GEOMETRY_STATIC_ASSERT(util::is_linear<linear_t>::value,
        "It must be possible to store linear geometry in OutputGeometry.", linear_t);
    BOOST_GEOMETRY_STATIC_ASSERT(util::is_pointlike<pointlike_t>::value,
        "It must be possible to store pointlike geometry in OutputGeometry.", pointlike_t);

    template <typename Geometry, typename Strategies>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategies const& strategies)
    {
        polygonal_t polygonal;
        convex_hull_out<polygonal_t>::apply(geometry, polygonal, strategies);
        // Empty input is checked so the output shouldn't be empty
        auto&& out_ring = ring(polygonal);

        if (boost::size(out_ring) == detail::minimum_ring_size<polygonal_t>::value)
        {
            using detail::equals::equals_point_point;
            if (equals_point_point(range::front(out_ring), range::at(out_ring, 1), strategies))
            {
                pointlike_t pointlike;
                move_to_pointlike(out_ring, pointlike);
                move_to_out(pointlike, out);
                return;
            }
            if (equals_point_point(range::front(out_ring), range::at(out_ring, 2), strategies))
            {
                linear_t linear;
                move_to_linear(out_ring, linear);
                move_to_out(linear, out);
                return;
            }
        }

        move_to_out(polygonal, out);
    }

private:
    template <typename Polygonal, util::enable_if_ring_t<Polygonal, int> = 0>
    static decltype(auto) ring(Polygonal const& polygonal)
    {
        return polygonal;
    }
    template <typename Polygonal, util::enable_if_polygon_t<Polygonal, int> = 0>
    static decltype(auto) ring(Polygonal const& polygonal)
    {
        return exterior_ring(polygonal);
    }
    template <typename Polygonal, util::enable_if_multi_polygon_t<Polygonal, int> = 0>
    static decltype(auto) ring(Polygonal const& polygonal)
    {
        return exterior_ring(range::front(polygonal));
    }

    template <typename Range, typename Linear, util::enable_if_segment_t<Linear, int> = 0>
    static void move_to_linear(Range & out_range, Linear & seg)
    {
        detail::assign_point_to_index<0>(range::front(out_range), seg);
        detail::assign_point_to_index<1>(range::at(out_range, 1), seg);
    }
    template <typename Range, typename Linear, util::enable_if_linestring_t<Linear, int> = 0>
    static void move_to_linear(Range & out_range, Linear & ls)
    {
        std::move(boost::begin(out_range), boost::begin(out_range) + 2, range::back_inserter(ls));
    }
    template <typename Range, typename Linear, util::enable_if_multi_linestring_t<Linear, int> = 0>
    static void move_to_linear(Range & out_range, Linear & mls)
    {
        typename boost::range_value<Linear>::type ls;
        std::move(boost::begin(out_range), boost::begin(out_range) + 2, range::back_inserter(ls));
        range::push_back(mls, std::move(ls));
    }

    template <typename Range, typename PointLike, util::enable_if_point_t<PointLike, int> = 0>
    static void move_to_pointlike(Range & out_range, PointLike & pt)
    {
        pt = range::front(out_range);
    }
    template <typename Range, typename PointLike, util::enable_if_multi_point_t<PointLike, int> = 0>
    static void move_to_pointlike(Range & out_range, PointLike & mpt)
    {
        range::push_back(mpt, std::move(range::front(out_range)));
    }

    template
    <
        typename Geometry, typename OutputGeometry_,
        util::enable_if_geometry_collection_t<OutputGeometry_, int> = 0
    >
    static void move_to_out(Geometry & g, OutputGeometry_ & out)
    {
        range::emplace_back(out, std::move(g));
    }
    template
    <
        typename Geometry, typename OutputGeometry_,
        util::enable_if_dynamic_geometry_t<OutputGeometry_, int> = 0
    >
    static void move_to_out(Geometry & g, OutputGeometry_ & out)
    {
        out = std::move(g);
    }
};

template <typename OutputGeometry>
struct convex_hull_out<OutputGeometry, dynamic_geometry_tag>
    : convex_hull_out<OutputGeometry, geometry_collection_tag>
{};

// For backward compatibility
template <typename OutputGeometry>
struct convex_hull_out<OutputGeometry, linestring_tag>
    : convex_hull_out<OutputGeometry, ring_tag>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace resolve_strategy {

template <typename Strategies>
struct convex_hull
{
    template <typename Geometry, typename OutputGeometry>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategies const& strategies)
    {
        dispatch::convex_hull_out<OutputGeometry>::apply(geometry, out, strategies);
    }
};

template <>
struct convex_hull<default_strategy>
{
    template <typename Geometry, typename OutputGeometry>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             default_strategy const&)
    {
        using strategy_type = typename detail::convex_hull::default_strategy
            <
                Geometry
            >::type;

        dispatch::convex_hull_out<OutputGeometry>::apply(geometry, out, strategy_type());
    }
};

} // namespace resolve_strategy

namespace resolve_dynamic {

template <typename Geometry, typename Tag = typename tag<Geometry>::type>
struct convex_hull
{
    template <typename OutputGeometry, typename Strategy>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategy const& strategy)
    {
        concepts::check_concepts_and_equal_dimensions<
            const Geometry,
            OutputGeometry
        >();

        resolve_strategy::convex_hull<Strategy>::apply(geometry, out, strategy);
    }
};

template <typename Geometry>
struct convex_hull<Geometry, dynamic_geometry_tag>
{
    template <typename OutputGeometry, typename Strategy>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategy const& strategy)
    {
        traits::visit<Geometry>::apply([&](auto const& g)
        {
            convex_hull<util::remove_cref_t<decltype(g)>>::apply(g, out, strategy);
        }, geometry);
    }
};

} // namespace resolve_dynamic

/*!
\brief \brief_calc{convex hull} \brief_strategy
\ingroup convex_hull
\details \details_calc{convex_hull,convex hull} \brief_strategy.
\tparam Geometry the input geometry type
\tparam OutputGeometry the output geometry type
\tparam Strategy the strategy type
\param geometry \param_geometry,  input geometry
\param out \param_geometry \param_set{convex hull}
\param strategy \param_strategy{area}

\qbk{distinguish,with strategy}

\qbk{[include reference/algorithms/convex_hull.qbk]}
 */
template<typename Geometry, typename OutputGeometry, typename Strategy>
inline void convex_hull(Geometry const& geometry, OutputGeometry& out, Strategy const& strategy)
{
    if (geometry::is_empty(geometry))
    {
        // Leave output empty
        return;
    }

    resolve_dynamic::convex_hull<Geometry>::apply(geometry, out, strategy);
}

/*!
\brief \brief_calc{convex hull}
\ingroup convex_hull
\details \details_calc{convex_hull,convex hull}.
\tparam Geometry the input geometry type
\tparam OutputGeometry the output geometry type
\param geometry \param_geometry,  input geometry
\param hull \param_geometry \param_set{convex hull}

\qbk{[include reference/algorithms/convex_hull.qbk]}
 */
template<typename Geometry, typename OutputGeometry>
inline void convex_hull(Geometry const& geometry, OutputGeometry& hull)
{
    geometry::convex_hull(geometry, hull, default_strategy());
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_CONVEX_HULL_INTERFACE_HPP