//#####################################################################
// Copyright 2003-2009, Zhaosheng Bao, Ron Fedkiw, Geoffrey Irving, Sergey Levine, Igor Neverov, Craig Schroeder, Andrew Selle, Tamar Shinar, Eftychios Sifakis, Jonathan Su.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class DEFORMABLE_BODY_COLLECTION
//#####################################################################
#include <Deformables/Deformable_Objects/DEFORMABLE_BODY_COLLECTION.h>
#include <Core/Data_Structures/SPARSE_UNION_FIND.h>
#include <Core/Diff_Tools/Diff_Test.h>
#include <Core/Log/LOG.h>
#include <Core/Log/SCOPE.h>
#include <Core/Math_Tools/Robust_Arithmetic.h>
#include <Core/Meshes/MESH_OBJECT.h>
#include <Core/Random_Numbers/RANDOM_NUMBERS.h>
#include <Core/Read_Write/FILE_UTILITIES.h>
#include <Core/Utilities/VIEWER_DIR.h>
#include <Core/Viewer/VIEWER_OUTPUT.h>
#include <Rigids/Collisions/COLLISION_BODY_COLLECTION.h>
#include <Deformables/Bindings/SOFT_BINDINGS.h>
#include <Deformables/Collisions_And_Interactions/DEFORMABLE_OBJECT_COLLISIONS.h>
#include <Deformables/Collisions_And_Interactions/TRIANGLE_COLLISION_PARAMETERS.h>
#include <Deformables/Collisions_And_Interactions/TRIANGLE_COLLISIONS.h>
#include <Deformables/Collisions_And_Interactions/TRIANGLE_REPULSIONS.h>
#include <Deformables/Forces/COLLISION_PENALTY_FORCES.h>
#include <Deformables/Forces/DEFORMABLES_EXAMPLE_FORCES_AND_VELOCITIES.h>
#include <Deformables/Forces/DEFORMABLES_FORCES.h>
using namespace PhysBAM;
//#####################################################################
// Constructor
//#####################################################################
template<class TV> DEFORMABLE_BODY_COLLECTION<TV>::
DEFORMABLE_BODY_COLLECTION(DEFORMABLE_PARTICLES<TV>* particles_input,COLLISION_BODY_COLLECTION<TV>* collision_body_list)
    :particles(particles_input?*particles_input:*new DEFORMABLE_PARTICLES<TV>),simulate(true),
    binding_list(*new BINDING_LIST<TV>(*this)),soft_bindings(*new SOFT_BINDINGS<TV>(binding_list)),implicit_damping(true),
    print_diagnostics(false),print_residuals(false),print_energy(false),iterations_used_diagnostic(0),
    triangle_repulsions_and_collisions_geometry(*new TRIANGLE_REPULSIONS_AND_COLLISIONS_GEOMETRY<TV>(*this)),
    triangle_repulsions(*new TRIANGLE_REPULSIONS<TV>(triangle_repulsions_and_collisions_geometry)),
    triangle_collisions(*new TRIANGLE_COLLISIONS<TV>(triangle_repulsions_and_collisions_geometry,triangle_repulsions.repulsion_thickness)),
    collisions(*new DEFORMABLE_OBJECT_COLLISIONS<TV>(particles,*this,structures,collision_body_list?*collision_body_list:*new COLLISION_BODY_COLLECTION<TV>)),
    use_embedded_collisions(false),use_nonembedded_self_collision(false),
    own_particles(!particles_input),own_collision_body_collection(!collision_body_list)
{
    particles.Store_Velocity();
    particles.Store_Mass();
}
//#####################################################################
// Destructor
//#####################################################################
template<class TV> DEFORMABLE_BODY_COLLECTION<TV>::
~DEFORMABLE_BODY_COLLECTION()
{
    deformables_forces.Delete_Pointers_And_Clean_Memory();
    structures.Delete_Pointers_And_Clean_Memory();
    delete &soft_bindings;
    delete &binding_list;
    if(own_collision_body_collection) delete &collisions.collision_body_list;
    delete &collisions;
    if(own_particles) delete &particles;
    delete &triangle_repulsions;
    delete &triangle_collisions;
    delete &triangle_repulsions_and_collisions_geometry;
}
//#####################################################################
// Function Initialize_Triangle_Collisions
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Initialize(TRIANGLE_COLLISION_PARAMETERS<TV>& triangle_collision_parameters)
{
    if(!triangle_collision_parameters.perform_self_collision || 
        (!triangle_repulsions_and_collisions_geometry.structures.m && !triangle_collision_parameters.initialize_collisions_without_objects))
        return;
    LOG::SCOPE scope("Initializing triangle collisions","Initializing triangle collisions");

    triangle_repulsions_and_collisions_geometry.Initialize(triangle_collision_parameters);
    if(triangle_collision_parameters.check_initial_mesh_for_self_intersection && triangle_repulsions_and_collisions_geometry.Check_For_Intersection(true)) PHYSBAM_FATAL_ERROR();
    triangle_repulsions.Initialize(triangle_collision_parameters);
    triangle_collisions.Initialize(triangle_collision_parameters);
}
//#####################################################################
// Function Update_Collision_Penalty_Forces_And_Derivatives
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Update_Collision_Penalty_Forces_And_Derivatives()
{
    for(int i=0;i<collision_penalty_forces.m;i++) collision_penalty_forces(i)->Update_Forces_And_Derivatives();
}
//#####################################################################
// Function Read_Dynamic_Variables
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Read_Dynamic_Variables(const VIEWER_DIR& viewer_dir)
{
    Read_From_File(viewer_dir.Frame_File("deformable_object_particles"),particles);
    // if number==0, the particles format doesn't remember the set of attributes, so the following line makes restarts look more exact
    particles.Store_Velocity();
    // read in bindings
    std::string binding_state_name=viewer_dir.Frame_File("bindings");
    if(File_Exists(binding_state_name))
        Read_From_File(binding_state_name,binding_list);
    else binding_list.Clean_Memory();
    std::string soft_binding_state_name=viewer_dir.Frame_File("soft_bindings");
    if(File_Exists(soft_binding_state_name))
        Read_From_File(soft_binding_state_name,soft_bindings);
    else soft_bindings.Clean_Memory();
    // recompute auxiliary mass data (this data is destroyed when particles and read, and mass might have changed)
    soft_bindings.Set_Mass_From_Effective_Mass();
}
//#####################################################################
// Function Write_Dynamic_Variables
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Write_Dynamic_Variables(const STREAM_TYPE stream_type,const VIEWER_DIR& viewer_dir) const
{
    if(binding_list.bindings.m>0)
        Write_To_File(stream_type,viewer_dir.Frame_File("bindings"),binding_list);
    if(soft_bindings.bindings.m>0)
        Write_To_File(stream_type,viewer_dir.Frame_File("soft_bindings"),soft_bindings);
}
//#####################################################################
// Function Read
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Read(const VIEWER_DIR& viewer_dir,const bool read_static_variables)
{
    particles.Store_Mass();
    if(read_static_variables) Read_Static_Variables(viewer_dir);
    Read_Dynamic_Variables(viewer_dir);
}
//#####################################################################
// Function Write
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Write(const STREAM_TYPE stream_type,const VIEWER_DIR& viewer_dir,const bool static_variables_every_frame,const bool write_from_every_process) const
{
    if(static_variables_every_frame || viewer_dir.First_Frame())
        Write_Static_Variables(stream_type,viewer_dir,static_variables_every_frame);
    Write_Dynamic_Variables(stream_type,viewer_dir);
}
//#####################################################################
// Function Register_Output_Files
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Register_Output_Files(VIEWER_OUTPUT& viewer_output)
{
    viewer_output.Add([this,&viewer_output](){Write(viewer_output.stream_type,viewer_output.viewer_dir,false,true);},false);
    auto particles_name="deformable_object_particles";
    viewer_output.Add(particles_name,particles,false,{.draw=false});

    for(int i=0;i<structures.m;i++)
    {
        auto name=LOG::sprintf("structure-%d",i);
        auto s=structures(i);
        if(auto* o=dynamic_cast<MESH_OBJECT<TV,1>*>(s))
        {
            viewer_output.Write(name+".mesh",o->mesh,true);
            viewer_output.template Add_Split_Mesh<TV,1>(name,name+".mesh",particles_name,{});
        }
        else if(auto* o=dynamic_cast<MESH_OBJECT<TV,std::min(TV::m,2)>*>(s))
        {
            viewer_output.Write(name+".mesh",o->mesh,true);
            viewer_output.template Add_Split_Mesh<TV,std::min(TV::m,2)>(name,name+".mesh",particles_name,{});
        }
        else if(auto* o=dynamic_cast<MESH_OBJECT<TV,std::min(TV::m,3)>*>(s))
        {
            viewer_output.Write(name+".mesh",o->mesh,true);
            viewer_output.template Add_Split_Mesh<TV,std::min(TV::m,3)>(name,name+".mesh",particles_name,{});
        }
    }
}
//#####################################################################
// Function Update_Simulated_Particles
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Update_Simulated_Particles()
{
    int particles_number=particles.Size();

    ARRAY<bool> particle_is_simulated(particles_number);for(int i=0;i<particles_number;i++) particle_is_simulated(i)=true;
    particle_is_simulated.Subset(particles.deletion_list).Fill(false);

    simulated_particles.Remove_All();
    dynamic_particles.Remove_All();

    for(int p=0;p<particles_number;p++) if(particle_is_simulated(p)) simulated_particles.Append(p);
    binding_list.Clear_Hard_Bound_Particles(particle_is_simulated); // Prevent hard bound particles from being added to dynamic_particles
    for(int p=0;p<particles_number;p++) if(particle_is_simulated(p)) dynamic_particles.Append(p);

    ARRAY<bool> particle_is_simulated_actual(particles_number);
    INDIRECT_ARRAY<ARRAY<bool>,ARRAY<int>&> simulated_subset=particle_is_simulated_actual.Subset(simulated_particles);
    simulated_subset.Fill(true);
    for(int i=0;i<deformables_forces.m;i++) deformables_forces(i)->Update_Mpi(particle_is_simulated_actual);

    collisions.Update_Simulated_Particles();
}
//#####################################################################
// Function Update_CFL
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Update_CFL()
{
    bool cfl_valid=true;
    if(deformables_forces.m){
        for(int i=0;i<deformables_forces.m;i++){if(!deformables_forces(i)->CFL_Valid()){cfl_valid=false;break;}}}
    else cfl_valid=false;
    if(!cfl_valid){
        frequency.Resize(particles.Size(),no_init);
        frequency.Subset(simulated_particles).Fill(T_FREQUENCY_DEFORMABLE());

        for(int i=0;i<deformables_forces.m;i++){deformables_forces(i)->Initialize_CFL(frequency);deformables_forces(i)->Validate_CFL();}
        cfl_elastic=FLT_MAX;cfl_damping=FLT_MAX;
        for(int i=0;i<simulated_particles.m;i++){int p=simulated_particles(i);
            cfl_elastic=min(cfl_elastic,Robust_Inverse(sqrt(frequency(p).elastic_squared)));
            cfl_damping=min(cfl_damping,Robust_Inverse(frequency(p).damping));}}
}
//#####################################################################
// Function CFL
//#####################################################################
template<class TV> typename TV::SCALAR DEFORMABLE_BODY_COLLECTION<TV>::
CFL(const bool verbose)
{
    T dt_elastic_and_damping=CFL_Elastic_And_Damping(),dt_strain_rate=CFL_Strain_Rate();
    if(verbose){
        LOG::cout<<"dt_elastic_and_damping = "<<dt_elastic_and_damping<<std::endl;
        LOG::cout<<"dt_strain_rate = "<<dt_strain_rate<<std::endl;
        LOG::cout<<"min = "<<min(dt_elastic_and_damping,dt_strain_rate)<<std::endl;}
    return min(dt_elastic_and_damping,dt_strain_rate);
}
//#####################################################################
// Function CFL_Elastic_And_Damping
//#####################################################################
template<class TV> typename TV::SCALAR DEFORMABLE_BODY_COLLECTION<TV>::
CFL_Elastic_And_Damping()
{
    T dt_elastic=CFL_Elastic();
    T dt_damping=FLT_MAX;if(!implicit_damping) dt_damping=CFL_Damping();
    T one_over_dt_full=1/dt_elastic+1/dt_damping;
    return Robust_Divide((T)1,one_over_dt_full);
}
//#####################################################################
// Function CFL_Elastic
//#####################################################################
template<class TV> typename TV::SCALAR DEFORMABLE_BODY_COLLECTION<TV>::
CFL_Elastic()
{
    Update_CFL();
    return cfl_elastic;
}
//#####################################################################
// Function CFL_Damping
//#####################################################################
template<class TV> typename TV::SCALAR DEFORMABLE_BODY_COLLECTION<TV>::
CFL_Damping()
{
    Update_CFL();
    return cfl_damping;
}
//#####################################################################
// Function CFL_Strain_Rate
//#####################################################################
template<class TV> typename TV::SCALAR DEFORMABLE_BODY_COLLECTION<TV>::
CFL_Strain_Rate()
{
    T dt_strain=FLT_MAX;
    for(int k=0;k<deformables_forces.m;k++) if(deformables_forces(k)->limit_time_step_by_strain_rate) dt_strain=min(dt_strain,deformables_forces(k)->CFL_Strain_Rate()); // otherwise not included
    return dt_strain;
}
//#####################################################################
// Function Update_Position_Based_State
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Update_Position_Based_State(const T time,const bool is_position_update,const bool update_hessian)
{
    for(int k=0;k<deformables_forces.m;k++)
        deformables_forces(k)->Update_Position_Based_State(time,is_position_update,update_hessian);
}
//#####################################################################
// Function Add_Forces
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Add_Forces(ARRAY_VIEW<TV> F_full,const T time) const
{
    assert(F_full.Size()==particles.Size());
    for(int k=0;k<deformables_forces.m;k++)
        deformables_forces(k)->Add_Forces(F_full,time);
}
//#####################################################################
// Function Add_Velocity_Dependent_Forces
//#####################################################################
// can depend on position too
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Add_Velocity_Dependent_Forces(ARRAY_VIEW<const TV> V_full,ARRAY_VIEW<TV> F_full,const T time) const
{
    assert(F_full.Size()==particles.Size());
    for(int k=0;k<deformables_forces.m;k++)
        deformables_forces(k)->Add_Velocity_Dependent_Forces(V_full,F_full,time);
}
//#####################################################################
// Function Implicit_Velocity_Independent_Forces
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Add_Force_Derivatives(ARRAY_VIEW<const TV> V_full,ARRAY_VIEW<TV> F_full,const T time,bool transpose) const
{
    assert(V_full.Size()==particles.Size() && F_full.Size()==particles.Size());
    for(int k=0;k<deformables_forces.m;k++)
        deformables_forces(k)->Add_Force_Derivatives(V_full,F_full,time,transpose);
}
//#####################################################################
// Function Add_Force
//#####################################################################
template<class TV> int DEFORMABLE_BODY_COLLECTION<TV>::
Add_Force(DEFORMABLES_FORCES<TV>* force)
{
    deformables_forces.Append(force);
    force->Set_CFL_Number(cfl_number);
    return deformables_forces.m;
}
//#####################################################################
// Function Set_CFL_Number
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Set_CFL_Number(const T cfl_number_input)
{
    cfl_number=cfl_number_input;
    for(int i=0;i<deformables_forces.m;i++) deformables_forces(i)->Set_CFL_Number(cfl_number_input);
}
//#####################################################################
// Function Test_Energy
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Test_Forces(const T time)
{
    for(auto f:deformables_forces) f->Test_Diff(time);
}
//#####################################################################
// Function Read_Static_Variables
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Read_Static_Variables(const VIEWER_DIR& viewer_dir)
{
    std::string file=viewer_dir.Frame_File("deformable_object_structures");
    if(!File_Exists(file))
        file=viewer_dir.Common_File("deformable_object_structures");
    FILE_ISTREAM input(file);
    Read_Structure_Array(input,structures);
}
//#####################################################################
// Function Write_Static_Variables
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Write_Static_Variables(const STREAM_TYPE stream_type,const VIEWER_DIR& viewer_dir,bool static_variables_every_frame) const
{
    std::string file=static_variables_every_frame?
        viewer_dir.Frame_File("deformable_object_structures"):
        viewer_dir.Common_File("deformable_object_structures");
    FILE_OSTREAM output(stream_type,file);
    Write_Structure_Array(output,(const STRUCTURE<TV>**)structures.Get_Array_Pointer(),structures.m);
}
//#####################################################################
// Function Compute_Auxiliary_Attributes
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Compute_Auxiliary_Attributes()
{
    ARRAY<int> a(soft_bindings.particles.Size());
    Set_Counting(a);
    Compute_Auxiliary_Attributes(a,false);
}
//#####################################################################
// Function Compute_Auxiliary_Attributes
//#####################################################################
template<class TV> void DEFORMABLE_BODY_COLLECTION<TV>::
Compute_Auxiliary_Attributes(const ARRAY<int>& indices,const bool copy_existing_elements)
{
    if(particles.template Get_Array<T>("one_over_mass"))
        particles.Remove_Array_Using_Index(particles.Get_Attribute_Index("one_over_mass"));
    particles.template Add_Array<T>("one_over_mass",&particles.one_over_mass);
    if(particles.template Get_Array<T>("effective_mass"))
        particles.Remove_Array_Using_Index(particles.Get_Attribute_Index("effective_mass"));
    particles.template Add_Array<T>("effective_mass",&particles.effective_mass);
    if(particles.template Get_Array<T>("one_over_effective_mass"))
        particles.Remove_Array_Using_Index(particles.Get_Attribute_Index("one_over_effective_mass"));
    particles.template Add_Array<T>("one_over_effective_mass",&particles.one_over_effective_mass);
    for(int i=0;i<indices.Size();i++){int p=indices(i);
        particles.one_over_mass(p)=Robust_Inverse(particles.mass(p));}
    for(int i=0;i<indices.Size();i++){int p=indices(i);
        particles.one_over_effective_mass(p)=soft_bindings.One_Over_Effective_Mass(p);
        particles.effective_mass(p)=Robust_Inverse(particles.one_over_effective_mass(p));}
}
//#####################################################################
namespace PhysBAM{
template class DEFORMABLE_BODY_COLLECTION<VECTOR<float,1> >;
template class DEFORMABLE_BODY_COLLECTION<VECTOR<float,2> >;
template class DEFORMABLE_BODY_COLLECTION<VECTOR<float,3> >;
template class DEFORMABLE_BODY_COLLECTION<VECTOR<double,1> >;
template class DEFORMABLE_BODY_COLLECTION<VECTOR<double,2> >;
template class DEFORMABLE_BODY_COLLECTION<VECTOR<double,3> >;
}
