//#####################################################################
// Copyright 2013.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
#include <Solids/Solids_Evolution/BACKWARD_EULER_MINIMIZATION_SYSTEM.h>
#include <Rigids/Rigid_Bodies/RIGID_BODY.h>
#include <Rigids/Rigid_Bodies/RIGID_BODY_COLLECTION.h>
#include <Deformables/Bindings/BINDING_LIST.h>
#include <Deformables/Deformable_Objects/DEFORMABLE_BODY_COLLECTION.h>
#include <Deformables/Particles/DEFORMABLE_PARTICLES.h>
#include <Solids/Forces_And_Torques/EXAMPLE_FORCES_AND_VELOCITIES.h>
#include <Solids/Solids/SOLID_BODY_COLLECTION.h>
#ifdef USE_OPENMP
#include <omp.h>
#endif
namespace PhysBAM{
//#####################################################################
// Constructor
//#####################################################################
template<class TV> BACKWARD_EULER_MINIMIZATION_SYSTEM<TV>::
BACKWARD_EULER_MINIMIZATION_SYSTEM(SOLID_BODY_COLLECTION<TV>& solid_body_collection,EXAMPLE_FORCES_AND_VELOCITIES<TV>* example_forces_and_velocities)
    :KRYLOV_SYSTEM_BASE<T>(false,false),solid_body_collection(solid_body_collection),dt(0),time(0),use_l_inf_norm(false),tmp(0),forced_collisions(),example_forces_and_velocities(example_forces_and_velocities)
{
}
//#####################################################################
// Destructor
//#####################################################################
template<class TV> BACKWARD_EULER_MINIMIZATION_SYSTEM<TV>::
~BACKWARD_EULER_MINIMIZATION_SYSTEM()
{
}
//#####################################################################
// Function Multiply
//#####################################################################
template<class TV> void BACKWARD_EULER_MINIMIZATION_SYSTEM<TV>::
Multiply(const KRYLOV_VECTOR_BASE<T>& BV,KRYLOV_VECTOR_BASE<T>& BF,bool transpose) const
{
    const GENERALIZED_VELOCITY<TV>& V=debug_cast<const GENERALIZED_VELOCITY<TV>&>(BV);
    GENERALIZED_VELOCITY<TV>& F=debug_cast<GENERALIZED_VELOCITY<TV>&>(BF);
    GENERALIZED_VELOCITY<TV>& t=debug_cast<GENERALIZED_VELOCITY<TV>&>(*tmp);
    DEFORMABLE_PARTICLES<TV>& particles=solid_body_collection.deformable_body_collection.particles;
    RIGID_BODY_PARTICLES<TV>& rigid_body_particles=solid_body_collection.rigid_body_collection.rigid_body_particles;

    t=V;
    solid_body_collection.deformable_body_collection.binding_list.Clamp_Particles_To_Embedded_Velocities(t.V.array);
    for(int i=0;i<collisions.m;i++){
        const COLLISION& c=collisions(i);
        t.V.array(c.p).Project_Orthogonal_To_Unit_Direction(c.n);}

    F.V.array.Fill(TV());
    F.rigid_V.array.Fill(TWIST<TV>());
    solid_body_collection.Add_Force_Derivatives(t,F,time,transpose);

#pragma omp parallel for
    for(int p=0;p<particles.number;p++) F.V.array(p)=particles.mass(p)*t.V.array(p)-dt*dt*F.V.array(p);

#pragma omp parallel for
    for(int p=0;p<rigid_body_particles.number;p++){
        RIGID_BODY<TV>& rigid_body=solid_body_collection.rigid_body_collection.Rigid_Body(p);
        if(rigid_body.Has_Infinite_Inertia()) continue;
        F.rigid_V.array(p)=rigid_body.Inertia_Times(t.rigid_V.array(p))-dt*dt*F.rigid_V.array(p);}

    for(int i=0;i<collisions.m;i++){
        const COLLISION& c=collisions(i);
        TV& v=F.V.array(c.p),tt=V.V.array(c.p);
        v-=c.n*c.n.Dot(v)+dt*c.n_dE*(c.H*tt)+dt*c.n.Dot(tt)*c.H_dE+dt*c.H_dE.Dot(tt)*c.n;}

    solid_body_collection.deformable_body_collection.binding_list.Distribute_Force_To_Parents(F.V.array);
    solid_body_collection.deformable_body_collection.binding_list.Clear_Hard_Bound_Particles(F.V.array);
}
//#####################################################################
// Function Inner_Product
//#####################################################################
template<class TV> auto BACKWARD_EULER_MINIMIZATION_SYSTEM<TV>::
Inner_Product(const KRYLOV_VECTOR_BASE<T>& BV0,const KRYLOV_VECTOR_BASE<T>& BV1) const -> D
{
    const GENERALIZED_VELOCITY<TV>& V0=debug_cast<const GENERALIZED_VELOCITY<TV>&>(BV0),&V1=debug_cast<const GENERALIZED_VELOCITY<TV>&>(BV1);
    return V0.V.Dot_Double_Precision(V1.V)+V0.rigid_V.Dot_Double_Precision(V1.rigid_V);
}
//#####################################################################
// Function Inner_Product_Operator
//#####################################################################
template<class TV> void BACKWARD_EULER_MINIMIZATION_SYSTEM<TV>::
Inner_Product_Operator(KRYLOV_VECTOR_BASE<T>& x) const
{
}
//#####################################################################
// Function Convergence_Norm
//#####################################################################
template<class TV> typename TV::SCALAR BACKWARD_EULER_MINIMIZATION_SYSTEM<TV>::
Convergence_Norm(const KRYLOV_VECTOR_BASE<T>& BR) const
{
    if(use_l_inf_norm){
        T l_inf=0;
        for(int i=0;i<BR.Raw_Size();i++)
            l_inf=maxabs(l_inf,BR.Raw_Get(i));
        return l_inf;}
    return sqrt(Inner_Product(BR,BR));
}
//#####################################################################
// Function Project
//#####################################################################
template<class TV> void BACKWARD_EULER_MINIMIZATION_SYSTEM<TV>::
Project(KRYLOV_VECTOR_BASE<T>& BV) const
{
    GENERALIZED_VELOCITY<TV>& V=debug_cast<GENERALIZED_VELOCITY<TV>&>(BV);
    if(example_forces_and_velocities)
        if(example_forces_and_velocities->zero_out_enslaved_velocity_nodes)
            example_forces_and_velocities->zero_out_enslaved_velocity_nodes(V.V.array,time);
}
//#####################################################################
// Function Project_Nullspace
//#####################################################################
template<class TV> void BACKWARD_EULER_MINIMIZATION_SYSTEM<TV>::
Project_Nullspace(KRYLOV_VECTOR_BASE<T>& V) const
{
}
//#####################################################################
// Function Apply_Preconditioner
//#####################################################################
template<class TV> void BACKWARD_EULER_MINIMIZATION_SYSTEM<TV>::
Apply_Preconditioner(const KRYLOV_VECTOR_BASE<T>& V,KRYLOV_VECTOR_BASE<T>& R) const
{
}
//#####################################################################
// Function Set_Boundary_Conditions
//#####################################################################
template<class TV> void BACKWARD_EULER_MINIMIZATION_SYSTEM<TV>::
Set_Boundary_Conditions(KRYLOV_VECTOR_BASE<T>& V) const
{
}
template class BACKWARD_EULER_MINIMIZATION_SYSTEM<VECTOR<float,1> >;
template class BACKWARD_EULER_MINIMIZATION_SYSTEM<VECTOR<float,2> >;
template class BACKWARD_EULER_MINIMIZATION_SYSTEM<VECTOR<float,3> >;
template class BACKWARD_EULER_MINIMIZATION_SYSTEM<VECTOR<double,1> >;
template class BACKWARD_EULER_MINIMIZATION_SYSTEM<VECTOR<double,2> >;
template class BACKWARD_EULER_MINIMIZATION_SYSTEM<VECTOR<double,3> >;
}
