//#####################################################################
// Copyright 2007, Geoffrey Irving, Craig Schroeder, Andrew Selle.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
#include <Deformables/Forces/ANGLE_SPRINGS.h>
#include <Core/Auto_Diff/AUTO_HESS_EXT.h>
#include <Core/Math_Tools/cyclic_shift.h>
#include <Core/Meshes/SEGMENT_MESH.h>
#include <Core/Meshes/TRIANGULATED_SURFACE.h>
#include <Deformables/Particles/DEFORMABLE_PARTICLES.h>
namespace PhysBAM{

//#####################################################################
// Constructor
//#####################################################################
template<class TV> ANGLE_SPRINGS<TV>::
ANGLE_SPRINGS(DEFORMABLE_PARTICLES<TV>& particles,SIMPLEX_MESH<d-1>& mesh)
    :BASE(particles),mesh(mesh)
{
    mesh.Initialize_Adjacent_Elements();
    Update_Mesh();
}
template<class TV,class T>
T Compute(const VECTOR<TV,2+1>& X,T a0,T k,T& pe,
    VECTOR<TV,TV::m+1>& f,MATRIX<MATRIX<T,TV::m>,TV::m+1>& H,bool angle_only)
{
    typedef DIFF_LAYOUT<T,2,2,2> LAYOUT;
    auto X0=Hess_From_Var<LAYOUT,0>(X(0));
    auto X1=Hess_From_Var<LAYOUT,1>(X(1));
    auto X2=Hess_From_Var<LAYOUT,2>(X(2));
    auto u=X1-X0;
    auto v=X2-X1;
    MATRIX<T,2> W(0,1,-1,0);
    auto s=u.Dot(W*v);
    auto c=u.Dot(v);
    auto a=atan2(s,c)-a0;
    if(angle_only) return a.x;
    auto E=k*sqr(a)/(u.Magnitude()+v.Magnitude());
    pe=E.x;
    Extract<0>(f,E.dx);
    Extract<0,0>(H,E.ddx);
    return a.x;
}
template<class TV,class T>
T Compute(const VECTOR<TV,3+1>& X,T a0,T k,T& pe,
    VECTOR<TV,TV::m+1>& f,MATRIX<MATRIX<T,TV::m>,TV::m+1>& H,bool angle_only)
{
    PHYSBAM_FATAL_ERROR("test before using");
    typedef DIFF_LAYOUT<T,3,3,3,3> LAYOUT;
    auto X0=Hess_From_Var<LAYOUT,0>(X(0));
    auto X1=Hess_From_Var<LAYOUT,1>(X(1));
    auto X2=Hess_From_Var<LAYOUT,2>(X(2));
    auto X3=Hess_From_Var<LAYOUT,3>(X(3));
    auto w=X2-X1;
    auto z=X1-X3;
    auto u=(X0-X1).Cross(w);
    auto v=z.Cross(w);
    auto s=u.Dot(z)*w.Magnitude();
    auto c=u.Dot(v);
    auto a=atan2(s,c)-a0;
    if(angle_only) return a.x;
    auto E=(T)1.5*k*sqr(a)/(u.Magnitude()+v.Magnitude());
    pe=E.x;
    Extract<0>(f,E.dx);
    Extract<0,0>(H,E.ddx);
    return a.x;
}

//#####################################################################
// Function Update_Position_Based_State
//#####################################################################
template<class TV> void ANGLE_SPRINGS<TV>::
Update_Position_Based_State(const T time,const bool is_position_update,const bool update_hessian)
{
    for(auto& d:data)
        Compute(VECTOR<TV,TV::m+1>(particles.X.Subset(d.e)),
            d.rest_angle,stiffness,d.E,d.dE,d.ddE,false);
}
//#####################################################################
// Function Add_Dependencies
//#####################################################################
template<class TV> void ANGLE_SPRINGS<TV>::
Add_Dependencies(SEGMENT_MESH& dependency_mesh) const
{
    mesh.Add_Dependencies(dependency_mesh);
}
//#####################################################################
// Function Update_Mesh
//#####################################################################
template<class TV> void ANGLE_SPRINGS<TV>::
Update_Mesh()
{
    data.Remove_All();
    for(int i=0;i<mesh.elements.m;i++)
    {
        VECTOR<int,TV::m> e0(mesh.elements(i));
        for(auto j:(*mesh.adjacent_elements)(i))
        {
            VECTOR<int,TV::m> e1(mesh.elements(j));
            for(int k=0;k<TV::m-1 && e1.Contains(e0.x);k++) cyclic_shift(e0);
            DATA d;
            d.e=e0.Append(e1.Sum()-e0.Sum()+e0.x);
            d.rest_angle=Compute(VECTOR<TV,TV::m+1>(particles.X.Subset(d.e)),
                (T)0,stiffness,d.E,d.dE,d.ddE,true);
            data.Append(d);
        }
    }
}
//#####################################################################
// Function Update_Mpi
//#####################################################################
template<class TV> void ANGLE_SPRINGS<TV>::
Update_Mpi(const ARRAY<bool>& particle_is_simulated)
{
}
//#####################################################################
// Function Add_Forces
//#####################################################################
template<class TV> void ANGLE_SPRINGS<TV>::
Add_Forces(ARRAY_VIEW<TV> F,const T time) const
{
    for(const auto& d:data) F.Subset(d.e)-=d.dE;
}
//#####################################################################
// Function Add_Velocity_Dependent_Forces
//#####################################################################
template<class TV> void ANGLE_SPRINGS<TV>::
Add_Velocity_Dependent_Forces(ARRAY_VIEW<const TV> V,ARRAY_VIEW<TV> F,const T time) const
{
}
//#####################################################################
// Function Compute_Energy
//#####################################################################
template<class TV> auto ANGLE_SPRINGS<TV>::
Potential_Energy(const T time) const -> T
{
    T E=0;
    for(auto& d:data) E+=d.E;
    return E;
}
//#####################################################################
// Function Add_Force_Derivatives
//#####################################################################
template<class TV> void ANGLE_SPRINGS<TV>::
Add_Force_Derivatives(ARRAY_VIEW<const TV> V,
    ARRAY_VIEW<TV> F,const T time,bool transpose) const
{
    for(const auto& d:data)
    {
        for(int i=0;i<TV::m+1;i++)
        {
            TV f;
            for(int j=0;j<TV::m+1;j++)
                f+=d.ddE(i,j)*V(d.e(j));
            F(d.e(i))-=f;
        }
    }
}
//#####################################################################
template class ANGLE_SPRINGS<VECTOR<float,2> >;
template class ANGLE_SPRINGS<VECTOR<float,3> >;
template class ANGLE_SPRINGS<VECTOR<double,2> >;
template class ANGLE_SPRINGS<VECTOR<double,3> >;
}
