//#####################################################################
// Copyright 2006-2008, Zhaosheng Bao, Ron Fedkiw, Geoffrey Irving, Sergey Koltakov, Craig Schroeder, Andrew Selle, Craig Schroeder, Tamar Shinar, Eftychios Sifakis, Joseph Teran.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class BACKWARD_EULER_SYSTEM
//#####################################################################
#include <Solids/Solids_Evolution/BACKWARD_EULER_SYSTEM.h>
#include <Core/Grids/GRID.h>
#include <Rigids/Articulated_Rigid_Bodies/ARTICULATED_RIGID_BODY_2D.h>
#include <Rigids/Articulated_Rigid_Bodies/ARTICULATED_RIGID_BODY_3D.h>
#include <Rigids/Particles/RIGID_BODY_PARTICLES.h>
#include <Rigids/Rigid_Bodies/RIGID_BODY_COLLECTION.h>
#include <Rigids/Rigid_Bodies/RIGID_BODY_STATE.h>
#include <Rigids/Rigid_Body_Clusters/RIGID_BODY_CLUSTER_BINDINGS.h>
#include <Deformables/Bindings/BINDING_LIST.h>
#include <Deformables/Collisions_And_Interactions/DEFORMABLE_OBJECT_COLLISIONS.h>
#include <Deformables/Collisions_And_Interactions/TRIANGLE_REPULSIONS.h>
#include <Deformables/Deformable_Objects/DEFORMABLE_BODY_COLLECTION.h>
#include <Solids/Collisions/RIGID_DEFORMABLE_COLLISIONS.h>
#include <Solids/Solids/SOLID_BODY_COLLECTION.h>
#include <Solids/Solids/SOLIDS_PARAMETERS.h>
#include <Solids/Solids_Evolution/IMPLICIT_SOLVE_PARAMETERS.h>
#include <Solids/Solids_Evolution/NEWMARK_EVOLUTION.h>
#include <climits>
using namespace PhysBAM;
//#####################################################################
// Constructor
//#####################################################################
template<class TV> BACKWARD_EULER_SYSTEM<TV>::
BACKWARD_EULER_SYSTEM(SOLIDS_EVOLUTION<TV>* solids_evolution_input,SOLID_BODY_COLLECTION<TV>& solid_body_collection,const T dt,const T current_velocity_time,
    const T current_position_time,ARTICULATED_RIGID_BODY<TV>* arb_input,TRIANGLE_REPULSIONS<TV>* repulsions_input,const bool velocity_update_input,bool fully_implicit)
    :KRYLOV_SYSTEM_BASE<typename TV::SCALAR>(false,true),solids_evolution(solids_evolution_input),solid_body_collection(solid_body_collection),
    dt(dt),current_velocity_time(current_velocity_time),current_position_time(current_position_time),//mass(deformable_object),
    arb(arb_input),repulsions(repulsions_input),velocity_update(velocity_update_input),project_nullspace_frequency(INT_MAX),
    fully_implicit(fully_implicit),projection_data(solid_body_collection)
{
    if(repulsions) repulsions->Set_Collision_Pairs(projection_data.point_face_precomputed,projection_data.edge_edge_precomputed,projection_data.point_face_pairs,projection_data.edge_edge_pairs,(T)1);
    if(arb) arb->Initialize_Poststabilization_Projection();
    // TODO: compare enforcing constraints at n+1/2 and n+1
    //if(solids_evolution.solids_parameters.use_rigid_deformable_contact && solid_body_collection.deformable_object.collisions.collisions_on)
    //    solids_evolution.rigid_deformable_collisions->Initialize_All_Contact_Projections();
}
//#####################################################################
// Destructor
//#####################################################################
template<class TV> BACKWARD_EULER_SYSTEM<TV>::
~BACKWARD_EULER_SYSTEM()
{
}
//#####################################################################
// Function Force
//#####################################################################
template<class TV> void BACKWARD_EULER_SYSTEM<TV>::
Force(const VECTOR_T& V,VECTOR_T& F) const
{
    F.V.array.Subset(solid_body_collection.deformable_body_collection.simulated_particles).Fill(TV());
    F.rigid_V.array.Subset(solid_body_collection.rigid_body_collection.simulated_rigid_body_particles).Fill(TWIST<TV>());
    solid_body_collection.rigid_body_collection.rigid_body_cluster_bindings.Clamp_Particles_To_Embedded_Velocities(V.rigid_V.array);

    solid_body_collection.deformable_body_collection.binding_list.Clamp_Particles_To_Embedded_Velocities(V.V.array,V.rigid_V.array);
    if(fully_implicit){
        solid_body_collection.Add_Force_Derivatives(V,F,current_velocity_time+dt);
        F*=dt;}
    // we don't inherit velocity dependent forces to the drifted particles (contrary to the explicit velocity independent ones) because that would compromise symmetry
    solid_body_collection.Add_Velocity_Dependent_Forces(V,F,current_velocity_time+dt);
    solid_body_collection.deformable_body_collection.binding_list.Distribute_Force_To_Parents(F.V.array,F.rigid_V.array);
    solid_body_collection.rigid_body_collection.rigid_body_cluster_bindings.Distribute_Force_To_Parents(F.rigid_V.array);
}
//#####################################################################
// Function Multiply
//#####################################################################
template<class TV> void BACKWARD_EULER_SYSTEM<TV>::
Multiply(const KRYLOV_VECTOR_BASE<T>& BV,KRYLOV_VECTOR_BASE<T>& BF,bool transpose) const
{
    const VECTOR_T& V=debug_cast<const VECTOR_T&>(BV);VECTOR_T& F=debug_cast<VECTOR_T&>(BF);
    Force(V,F);
    for(int i=0;i<V.V.Size();i++) F.V(i)=V.V(i)-dt*projection_data.mass.one_over_mass(i)*F.V(i);
    for(int i=0;i<V.rigid_V.Size();i++) F.rigid_V(i)=V.rigid_V(i)-dt*(projection_data.mass.world_space_rigid_mass_inverse(i)*F.rigid_V(i));
}
//#####################################################################
// Function Set_Global_Boundary_Conditions
//#####################################################################
template<class TV> void BACKWARD_EULER_SYSTEM<TV>::
Set_Global_Boundary_Conditions(VECTOR_T& V,ARRAY<TV>& X_save,ARRAY<FRAME<TV> >& rigid_frame_save,
    ARRAY<TWIST<TV> >& rigid_velocity_save,ARRAY<typename TV::SPIN>& rigid_angular_momentum_save,ARRAY<TV>& V_save,bool test_system,bool print_matrix) const
{
    if(!solids_evolution) return;
    SOLIDS_PARAMETERS<TV>& solids_parameters=solids_evolution->solids_parameters;
    solids_evolution->Set_External_Velocities(V.V.array,current_velocity_time+dt);
    solids_evolution->kinematic_evolution.Set_External_Velocities(V.rigid_V.array,current_velocity_time+dt);
    // TODO: make Solve_Velocities_for_PD take rigid_V.array and call that instead
    if(arb){
        PHYSBAM_ASSERT(ARRAY_VIEW<TWIST<TV> >::Same_Array(V.rigid_V.array,solid_body_collection.rigid_body_collection.rigid_body_particles.twist));
        arb->Apply_Poststabilization(test_system,print_matrix); // Do not project out pd directions here
        if(arb->Has_Actuators() && arb->constrain_pd_directions){
            arb->Compute_Position_Based_State(dt,current_velocity_time);
            arb->Solve_Velocities_for_PD(current_velocity_time,dt,solids_parameters.implicit_solve_parameters.test_system,solids_parameters.implicit_solve_parameters.print_matrix);}}
    if(velocity_update){
        PHYSBAM_ASSERT(ARRAY_VIEW<TV>::Same_Array(V.V.array,solid_body_collection.deformable_body_collection.particles.V) && ARRAY_VIEW<TWIST<TV> >::Same_Array(V.rigid_V.array,solid_body_collection.rigid_body_collection.rigid_body_particles.twist));
        if(solids_parameters.use_post_cg_constraints){// TODO: may just want to call Apply_Constraints in this case too
            if(solids_evolution->solids_parameters.use_rigid_deformable_contact && solid_body_collection.deformable_body_collection.collisions.collisions_on)
                solids_evolution->rigid_deformable_collisions->Set_Collision_Velocities(V.V.array,V.rigid_V.array,X_save,rigid_frame_save,rigid_velocity_save,rigid_angular_momentum_save,V_save);
            if(repulsions) repulsions->Adjust_Velocity_For_Self_Repulsion_Using_History(dt,false,false);}}
}
//#####################################################################
// Function Project
//#####################################################################
template<class TV> void BACKWARD_EULER_SYSTEM<TV>::
Project(KRYLOV_VECTOR_BASE<T>& BV) const
{
    if(!solids_evolution) return;
    VECTOR_T& V=debug_cast<VECTOR_T&>(BV);
    // Applying the projections in this order is equivalent to repeating Zero_Out_Enslaved_Velocity_Nodes after Poststabilization_Projection, which is a (mass) symmetric projection.
    solids_evolution->Zero_Out_Enslaved_Velocity_Nodes(V.V.array,current_velocity_time+dt);
    solids_evolution->Zero_Out_Enslaved_Velocity_Nodes(V.rigid_V.array,current_velocity_time+dt);
    if(!velocity_update){if(arb) arb->Poststabilization_Projection(V.rigid_V.array,true);return;}
    for(int i=0;i<solids_evolution->solids_parameters.implicit_solve_parameters.cg_projection_iterations;i++){
        int middle_projection=1;
        if(solids_evolution->solids_parameters.use_rigid_deformable_contact && solid_body_collection.deformable_body_collection.collisions.collisions_on){
            solids_evolution->rigid_deformable_collisions->Project_Contact_Pairs(V.V.array,V.rigid_V.array);middle_projection=2;}
        // TODO: arb projections should also project out changes in pd controlled directions
        if(arb){arb->Poststabilization_Projection(V.rigid_V.array,true);middle_projection=3;}
        if(projection_data.point_face_precomputed.m || projection_data.edge_edge_precomputed.m){
            TRIANGLE_REPULSIONS<TV>::Project_All_Moving_Constraints(projection_data.point_face_precomputed,projection_data.edge_edge_precomputed,V.V.array);middle_projection=4;}
        if(arb && middle_projection!=3) arb->Poststabilization_Projection(V.rigid_V.array,true);
        if(solids_evolution->solids_parameters.use_rigid_deformable_contact && solid_body_collection.deformable_body_collection.collisions.collisions_on && middle_projection!=2)
            solids_evolution->rigid_deformable_collisions->Project_Contact_Pairs(V.V.array,V.rigid_V.array);
        if(middle_projection!=1){
            solids_evolution->Zero_Out_Enslaved_Velocity_Nodes(V.V.array,current_velocity_time+dt);
            solids_evolution->Zero_Out_Enslaved_Velocity_Nodes(V.rigid_V.array,current_velocity_time+dt);}}
}
//#####################################################################
// Function Inner_Product
//#####################################################################
template<class TV> auto BACKWARD_EULER_SYSTEM<TV>::
Inner_Product(const KRYLOV_VECTOR_BASE<T>& BV0,const KRYLOV_VECTOR_BASE<T>& BV1) const -> D
{
    const VECTOR_T& V0=debug_cast<const VECTOR_T&>(BV0),&V1=debug_cast<const VECTOR_T&>(BV1);
    D inner_product{};
    for(int i=0;i<V0.V.Size();i++)
        inner_product+=projection_data.mass.mass(i)*V0.V(i).Dot(V1.V(i));
    for(int i=0;i<V0.rigid_V.Size();i++)
        inner_product+=projection_data.mass.world_space_rigid_mass(i).Inner_Product(V0.rigid_V(i),V1.rigid_V(i));
    return inner_product;
}
//#####################################################################
// Function Inner_Product_Operator
//#####################################################################
template<class TV> void BACKWARD_EULER_SYSTEM<TV>::
Inner_Product_Operator(KRYLOV_VECTOR_BASE<T>& x) const
{
    PHYSBAM_NOT_IMPLEMENTED();
}
//#####################################################################
// Function Convergence_Norm
//#####################################################################
template<class TV> typename TV::SCALAR BACKWARD_EULER_SYSTEM<TV>::
Convergence_Norm(const KRYLOV_VECTOR_BASE<T>& BR) const
{
    const VECTOR_T& R=debug_cast<const VECTOR_T&>(BR);
    T convergence_norm_squared=sqr(R.V.Maximum_Magnitude());
    for(int p=0;p<R.rigid_V.Size();p++){
        const TWIST<TV>& twist=R.rigid_V(p);
        const RIGID_BODY_MASS<TV,true> &rigid_mass=projection_data.mass.world_space_rigid_mass(p),&rigid_mass_inverse=projection_data.mass.world_space_rigid_mass_inverse(p);
        convergence_norm_squared=max(convergence_norm_squared,
            twist.linear.Magnitude_Squared()+rigid_mass_inverse.mass*twist.angular.Dot(rigid_mass.inertia_tensor*twist.angular));}
    T convergence_norm=sqrt(convergence_norm_squared);
    return convergence_norm;
}
//#####################################################################
// Function Project_Nullspace
//#####################################################################
template<class TV> void BACKWARD_EULER_SYSTEM<TV>::
Project_Nullspace(KRYLOV_VECTOR_BASE<T>& V) const
{
    if(++projection_data.project_nullspace_counter>=project_nullspace_frequency){Project(V);projection_data.project_nullspace_counter=0;}
}
//#####################################################################
// Function Apply_Preconditioner
//#####################################################################
template<class TV> void BACKWARD_EULER_SYSTEM<TV>::
Apply_Preconditioner(const KRYLOV_VECTOR_BASE<T>& V,KRYLOV_VECTOR_BASE<T>& R) const
{
    R.Copy((T)1,V);
}
//#####################################################################
// Constructor
//#####################################################################
template<class TV> GENERALIZED_MASS<TV>::
GENERALIZED_MASS(SOLID_BODY_COLLECTION<TV>& solid_body_collection)
    :mass(solid_body_collection.deformable_body_collection.particles.mass,solid_body_collection.deformable_body_collection.dynamic_particles),
    one_over_mass(solid_body_collection.deformable_body_collection.particles.one_over_mass,solid_body_collection.deformable_body_collection.dynamic_particles),
    rigid_mass(solid_body_collection.rigid_body_collection.rigid_body_particles.mass,solid_body_collection.rigid_body_collection.dynamic_rigid_body_particles),
    rigid_inertia_tensor(solid_body_collection.rigid_body_collection.rigid_body_particles.inertia_tensor,solid_body_collection.rigid_body_collection.dynamic_rigid_body_particles),
    world_space_rigid_mass_inverse(world_space_rigid_mass_inverse_full,rigid_inertia_tensor.indices)
{
    Initialize_World_Space_Masses(solid_body_collection);
}
template<class TV> GENERALIZED_MASS<TV>::
~GENERALIZED_MASS()
{
}
//#####################################################################
// Function Initialize_World_Space_Masses
//#####################################################################
template<class TV> void GENERALIZED_MASS<TV>::
Initialize_World_Space_Masses(const SOLID_BODY_COLLECTION<TV>& solid_body_collection)
{
    world_space_rigid_mass.Resize(solid_body_collection.rigid_body_collection.dynamic_rigid_body_particles.m,no_init);
    world_space_rigid_mass_inverse_full.Resize(rigid_mass.array.m,no_init);

    for(int i=0;i<solid_body_collection.rigid_body_collection.dynamic_rigid_body_particles.m;i++){int p=solid_body_collection.rigid_body_collection.dynamic_rigid_body_particles(i);
        RIGID_BODY_MASS<TV> rm(rigid_mass.array(i),rigid_inertia_tensor.array(i));
        world_space_rigid_mass(i)=solid_body_collection.rigid_body_collection.State(p).World_Space_Rigid_Mass(rm);
        world_space_rigid_mass_inverse(i)=solid_body_collection.rigid_body_collection.State(i).World_Space_Rigid_Mass_Inverse(rm);}
}
//#####################################################################
// Function Inverse_Multiply
//#####################################################################
template<class TV> void GENERALIZED_MASS<TV>::
Inverse_Multiply(const GENERALIZED_VELOCITY<TV>& V,GENERALIZED_VELOCITY<TV>& F,bool include_static) const
{
    for(int i=0;i<F.V.Size();i++) F.V(i)=one_over_mass(i)*V.V(i);
    for(int i=0;i<F.rigid_V.Size();i++) F.rigid_V(i)=world_space_rigid_mass_inverse(i)*V.rigid_V(i);
    if(include_static) F.kinematic_and_static_rigid_V*=0;
}
//#####################################################################
// Function Multiply
//#####################################################################
template<class TV> void GENERALIZED_MASS<TV>::
Multiply(const GENERALIZED_VELOCITY<TV>& V,GENERALIZED_VELOCITY<TV>& F,bool include_static) const
{
    for(int i=0;i<F.V.Size();i++) F.V(i)=mass(i)*V.V(i);
    for(int i=0;i<F.rigid_V.Size();i++) F.rigid_V(i)=world_space_rigid_mass(i)*V.rigid_V(i);
    if(include_static) F.kinematic_and_static_rigid_V*=0;
}
//#####################################################################
namespace PhysBAM{
template class GENERALIZED_MASS<VECTOR<float,1> >;
template class GENERALIZED_MASS<VECTOR<float,2> >;
template class GENERALIZED_MASS<VECTOR<float,3> >;
template class BACKWARD_EULER_SYSTEM<VECTOR<float,1> >;
template class BACKWARD_EULER_SYSTEM<VECTOR<float,2> >;
template class BACKWARD_EULER_SYSTEM<VECTOR<float,3> >;
template class GENERALIZED_MASS<VECTOR<double,1> >;
template class GENERALIZED_MASS<VECTOR<double,2> >;
template class GENERALIZED_MASS<VECTOR<double,3> >;
template class BACKWARD_EULER_SYSTEM<VECTOR<double,1> >;
template class BACKWARD_EULER_SYSTEM<VECTOR<double,2> >;
template class BACKWARD_EULER_SYSTEM<VECTOR<double,3> >;
}
