//#####################################################################
// Copyright 2002-2023, Ronald Fedkiw, Geoffrey Irving, Neil Molino, Craig Schroeder, Jonathan Su.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class CYLINDER
//##################################################################### 
#include <Core/Basic_Geometry/CYLINDER.h>
#include <Core/Basic_Geometry/ORIENTED_BOX.h>
#include <Core/Basic_Geometry/RAY.h>
#include <Core/Basic_Geometry/RAY_HIT.h>
#include <Core/Basic_Geometry/SPHERE.h>
#include <Core/Implicit_Objects/INTERSECTION_HELPERS.h>
#include <Core/Math_Tools/cube.h>
#include <Core/Matrices/FRAME.h>
#include <Core/Meshes/TRIANGULATED_AREA.h>
#include <Core/Meshes/TRIANGULATED_SURFACE.h>
#include <Core/Random_Numbers/RANDOM_NUMBERS.h>
#include <Core/Read_Write/READ_WRITE.h>
namespace PhysBAM{
//#####################################################################
// Function Normal
//#####################################################################
template<class T> VECTOR<T,3> CYLINDER<T>::
Normal(const TV& location,const int aggregate) const 
{
    assert(aggregate >= 1 && aggregate <= 3);
    if(aggregate == 1) return (location-plane1.x0).Projected_Orthogonal_To_Unit_Direction(plane1.normal).Normalized(); // CYLINDER
    else if(aggregate == 2) return plane1.Normal();
    else return plane2.Normal();
}
//#####################################################################
// Function Inside
//#####################################################################
template<class T> bool CYLINDER<T>::
Inside(const TV& location,const T thickness_over_two) const 
{
    if(!plane1.Inside(location,thickness_over_two) || !plane2.Inside(location,thickness_over_two)) return false;
    return (location-plane1.x0).Projected_Orthogonal_To_Unit_Direction(plane1.normal).Magnitude_Squared() <= sqr(radius-thickness_over_two);
}
//#####################################################################
// Function Lazy_Inside
//#####################################################################
template<class T> bool CYLINDER<T>::
Lazy_Inside(const TV& location) const 
{
    if(!plane1.Lazy_Inside(location) || !plane2.Lazy_Inside(location)) return false;
    return (location-plane1.x0).Projected_Orthogonal_To_Unit_Direction(plane1.normal).Magnitude_Squared() <= sqr(radius);
}
//#####################################################################
// Function Outside
//#####################################################################
template<class T> bool CYLINDER<T>::
Outside(const TV& location,const T thickness_over_two) const  
{
    if(plane1.Outside(location,thickness_over_two) || plane2.Outside(location,thickness_over_two)) return true;
    return (location-plane1.x0).Projected_Orthogonal_To_Unit_Direction(plane1.normal).Magnitude_Squared() >= sqr(radius+thickness_over_two);
}
//#####################################################################
// Function Lazy_Outside
//#####################################################################
template<class T> bool CYLINDER<T>::
Lazy_Outside(const TV& location) const  
{
    if(plane1.Lazy_Outside(location) || plane2.Lazy_Outside(location)) return true;
    return (location-plane1.x0).Projected_Orthogonal_To_Unit_Direction(plane1.normal).Magnitude_Squared() >= sqr(radius);
}
//#####################################################################
// Function Boundary
//#####################################################################
template<class T> bool CYLINDER<T>::
Boundary(const TV& location,const T thickness_over_two) const
{
    return !Inside(location,thickness_over_two) && !Outside(location,thickness_over_two);
}
//#####################################################################
// Function Surface
//#####################################################################
template<class T> VECTOR<T,3> CYLINDER<T>::
Surface(const TV& location) const
{
    TV v=location-plane1.x0;T axial_distance=-v.Dot(plane1.normal);
    TV radial_direction=v+axial_distance*plane1.normal;T radial_distance=radial_direction.Normalize();
    T radial_distance_minus_radius=radial_distance-radius;
    if(radial_distance_minus_radius>0 || (radial_distance_minus_radius>-axial_distance && radial_distance_minus_radius>axial_distance-height)) // closest point is on infinite CYLINDER
        return plane1.x0-clamp(axial_distance,(T)0,height)*plane1.normal+radius*radial_direction;
    if(axial_distance < height-axial_distance) return location+axial_distance*plane1.normal; // closest point is on plane1
    else return location-(height-axial_distance)*plane1.normal; // closest point is on plane2
}
//#####################################################################
// Function Signed_Distance
//#####################################################################
template<class T> T CYLINDER<T>::
Signed_Distance(const TV& location) const
{  
    TV v=location-plane1.x0;
    T plane1_distance=v.Dot(plane1.normal),plane_distance=max(plane1_distance,-height-plane1_distance);
    T cylinder_distance=(v-plane1_distance*plane1.normal).Magnitude()-radius;
    return cylinder_distance>0 && plane_distance>0?sqrt(sqr(cylinder_distance)+sqr(plane_distance)):max(cylinder_distance,plane_distance);
}
//#####################################################################
// Function Normal
//#####################################################################
template<class T> VECTOR<T,3> CYLINDER<T>::
Normal(const TV& location) const
{
    TV v=location-plane1.x0;
    T plane1_distance=v.Dot(plane1.normal),plane2_distance=-height-plane1_distance;
    TV infinite_cylinder_normal=v-plane1_distance*plane1.normal;
    T cylinder_distance=infinite_cylinder_normal.Normalize()-radius;
    if(plane1_distance>=plane2_distance){
        if(cylinder_distance>0 && plane1_distance>0){
            T magnitude=sqrt(sqr(cylinder_distance)+sqr(plane1_distance));
            return cylinder_distance/magnitude*infinite_cylinder_normal+plane1_distance/magnitude*plane1.normal;}
        else if(cylinder_distance>plane1_distance) return infinite_cylinder_normal;
        return plane1.normal;}
    if(cylinder_distance>0 && plane2_distance>0){
        T magnitude=sqrt(sqr(cylinder_distance)+sqr(plane2_distance));
        return cylinder_distance/magnitude*infinite_cylinder_normal+plane2_distance/magnitude*plane2.normal;}
    else if(cylinder_distance>plane2_distance) return infinite_cylinder_normal;
    return plane2.normal;
}
//#####################################################################
// Function Hessian
//#####################################################################
template<class T> SYMMETRIC_MATRIX<T,3> CYLINDER<T>::
Hessian(const TV& X) const
{
    TV v=X-plane1.x0;
    T plane1_distance=v.Dot(plane1.normal),plane_distance=max(plane1_distance,-height-plane1_distance);
    TV infinite_cylinder_normal=v-plane1_distance*plane1.normal;
    T norm_infinite_cylinder_normal=infinite_cylinder_normal.Normalize();
    T cylinder_distance=norm_infinite_cylinder_normal-radius;

    bool edge=(cylinder_distance>0 && plane_distance>0),face=plane_distance>cylinder_distance;
    if(!edge && face) return SYMMETRIC_MATRIX<T,3>();

    TV diff_cylinder_distance=infinite_cylinder_normal-plane1.normal.Dot(infinite_cylinder_normal)*plane1.normal;
    T norm2_v=v.Magnitude_Squared();
    T Hd=cube(norm_infinite_cylinder_normal);
    SYMMETRIC_MATRIX<T,3> H0=norm2_v-Outer_Product(v);
    SYMMETRIC_MATRIX<T,3> H1=norm2_v*Outer_Product(plane1.normal);
    SYMMETRIC_MATRIX<T,3> H2=plane1_distance*Symmetric_Outer_Product(plane1.normal,v);
    SYMMETRIC_MATRIX<T,3> hess_cylinder_distance=(H0-H1+H2-sqr(plane1_distance))/Hd;
    if(!edge) return hess_cylinder_distance;

    TV diff_plane_distance=(plane1_distance>-height-plane1_distance)?plane1.normal:-plane1.normal;
    T a=sqrt(sqr(cylinder_distance)+sqr(plane_distance));
    TV da=(cylinder_distance*diff_cylinder_distance+plane_distance*diff_plane_distance)/a;
    SYMMETRIC_MATRIX<T,3> A0=Outer_Product(diff_cylinder_distance);
    SYMMETRIC_MATRIX<T,3> A1=cylinder_distance*hess_cylinder_distance;
    SYMMETRIC_MATRIX<T,3> A2=Outer_Product(plane1.normal);
    SYMMETRIC_MATRIX<T,3> A3=Outer_Product(da);
    return (A0+A1+A2-A3)/a;
}
//#####################################################################
// Function Bounding_Box
//#####################################################################
template<class T> RANGE<VECTOR<T,3> > CYLINDER<T>::
Bounding_Box() const
{
    TV C=(plane1.x0+plane2.x0)/2,u=plane1.normal,w=height/2*abs(u),mags;
    auto M=Outer_Product(u,u)-1;
    for(int i=0;i<TV::m;i++) mags(i)=M.Column(i).Magnitude();
    return RANGE<TV>(C).Thickened(w+mags*radius);
}
//#####################################################################
// Function Volume
//#####################################################################
template<class T> T CYLINDER<T>::
Volume() const
{
    return pi_v<T>*height*sqr(radius); 
}
//#####################################################################
// Function Read
//#####################################################################
template<class T> template<class RW> void CYLINDER<T>::
Read(UNTYPED_ISTREAM<RW>& input)
{
    Read_Binary_Group(input,plane1.x0,plane2.x0,radius);
    Set_Endpoints(plane1.x0,plane2.x0);
}
//#####################################################################
// Function Write
//#####################################################################
template<class T> template<class RW> void CYLINDER<T>::
Write(UNTYPED_OSTREAM<RW>& output) const
{
    Write_Binary_Group(output,plane1.x0,plane2.x0,radius);
}
//#####################################################################
// Function Fill_Uniform
//#####################################################################
template<class T> void CYLINDER<T>::
Fill_Uniform(RANDOM_NUMBERS<T>& rand,const RANGE<TV>& box)
{
    TV X,Y;
    rand.Fill_Uniform(X,box);
    rand.Fill_Uniform(Y,box);
    TV C=(X+Y)/2,u=(Y-X).Normalized(),w=(Y-X).Magnitude()/2*abs(u),cm;
    auto R=(box-C).Thickened(-w);
    auto RS=R.max_corner.Componentwise_Min(-R.min_corner);
    auto M=Outer_Product(u,u)-1;
    for(int i=0;i<TV::m;i++) cm(i)=M.Column(i).Magnitude();
    T max_r=(RS/cm).Min(),radius;
    rand.Fill_Uniform(radius,0,max_r);
    *this=CYLINDER<T>(X,Y,radius);
}
//#####################################################################
// Function Scaled
//#####################################################################
template<class T,class TV> CYLINDER<T>
Scaled(const CYLINDER<T>& b,const TV& center,T scale)
{
    return CYLINDER<T>((b.plane1.x0-center)*scale+center,
        (b.plane2.x0-center)*scale+center,b.radius*scale);
}
//#####################################################################
// Function Transformed
//#####################################################################
template<class T,class TV> CYLINDER<T>
Transformed(const CYLINDER<T>& b,const FRAME<TV>& f)
{
    return CYLINDER<T>(f*b.plane1.x0,f*b.plane2.x0,b.radius);
}
template<class T> TRIANGULATED_SURFACE<T>* Generate_Triangles(const CYLINDER<T>& cylinder,const int resolution_height,const int resolution_radius)
{
    typedef VECTOR<T,3> TV;
    TRIANGULATED_SURFACE<T>* surface=TRIANGULATED_SURFACE<T>::Create();
    SPHERE<VECTOR<T,2> > circle({},cylinder.radius);
    TRIANGULATED_AREA<T>* area=Generate_Triangles(circle,resolution_radius);
    int outer_start=area->particles.X.m-6*resolution_radius;
    surface->mesh.elements=area->mesh.elements;
    for(auto&e:area->mesh.elements) std::swap(e.x,e.y);

    TRIANGLE_MESH tm;
    tm.Initialize_Cylinder_Mesh(resolution_height+1,6*resolution_radius,false);
    tm.elements.Flattened()+=outer_start;
    surface->mesh.elements.Append(tm.elements);
    int num_p=area->particles.X.m+6*resolution_radius*resolution_height;
    for(auto& i:area->mesh.elements.Flattened())
    {
        if(i<outer_start) i+=num_p;
        else i+=6*resolution_radius*resolution_height;
    }
    surface->mesh.elements.Append(area->mesh.elements);
    surface->particles.Add_Elements(2*area->particles.X.m+6*resolution_radius*(resolution_height-1));
    TV X=cylinder.plane1.x0,Z=cylinder.plane2.x0;
    TV u=cylinder.plane2.normal;
    TV v=u.Unit_Orthogonal_Vector();
    TV w=v.Cross(u);
    for(int i=0;i<outer_start;i++)
    {
        auto Y=area->particles.X(i);
        surface->particles.X(i)=X+v*Y.x+w*Y.y;
        surface->particles.X(i+num_p)=Z+v*Y.x+w*Y.y;
    }
    TV dr=(Z-X)/resolution_height;
    for(int l=0;l<=resolution_height;l++)
    {
        int start=outer_start+6*resolution_radius*l;
        for(int i=0;i<6*resolution_radius;i++)
        {
            auto Y=area->particles.X(i+outer_start);
            surface->particles.X(i+start)=X+l*dr+v*Y.x+w*Y.y;
        }
    }
    delete area;
    surface->Update_Number_Nodes();
    return surface;
}
//#####################################################################
// Function Intersection_Interval_Infinite
//#####################################################################
template<class T> VECTOR<RAY_HIT<T>,2>
Intersection_Interval_Infinite(const RAY<VECTOR<T,3> >& ray,const CYLINDER<T>& cylinder,T isocontour)
{
    typedef VECTOR<T,3> TV;
    typedef RAY_HIT<T> H;
    const auto& n=cylinder.plane1.normal;
    TV w=ray.endpoint-cylinder.plane1.x0;
    TV f=w-w.Dot(n)*n;
    TV g=ray.direction-ray.direction.Dot(n)*n;
    T r2=sqr(cylinder.radius+isocontour);
    T a=g.Dot(g);
    T b=f.Dot(g);
    T c=f.Dot(f)-r2;
    T d=b*b-a*c;
    if(c<0) return {H{0},H{(-b+sqrt(d))/a}};
    if(b>=0 || d<0) return {};
    T e=sqrt(d);
    return {H{(-b-e)/a},H{(-b+e)/a}};
}
//#####################################################################
// Function Intersects
//#####################################################################
template<class T> RAY_HIT<T> Intersects(const RAY<VECTOR<T,3> >& ray,const CYLINDER<T>& cylinder,const T isocontour)
{
    return Ray_Hit(Intersection_Interval(ray,cylinder,isocontour));
}
//#####################################################################
// Function Intersection_Interval
//#####################################################################
template<class T> VECTOR<RAY_HIT<T>,6>
Intersection_Interval(const RAY<VECTOR<T,3> >& ray,const CYLINDER<T>& cylinder,
    T isocontour)
{
    auto i1=Intersection_Interval_Infinite(ray,cylinder,isocontour);
    auto i2=Intersection_Interval(ray,cylinder.plane1,isocontour);
    auto i3=Intersection_Interval(ray,cylinder.plane2,isocontour);
    Set_Id(i1,1);
    Set_Id(i2,2);
    Set_Id(i3,3);
    return Intersection(Intersection(i1,i2),i3);
}
//#####################################################################
// Function All_Intersections
//#####################################################################
template<class TV,class T> void
All_Intersections(ARRAY<RAY_HIT<T> >& hits,const RAY<TV>& ray,
    const CYLINDER<T>& cylinder,const T isocontour)
{
    auto iv=Intersection_Interval(ray,cylinder,isocontour);
    hits=iv.Array_View(0,Actual_Length(iv));
}
//#####################################################################
template class CYLINDER<float>;
template class CYLINDER<double>;
template CYLINDER<double> Scaled<double,VECTOR<double,3> >(CYLINDER<double> const&,VECTOR<double,3> const&,double);
template CYLINDER<double> Transformed<double,VECTOR<double,3> >(CYLINDER<double> const&,FRAME<VECTOR<double,3> > const&);
template CYLINDER<float> Scaled<float,VECTOR<float,3> >(CYLINDER<float> const&,VECTOR<float,3> const&,float);
template CYLINDER<float> Transformed<float,VECTOR<float,3> >(CYLINDER<float> const&,FRAME<VECTOR<float,3> > const&);
template TRIANGULATED_SURFACE<float>* Generate_Triangles(const CYLINDER<float>&,const int,const int);
template TRIANGULATED_SURFACE<double>* Generate_Triangles(const CYLINDER<double>&,const int,const int);
template RAY_HIT<float> Intersects(const RAY<VECTOR<float,3> >&,const CYLINDER<float>&,const float);
template RAY_HIT<double> Intersects(const RAY<VECTOR<double,3> >&,const CYLINDER<double>&,const double);
template VECTOR<RAY_HIT<double>,2> Intersection_Interval_Infinite<double>(RAY<VECTOR<double,3> > const&,CYLINDER<double> const&,double);
template VECTOR<RAY_HIT<float>,2> Intersection_Interval_Infinite<float>(RAY<VECTOR<float,3> > const&,CYLINDER<float> const&,float);
template void All_Intersections<VECTOR<double,3>,double>(ARRAY<RAY_HIT<double>,int>&,RAY<VECTOR<double,3> > const&,CYLINDER<double> const&,double);
template void All_Intersections<VECTOR<float,3>,float>(ARRAY<RAY_HIT<float>,int>&,RAY<VECTOR<float,3> > const&,CYLINDER<float> const&,float);
template void CYLINDER<double>::Read(UNTYPED_ISTREAM<char>&);
template void CYLINDER<double>::Read(UNTYPED_ISTREAM<double>&);
template void CYLINDER<double>::Read(UNTYPED_ISTREAM<float>&);
template void CYLINDER<double>::Write(UNTYPED_OSTREAM<char>&) const;
template void CYLINDER<double>::Write(UNTYPED_OSTREAM<double>&) const;
template void CYLINDER<double>::Write(UNTYPED_OSTREAM<float>&) const;
template void CYLINDER<float>::Read(UNTYPED_ISTREAM<char>&);
template void CYLINDER<float>::Read(UNTYPED_ISTREAM<double>&);
template void CYLINDER<float>::Read(UNTYPED_ISTREAM<float>&);
template void CYLINDER<float>::Write(UNTYPED_OSTREAM<char>&) const;
template void CYLINDER<float>::Write(UNTYPED_OSTREAM<double>&) const;
template void CYLINDER<float>::Write(UNTYPED_OSTREAM<float>&) const;
}
