//##################################################################### 
// Copyright 2021, Craig Schroeder.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
#include <Core/Interpolation/DIV_FREE_INTERPOLATION_C1_INTERP.h>
#include <Core/Interpolation/GENERAL_INTERPOLATION_AXIS_FACE_DEFINITION.h>
#include <Core/Interpolation/INTERPOLATION_SPLINES.h>

namespace PhysBAM {
template<int a,int d,class T> auto
DIV_FREE_INTERPOLATION_C1_INTERP_STENCIL<2>::
Value(T t) -> VECTOR<VECTOR<T,3+(a==d)>,4>
{
    if constexpr(a==d)
    {
        T k1=8./35,k3=-4;
        auto u=Make_Powers<5>(t);
        auto p0=Evaluate_Spline(IS::B3,u);
        auto p1=Evaluate_Spline(IS::F5,u);
        auto p2=Evaluate_Spline(IS::C4,u);
        auto p3=Evaluate_Spline(IS::D5,u);
        return {p0+k3*p3,k1*p1,k1*p2,k3*p0};
    }
    else
    {
        auto u=Make_Powers<4>(t);
        auto p0=Evaluate_Spline(IS::B2,u);
        auto p1=Evaluate_Spline(IS::F4,u);
        auto p2=Evaluate_Spline(IS::C3,u);
        auto p3=Evaluate_Spline(IS::D4,u);
        return {p0,p2,p1,p3};
    }
}

template<int a,int d,class T> auto
DIV_FREE_INTERPOLATION_C1_INTERP_STENCIL<2>::
Diff(T t) -> VECTOR<VECTOR<T,3+(a==d)>,4> 
{
    if constexpr(a==d)
    {
        T k1=8./35,k3=-4;
        auto u=Make_Powers<4>(t);
        auto p0=Evaluate_Spline_Diff(IS::B3,u);
        auto p1=Evaluate_Spline_Diff(IS::F5,u);
        auto p2=Evaluate_Spline_Diff(IS::C4,u);
        auto p3=Evaluate_Spline_Diff(IS::D5,u);
        return {p0+k3*p3,k1*p1,k1*p2,k3*p0};
    }
    else
    {
        auto u=Make_Powers<3>(t);
        auto p0=Evaluate_Spline_Diff(IS::B2,u);
        auto p1=Evaluate_Spline_Diff(IS::F4,u);
        auto p2=Evaluate_Spline_Diff(IS::C3,u);
        auto p3=Evaluate_Spline_Diff(IS::D4,u);
        return {p0,p2,p1,p3};
    }
}
template<int a,int d,class T> auto
DIV_FREE_INTERPOLATION_C1_INTERP_STENCIL<2>::
Hess(T t) -> VECTOR<VECTOR<T,3+(a==d)>,4> 
{
    if constexpr(a==d)
    {
        T k1=8./35,k3=-4;
        auto u=Make_Powers<3>(t);
        auto p0=Evaluate_Spline_Hess(IS::B3,u);
        auto p1=Evaluate_Spline_Hess(IS::F5,u);
        auto p2=Evaluate_Spline_Hess(IS::C4,u);
        auto p3=Evaluate_Spline_Hess(IS::D5,u);
        return {p0+k3*p3,k1*p1,k1*p2,k3*p0};
    }
    else
    {
        auto u=Make_Powers<2>(t);
        auto p0=Evaluate_Spline_Hess(IS::B2,u);
        auto p1=Evaluate_Spline_Hess(IS::F4,u);
        auto p2=Evaluate_Spline_Hess(IS::C3,u);
        auto p3=Evaluate_Spline_Hess(IS::D4,u);
        return {p0,p2,p1,p3};
    }
}

template<int a,int d,class T> auto
DIV_FREE_INTERPOLATION_C1_INTERP_STENCIL<3>::
Value(T t) -> VECTOR<VECTOR<T,3+(a==d)>,10> 
{
    if constexpr(a==d)
    {
        T k0=1./7,k3=1./21;
        auto u=Make_Powers<5>(t);
        auto p0=Evaluate_Spline(IS::G5,u)*k0;
        auto p1=Evaluate_Spline(IS::D5,u)*k0;
        auto bs=Evaluate_Spline(IS::B3,u),p2=bs*k0;
        auto p3=Evaluate_Spline(IS::H4,u)*k3;
        auto p4=Evaluate_Spline(IS::C4,u)*k3;
        return {p0,p0,p1,p1,p2,p2,p3,p4,p4,bs};
    }
    else
    {
        auto u=Make_Powers<4>(t);
        auto p0=Evaluate_Spline(IS::G4,u);
        auto p1=Evaluate_Spline(IS::D4,u);
        auto p2=Evaluate_Spline(IS::B2,u);
        auto p3=Evaluate_Spline(IS::H3,u);
        auto p4=Evaluate_Spline(IS::C3,u);
        if constexpr((a-d-1)%3==0) return {p1,p2,p2,p0,p0,p1,p4,p3,p4,p2};
        else return {p2,p1,p0,p2,p1,p0,p4,p4,p3,p2};
    }
}

template<int a,int d,class T> auto
DIV_FREE_INTERPOLATION_C1_INTERP_STENCIL<3>::
Diff(T t) -> VECTOR<VECTOR<T,3+(a==d)>,10> 
{
    if constexpr(a==d)
    {
        T k0=1./7,k3=1./21;
        auto u=Make_Powers<4>(t);
        auto p0=Evaluate_Spline_Diff(IS::G5,u)*k0;
        auto p1=Evaluate_Spline_Diff(IS::D5,u)*k0;
        auto bs=Evaluate_Spline_Diff(IS::B3,u),p2=bs*k0;
        auto p3=Evaluate_Spline_Diff(IS::H4,u)*k3;
        auto p4=Evaluate_Spline_Diff(IS::C4,u)*k3;
        return {p0,p0,p1,p1,p2,p2,p3,p4,p4,bs};
    }
    else
    {
        auto u=Make_Powers<3>(t);
        auto p0=Evaluate_Spline_Diff(IS::G4,u);
        auto p1=Evaluate_Spline_Diff(IS::D4,u);
        auto p2=Evaluate_Spline_Diff(IS::B2,u);
        auto p3=Evaluate_Spline_Diff(IS::H3,u);
        auto p4=Evaluate_Spline_Diff(IS::C3,u);
        if constexpr((a-d-1)%3==0) return {p1,p2,p2,p0,p0,p1,p4,p3,p4,p2};
        else return {p2,p1,p0,p2,p1,p0,p4,p4,p3,p2};
    }
}

template<int a,int d,class T> auto
DIV_FREE_INTERPOLATION_C1_INTERP_STENCIL<3>::
Hess(T t) -> VECTOR<VECTOR<T,3+(a==d)>,10> 
{
    if constexpr(a==d)
    {
        T k0=1./7,k3=1./21;
        auto u=Make_Powers<3>(t);
        auto p0=Evaluate_Spline_Hess(IS::G5,u)*k0;
        auto p1=Evaluate_Spline_Hess(IS::D5,u)*k0;
        auto bs=Evaluate_Spline_Hess(IS::B3,u),p2=bs*k0;
        auto p3=Evaluate_Spline_Hess(IS::H4,u)*k3;
        auto p4=Evaluate_Spline_Hess(IS::C4,u)*k3;
        return {p0,p0,p1,p1,p2,p2,p3,p4,p4,bs};
    }
    else
    {
        auto u=Make_Powers<2>(t);
        auto p0=Evaluate_Spline_Hess(IS::G4,u);
        auto p1=Evaluate_Spline_Hess(IS::D4,u);
        auto p2=Evaluate_Spline_Hess(IS::B2,u);
        auto p3=Evaluate_Spline_Hess(IS::H3,u);
        auto p4=Evaluate_Spline_Hess(IS::C3,u);
        if constexpr((a-d-1)%3==0) return {p1,p2,p2,p0,p0,p1,p4,p3,p4,p2};
        else return {p2,p1,p0,p2,p1,p0,p4,p4,p3,p2};
    }
}
template class GENERAL_INTERPOLATION_AXIS_FACE<VECTOR<double,2>,double,DIV_FREE_INTERPOLATION_C1_INTERP_STENCIL<2>,false,ARRAY<double,FACE_INDEX<2> > >;
template class GENERAL_INTERPOLATION_AXIS_FACE<VECTOR<double,3>,double,DIV_FREE_INTERPOLATION_C1_INTERP_STENCIL<3>,false,ARRAY<double,FACE_INDEX<3> > >;
template class GENERAL_INTERPOLATION_AXIS_FACE<VECTOR<float,2>,float,DIV_FREE_INTERPOLATION_C1_INTERP_STENCIL<2>,false,ARRAY<float,FACE_INDEX<2> > >;
template class GENERAL_INTERPOLATION_AXIS_FACE<VECTOR<float,3>,float,DIV_FREE_INTERPOLATION_C1_INTERP_STENCIL<3>,false,ARRAY<float,FACE_INDEX<3> > >;
}
