//#####################################################################
// Copyright 2003-2007, Zhaosheng Bao, Ron Fedkiw, Eran Guendelman, Don Hatch, Geoffrey Irving, Joseph Teran.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
#include <Rigids/Rigid_Bodies/MASS_PROPERTIES.h>
#include <Core/Log/LOG.h>
#include <Core/Math_Tools/FACTORIAL.h>
#include <Core/Matrices/DIAGONAL_MATRIX.h>
#include <Core/Matrices/FRAME.h>
#include <Core/Matrices/MATRIX.h>
#include <Core/Matrices/SYMMETRIC_MATRIX.h>
#include <Core/Meshes/POINT_SIMPLICES_1D.h>
#include <Core/Meshes/SEGMENTED_CURVE.h>
#include <Core/Meshes/TRIANGULATED_SURFACE.h>
#include <Deformables/Forces/STRAIN_MEASURE.h>
using namespace PhysBAM;
//#####################################################################
// Function Compute_Properties
//#####################################################################
namespace{
template<class T,int d> inline SYMMETRIC_MATRIX<T,1> Scaled_Element_Covariance(const T scaled_element_volume,const MATRIX<T,2,d>& DX) // actually returns only the trace of the covariance matrix
{
    static const SYMMETRIC_MATRIX<T,d> canonical=(T)1+SYMMETRIC_MATRIX<T,d>::Unit_Matrix();
    return SYMMETRIC_MATRIX<T,1>(scaled_element_volume*DX.Double_Contract(DX*canonical));
}
template<class T,int d> inline SYMMETRIC_MATRIX<T,3> Scaled_Element_Covariance(const T scaled_element_volume,const MATRIX<T,3,d>& DX)
{
    static const SYMMETRIC_MATRIX<T,d> canonical=(T)1+SYMMETRIC_MATRIX<T,d>::Unit_Matrix();
    return Conjugate(DX,scaled_element_volume*canonical);
}
template<class T> inline T Inertia_Tensor_From_Covariance(const T covariance_trace)
{
    return covariance_trace;
}
template<class T> inline SYMMETRIC_MATRIX<T,3> Inertia_Tensor_From_Covariance(const SYMMETRIC_MATRIX<T,3>& covariance)
{
    return covariance.Trace()-covariance;
}}
template<class TV,int d> template<bool thin_shell> void MASS_PROPERTIES<TV,d>::
Compute_Properties(MASS_PROPERTIES<TV,d>&,NORMAL_IMPLEMENTATION)
{
    typedef typename TV::SCALAR T;

    static const bool filled=!thin_shell;
    if(!object.mesh.elements.m) PHYSBAM_FATAL_ERROR("mesh has no elements");
    if(filled && d!=TV::m-1) PHYSBAM_FATAL_ERROR("only codimension 1 objects can be filled");

    // compute volume and center
    const TV base=object.particles.X(object.mesh.elements(0)[0]);
    T scaled_volume=0; // (d+filled)!*volume
    TV scaled_center_times_volume; // (d+1+filled)!*center*volume
    TV particle_X[d+1];
    for(int t=0;t<object.mesh.elements.m;t++){const VECTOR<int,d+1>& nodes=object.mesh.elements(t);
        MATRIX<T,TV::m,d+1> DX;
        for(int i=0;i<nodes.m;i++) particle_X[i]=object.particles.X(nodes(i)); // THIS NEEDS TO BE HERE BECAUSE OF A COMPILER BUG IN GCC 4.0.2
        for(int i=0;i<nodes.m;i++) DX.Set_Column(i,particle_X[i]-base);
        T scaled_element_volume=filled?DX.Parallelepiped_Measure():STRAIN_MEASURE<TV,d>::Ds(object.particles.X,nodes).Parallelepiped_Measure();
        if(t==-1){LOG::cout<<object.particles.X(nodes(0))<<object.particles.X(nodes(1))<<std::endl; // THIS NEEDS TO BE HERE BECAUSE OF A COMPILER BUG IN GCC 4.0.1
            LOG::cout<<object.particles.X(nodes(0))<<object.particles.X(nodes(2))<<std::endl;
            LOG::cout<<object.particles.X(nodes(2))<<base<<std::endl;}
        scaled_volume+=scaled_element_volume;
        TV sum;
        for(int i=0;i<DX.n;i++) sum+=DX.Column(i);
        scaled_center_times_volume+=scaled_element_volume*sum;}
    volume=(T)1/factorial(d+filled)*scaled_volume;
    if(!volume) PHYSBAM_FATAL_ERROR("zero volume");
    center=base+(T)1/(d+1+filled)*scaled_center_times_volume/scaled_volume;

    // compute inertia tensor (see http://number-none.com/blow/inertia for explanation of filled case)
    SYMMETRIC_MATRIX<T,TV::SPIN::m> scaled_covariance=SYMMETRIC_MATRIX<T,TV::SPIN::m>(); // (d+2+filled)!*covariance (or trace(covariance) in 2d)
    for(int t=0;t<object.mesh.elements.m;t++){const VECTOR<int,d+1>& nodes=object.mesh.elements(t);
        MATRIX<T,TV::m,d+1> DX;
        for(int i=0;i<nodes.m;i++) particle_X[i]=object.particles.X(nodes(i)); // THIS NEEDS TO BE HERE BECAUSE OF A COMPILER BUG IN GCC 4.0.2
        for(int i=0;i<nodes.m;i++) DX.Set_Column(i,particle_X[i]-center);
        T scaled_element_volume=filled?DX.Parallelepiped_Measure():STRAIN_MEASURE<TV,d>::Ds(object.particles.X,nodes).Parallelepiped_Measure();
        scaled_covariance+=Scaled_Element_Covariance(scaled_element_volume,DX);}
    
    inertia_tensor_over_density=Inertia_Tensor_From_Covariance((T)1/factorial(d+2+filled)*scaled_covariance);
    LOG::cout<<"inertia tensor over density "<<inertia_tensor_over_density<<std::endl; // THIS NEEDS TO BE HERE BECAUSE OF A COMPILER BUG IN GCC 4.0.2 (Frank wants it to stay std::endl)
}
template<class TV,int d> template<bool thin_shell> void MASS_PROPERTIES<TV,d>::
Compute_Properties(MASS_PROPERTIES<TV,d>&,DUMMY_IMPLEMENTATION )
{
    PHYSBAM_NOT_IMPLEMENTED();
}
//#####################################################################
// Constructor
//#####################################################################
template<class TV,int d> MASS_PROPERTIES<TV,d>::
MASS_PROPERTIES(const T_SIMPLICIAL_OBJECT& object,const bool thin_shell)
    :object(object),mass(1),density(1),use_mass(true)
{
    if(thin_shell) Compute_Properties<true>(*this,ACCESS_IMPLEMENTATION());else Compute_Properties<false>(*this,ACCESS_IMPLEMENTATION());
}
//#####################################################################
// Function Volume
//#####################################################################
template<class T_SIMPLICIAL_OBJECT> typename T_SIMPLICIAL_OBJECT::SCALAR
Shell_Volume_Helper(const T_SIMPLICIAL_OBJECT& object)
{
    typedef typename T_SIMPLICIAL_OBJECT::SCALAR T;typedef typename T_SIMPLICIAL_OBJECT::VECTOR_T TV;
    static const int d=T_SIMPLICIAL_OBJECT::MESH::dimension;
    T scaled_volume=0; // d!*volume
    for(int t=0;t<object.mesh.elements.m;t++){const VECTOR<int,d+1>& nodes=object.mesh.elements(t);
        MATRIX<T,TV::m,d> Ds=STRAIN_MEASURE<TV,d>::Ds(object.particles.X,nodes);
        scaled_volume+=Ds.Parallelepiped_Measure();}
    return (T)1/factorial(d)*scaled_volume;
}
template<class T> T
Shell_Volume_Helper(const POINT_SIMPLICES_1D<T>& object)
{
    PHYSBAM_NOT_IMPLEMENTED();
}
template<class TV,int d> typename TV::SCALAR MASS_PROPERTIES<TV,d>::
Thin_Shell_Volume(const T_SIMPLICIAL_OBJECT& object)
{
    if(!object.mesh.elements.m) PHYSBAM_FATAL_ERROR("mesh has no elements");
    return Shell_Volume_Helper(object);
}
//#####################################################################
// Function Transform_To_Object_Frame
//#####################################################################
namespace{
template<class T> ROTATION<VECTOR<T,1> > Diagonalize_Inertia_Tensor(DIAGONAL_MATRIX<T,0>& object_space_inertia_tensor,const SYMMETRIC_MATRIX<T,0> inertia_tensor)
{
    return ROTATION<VECTOR<T,1> >();
}
template<class T> ROTATION<VECTOR<T,2> > Diagonalize_Inertia_Tensor(DIAGONAL_MATRIX<T,1>& object_space_inertia_tensor,const SYMMETRIC_MATRIX<T,1>& inertia_tensor)
{
    object_space_inertia_tensor=DIAGONAL_MATRIX<T,1>(inertia_tensor(0,0));
    return ROTATION<VECTOR<T,2> >();
}
template<class T> ROTATION<VECTOR<T,3> > Diagonalize_Inertia_Tensor(DIAGONAL_MATRIX<T,3>& object_space_inertia_tensor,const SYMMETRIC_MATRIX<T,3>& inertia_tensor)
{
    auto [ev,R]=inertia_tensor.Solve_Eigenproblem();
    object_space_inertia_tensor=ev;
    return ROTATION<VECTOR<T,3> >(R);
}}
template<class TV,int d> void MASS_PROPERTIES<TV,d>::
Transform_To_Object_Frame(FRAME<TV>& frame,DIAGONAL_MATRIX<T,TV::SPIN::m>& object_space_inertia_tensor) const
{
    frame.t=center;
    frame.r=Diagonalize_Inertia_Tensor<T>(object_space_inertia_tensor,Inertia_Tensor());
}
//#####################################################################
// Function Transform_To_Object_Frame
//#####################################################################
template<class TV,int d> void MASS_PROPERTIES<TV,d>::
Transform_To_Object_Frame(FRAME<TV>& frame,DIAGONAL_MATRIX<T,TV::SPIN::m>& object_space_inertia_tensor,GEOMETRY_PARTICLES<TV>& point_cloud) const
{
    Transform_To_Object_Frame(frame,object_space_inertia_tensor);
    if(frame.r==ROTATION<TV>()) point_cloud.X-=frame.t;
    else{
        PHYSBAM_ASSERT(TV::m==3);
        for(int i=0;i<point_cloud.X.m;i++)
            point_cloud.X(i)=frame.Inverse()*point_cloud.X(i);}
}
//#####################################################################
namespace PhysBAM{
template class MASS_PROPERTIES<VECTOR<float,1>,0>;
template class MASS_PROPERTIES<VECTOR<float,2>,1>;
template class MASS_PROPERTIES<VECTOR<float,3>,1>;
template class MASS_PROPERTIES<VECTOR<float,3>,2>;
template class MASS_PROPERTIES<VECTOR<double,1>,0>;
template class MASS_PROPERTIES<VECTOR<double,2>,1>;
template class MASS_PROPERTIES<VECTOR<double,3>,1>;
template class MASS_PROPERTIES<VECTOR<double,3>,2>;
}
