//#####################################################################
// Copyright 2003-2009, Ron Fedkiw, Jon Gretarsson, Eran Guendelman, Geoffrey Irving, Sergey Koltakov, Nipun Kwatra, Duc Nguyen, Andrew Selle, Eftychios Sifakis, Joseph Teran.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class SEGMENTED_CURVE
//##################################################################### 
#include <Core/Meshes/SEGMENTED_CURVE.h>
#include <Core/Arrays/ARRAYS_ND.h>
#include <Core/Arrays/INDIRECT_ARRAY.h>
#include <Core/Basic_Geometry/RAY.h>
#include <Core/Basic_Geometry/RAY_HIT.h>
#include <Core/Basic_Geometry/SEGMENT_1D.h>
#include <Core/Basic_Geometry/SEGMENT_2D.h>
#include <Core/Basic_Geometry/SEGMENT_3D.h>
#include <Core/Data_Structures/HASHTABLE.h>
#include <Core/Grids/GRID.h>
#include <Core/Intersections/SEGMENT_2D_SEGMENT_2D_INTERSECTION.h>
#include <Core/Log/LOG.h>
#include <Core/Math_Tools/RANGE.h>
#include <Core/Meshes/POINT_SIMPLICES_1D.h>
#include <Core/Registry/STRUCTURE_REGISTRY.h>
#include <Core/Spatial_Acceleration/MESH_HIERARCHY.h>
#include <complex>
namespace PhysBAM{
//#####################################################################
// Constructor
//#####################################################################
template<class TV> SEGMENTED_CURVE_BASE<TV>::
SEGMENTED_CURVE_BASE()
    :MESH_OBJECT_BASE<TV,SEGMENT_MESH>(*new SEGMENT_MESH,*new GEOMETRY_PARTICLES<TV>),segment_list(0),point_simplices_1d(0)
{
    this->need_destroy_mesh=this->need_destroy_particles=true;
}
//#####################################################################
// Constructor
//#####################################################################
template<class TV> SEGMENTED_CURVE_BASE<TV>::
SEGMENTED_CURVE_BASE(SEGMENT_MESH& segment_mesh_input,GEOMETRY_PARTICLES<TV>& particles_input)
    :MESH_OBJECT_BASE<TV,SEGMENT_MESH>(segment_mesh_input,particles_input),segment_list(0),point_simplices_1d(0)
{
}
//#####################################################################
// Destructor
//#####################################################################
template<class TV> SEGMENTED_CURVE_BASE<TV>::
~SEGMENTED_CURVE_BASE()
{
}
//#####################################################################
// Function Clean_Memory
//#####################################################################
template<class TV> void SEGMENTED_CURVE_BASE<TV>::
Clean_Memory()
{
    MESH_OBJECT_BASE<TV,SEGMENT_MESH>::Clean_Memory();
    delete segment_list;segment_list=0;
}
//#####################################################################
// Function Refresh_Auxiliary_Structures_Helper
//#####################################################################
template<class TV> void SEGMENTED_CURVE_BASE<TV>::
Refresh_Auxiliary_Structures_Helper()
{
    if(segment_list) Update_Segment_List();
    if(hierarchy) this->Initialize_Hierarchy();
}
//#####################################################################
// Function Update_Segment_List
//#####################################################################
template<class TV> void SEGMENTED_CURVE_BASE<TV>::
Update_Segment_List() // updates the segments assuming the particle positions are already updated
{
    if(!segment_list) segment_list=new ARRAY<typename BASIC_GEOMETRY_POLICY<TV>::SEGMENT>(mesh.elements.m);
    for(int s=0;s<mesh.elements.m;s++){
        int i,j;mesh.elements(s).Get(i,j);
        (*segment_list)(s)=typename BASIC_GEOMETRY_POLICY<TV>::SEGMENT(particles.X(i),particles.X(j));}
}
//#####################################################################
// Function Initialize_Straight_Mesh_And_Particles
//#####################################################################
template<class TV> void SEGMENTED_CURVE_BASE<TV>::  
Initialize_Straight_Mesh_And_Particles(const GRID<VECTOR<T,1>>& grid)
{
    Clean_Memory();
    mesh.Initialize_Straight_Mesh(grid.Numbers_Of_Nodes().x);
    particles.Preallocate(grid.Numbers_Of_Nodes().x);
    for(int i=0;i<grid.Numbers_Of_Nodes().x;i++) particles.X(particles.Add_Element()).x=grid.Node_X(VECTOR<int,1>(i)).x;
}
//#####################################################################
// Function Initialize_Circle_Mesh_And_Particles
//#####################################################################
template<class TV> void SEGMENTED_CURVE_BASE<TV>::  
Initialize_Circle_Mesh_And_Particles(const int m,const T radius)
{
    if constexpr(TV::m==1) PHYSBAM_NOT_IMPLEMENTED();
    else{
    Clean_Memory();
    mesh.Initialize_Straight_Mesh(m,true);
    particles.Add_Elements(m);
    for(int p=0;p<m;p++){
        std::complex<T> X=std::polar(radius,(T)2*pi_v<T>/m*p);
        particles.X(p)=TV(VECTOR<T,2>(X.real(),X.imag()));}}
}
//#####################################################################
// Function Closest_Point_On_Curve
//#####################################################################
template<class TV> TV SEGMENTED_CURVE_BASE<TV>::  
Closest_Point_On_Curve(const TV& location,T thickness_over_two,int* closest_segment,T* distance) const
{
    T min_distance_squared=FLT_MAX;TV point;
    for(int i=0;i<mesh.elements.m;i++){
        T_SEGMENT segment=segment_list?(*segment_list)(i):Get_Element(i);
        TV new_point=segment.Closest_Point_On_Segment(location);
        T distance_squared=(new_point-location).Magnitude_Squared();
        if(distance_squared < min_distance_squared){
            min_distance_squared=distance_squared;point=new_point;
            if(closest_segment) *closest_segment=i;}}
    if(distance) *distance=sqrt(min_distance_squared);
    return point;
}
//#####################################################################
// Function Rescale
//#####################################################################
template<class TV> void SEGMENTED_CURVE_BASE<TV>::
Rescale(const TV& scaling)
{
    for(int k=0;k<particles.Size();k++) particles.X(k)*=scaling;
    if(segment_list) Update_Segment_List();
    if(bounding_box) Update_Bounding_Box();
}
//#####################################################################
// Function Total_Length
//#####################################################################
template<class TV> typename TV::SCALAR SEGMENTED_CURVE_BASE<TV>::
Total_Length() const
{
    T length=0;
    for(int t=0;t<mesh.elements.m;t++){
        int node1=mesh.elements(t)(0),node2=mesh.elements(t)(1);
        length+=(particles.X(node1)-particles.X(node2)).Magnitude();}
    return length;
}
template<class T> void Initialize_Boundary_Object_Helper(SEGMENTED_CURVE_BASE<VECTOR<T,1> >& sc)
{
    delete sc.point_simplices_1d;
    if(!sc.mesh.boundary_mesh) sc.mesh.Initialize_Boundary_Mesh();
    sc.point_simplices_1d=new POINT_SIMPLICES_1D<T>(*sc.mesh.boundary_mesh,sc.particles);
}
template<class T> void Initialize_Boundary_Object_Helper(SEGMENTED_CURVE_BASE<VECTOR<T,2> >& sc){PHYSBAM_NOT_IMPLEMENTED();}
template<class T> void Initialize_Boundary_Object_Helper(SEGMENTED_CURVE_BASE<VECTOR<T,3> >& sc){PHYSBAM_NOT_IMPLEMENTED();}
//#####################################################################
// Function Get_Boundary_Object
//#####################################################################
template<class TV> POINT_SIMPLICES_1D<typename TV::SCALAR>& SEGMENTED_CURVE_BASE<TV>::
Get_Boundary_Object()
{
    Initialize_Boundary_Object_Helper(*this);
    return *point_simplices_1d;
}
//#####################################################################
// Function Inside_Any_Simplex
//#####################################################################
template<class TV> bool SEGMENTED_CURVE_BASE<TV>::
Inside_Any_Simplex(const TV& location,int& segment_id,const T thickness_over_two) const
{
    assert(hierarchy);
    assert(segment_list);
    ARRAY<int> nearby_segments;
    bool found=false;
    hierarchy->Traverse(location,
        [&,this,thickness_over_two](int s)
        {
            T_SEGMENT& segment=(*segment_list)(s);
            if(segment.Inside(location,thickness_over_two))
            {
                segment_id=s;
                found=true;
                return false;
            }
            return true;
        },thickness_over_two);
    return found;
}
//#####################################################################
// Constructor
//#####################################################################
template<class T> MESH_OBJECT<VECTOR<T,2>,1>::
MESH_OBJECT()
{
}
//#####################################################################
// Constructor
//#####################################################################
template<class T> MESH_OBJECT<VECTOR<T,2>,1>::
MESH_OBJECT(SEGMENT_MESH& segment_mesh_input,GEOMETRY_PARTICLES<TV>& particles_input)
    :SEGMENTED_CURVE_BASE<TV>(segment_mesh_input,particles_input)
{
}
//#####################################################################
// Function Calculate_Signed_Distance
//#####################################################################
template<class T> T MESH_OBJECT<VECTOR<T,2>,1>:: 
Calculate_Signed_Distance(const TV& location,T thickness_over_two) const
{
    T distance;
    Closest_Point_On_Curve(location,thickness_over_two,0,&distance);
    if(Inside(location,thickness_over_two)) distance*=-1;
    return distance;
}
//#####################################################################
// Function Inside
//#####################################################################
template<class T> bool MESH_OBJECT<VECTOR<T,2>,1>::  
Inside(const TV& location,T thickness_over_two) const
{
    // early pruning
    if(bounding_box){if(bounding_box->Outside(location,thickness_over_two)) return false;}

    // Not totally robust yet
    T total_signed_angle=0;
    for(int i=0;i<mesh.elements.m;i++){
        SEGMENT_2D<T> segment=(segment_list)?(*segment_list)(i):Get_Element(i);
        TV v1=segment.X.x-location;
        TV v2=segment.X.y-location;
        VECTOR<T,1> cross=v1.Cross(v2);
        T signed_angle=v1.Angle_Between(v2);
        if(cross.x<0) signed_angle*=-1;
        total_signed_angle+=signed_angle;}
    T total_angle=abs(total_signed_angle);

    return (abs(total_angle-2*pi)<abs(total_signed_angle-0));
}
//#####################################################################
// Function Outside
//#####################################################################
template<class T> bool MESH_OBJECT<VECTOR<T,2>,1>::  
Outside(const TV& location,T thickness_over_two) const
{
    return !Inside(location,thickness_over_two);
}
//#####################################################################
// Function Outside
//#####################################################################
template<class T> bool MESH_OBJECT<VECTOR<T,2>,1>::  
Boundary(const TV& location,T thickness_over_two) const
{
    PHYSBAM_NOT_IMPLEMENTED();
}
//#####################################################################
// Function Inside_Any_Segment
//#####################################################################
template<class T> bool MESH_OBJECT<VECTOR<T,2>,1>::  
Inside_Any_Segment(const TV& point,int& segment_id,const T thickness_over_two) const
{
    for(int i=0;i<mesh.elements.m;i++){
        SEGMENT_2D<T> segment=(segment_list)?(*segment_list)(i):Get_Element(i);
        if(segment.Inside(point,thickness_over_two)){segment_id=i;return true;}}
    return false;
}
//#####################################################################
// Function Normal
//#####################################################################
template<class T> VECTOR<T,2> MESH_OBJECT<VECTOR<T,2>,1>::
Normal(const int aggregate) const
{
    SEGMENT_2D<T> segment=(segment_list)?(*segment_list)(aggregate):Get_Element(aggregate);
    return segment.Normal();
}
//#####################################################################
// Function Find_First_Segment_Segment_Intersection
//#####################################################################
template<class T> bool MESH_OBJECT<VECTOR<T,2>,1>::
Find_First_Segment_Segment_Intersection(const SEGMENT_MESH& test_segment_mesh,ARRAY_VIEW<const TV> X,const T thickness_over_2,const int max_coarsening_attempts,
    const bool update_bounding_boxes)
{
    if(Segment_Segment_Intersection(test_segment_mesh,X,thickness_over_2,update_bounding_boxes)){
        LOG::cout<<"SELF INTERSECTIONS !"<<std::endl;
        return true;}
    else{
        for(int loops=0;loops<max_coarsening_attempts;loops++){
            T distance=pow((T)2,loops)*thickness_over_2;
            if(Segment_Segment_Intersection(test_segment_mesh,X,distance,false)){
                LOG::cout<<"collision at a proximity < "<<distance<<std::endl;
                return true;}
            else LOG::cout<<"ok at a proximity = "<<distance<<std::endl;
    }}
    return false;
}
//#####################################################################
// Function Segment_Segment_Intersection
//#####################################################################
template<class T> bool MESH_OBJECT<VECTOR<T,2>,1>::
Segment_Segment_Intersection(const SEGMENT_MESH& test_segment_mesh,ARRAY_VIEW<const TV> X,const T thickness_over_2,const bool update_bounding_boxes,
    ARRAY<VECTOR<int,2> >* intersecting_segment_segment_pairs)
{  
    bool intersection=false;
    ARRAY<ARRAY<int> > segments_near_segments(test_segment_mesh.elements.m);Get_Segments_Near_Segments(segments_near_segments,test_segment_mesh,X,thickness_over_2,update_bounding_boxes);
    for(int s1=0;s1<test_segment_mesh.elements.m;s1++){
        SEGMENT_2D<T> segment1(X(test_segment_mesh.elements(s1)(0)),X(test_segment_mesh.elements(s1)(1)));
        for(int k=0;k<segments_near_segments(s1).m;k++){int s2=segments_near_segments(s1)(k);
            SEGMENT_2D<T> segment2(X(mesh.elements(s2)(0)),X(mesh.elements(s2)(1)));
            if(Intersects(segment1,segment2,thickness_over_2)){intersection=true;
                if(intersecting_segment_segment_pairs) intersecting_segment_segment_pairs->Append(VECTOR<int,2>(s1,s2));else return true;}}}
    return intersection;
}
//#####################################################################
// Function Get_Triangles_Near_Edges
//#####################################################################
template<class T> void MESH_OBJECT<VECTOR<T,2>,1>::
Get_Segments_Near_Segments(ARRAY<ARRAY<int> >& segments_near_segments,const SEGMENT_MESH& test_segment_mesh,ARRAY_VIEW<const TV> X,const T thickness_over_2,
    const bool update_bounding_boxes)
{  
    bool hierarchy_defined=hierarchy!=0;
    if(!hierarchy_defined) Initialize_Hierarchy(false);
    if(!hierarchy_defined || update_bounding_boxes) hierarchy->Compute(X,false);

    for(int k=0;k<test_segment_mesh.elements.m;k++){
        int node1,node2;test_segment_mesh.elements(k).Get(node1,node2);
        RANGE<TV> box(X(node1));box.Enlarge_To_Include_Point(X(node2));
        hierarchy->Intersection_List(box,segments_near_segments(k),thickness_over_2);
        for(int kk=0;kk<segments_near_segments(k).m;kk++){int s=segments_near_segments(k)(kk); // remove elements that contain a node of the segment being tested
            for(int i=0;i<2;i++) if(mesh.elements(s)(i) == node1 || mesh.elements(s)(i) == node2){segments_near_segments(k).Remove_Index_Lazy(kk);kk--;break;}}}
    
    if(!hierarchy_defined){delete hierarchy;hierarchy=0;}
}
//#####################################################################
// Function Get_Vertex_Normals
//#####################################################################
template<class T> void MESH_OBJECT<VECTOR<T,2>,1>::
Get_Vertex_Normals(ARRAY<TV>& normals) const
{
    normals.Resize(particles.number,init_all);
    ARRAY<int> vertex_edge_numbers(particles.number);

    for(int e=0;e<mesh.elements.m;e++){
        int i,j;mesh.elements(e).Get(i,j);
        vertex_edge_numbers(i)++;
        vertex_edge_numbers(j)++;
        TV segment_normal=SEGMENT_2D<T>(particles.X(i),particles.X(j)).Normal();
        normals(i)+=segment_normal;
        normals(j)+=segment_normal;}
    for(int i=0;i<normals.Size();i++) normals(i)/=vertex_edge_numbers(i);
}
//#####################################################################
// Function Intersects
//#####################################################################
template<class T> RAY_HIT<T> Closest_Non_Intersecting_Point(const RAY<VECTOR<T,2> >& ray,
    const SEGMENTED_CURVE<VECTOR<T,2> >& curve,const T thickness_over_two)
{
    if(curve.bounding_box)
        if(curve.bounding_box->Outside(ray.endpoint,5*thickness_over_two))
            if(!Intersects(ray,*curve.bounding_box,4*thickness_over_two).Hit())
                return {};
    RAY_HIT<T> best;
    for(int i=0;i<curve.mesh.elements.m;i++){
        SEGMENT_2D<T> segment=(curve.segment_list)?(*curve.segment_list)(i):curve.Get_Element(i);
        best=best.Best(Closest_Non_Intersecting_Point(ray,segment,thickness_over_two).Set_Id(i));}
    return best;
}
//#####################################################################
template class SEGMENTED_CURVE_BASE<VECTOR<float,1> >;
template class SEGMENTED_CURVE_BASE<VECTOR<float,2> >;
template class SEGMENTED_CURVE_BASE<VECTOR<float,3> >;
template class SEGMENTED_CURVE_BASE<VECTOR<double,1> >;
template class SEGMENTED_CURVE_BASE<VECTOR<double,2> >;
template class SEGMENTED_CURVE_BASE<VECTOR<double,3> >;
template class MESH_OBJECT<VECTOR<float,1>,1>;
template class MESH_OBJECT<VECTOR<float,2>,1>;
template class MESH_OBJECT<VECTOR<float,3>,1>;
template class MESH_OBJECT<VECTOR<double,1>,1>;
template class MESH_OBJECT<VECTOR<double,2>,1>;
template class MESH_OBJECT<VECTOR<double,3>,1>;
// template RAY_HIT<float> Intersects(const RAY<VECTOR<float,2> >&,
//     const SEGMENTED_CURVE<VECTOR<float,2> >&, const float);
template RAY_HIT<float> Closest_Non_Intersecting_Point(
    const RAY<VECTOR<float,2> >&,const SEGMENTED_CURVE<VECTOR<float,2> >&,const float);
// template RAY_HIT<double> Intersects(const RAY<VECTOR<double,2> >&,
//     const SEGMENTED_CURVE<VECTOR<double,2> >&, const double);
template RAY_HIT<double> Closest_Non_Intersecting_Point(
    const RAY<VECTOR<double,2> >&,const SEGMENTED_CURVE<VECTOR<double,2> >&,const double);
}
