//#####################################################################
// Copyright 2003-2008, Eran Guendelman, Geoffrey Irving, Sergey Koltakov.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class CONE
//##################################################################### 
#include <Core/Basic_Geometry/CONE.h>
#include <Core/Basic_Geometry/RAY.h>
#include <Core/Basic_Geometry/RAY_HIT.h>
#include <Core/Implicit_Objects/IMPLICIT_OBJECT.h>
#include <Core/Implicit_Objects/INTERSECTION_HELPERS.h>
#include <Core/Math_Tools/RANGE.h>
#include <Core/Matrices/FRAME.h>
#include <Core/Random_Numbers/RANDOM_NUMBERS.h>
#include <Core/Read_Write/READ_WRITE.h>
#include <Core/Vectors/VECTOR.h>
namespace PhysBAM{

namespace{
template<class T,class TV>
struct HELPER
{
    T p,cr,ch=0,x,y,yy;
    TV dy,d;
    int cs;

    HELPER(const CONE<T>& cone,const TV& X)
    {
        d=cone.dir;
        TV u=X-cone.base;
        x=u.Dot(d);
        TV w=u-d*x;
        dy=w;
        yy=y=dy.Normalize();
        if(x<=0 && y<=cone.radius){p=-x;cs=0;return;}
        T yc=y-cone.radius;
        if(yc*cone.radius>cone.height*x){y=yc;p=hypot(x,y);cs=1;return;}
        T xc=x-cone.height;
        if(xc*cone.height>cone.radius*y){x=xc;p=hypot(x,y);cs=1;return;}
        T h=hypot(cone.radius,cone.height);
        cr=cone.radius/h;
        ch=cone.height/h;
        T z=xc*cr+y*ch;
        if(-x>=z){p=-x;cs=0;return;}
        p=z;
        cs=2;
    }

    TV N() const
    {
        if(cs==0) return -d;
        if(cs==1) return x/p*d+y/p*dy;
        return d*cr+dy*ch;
    }

    SYMMETRIC_MATRIX<T,3> H() const
    {
        if(cs==0) return {};
        SYMMETRIC_MATRIX<T,3> ddy=((T)1-Outer_Product(d)-Outer_Product(dy))/yy;
        if(cs==1) return y/p*ddy+Outer_Product(x/p*dy-y/p*d)/p;
        return ddy*ch;
    }
};
}
//#####################################################################
// Function Bounding_Box
//#####################################################################
template<class T> RANGE<VECTOR<T,3> > CONE<T>::
Bounding_Box() const
{
    TV w=sqrt((T)1-sqr(dir))*radius;
    RANGE<TV> range(-w,w);
    range.Enlarge_To_Include_Point(dir*height);
    return range+base;
}
//#####################################################################
// Function Signed_Distance
//#####################################################################
template<class T> T CONE<T>::
Signed_Distance(const TV& X) const
{
    HELPER<T,TV> h(*this,X);
    return h.p;
}
//#####################################################################
// Function Suface
//#####################################################################
template<class T> VECTOR<T,3> CONE<T>::
Surface(const TV& X) const 
{
    HELPER<T,TV> h(*this,X);
    return X-h.p*h.N();
}
//#####################################################################
// Function Normal
//#####################################################################
template<class T> VECTOR<T,3> CONE<T>::
Normal(const TV& X) const 
{
    HELPER<T,TV> h(*this,X);
    return h.N();
}
//#####################################################################
// Function Normal
//#####################################################################
template<class T> VECTOR<T,3> CONE<T>::
Normal(const TV& X,const int aggregate) const 
{
    return Normal(X);
}
//#####################################################################
// Function Hessian
//#####################################################################
template<class T> SYMMETRIC_MATRIX<T,3> CONE<T>::
Hessian(const TV& X) const
{
    HELPER<T,TV> h(*this,X);
    return h.H();
}
//#####################################################################
// Function Lazy_Inside
//#####################################################################
template<class T> bool CONE<T>::
Lazy_Inside(const TV& X) const 
{
    TV u=X-base;
    T x=u.Dot(dir);
    if(x<0) return false;
    T y=(u-dir*x).Magnitude();
    return y*height<=radius*(height-x);
}
//#####################################################################
// Function Lazy_Outside
//#####################################################################
template<class T> bool CONE<T>::
Lazy_Outside(const TV& X) const  
{
    return !Lazy_Inside(X);
}
//#####################################################################
// Function Inside
//#####################################################################
template<class T> bool CONE<T>::
Inside(const TV& X,const T thickness_over_two) const 
{
    return Signed_Distance(X)<-thickness_over_two;
}
//#####################################################################
// Function Outside
//#####################################################################
template<class T> bool CONE<T>::
Outside(const TV& X,const T thickness_over_two) const  
{
    return Signed_Distance(X)>thickness_over_two;
}
//#####################################################################
// Function Boundary
//#####################################################################
template<class T> bool CONE<T>::
Boundary(const TV& X,const T thickness_over_two) const
{
    T d=Signed_Distance(X);
    return d<=thickness_over_two && d>=-thickness_over_two;
}
//#####################################################################
// Function Name
//#####################################################################
template<class T> std::string CONE<T>::
Name()
{
    return "CONE<T>";
}
//#####################################################################
// Function Volume
//#####################################################################
template<class T> T CONE<T>::
Volume() const
{
    return pi_v<T>/3*sqr(radius)*height;
}
//#####################################################################
// Function Read
//#####################################################################
template<class T> template<class RW> void CONE<T>::
Read(UNTYPED_ISTREAM<RW>& input)
{
    Read_Binary_Group(input,base,dir,radius,height);
}
//#####################################################################
// Function Write
//#####################################################################
template<class T> template<class RW> void CONE<T>::
Write(UNTYPED_OSTREAM<RW>& output) const
{
    Write_Binary_Group(output,base,dir,radius,height);
}
//#####################################################################
// Function Fill_Uniform
//#####################################################################
template<class T> void CONE<T>::
Fill_Uniform(RANDOM_NUMBERS<T>& rand,const RANGE<TV>& box)
{
    rand.Fill_Uniform(base,box);
    dir=rand.template Get_Direction<TV>();
    auto bx=box-base;
    TV u=bx.max_corner.Componentwise_Min(-bx.min_corner);
    auto a=(box-base).min_corner/dir;
    auto b=(box-base).max_corner/dir;
    auto max_h=a.Componentwise_Max(b).Min();
    rand.Fill_Uniform(height,0,max_h);
    rand.Fill_Uniform(radius,0,(u/sqrt((T)1-sqr(dir))).Min());
}
//#####################################################################
// Function Scaled
//#####################################################################
template<class T,class TV> CONE<T>
Scaled(const CONE<T>& b,const TV& center,T scale)
{
    auto h(b);
    h.base=(h.base-center)*scale+center;
    h.radius*=scale;
    h.height*=scale;
    return h;
}
//#####################################################################
// Function Transformed
//#####################################################################
template<class T,class TV> CONE<T>
Transformed(const CONE<T>& b,const FRAME<TV>& f)
{
    auto h(b);
    h.base=f*h.base;
    h.dir=f.r.Rotate(h.dir);
    return h;
}
//#####################################################################
// Function Intersects
//#####################################################################
template<class T> RAY_HIT<T>
Intersects(const RAY<VECTOR<T,3> >& ray,const CONE<T>& cone,T isocontour)
{
    return Ray_Hit(Intersection_Interval(ray,cone,isocontour));
}
//#####################################################################
// Function Intersection_Interval
//#####################################################################
template<class T> VECTOR<RAY_HIT<T>,2>
Intersection_Interval(const RAY<VECTOR<T,3> >& ray,const CONE<T>& cone,T isocontour)
{
    auto iv0=Intersection_Interval_Infinite_Double_Cone(ray,cone,isocontour);
    auto iv1=Intersection_Interval(ray,PLANE<T>(-cone.dir,cone.base),isocontour);
    auto iv2=Intersection_Interval(ray,PLANE<T>(cone.dir,cone.base+cone.height*cone.dir),isocontour);
    Set_Id(iv0,0);
    Set_Id(iv1,1);
    Set_Id(iv2,2);
    auto all=Intersection(Intersection(iv1,iv2),iv0);
    return {all(0),all(1)};
}
//#####################################################################
// Function Intersection_Interval_Infinite_Double_Cone
//#####################################################################
template<class T> VECTOR<RAY_HIT<T>,3>
Intersection_Interval_Infinite_Double_Cone(const RAY<VECTOR<T,3> >& ray,
    const CONE<T>& cone,T isocontour)
{
    typedef VECTOR<T,3> TV;typedef RAY_HIT<T> H;
    TV B=cone.base;
    T r=cone.radius,h=cone.height;
    if(isocontour)
    {
        T scale=hypot(h,r)/(h*r)*isocontour+1;
        r*=scale;
        h*=scale;
        B-=cone.dir*isocontour;
    }
    T r2=r*r,h2=h*h;
    TV w0=ray.endpoint-B,w1=ray.direction;
    T s0=w0.Dot(cone.dir),s1=w1.Dot(cone.dir),ss=s0-h;
    TV x0=w0-s0*cone.dir,x1=w1-s1*cone.dir;
    T a=h2*x1.Dot(x1)-r2*s1*s1;
    T b=h2*x0.Dot(x1)-r2*ss*s1;
    T c=h2*x0.Dot(x0)-r2*ss*ss; // inside when a*t*t+2*b*t+c<=0
    T d=b*b-a*c;
    if(d<0) return {};
    T e=sqrt(d);
    T t0=-(b-e)/a;
    T t1=-(b+e)/a;
    if(t1<t0) std::swap(t0,t1);
    int n=0;
    VECTOR<RAY_HIT<T>,3> v;
    if(c<0) v(n++).Set(0);
    if(t0>0) v(n++).Set(t0);
    if(t1>0) v(n++).Set(t1);
    return v;
}
//#####################################################################
// Function All_Intersections
//#####################################################################
template<class TV,class T> void
All_Intersections(ARRAY<RAY_HIT<T> >& hits,const RAY<TV>& ray,const CONE<T>& cone,const T isocontour)
{
    auto iv=Intersection_Interval(ray,cone,isocontour);
    hits=iv.Array_View(0,Actual_Length(iv));
}
//#####################################################################
template class CONE<float>;
template class CONE<double>;
template CONE<double> Scaled<double,VECTOR<double,3> >(CONE<double> const&,VECTOR<double,3> const&,double);
template CONE<double> Transformed<double,VECTOR<double,3> >(CONE<double> const&,FRAME<VECTOR<double,3> > const&);
template CONE<float> Scaled<float,VECTOR<float,3> >(CONE<float> const&,VECTOR<float,3> const&,float);
template CONE<float> Transformed<float,VECTOR<float,3> >(CONE<float> const&,FRAME<VECTOR<float,3> > const&);
template RAY_HIT<double> Intersects<double>(RAY<VECTOR<double,3> > const&,CONE<double> const&,double);
template RAY_HIT<float> Intersects<float>(RAY<VECTOR<float,3> > const&,CONE<float> const&,float);
template void All_Intersections<VECTOR<double,3>,double>(ARRAY<RAY_HIT<double>,int>&,RAY<VECTOR<double,3> > const&,CONE<double> const&,double);
template void All_Intersections<VECTOR<float,3>,float>(ARRAY<RAY_HIT<float>,int>&,RAY<VECTOR<float,3> > const&,CONE<float> const&,float);
template void CONE<double>::Read(UNTYPED_ISTREAM<char>&);
template void CONE<double>::Read(UNTYPED_ISTREAM<double>&);
template void CONE<double>::Read(UNTYPED_ISTREAM<float>&);
template void CONE<double>::Write(UNTYPED_OSTREAM<char>&) const;
template void CONE<double>::Write(UNTYPED_OSTREAM<double>&) const;
template void CONE<double>::Write(UNTYPED_OSTREAM<float>&) const;
template void CONE<float>::Read(UNTYPED_ISTREAM<char>&);
template void CONE<float>::Read(UNTYPED_ISTREAM<double>&);
template void CONE<float>::Read(UNTYPED_ISTREAM<float>&);
template void CONE<float>::Write(UNTYPED_OSTREAM<char>&) const;
template void CONE<float>::Write(UNTYPED_OSTREAM<double>&) const;
template void CONE<float>::Write(UNTYPED_OSTREAM<float>&) const;
}
