//#####################################################################
// Copyright 2006-2007, Geoffrey Irving, 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.
//#####################################################################
#include <Core/Basic_Geometry/TORUS.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/Matrices/FRAME.h>
#include <Core/Matrices/LAPACK.h>
#include <Core/Meshes/TRIANGULATED_SURFACE.h>
#include <Core/Polynomials/Polynomial_Roots.h>
#include <Core/Random_Numbers/RANDOM_NUMBERS.h>
#include <Core/Read_Write/READ_WRITE.h>
namespace PhysBAM{
//#####################################################################
// Function Fill_Uniform
//#####################################################################
template<class T> void TORUS<T>::
Fill_Uniform(RANDOM_NUMBERS<T>& rand,const RANGE<TV>& box)
{
    rand.Fill_Uniform(center,box.min_corner,box.max_corner);
    axis=rand.template Get_Direction<TV>();
    auto R=box-center;
    auto S=R.max_corner.Componentwise_Min(-R.min_corner);
    T x=sqr(axis.x),y=sqr(axis.y),z=sqr(axis.z);
    auto u=sqrt(TV(y+z,x+z,x+y));
    rand.Fill_Uniform(outer_radius,0,(S/u).Min());
    rand.Fill_Uniform(inner_radius,0,(S-u*outer_radius).Min());
}
//#####################################################################
// Function Signed_Distance
//#####################################################################
template<class T> T TORUS<T>::
Signed_Distance(const TV& X) const
{
    TV x=X-center;
    T axial=x.Dot(axis);
    T radial=(x-axial*axis).Magnitude();
    return sqrt(sqr(radial-outer_radius)+sqr(axial))-inner_radius;
}
//#####################################################################
// Function Normal
//#####################################################################
template<class T> VECTOR<T,3> TORUS<T>::
Normal(const TV& X) const
{
    TV Y=X-center;
    T a=Y.Dot(axis);
    TV dw=Y-a*axis;
    T w=dw.Normalize();
    T z=w-outer_radius,p=hypot(z,a),e=z/p;
    return a/p*axis+e*dw;
}
//#####################################################################
// Function Hessian
//#####################################################################
template<class T> SYMMETRIC_MATRIX<T,3> TORUS<T>::
Hessian(const TV& X) const
{
    TV Y=X-center;
    T a=Y.Dot(axis);
    TV dw=Y-a*axis;
    T w=dw.Normalize();
    T z=w-outer_radius,p=hypot(z,a),e=z/p;
    return e/w*((T)1-Outer_Product(axis)-Outer_Product(dw))+Outer_Product(a/p*dw-e*axis)/p;
}
//#####################################################################
// Function Bounding_Box
//#####################################################################
template<class T> RANGE<VECTOR<T,3> > TORUS<T>::
Bounding_Box() const
{
    T x=sqr(axis.x),y=sqr(axis.y),z=sqr(axis.z);
    TV corner_offset=sqrt(TV(y+z,x+z,x+y))*outer_radius+inner_radius;
    return RANGE<TV>(center-corner_offset,center+corner_offset);
}
//######################################################################
// Function Volume
//#####################################################################
template<class T> T TORUS<T>::
Volume() const
{
    return 2*sqr(pi_v<T>)*sqr(inner_radius)*outer_radius;
}
//#####################################################################
// Function Read
//#####################################################################
template<class T> template<class RW> void TORUS<T>::
Read(UNTYPED_ISTREAM<RW>& input)
{
    Read_Binary_Group(input,center,axis,inner_radius,outer_radius);
}
//#####################################################################
// Function Write
//#####################################################################
template<class T> template<class RW> void TORUS<T>::
Write(UNTYPED_OSTREAM<RW>& output) const
{
    Write_Binary_Group(output,center,axis,inner_radius,outer_radius);
}
//#####################################################################
// Function Scaled
//#####################################################################
template<class T,class TV> TORUS<T>
Scaled(const TORUS<T>& b,const TV& center,T scale)
{
    return TORUS<T>((b.center-center)*scale+center,b.axis,
        b.inner_radius*scale,b.outer_radius*scale);
}
//#####################################################################
// Function Transformed
//#####################################################################
template<class T,class TV> TORUS<T>
Transformed(const TORUS<T>& b,const FRAME<TV>& f)
{
    return TORUS<T>(f*b.center,f.r.Rotate(b.axis),b.inner_radius,b.outer_radius);
}
//#####################################################################
// Function Generate_Triangles
//#####################################################################
// m is inner_resolution, n is outer_resolution
template<class T> TRIANGULATED_SURFACE<T>*
Generate_Triangles(const TORUS<T>& torus,const int m,const int n)
{
    typedef VECTOR<T,3> TV;
    T mscale=pi_v<T>*2/m,nscale=pi_v<T>*2/n;
    TV y=torus.axis.Unit_Orthogonal_Vector();
    MATRIX<T,3> transform{y,torus.axis.Cross(y),torus.axis};
    TRIANGULATED_SURFACE<T>* surface=TRIANGULATED_SURFACE<T>::Create();
    GEOMETRY_PARTICLES<TV>& particles=surface->particles;particles.Add_Elements(m*n);
    TRIANGLE_MESH& mesh=surface->mesh;mesh.number_nodes=m*n;mesh.elements.Preallocate(2*m*n);
    for(int i=0;i<m;++i){
        T axial=torus.inner_radius*cos(i*mscale),inplane=torus.inner_radius*sin(i*mscale)+torus.outer_radius;
        for(int j=0;j<n;j++){
            T cj=cos(j*nscale),sj=sin(j*nscale);
            particles.X(i+j*m)=torus.center+transform*TV(cj*inplane,sj*inplane,axial);
            int ni=(i+1)%m,nj=(j+1)%n,p00=i+j*m,p01=i+nj*m,p10=ni+j*m,p11=p10+p01-p00;
            mesh.elements.Append(VECTOR<int,3>(p00,p10,p01));
            mesh.elements.Append(VECTOR<int,3>(p11,p01,p10));}}
    return surface;
}
//#####################################################################
// Function Intersection_Interval
//#####################################################################
template<class T> VECTOR<RAY_HIT<T>,4>
Intersection_Interval(const RAY<VECTOR<T,3> >& ray,const TORUS<T>& torus,T isocontour)
{
    typedef VECTOR<T,3> TV;typedef RAY_HIT<T> H;
    T i2=sqr(torus.inner_radius+isocontour);
    T o2=sqr(torus.outer_radius);
    TV u=ray.endpoint-torus.center;
    TV v=ray.direction;
    TV uk=u-u.Dot(torus.axis)*torus.axis;
    TV vk=v-v.Dot(torus.axis)*torus.axis;
    T m0=u.Dot(u)+o2-i2;
    T m1=2*u.Dot(v);
//    T m2=1;//v.Dot(v);
    T n0=uk.Dot(uk);
    T n1=2*uk.Dot(vk);
    T n2=vk.Dot(vk);
    VECTOR<T,5> coeffs={m0*m0-4*o2*n0,2*m0*m1-4*n1*o2,2*m0+m1*m1-4*n2*o2,2*m1,1};
    VECTOR<T,4> r_ev;
    int num_roots=Polynomial_Roots(coeffs,r_ev);
    assert(num_roots%2==0);
    std::sort(r_ev.begin(),r_ev.begin()+num_roots);
    if(num_roots==4 && r_ev(1)>0) return {H{max((T)0,r_ev(0))},H{r_ev(1)},H{r_ev(2)},H{r_ev(3)}};
    if(num_roots==4 && r_ev(3)>0) return {H{max((T)0,r_ev(2))},H{r_ev(3)},H{},H{}};
    if(num_roots==2 && r_ev(1)>0) return {H{max((T)0,r_ev(0))},H{r_ev(1)},H{},H{}};
    return {};
}
//#####################################################################
// Function Intersects
//#####################################################################
template<class T> RAY_HIT<T>
Intersects(const RAY<VECTOR<T,3> >& ray,const TORUS<T>& torus,T isocontour)
{
    return Ray_Hit(Intersection_Interval(ray,torus,isocontour));
}
//#####################################################################
// Function All_Intersections
//#####################################################################
template<class TV,class T> void
All_Intersections(ARRAY<RAY_HIT<T> >& hits,const RAY<TV>& ray,
    const TORUS<T>& torus,const T isocontour)
{
    auto iv=Intersection_Interval(ray,torus,isocontour);
    hits=iv.Array_View(0,Actual_Length(iv));
}
template class TORUS<float>;
template class TORUS<double>;
template TORUS<double> Scaled<double,VECTOR<double,3> >(TORUS<double> const&,VECTOR<double,3> const&,double);
template TORUS<double> Transformed<double,VECTOR<double,3> >(TORUS<double> const&,FRAME<VECTOR<double,3> > const&);
template TORUS<float> Scaled<float,VECTOR<float,3> >(TORUS<float> const&,VECTOR<float,3> const&,float);
template TORUS<float> Transformed<float,VECTOR<float,3> >(TORUS<float> const&,FRAME<VECTOR<float,3> > const&);
template TRIANGULATED_SURFACE<float>* Generate_Triangles(const TORUS<float>&,const int,const int);
template TRIANGULATED_SURFACE<double>* Generate_Triangles(const TORUS<double>&,const int,const int);
template RAY_HIT<double> Intersects<double>(RAY<VECTOR<double,3> > const&,TORUS<double> const&,double);
template RAY_HIT<float> Intersects<float>(RAY<VECTOR<float,3> > const&,TORUS<float> const&,float);
template void All_Intersections<VECTOR<double,3>,double>(ARRAY<RAY_HIT<double>,int>&,RAY<VECTOR<double,3> > const&,TORUS<double> const&,double);
template void All_Intersections<VECTOR<float,3>,float>(ARRAY<RAY_HIT<float>,int>&,RAY<VECTOR<float,3> > const&,TORUS<float> const&,float);
template void TORUS<double>::Read(UNTYPED_ISTREAM<char>&);
template void TORUS<double>::Read(UNTYPED_ISTREAM<double>&);
template void TORUS<double>::Read(UNTYPED_ISTREAM<float>&);
template void TORUS<double>::Write(UNTYPED_OSTREAM<char>&) const;
template void TORUS<double>::Write(UNTYPED_OSTREAM<double>&) const;
template void TORUS<double>::Write(UNTYPED_OSTREAM<float>&) const;
template void TORUS<float>::Read(UNTYPED_ISTREAM<char>&);
template void TORUS<float>::Read(UNTYPED_ISTREAM<double>&);
template void TORUS<float>::Read(UNTYPED_ISTREAM<float>&);
template void TORUS<float>::Write(UNTYPED_OSTREAM<char>&) const;
template void TORUS<float>::Write(UNTYPED_OSTREAM<double>&) const;
template void TORUS<float>::Write(UNTYPED_OSTREAM<float>&) const;
}
