//#####################################################################
// Copyright 2003-2007, Zhaosheng Bao, Ronald Fedkiw, Geoffrey Irving, Neil Molino, Andrew Selle, 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.
//#####################################################################
#include <Deformables/Forces/LINEAR_ALTITUDE_SPRINGS_S3D.h>
#include <Core/Basic_Geometry/SEGMENT_3D.h>
#include <Core/Log/LOG.h>
#include <Core/Math_Tools/Robust_Arithmetic.h>
#include <Deformables/Particles/DEFORMABLE_PARTICLES.h>
using ::std::sqrt;
using namespace PhysBAM;
//#####################################################################
// Function Set_Restlength_From_Particles
//#####################################################################
template<class T> void LINEAR_ALTITUDE_SPRINGS_S3D<T>::
Set_Restlength_From_Particles()
{
    Set_Restlength_From_Material_Coordinates(particles.X);
}
//#####################################################################
// Function Set_Restlength_From_Material_Coordinates
//#####################################################################
template<class T> void LINEAR_ALTITUDE_SPRINGS_S3D<T>::
Set_Restlength_From_Material_Coordinates(ARRAY_VIEW<const TV> X)
{
    Invalidate_CFL();
    for(int t=0;t<mesh.elements.m;t++){
        int i=mesh.elements(t)(0),j=mesh.elements(t)(1),k=mesh.elements(t)(2);
        parameters(t)(0).restlength=SEGMENT_3D<T>(X(j),X(k)).Distance_From_Point_To_Line(X(i));
        parameters(t)(1).restlength=SEGMENT_3D<T>(X(i),X(k)).Distance_From_Point_To_Line(X(j));
        parameters(t)(2).restlength=SEGMENT_3D<T>(X(i),X(j)).Distance_From_Point_To_Line(X(k));
        for(int k=0;k<3;k++) parameters(t)(k).visual_restlength=parameters(t)(k).restlength;}
}
//#####################################################################
// Function Set_Overdamping_Fraction
//#####################################################################
template<class T> void LINEAR_ALTITUDE_SPRINGS_S3D<T>::
Set_Overdamping_Fraction(const T overdamping_fraction) // 1 is critically damped
{
    for(int t=0;t<mesh.elements.m;t++){
        int i,j,k;mesh.elements(t).Get(i,j,k);
        T harmonic_mass=Pseudo_Inverse(particles.one_over_effective_mass(i)+(T).25*(particles.one_over_effective_mass(j)+particles.one_over_effective_mass(k)));
        parameters(t)(0).damping=overdamping_fraction*2*sqrt(parameters(t)(0).youngs_modulus*parameters(t)(0).restlength*harmonic_mass);
        harmonic_mass=Pseudo_Inverse(particles.one_over_effective_mass(j)+(T).25*(particles.one_over_effective_mass(i)+particles.one_over_effective_mass(k)));
        parameters(t)(1).damping=overdamping_fraction*2*sqrt(parameters(t)(1).youngs_modulus*parameters(t)(1).restlength*harmonic_mass);
        harmonic_mass=Pseudo_Inverse(particles.one_over_effective_mass(k)+(T).25*(particles.one_over_effective_mass(j)+particles.one_over_effective_mass(i)));
        parameters(t)(2).damping=overdamping_fraction*2*sqrt(parameters(t)(2).youngs_modulus*parameters(t)(2).restlength*harmonic_mass);}
    Invalidate_CFL();
}
//#####################################################################
// Function Set_Overdamping_Fraction
//#####################################################################
template<class T> void LINEAR_ALTITUDE_SPRINGS_S3D<T>::
Set_Overdamping_Fraction(const ARRAY<VECTOR<T,3> >& overdamping_fraction) // 1 is critically damped
{
    for(int t=0;t<mesh.elements.m;t++){
        int i,j,k;mesh.elements(t).Get(i,j,k);
        T harmonic_mass=Pseudo_Inverse(particles.one_over_effective_mass(i)+(T).25*(particles.one_over_effective_mass(j)+particles.one_over_effective_mass(k)));
        parameters(t)(0).damping=overdamping_fraction(t)(0)*2*sqrt(parameters(t)(0).youngs_modulus*parameters(t)(0).restlength*harmonic_mass);
        harmonic_mass=Pseudo_Inverse(particles.one_over_effective_mass(j)+(T).25*(particles.one_over_effective_mass(i)+particles.one_over_effective_mass(k)));
        parameters(t)(1).damping=overdamping_fraction(t)(1)*2*sqrt(parameters(t)(1).youngs_modulus*parameters(t)(1).restlength*harmonic_mass);
        harmonic_mass=Pseudo_Inverse(particles.one_over_effective_mass(k)+(T).25*(particles.one_over_effective_mass(j)+particles.one_over_effective_mass(i)));
        parameters(t)(2).damping=overdamping_fraction(t)(2)*2*sqrt(parameters(t)(2).youngs_modulus*parameters(t)(2).restlength*harmonic_mass);}
    Invalidate_CFL();
}
//#####################################################################
// Function Update_Position_Based_State
//#####################################################################
template<class T> void LINEAR_ALTITUDE_SPRINGS_S3D<T>::
Update_Position_Based_State(const T time,const bool is_position_update,const bool update_hessian)
{
    ARRAY_VIEW<const TV> X(particles.X);int node1,node2,node3; // node1 is the isolated vertex and nodes2,3 are the segment
    int used_springs=0,total_elements=0;
    spring_states.Resize(mesh.elements.m);
    for(int t:force_elements){ // use shortest spring only
        int i,j,k;mesh.elements(t).Get(i,j,k);
        int hmin=-1;T cross_length_max=-FLT_MAX;
        if(is_position_update){
            for(int h=0;h<3;h++){
                switch(h){case 0:node2=j;node3=k;break;case 1:node2=k;node3=i;break;default:node2=i;node3=j;}
                T cross_length=(X(node3)-X(node2)).Magnitude_Squared();if(cross_length>cross_length_max){hmin=h;cross_length_max=cross_length;}}}
        else hmin=spring_states(t).node;
        switch(hmin){case 0:node1=i;node2=j;node3=k;break;case 1:node1=j;node2=k;node3=i;break;default:node1=k;node2=i;node3=j;}
        TV direction=SEGMENT_3D<T>::Normal(X(node2),X(node3),X(node1));
        if(triangle_inverted && (*triangle_inverted)(t)) direction=-direction;
        T rl=parameters(t)(hmin).restlength,current_length=direction.Dot(X(node1)-X(node2));
        SPRING_STATE& state=spring_states(t);
        total_elements++;
        if(use_springs_compressed_beyond_threshold && current_length-rl>-spring_compression_fraction_threshold*rl) state.node=-1;
        else{
            used_springs++;
            state.current_length=current_length;state.coefficient=parameters(t)(hmin).damping/rl;
            state.barycentric=SEGMENT_3D<T>::Clamped_Barycentric_Coordinates(X(node1),X(node2),X(node3));
            if(use_plasticity) Compute_Plasticity(hmin,t,current_length);
            state.direction=direction;state.node=hmin;}}

    if(print_number_used) LOG::cout<<"using "<<used_springs<<" of "<<total_elements<<" altitude springs"<<std::endl;
    if(!mesh.incident_elements) mesh.Initialize_Incident_Elements();
}
//#####################################################################
// Function Add_Forces
//#####################################################################
template<class T> void LINEAR_ALTITUDE_SPRINGS_S3D<T>::
Add_Forces(ARRAY_VIEW<TV> F,const T time) const
{
    for(int t:force_elements){
        const SPRING_STATE& state=spring_states(t);
        if(state.node>=0){
            int i,j,k;mesh.elements(t).Get(i,j,k);
            int node1,node2,node3; // node1 is the isolated vertex and nodes2,3 is the segment
            switch(state.node){case 0:node1=i;node2=j;node3=k;break;case 1:node1=j;node2=k;node3=i;break;default:node1=k;node2=i;node3=j;}
            const SPRING_PARAMETER& parameter=parameters(t)(state.node);
            T rl=parameter.restlength,vrl=parameter.visual_restlength;
            TV force=parameter.youngs_modulus/rl*(state.current_length-vrl)*state.direction;
            F(node1)-=force;F(node2)+=state.barycentric.x*force;F(node3)+=state.barycentric.y*force;}}
}
//#####################################################################
// Function Add_Velocity_Dependent_Forces
//#####################################################################
template<class T> void LINEAR_ALTITUDE_SPRINGS_S3D<T>::
Add_Velocity_Dependent_Forces(ARRAY_VIEW<const TV> V,ARRAY_VIEW<TV> F,const T time) const
{
    for(int t:force_elements){
        const SPRING_STATE& state=spring_states(t);
        if(state.node>=0){
            int i,j,k;mesh.elements(t).Get(i,j,k);
            int node1,node2,node3; // node1 is the isolated vertex and nodes2,3 are the segment
            switch(state.node){case 0:node1=i;node2=j;node3=k;break;case 1:node1=j;node2=k;node3=i;break;default:node1=k;node2=i;node3=j;}
            TV force=(state.coefficient*state.direction.Dot(V(node1)-state.barycentric.x*V(node2)-state.barycentric.y*V(node3)))*state.direction;
            F(node1)-=force;F(node2)+=state.barycentric.x*force;F(node3)+=state.barycentric.y*force;}}
}
//#####################################################################
// Function CFL_Strain_Rate
//#####################################################################
template<class T> T LINEAR_ALTITUDE_SPRINGS_S3D<T>::
CFL_Strain_Rate() const
{
    T max_strain_rate=0,dx;VECTOR<T,3> direction,v_interpolated; VECTOR<T,2> barycentric;int node1,node2,node3; // 1 is vertex, 2,3 is segment
    ARRAY<VECTOR<int,3> >& elements=mesh.elements;ARRAY_VIEW<const TV> X(particles.X),V(particles.V);
    for(int t:force_elements){ // use shortest spring only
        int i,j,k;elements(t).Get(i,j,k);
        int hmin=-1;T cross_length_max=-FLT_MAX;
        for(int h=0;h<3;h++){
            switch(h){case 0:node1=i;node2=j;node3=k;break;case 1:node1=j;node2=k;node3=i;break;default:node1=k;node2=i;node3=j;}
            T cross_length=(X(node3)-X(node2)).Magnitude_Squared();
            if(cross_length>cross_length_max){hmin=h;cross_length_max=cross_length;}}
        switch(hmin){case 0:node1=i;node2=j;node3=k;break;case 1:node1=j;node2=k;node3=i;break;default:node1=k;node2=i;node3=j;}
        direction=SEGMENT_3D<T>::Normal(X(node2),X(node3),X(node1));
        T rl=parameters(t)(hmin).restlength;
        if(use_rest_state_for_strain_rate) dx=rl;else dx=direction.Dot(X(node1)-X(node2));
        if(use_springs_compressed_beyond_threshold){
            T length;if(use_rest_state_for_strain_rate) length=direction.Dot(X(node1)-X(node2));else length=dx;
            if(length-rl>-spring_compression_fraction_threshold*rl) continue;}
        barycentric=SEGMENT_3D<T>::Clamped_Barycentric_Coordinates(X(node1),X(node2),X(node3));
        v_interpolated=barycentric.x*V(node2)+barycentric.y*V(node3);
        T strain_rate=direction.Dot(V(node1)-v_interpolated)/dx;max_strain_rate=max(max_strain_rate,abs(strain_rate));
        if(cache_strain){strains_of_spring(t)=VECTOR<T,2>(abs(strain_rate),abs((dx-parameters(t)(hmin).visual_restlength)/rl));}}
   return Robust_Divide(max_strain_per_time_step,max_strain_rate);
}
//#####################################################################
// Function Potential_Energy
//#####################################################################
template<class T> T LINEAR_ALTITUDE_SPRINGS_S3D<T>::
Potential_Energy(const int t,const T time) const
{
    const SPRING_STATE& state=spring_states(t);
    if(state.node>=0){
        int i,j,k;mesh.elements(t).Get(i,j,k);
        int node1,node2,node3; // node1 is the isolated vertex and nodes2,3 is the segment
        switch(state.node){case 0:node1=i;node2=j;node3=k;break;case 1:node1=j;node2=k;node3=i;break;default:node1=k;node2=i;node3=j;}
        const SPRING_PARAMETER& parameter=parameters(t)(state.node);
        T rl=parameter.restlength,vrl=parameter.visual_restlength;
        TV direction=SEGMENT_3D<T>::Normal(particles.X(node2),particles.X(node3),particles.X(node1));
        T current_length=direction.Dot(particles.X(node1)-particles.X(node2));
        return (T).5*parameter.youngs_modulus/rl*sqr(current_length-vrl);}
    else return 0;
}
//#####################################################################
// Function Potential_Energy
//#####################################################################
template<class T> T LINEAR_ALTITUDE_SPRINGS_S3D<T>::
Potential_Energy(const T time) const
{
    T potential_energy=0;
    for(int t:force_elements){
        potential_energy+=Potential_Energy(t,time);}
    return potential_energy;
}
//#####################################################################
namespace PhysBAM{
template class LINEAR_ALTITUDE_SPRINGS_S3D<float>;
template class LINEAR_ALTITUDE_SPRINGS_S3D<double>;
}
