//#####################################################################
// 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/Level_Sets/Visit_Ray_Grid.h>
#include <Core/Basic_Geometry/RAY.h>
#include <Core/Grids/GRID.h>
#include <Core/Math_Tools/Robust_Arithmetic.h>
namespace PhysBAM{

template<class TV> void
Visit_Ray_Grid(const RAY<TV>& ray,const GRID<TV>& grid,int ghost,
    const std::function<bool(const VECTOR<int,TV::m>&,typename TV::SCALAR,typename TV::SCALAR)>& func)
{
    typedef typename TV::SCALAR T;
    typedef VECTOR<int,TV::m> IV;

    RANGE<TV> ghost_domain=grid.Ghost_Domain(ghost);
    RANGE<IV> index_domain=grid.Cell_Indices(ghost);
    T start_t=0,end_t=0;
    if(!Get_Intersection_Range(ray,ghost_domain,start_t,end_t)) return;
    TV X=ray.Point(start_t);
    IV cell=index_domain.Clamp_Half_Open(grid.Cell_I(X));
    TV cell_X=grid.Node_X(cell);
    
    IV dir=IV()+1;
    TV dt,next_t;
    IV end_index;
    for(int i=0;i<TV::m;i++)
    {
        dt(i)=Robust_Divide(grid.dX(i),ray.direction(i));
        if(dt(i)<0)
        {
            dt(i)=-dt(i);
            dir(i)=-1;
            end_index(i)=index_domain.min_corner(i)-1;
        }
        else
        {
            cell_X(i)+=grid.dX(i);
            end_index(i)=index_domain.max_corner(i);
        }
        next_t(i)=max((T)0,start_t+Robust_Divide(cell_X(i)-X(i),ray.direction(i)));
    }
    if(!func(cell,start_t,next_t.Min())) return;
    while(1)
    {
        int a=next_t.Arg_Min();
        cell(a)+=dir(a);
        if(cell(a)==end_index(a)) break;
        T t=next_t(a);
        next_t(a)+=dt(a);
        if(!func(cell,t,next_t.Min())) return;
    }
}
template void Visit_Ray_Grid<VECTOR<double,1> >(RAY<VECTOR<double,1> > const&,
    GRID<VECTOR<double,1> > const&,int,
    std::function<bool (VECTOR<int,1> const&,double,double)> const&);
template void Visit_Ray_Grid<VECTOR<double,2> >(RAY<VECTOR<double,2> > const&,
    GRID<VECTOR<double,2> > const&,int,
    std::function<bool (VECTOR<int,2> const&,double,double)> const&);
template void Visit_Ray_Grid<VECTOR<double,3> >(RAY<VECTOR<double,3> > const&,
    GRID<VECTOR<double,3> > const&,int,
    std::function<bool (VECTOR<int,3> const&,double,double)> const&);
template void Visit_Ray_Grid<VECTOR<float,1> >(RAY<VECTOR<float,1> > const&,
    GRID<VECTOR<float,1> > const&,int,
    std::function<bool (VECTOR<int,1> const&,float,float)> const&);
template void Visit_Ray_Grid<VECTOR<float,2> >(RAY<VECTOR<float,2> > const&,
    GRID<VECTOR<float,2> > const&,int,
    std::function<bool (VECTOR<int,2> const&,float,float)> const&);
template void Visit_Ray_Grid<VECTOR<float,3> >(RAY<VECTOR<float,3> > const&,
    GRID<VECTOR<float,3> > const&,int,
    std::function<bool (VECTOR<int,3> const&,float,float)> const&);
}
