//#####################################################################
// Copyright 2002-2007, Robert Bridson, Ronald Fedkiw, Eran Guendelman, Geoffrey Irving, Avi Robinson-Mosher, Craig Schroeder, Tamar Shinar.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class SPHERE
//##################################################################### 
#include <Core/Basic_Geometry/SPHERE.h>
#include <Core/Basic_Geometry/RAY.h>
#include <Core/Basic_Geometry/RAY_HIT.h>
#include <Core/Geometry_Particles/GEOMETRY_PARTICLES.h>
#include <Core/Implicit_Objects/INTERSECTION_HELPERS.h>
#include <Core/Math_Tools/cube.h>
#include <Core/Matrices/FRAME.h>
#include <Core/Meshes/SEGMENTED_CURVE.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>
#include <Core/Vectors/VECTOR.h>
namespace PhysBAM{
//#####################################################################
// Function Fill_Uniform
//#####################################################################
template<class TV> void SPHERE<TV>::
Fill_Uniform(RANDOM_NUMBERS<T>& rand,const RANGE<TV>& box)
{
    rand.Fill_Uniform(radius,0,box.Edge_Lengths().Min()/2);
    rand.Fill_Uniform(center,box.min_corner+radius,box.max_corner-radius);
}
//#####################################################################
// Function Sector_Volumes
//#####################################################################
// Also see http://mathworld.wolfram.com/CircularSegment.html for fast approximate formulas
template<class T> void Sector_Volumes_Helper(const SPHERE<VECTOR<T,2> >& circle,const VECTOR<T,2>& origin,T volumes[4],const T thickness_over_two)
{
    typedef VECTOR<T,2> TV;
    T radius=circle.radius;
    TV center=circle.center;

    // sectors in usual order (left to right, bottom to top)
    if(circle.Inside(origin,thickness_over_two)){
        T radius_squared=sqr(radius),y_discriminant=sqrt(radius_squared-sqr(origin.y-center.y)),x_discriminant=sqrt(radius_squared-sqr(origin.x-center.x));
        T x0=center.x+y_discriminant,x1=center.x-y_discriminant,y0=center.y+x_discriminant,y1=center.y-x_discriminant; // intersection points
        T xl1=x0-origin.x,xl2=origin.x-x1,yl1=y0-origin.y,yl2=origin.y-y1; // the lengths of the axes inside the circle
        T ry_times_x_discriminant=(center.y-origin.y)*x_discriminant,rx_times_y_discriminant=(center.x-origin.x)*y_discriminant;
        volumes[0]=(T).5*xl2*yl2+circle.Circular_Segment_Area(max((T)0,radius-sqrt((T).5*(radius_squared+ry_times_x_discriminant+rx_times_y_discriminant))));
        volumes[1]=(T).5*xl1*yl2+circle.Circular_Segment_Area(max((T)0,radius-sqrt((T).5*(radius_squared+ry_times_x_discriminant-rx_times_y_discriminant))));
        volumes[2]=(T).5*xl2*yl1+circle.Circular_Segment_Area(max((T)0,radius-sqrt((T).5*(radius_squared-ry_times_x_discriminant+rx_times_y_discriminant))));
        volumes[3]=(T).5*xl1*yl1+circle.Circular_Segment_Area(max((T)0,radius-sqrt((T).5*(radius_squared-ry_times_x_discriminant-rx_times_y_discriminant))));}
    else if(circle.Bounding_Box().Inside(origin,thickness_over_two)){
        T horizontal_area=circle.Circular_Segment_Area(max((T)0,radius-abs(center.x-origin.x))),vertical_area=circle.Circular_Segment_Area(max((T)0,radius-abs(center.y-origin.y)));
        T remaining_area=circle.Size()-horizontal_area-vertical_area;
        if(origin.x>center.x){
            if(origin.y>center.y){volumes[0]=remaining_area;volumes[1]=horizontal_area;volumes[2]=vertical_area;volumes[3]=0;}
            else{volumes[0]=vertical_area;volumes[1]=0;volumes[2]=remaining_area;volumes[3]=horizontal_area;}}
        else{
            if(origin.y>center.y){volumes[0]=horizontal_area;volumes[1]=remaining_area;volumes[2]=0;volumes[3]=vertical_area;}
            else{volumes[0]=0;volumes[1]=vertical_area;volumes[2]=horizontal_area;volumes[3]=remaining_area;}}}
    else if(circle.Bounding_Box().Remove_Dimension(1).Inside(VECTOR<T,1>(origin.x),thickness_over_two)){ // cuts through vertically
        T left,right;
        T h=origin.x-center.x;
        if(h>0){right=circle.Circular_Segment_Area(max((T)0,radius-h));left=circle.Size()-right;}
        else{left=circle.Circular_Segment_Area(max((T)0,radius+h));right=circle.Size()-left;}
        if(origin.y>center.y){volumes[0]=left;volumes[1]=right;volumes[2]=volumes[3]=0;}
        else{volumes[2]=left;volumes[3]=right;volumes[0]=volumes[1]=0;}}
    else if(circle.Bounding_Box().Remove_Dimension(0).Inside(VECTOR<T,1>(origin.y),thickness_over_two)){
        T top,bottom;
        T h=origin.y-center.y;
        if(h>0){top=circle.Circular_Segment_Area(max((T)0,radius-h));bottom=circle.Size()-top;}
        else{bottom=circle.Circular_Segment_Area(max((T)0,radius+h));top=circle.Size()-bottom;}
        if(origin.x>center.x){volumes[0]=bottom;volumes[2]=top;volumes[1]=volumes[3]=0;}
        else{volumes[1]=bottom;volumes[3]=top;volumes[0]=volumes[2]=0;}}
    else{
        for(int i=0;i<4;i++) volumes[i]=0;
        volumes[(origin.y>center.y?0:2)+(origin.x>center.x?0:1)]=circle.Size();}
}
template<class T> void Sector_Volumes_Helper(const SPHERE<VECTOR<T,3> >& sphere,const VECTOR<T,3>& origin,T volumes[8],const T thickness_over_two)
{
    typedef VECTOR<T,3> TV;
    // TODO: this is temporary; gives box sectors, not sphere sectors
    const RANGE<TV> box=sphere.Bounding_Box();
    TV positive_lengths,max_corner=box.Maximum_Corner(),edge_lengths=box.Edge_Lengths();
    for(int i=0;i<3;i++) positive_lengths(i)=clamp(max_corner(i)-origin(i),(T)0,edge_lengths(i));
    for(int i=0;i<8;i++){volumes[i]=1;for(int j=0;j<3;j++) volumes[i]*=(i&(1<<j))?positive_lengths(j):edge_lengths(j)-positive_lengths(j);}
}
template<class TV> void SPHERE<TV>::
Sector_Volumes(const TV& origin,T volumes[1<<d],const T thickness_over_two) const
{
    Sector_Volumes_Helper(*this,origin,volumes,thickness_over_two);
}
//#####################################################################
// Function Octant_Volume_Helper
//#####################################################################
template<class T> T Octant_Volume_Helper(T x,T x1,T y,T y1)
{
    T s2=max(1-x1-y1,(T)0),s=sqrt(s2),twoxys=2*x*y*s;return ((T)1/6)*(atan2(-twoxys,s2-x1*y1)+(3-y1)*y*(atan2(x,s)-pi_v<T>/4)+(3-x1)*x*(atan2(y,s)-pi_v<T>/4)+twoxys);
}
//#####################################################################
// Function Octant_Volume_Internal
//#####################################################################
template<class T> T Octant_Volume_Internal(const VECTOR<T,3>& p)
{
    T x2=sqr(p.x),y2=sqr(p.y),z2=sqr(p.z);return Octant_Volume_Helper(p.x,x2,p.y,y2)+Octant_Volume_Helper(p.x,x2,p.z,z2)+Octant_Volume_Helper(p.y,y2,p.z,z2)-p.x*p.y*p.z+pi/6;
}
//#####################################################################
// Function Octant_Volume_Wedge
//#####################################################################
template<class T> T Octant_Volume_Wedge(T x,T y)
{
    T x2=sqr(x),y2=sqr(y);return 2*(Octant_Volume_Helper(x,x2,y,y2)-(pi_v<T>/24)*((3-x2)*x+(3-y2)*y)+pi/6);
}
//#####################################################################
// Function Octant_Volume_Cap
//#####################################################################
template<class T> T Octant_Volume_Cap(T z)
{
    return (pi_v<T>/3)*sqr(z-1)*(z+2);
}
//#####################################################################
// Function Name
//#####################################################################
template<class TV> std::string SPHERE<TV>::
Name()
{
    if(TV::m==2) return "CIRCLE<T>";
    else if(TV::m==3) return "SPHERE<T>";
    PHYSBAM_NOT_IMPLEMENTED();
}
//#####################################################################
// Function Circular_Segment_Area
//#####################################################################
template<class TV> auto SPHERE<TV>::
Circular_Segment_Area(const T h) const -> T
{
    static_assert(d==2);
    return sqr(radius)*acos((radius-h)/radius)-(radius-h)*sqrt(2*radius*h-sqr(h));
}
//#####################################################################
// Function Read
//#####################################################################
template<class TV> template<class RW> void SPHERE<TV>::
Read(UNTYPED_ISTREAM<RW>& input)
{
    Read_Binary_Group(input,center,radius);
}
//#####################################################################
// Function Write
//#####################################################################
template<class TV> template<class RW> void SPHERE<TV>::
Write(UNTYPED_OSTREAM<RW>& output) const
{
    Write_Binary_Group(output,center,radius);
}
//#####################################################################
// Function Scaled
//#####################################################################
template<class T,class TV> SPHERE<TV>
Scaled(const SPHERE<TV>& b,const TV& center,T scale)
{
    return SPHERE<TV>((b.center-center)*scale+center,b.radius*scale);
}
//#####################################################################
// Function Transformed
//#####################################################################
template<class TV> SPHERE<TV>
Transformed(const SPHERE<TV>& b,const FRAME<TV>& f)
{
    return SPHERE<TV>(f*b.center,b.radius);
}
//#####################################################################
// Function Generate_Triangles
//#####################################################################
template<class T> TRIANGULATED_SURFACE<T>* Generate_Triangles(const SPHERE<VECTOR<T,3> >& sphere,int levels)
{
    typedef VECTOR<T,3> TV;
    TRIANGULATED_SURFACE<T>* surface=TRIANGULATED_SURFACE<T>::Create();
    GEOMETRY_PARTICLES<TV>& particles=surface->particles;particles.Add_Elements(6);
    particles.X(0)=TV(-1,0,0);particles.X(1)=TV(1,0,0);particles.X(2)=TV(0,-1,0);
    particles.X(3)=TV(0,1,0);particles.X(4)=TV(0,0,-1);particles.X(5)=TV(0,0,1);
    ARRAY<VECTOR<int,3> >& triangles=surface->mesh.elements;triangles.Exact_Resize(8);
    triangles(0).Set(0,5,3);triangles(1).Set(0,2,5);triangles(2).Set(5,1,3);triangles(3).Set(5,2,1);
    triangles(4).Set(4,0,3);triangles(5).Set(4,2,0);triangles(6).Set(1,2,4);triangles(7).Set(1,4,3);
    surface->mesh.number_nodes=6;
    surface->mesh.Initialize_Neighbor_Nodes();
    for(int i=0;i<levels;i++) surface->Root_Three_Subdivide();
    for(int p=0;p<particles.Size();p++) particles.X(p)=sphere.center+sphere.radius*particles.X(p).Normalized();
    return surface;
}
template<class T> TRIANGULATED_AREA<T>* Generate_Triangles(const SPHERE<VECTOR<T,2> >& circle,int levels)
{
    assert(levels>=1);
    typedef VECTOR<T,2> TV;
    typedef VECTOR<int,3> E;
    TRIANGULATED_AREA<T>* area=TRIANGULATED_AREA<T>::Create();
    GEOMETRY_PARTICLES<TV>& particles=area->particles;particles.Add_Elements(1+3*levels*(levels+1));
    particles.X(0)=TV(0,0);
    for(int i=0,k=1;i<levels;i++)
        for(int j=0;j<i*6+6;j++){
            T a=pi_v<T>/3*(j+1-(T).5*i)/(i+1);
            particles.X(k++)=(T)(i+1)*TV(cos(a),sin(a));}
    for(int i=1;i<7;i++) area->mesh.elements.Append(E(0,i,i%6+1));
    for(int i=1,p=1;i<levels;i++){
        int n=i*6+6;
        int c=p+n-6;
        area->mesh.elements.Append(E(c-1,c+n-1,c));
        area->mesh.elements.Append(E(c,p,c-1));
        for(int j=0;j<n-1;j++){
            area->mesh.elements.Append(E(p,c+j,c+j+1));
            if(j%(i+1)!=i/2){
                area->mesh.elements.Append(E(c+j+1,p+1,p));
                p++;}}
        p=c;}
    particles.X=particles.X*(circle.radius/levels)+circle.center;
    area->Update_Number_Nodes();
    return area;
}
template<class T> SEGMENTED_CURVE<VECTOR<T,2> >* Tessellate_Boundary(const SPHERE<VECTOR<T,2> >& sphere,int levels,bool exp_scale)
{
    typedef VECTOR<T,2> TV;
    assert(levels>=1);
    SEGMENTED_CURVE<TV>* curve=SEGMENTED_CURVE<TV>::Create();
    int n=exp_scale?(1<<levels):levels;
    curve->particles.Add_Elements(n);
    for(int i=0;i<n;i++) curve->particles.X(i)=TV((T)cos(pi_v<T>*2*i/n),(T)sin(pi_v<T>*2*i/n))*sphere.radius+sphere.center;
    for(int i=0;i<n-1;i++) curve->mesh.elements.Append(VECTOR<int,2>(i,i+1));
    curve->mesh.elements.Append(VECTOR<int,2>(n-1,0));
    curve->Update_Number_Nodes();
    return curve;
}
//#####################################################################
// Function Intersects
//#####################################################################
template<class TV> RAY_HIT<typename TV::SCALAR>
Intersects(const RAY<TV>& ray,const SPHERE<TV>& sphere,typename TV::SCALAR isocontour)
{
    typedef typename TV::SCALAR T;
    // translate ray so that the sphere can be considered at the origin
    TV ray_endpoint=ray.endpoint-sphere.center;
    T radius=sphere.radius+isocontour,radius_squared=sqr(radius);
    T distance_squared=ray_endpoint.Magnitude_Squared();
    T c=distance_squared-radius_squared;
    T b=ray.direction.Dot(ray_endpoint);
    T d=sqr(b)-c;
    if(c<=0) return RAY_HIT<T>{-b+sqrt(d)}; // starts inside; large of the two roots
    if(b>=0) return {}; // no intersection - ray goes away from sphere
    if(d<0) return {}; // no intersection - ray misses sphere
    return RAY_HIT<T>{-b-sqrt(d)}; // smaller of the two roots
}
//#####################################################################
// Function All_Intersections
//#####################################################################
template<class TV,class T> void
All_Intersections(ARRAY<RAY_HIT<T> >& hits,const RAY<TV>& ray,
    const SPHERE<TV>& sphere,const T isocontour)
{
    auto iv=Intersection_Interval(ray,sphere,isocontour);
    hits=iv.Array_View(0,Actual_Length(iv));
}
//#####################################################################
// Function Intersection_Interval
//#####################################################################
template<class TV> VECTOR<RAY_HIT<typename TV::SCALAR>,2>
Intersection_Interval(const RAY<TV>& ray,const SPHERE<TV>& sphere,
    typename TV::SCALAR isocontour)
{
    typedef typename TV::SCALAR T;
    typedef RAY_HIT<T> H;
    // translate ray so that the sphere can be considered at the origin
    TV ray_endpoint=ray.endpoint-sphere.center;
    T radius=sphere.radius+isocontour,radius_squared=sqr(radius);
    T distance_squared=ray_endpoint.Magnitude_Squared();
    T c=distance_squared-radius_squared;
    T b=ray.direction.Dot(ray_endpoint);
    T d=sqr(b)-c;
    if(c<=0) return {H{0},H{-b+sqrt(d)}}; // starts inside; large of the two roots
    if(b>=0) return {}; // no intersection - ray goes away from sphere
    if(d<0) return {}; // no intersection - ray misses sphere
    d=sqrt(d);
    return {H{-b-d},H{-b+d}}; // both roots
}
//#####################################################################
template RAY_HIT<float> Intersects(const RAY<VECTOR<float,1> >&,const SPHERE<VECTOR<float,1> >&,float);
template RAY_HIT<float> Intersects(const RAY<VECTOR<float,2> >&,const SPHERE<VECTOR<float,2> >&,float);
template RAY_HIT<float> Intersects(const RAY<VECTOR<float,3> >&,const SPHERE<VECTOR<float,3> >&,float);
template RAY_HIT<double> Intersects(const RAY<VECTOR<double,1> >&,const SPHERE<VECTOR<double,1> >&,double);
template RAY_HIT<double> Intersects(const RAY<VECTOR<double,2> >&,const SPHERE<VECTOR<double,2> >&,double);
template RAY_HIT<double> Intersects(const RAY<VECTOR<double,3> >&,const SPHERE<VECTOR<double,3> >&,double);
//#####################################################################
#define P(...) __VA_ARGS__
#define INSTANTIATION_HELPER_T_TV(T,TV,d) \
    template void SPHERE<TV>::Sector_Volumes(const TV& origin,T volumes[1<<d],const T thickness_over_two) const; \
    template std::string SPHERE<TV>::Name();
#define INSTANTIATION_HELPER(T,d) INSTANTIATION_HELPER_T_TV(T,P(VECTOR<T,d> ),d)
INSTANTIATION_HELPER(float,2)
INSTANTIATION_HELPER(float,3)
INSTANTIATION_HELPER(double,2)
INSTANTIATION_HELPER(double,3)
template void SPHERE<VECTOR<double,2> >::Read(UNTYPED_ISTREAM<double>&);
template void SPHERE<VECTOR<double,2> >::Read(UNTYPED_ISTREAM<float>&);
template void SPHERE<VECTOR<double,2> >::Read(UNTYPED_ISTREAM<char>&);
template void SPHERE<VECTOR<double,2> >::Write(UNTYPED_OSTREAM<double>&) const;
template void SPHERE<VECTOR<double,2> >::Write(UNTYPED_OSTREAM<float>&) const;
template void SPHERE<VECTOR<double,2> >::Write(UNTYPED_OSTREAM<char>&) const;
template void SPHERE<VECTOR<double,3> >::Read(UNTYPED_ISTREAM<double>&);
template void SPHERE<VECTOR<double,3> >::Read(UNTYPED_ISTREAM<float>&);
template void SPHERE<VECTOR<double,3> >::Read(UNTYPED_ISTREAM<char>&);
template void SPHERE<VECTOR<double,3> >::Write(UNTYPED_OSTREAM<double>&) const;
template void SPHERE<VECTOR<double,3> >::Write(UNTYPED_OSTREAM<float>&) const;
template void SPHERE<VECTOR<double,3> >::Write(UNTYPED_OSTREAM<char>&) const;
template void SPHERE<VECTOR<float,2> >::Read(UNTYPED_ISTREAM<double>&);
template void SPHERE<VECTOR<float,2> >::Read(UNTYPED_ISTREAM<float>&);
template void SPHERE<VECTOR<float,2> >::Read(UNTYPED_ISTREAM<char>&);
template void SPHERE<VECTOR<float,2> >::Write(UNTYPED_OSTREAM<double>&) const;
template void SPHERE<VECTOR<float,2> >::Write(UNTYPED_OSTREAM<float>&) const;
template void SPHERE<VECTOR<float,2> >::Write(UNTYPED_OSTREAM<char>&) const;
template void SPHERE<VECTOR<float,3> >::Read(UNTYPED_ISTREAM<double>&);
template void SPHERE<VECTOR<float,3> >::Read(UNTYPED_ISTREAM<float>&);
template void SPHERE<VECTOR<float,3> >::Read(UNTYPED_ISTREAM<char>&);
template void SPHERE<VECTOR<float,3> >::Write(UNTYPED_OSTREAM<double>&) const;
template void SPHERE<VECTOR<float,3> >::Write(UNTYPED_OSTREAM<float>&) const;
template void SPHERE<VECTOR<float,3> >::Write(UNTYPED_OSTREAM<char>&) const;
template SPHERE<VECTOR<double,3> > Scaled<double,VECTOR<double,3> >(SPHERE<VECTOR<double,3> > const&,VECTOR<double,3> const&,double);
template SPHERE<VECTOR<double,3> > Transformed<VECTOR<double,3> >(SPHERE<VECTOR<double,3> > const&,FRAME<VECTOR<double,3> > const&);
template SPHERE<VECTOR<float,3> > Scaled<float,VECTOR<float,3> >(SPHERE<VECTOR<float,3> > const&,VECTOR<float,3> const&,float);
template SPHERE<VECTOR<float,3> > Transformed<VECTOR<float,3> >(SPHERE<VECTOR<float,3> > const&,FRAME<VECTOR<float,3> > const&);
template TRIANGULATED_SURFACE<float>* Generate_Triangles(const SPHERE<VECTOR<float,3> >&,int);
template TRIANGULATED_AREA<float>* Generate_Triangles(const SPHERE<VECTOR<float,2> >&,int);
template SEGMENTED_CURVE<VECTOR<float,2> >* Tessellate_Boundary(const SPHERE<VECTOR<float,2> >&,int,bool);
template TRIANGULATED_SURFACE<double>* Generate_Triangles(const SPHERE<VECTOR<double,3> >&,int);
template TRIANGULATED_AREA<double>* Generate_Triangles(const SPHERE<VECTOR<double,2> >&,int);
template SEGMENTED_CURVE<VECTOR<double,2> >* Tessellate_Boundary(const SPHERE<VECTOR<double,2> >&,int,bool);
template SPHERE<VECTOR<double,2> > Scaled<double,VECTOR<double,2> >(SPHERE<VECTOR<double,2> > const&,VECTOR<double,2> const&,double);
template SPHERE<VECTOR<double,2> > Transformed<VECTOR<double,2> >(SPHERE<VECTOR<double,2> > const&,FRAME<VECTOR<double,2> > const&);
template void All_Intersections<VECTOR<double,2>,double>(ARRAY<RAY_HIT<double>,int>&,RAY<VECTOR<double,2> > const&,SPHERE<VECTOR<double,2> > const&,double);
template void All_Intersections<VECTOR<double,3>,double>(ARRAY<RAY_HIT<double>,int>&,RAY<VECTOR<double,3> > const&,SPHERE<VECTOR<double,3> > const&,double);
template void All_Intersections<VECTOR<float,2>,float>(ARRAY<RAY_HIT<float>,int>&,RAY<VECTOR<float,2> > const&,SPHERE<VECTOR<float,2> > const&,float);
template void All_Intersections<VECTOR<float,3>,float>(ARRAY<RAY_HIT<float>,int>&,RAY<VECTOR<float,3> > const&,SPHERE<VECTOR<float,3> > const&,float);
template void SPHERE<VECTOR<double,2> >::Fill_Uniform(RANDOM_NUMBERS<double>&,RANGE<VECTOR<double,2> > const&);
template void SPHERE<VECTOR<double,3> >::Fill_Uniform(RANDOM_NUMBERS<double>&,RANGE<VECTOR<double,3> > const&);
template void SPHERE<VECTOR<float,2> >::Fill_Uniform(RANDOM_NUMBERS<float>&,RANGE<VECTOR<float,2> > const&);
template void SPHERE<VECTOR<float,3> >::Fill_Uniform(RANDOM_NUMBERS<float>&,RANGE<VECTOR<float,3> > const&);
template void SPHERE<VECTOR<double,1> >::Fill_Uniform(RANDOM_NUMBERS<double>&,RANGE<VECTOR<double,1> > const&);
template void SPHERE<VECTOR<float,1> >::Fill_Uniform(RANDOM_NUMBERS<float>&,RANGE<VECTOR<float,1> > const&);
}
