//#####################################################################
// Copyright 2015, Craig Schroeder.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
#include <Hybrid_Methods/Iterators/PARTICLE_GRID_WEIGHTS_SPLINE.h>
#include <Core/Grids/GRID.h>
#include <Core/Log/DEBUG_UTILITIES.h>
#include <Core/Math_Tools/cube.h>
#include <Core/Math_Tools/pow.h>
#include <Core/Math_Tools/RANGE_ITERATOR.h>
#include <Core/Matrices/DIAGONAL_MATRIX.h>
#include <Core/Symbolics/SYMBOLIC.h>
namespace PhysBAM{
//#####################################################################
// Constructor
//#####################################################################
template<class TV,int degree> PARTICLE_GRID_WEIGHTS_SPLINE<TV,degree>::
PARTICLE_GRID_WEIGHTS_SPLINE(const GRID<TV>& grid,int threads)
    :BASE(threads),grid(grid)
{
    this->stencil_width=degree+1;
    this->Dp=sqr(grid.dX.Max())/(6-degree);
    this->Dp_inv=1/this->Dp;
}
//#####################################################################
// Destructor
//#####################################################################
template<class TV,int degree> PARTICLE_GRID_WEIGHTS_SPLINE<TV,degree>::
~PARTICLE_GRID_WEIGHTS_SPLINE()
{
}
//#####################################################################
// Function Compute
//#####################################################################
template<class TV,int degree> void PARTICLE_GRID_WEIGHTS_SPLINE<TV,degree>::
Compute(const PRECOMPUTE_DATA& pd,typename BASE::SCRATCH& scratch,
    bool want_gradient,bool want_pseudo_gradient) const
{
    int pow_n=pow<TV::m,int>(n);
    scratch.index.Resize(pow_n);
    scratch.weight.Resize(pow_n);
    if(want_gradient) scratch.gradient(0).Resize(pow_n);
    if(want_pseudo_gradient) scratch.gradient(1).Resize(pow_n);

    int k=0;
    for(RANGE_ITERATOR<TV::m> it(RANGE<IV>(IV(),IV()+n));it.Valid();it.Next()){
        scratch.index(k)=pd.base+it.index;
        TV all_w;
        for(int i=0;i<TV::m;i++)
            all_w(i)=pd.w(it.index(i))(i);
        scratch.weight(k)=all_w.Product();
        if(want_gradient){
            TV dw(TV()+1);
            for(int j=0;j<TV::m;j++){
                T dj=pd.dw(it.index(j))(j);
                for(int i=0;i<TV::m;i++)
                    dw(i)*=(i==j)?dj:all_w(j);}
            scratch.gradient(0)(k)=dw;}
        if(want_pseudo_gradient)
        {
            TV Xi=grid.Node_X(scratch.index(k));
            TV dw=scratch.weight(k)*this->Dp_inv*(Xi-pd.X);
            scratch.gradient(1)(k)=dw;
        }
        k++;}
    scratch.m3=pd.m3;
    scratch.m4=pd.m4;
}
//#####################################################################
// Function Compute
//#####################################################################
template<class TV,int degree> void PARTICLE_GRID_WEIGHTS_SPLINE<TV,degree>::
Compute(int p,typename BASE::SCRATCH& scratch,bool want_gradient,
    bool want_pseudo_gradient) const
{
    Compute(precompute_data(p),scratch,want_gradient,want_pseudo_gradient);
}
//#####################################################################
// Function Compute
//#####################################################################
template<class TV,int degree> void PARTICLE_GRID_WEIGHTS_SPLINE<TV,degree>::
Compute(const TV& X,typename BASE::SCRATCH& scratch,bool want_gradient,
    bool want_pseudo_gradient) const
{
    PRECOMPUTE_DATA pd;
    Compute_Precompute_Data(pd,X);
    Compute(pd,scratch,want_gradient,want_pseudo_gradient);
}
//#####################################################################
// Function Compute_Weights
//#####################################################################
template<class TV> static void
Compute_Weights(VECTOR<TV,2>& w,VECTOR<TV,2>& dw,TV x,TV one_over_dX)
{
    typedef typename TV::SCALAR T;
    for(int k=0;k<TV::m;++k)
        if(x(k)<1e-24) x(k)=1e-10;
    w(0)=(T)1-x;
    dw(0)=-one_over_dX;
    x-=1;
    w(1)=(T)1+x;
    dw(1)=one_over_dX;
}
//#####################################################################
// Function Compute_Weights
//#####################################################################
template<class TV> static void
Compute_Weights(VECTOR<TV,3>& w,VECTOR<TV,3>& dw,TV x,TV one_over_dX)
{
    typedef typename TV::SCALAR T;
    w(0)=(T).5*sqr(x-(T)1.5);
    dw(0)=(x-(T)1.5)*one_over_dX;
    x-=(T)1;
    w(1)=-x*x+(T).75;
    dw(1)=(T)(-2)*x*one_over_dX;
    x-=(T)1;
    w(2)=(T).5*sqr(x+(T)1.5);
    dw(2)=(x+(T)1.5)*one_over_dX;
}
//#####################################################################
// Function Compute_Weights
//#####################################################################
template<class TV> static void
Compute_Weights(VECTOR<TV,4>& w,VECTOR<TV,4>& dw,TV x,TV one_over_dX)
{
    typedef typename TV::SCALAR T;
    x=clamp(x,TV()+1,TV()+2);
    TV x2,x3,z=(T)2-x,z2=z*z;
    w(0)=((T)1/6)*z2*z;
    dw(0)=-(T).5*one_over_dX*z2;
    x-=(T)1;
    x2=x*x;x3=x2*x;
    w(1)=(T).5*x3-x2+(T)2/3;
    dw(1)=((T)1.5*x2-(T)2*x)*one_over_dX;
    x-=(T)1;
    x2=x*x;x3=x2*x;
    w(2)=-(T).5*x3-x2+(T)2/3;
    dw(2)=(-(T)1.5*x2-(T)2*x)*one_over_dX;
    x-=(T)1;
    z=(T)2+x;z2=z*z;
    w(3)=((T)1/6)*z2*z;
    dw(3)=(T).5*one_over_dX*z2;
}
//#####################################################################
// Function Compute_Precompute_Data
//#####################################################################
template<class TV,int degree> void PARTICLE_GRID_WEIGHTS_SPLINE<TV,degree>::
Compute_Precompute_Data(PRECOMPUTE_DATA& pd,const TV& X) const
{
    pd.base=grid.Cell_I(X-(T).5*(degree-1)*grid.dX);
    pd.X=X;
    TV X_eval=X-grid.Node_X(pd.base);
    Compute_Weights(pd.w,pd.dw,X_eval*grid.one_over_dX,grid.one_over_dX);

    TV dx=grid.dX,d2=dx*dx,d4=d2*d2,x=X_eval-dx,x2=x*x;
    if(degree==2)
    {
        pd.m3=((T).25*d2-x2)*x;
        pd.m4=(-(T)1.5*d2+3*x2)*x2+(T).25*d4;
    }
    else
    {
        pd.m3=TV();
        pd.m4=(T)1/3*d4-(d2-2*dx*x+x2)*x2;
    }
}
//#####################################################################
// Function Update
//#####################################################################
template<class TV,int degree> void PARTICLE_GRID_WEIGHTS_SPLINE<TV,degree>::
Update(ARRAY_VIEW<const TV> X)
{
    precompute_data.Resize(X.m);
    for(int p=0;p<X.m;p++) Compute_Precompute_Data(precompute_data(p),X(p));
}
//#####################################################################
// Function Weights_Helper
//#####################################################################
template<class T> inline T
Weights_Helper(T x,VECTOR<int,1>*)
{
    if(x>1) return 0;
    return 1-x;
}
//#####################################################################
// Function Weights_Helper
//#####################################################################
template<class T> inline T
Weights_Helper(T x,VECTOR<int,2>*)
{
    if(x>(T)1.5) return 0;
    if(x>(T).5) return (T).5*sqr(x-(T)1.5);
    return -x*x+(T).75;
}
//#####################################################################
// Function Weights_Helper
//#####################################################################
template<class T> inline T
Weights_Helper(T x,VECTOR<int,3>*)
{
    if(x>2) return 0;
    if(x<1) return ((T).5*x-1)*x*x+(T)2/3;
    T z=2-x;
    return ((T)1/6)*z*z*z;
}
//#####################################################################
// Constructor
//#####################################################################
template<class TV,int degree> typename TV::SCALAR PARTICLE_GRID_WEIGHTS_SPLINE<TV,degree>::
Weight(const TV& u) const
{
    TV v=u*grid.one_over_dX,w;
    for(int i=0;i<TV::m;i++) w(i)=Weights_Helper(abs(v(i)),(VECTOR<int,degree>*)0);
    return w.Product();
}
//#####################################################################
// Function Particle_Ranges
//#####################################################################
template<class TV,int degree> void PARTICLE_GRID_WEIGHTS_SPLINE<TV,degree>::
Particle_Ranges(ARRAY<RANGE<IV> >& ranges) const
{
    ranges.Resize(precompute_data.m);
    for(int i=0;i<precompute_data.m;i++)
    {
        IV base=precompute_data(i).base;
        ranges(i)={base,base+n};
    }
}
//#####################################################################
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<float,1>,1>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<float,2>,1>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<float,3>,1>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<double,1>,1>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<double,2>,1>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<double,3>,1>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<float,1>,2>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<float,2>,2>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<float,3>,2>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<double,1>,2>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<double,2>,2>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<double,3>,2>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<float,1>,3>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<float,2>,3>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<float,3>,3>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<double,1>,3>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<double,2>,3>;
template class PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<double,3>,3>;
#ifdef USE_SYMBOLIC
template PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<SYMBOLIC,2>,2>::PARTICLE_GRID_WEIGHTS_SPLINE(GRID<VECTOR<SYMBOLIC,2> > const&,int);
template PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<SYMBOLIC,2>,3>::PARTICLE_GRID_WEIGHTS_SPLINE(GRID<VECTOR<SYMBOLIC,2> > const&,int);
template PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<SYMBOLIC,3>,2>::PARTICLE_GRID_WEIGHTS_SPLINE(GRID<VECTOR<SYMBOLIC,3> > const&,int);
template PARTICLE_GRID_WEIGHTS_SPLINE<VECTOR<SYMBOLIC,3>,3>::PARTICLE_GRID_WEIGHTS_SPLINE(GRID<VECTOR<SYMBOLIC,3> > const&,int);
#endif
}
