//#####################################################################
// Copyright 2003-2007, Zhaosheng Bao, Ronald Fedkiw, Geoffrey Irving, Neil Molino, Andrew Selle, 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_2D.h>
#include <Core/Basic_Geometry/SEGMENT_2D.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_2D<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_2D<T>::
Set_Restlength_From_Material_Coordinates(ARRAY_VIEW<TV> material_coordinates)
{
    for(int t=0;t<mesh.elements.m;t++){
        int i,j,k;mesh.elements(t).Get(i,j,k);
        parameters(t)(0).restlength=SEGMENT_2D<T>(material_coordinates(j),material_coordinates(k)).Distance_From_Point_To_Line(material_coordinates(i));
        parameters(t)(1).restlength=SEGMENT_2D<T>(material_coordinates(i),material_coordinates(k)).Distance_From_Point_To_Line(material_coordinates(j));
        parameters(t)(2).restlength=SEGMENT_2D<T>(material_coordinates(i),material_coordinates(j)).Distance_From_Point_To_Line(material_coordinates(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_2D<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 Update_Position_Based_State
//#####################################################################
template<class T> void LINEAR_ALTITUDE_SPRINGS_2D<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){
        int i,j,k;mesh.elements(t).Get(i,j,k);
        int hmin=0;T cross_length_max=-FLT_MAX;
        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;}}
        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_2D<T>::Normal(X(node3),X(node2));
        T rl=parameters(t)(hmin).restlength,current_length=direction.Dot(X(node1)-X(node2));
        SPRING_STATE& spring_state=spring_states(t);
        total_elements++;
        if(use_springs_compressed_beyond_threshold && current_length-rl>-spring_compression_fraction_threshold*rl) spring_state.node=-1;
        else{
            used_springs++;
            spring_state.current_length=current_length;spring_state.coefficient=parameters(t)(hmin).damping/rl;
            spring_state.barycentric=SEGMENT_2D<T>::Clamped_Barycentric_Coordinates(X(node1),X(node2),X(node3));
            if(use_plasticity) Compute_Plasticity(hmin,t,current_length);            
            spring_state.direction=direction;spring_state.node=hmin;}}
    if(print_number_used) LOG::cout<<"using "<<used_springs<<" of "<<total_elements<<" altitude springs"<<std::endl;
}
//#####################################################################
// Function Add_Forces
//#####################################################################
template<class T> void LINEAR_ALTITUDE_SPRINGS_2D<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
            const SPRING_PARAMETER& parameter=parameters(t)(state.node);
            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;}
            T rl=parameter.restlength,vrl=parameter.visual_restlength;
            TV force=parameter.youngs_modulus*(state.current_length-vrl)/rl*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_2D<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& spring_state=spring_states(t);
        if(spring_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(spring_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=(spring_state.coefficient*spring_state.direction.Dot(V(node1)-spring_state.barycentric.x*V(node2)-spring_state.barycentric.y*V(node3)))*spring_state.direction;
            F(node1)-=force;F(node2)+=spring_state.barycentric.x*force;F(node3)+=spring_state.barycentric.y*force;}}
}
//#####################################################################
// Function CFL_Strain_Rate
//#####################################################################
template<class T> T LINEAR_ALTITUDE_SPRINGS_2D<T>::
CFL_Strain_Rate() const
{
    T max_strain_rate=0,dx;TV direction,barycentric,v_interpolated;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){
        int i,j,k;elements(t).Get(i,j,k);
        int hmin=0;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_2D<T>::Normal(X(node2),X(node3));
        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_2D<T>::Clamped_Barycentric_Coordinates(X(node1),X(node2),X(node3));
        v_interpolated=barycentric.x*V(node2)+barycentric.y*V(node3);
        max_strain_rate=max(max_strain_rate,abs(direction.Dot(V(node1)-v_interpolated)/dx));}
    return Robust_Divide(max_strain_per_time_step,max_strain_rate);
}
//#####################################################################
namespace PhysBAM{
template class LINEAR_ALTITUDE_SPRINGS_2D<float>;
template class LINEAR_ALTITUDE_SPRINGS_2D<double>;
}
