//#####################################################################
// Copyright 2002-2006, Ronald Fedkiw, Neil Molino, Jerry Talton.
// 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/ELLIPSOID.h>
#include <Core/Arrays/ARRAY.h>
#include <Core/Basic_Geometry/RAY.h>
#include <Core/Basic_Geometry/RAY_HIT.h>
#include <Core/Basic_Geometry/SPHERE.h>
#include <Core/Grids/CELL_ITERATOR.h>
#include <Core/Math_Tools/minmax.h>
#include <Core/Random_Numbers/RANDOM_NUMBERS.h>
namespace PhysBAM{
//#####################################################################
// Function Normal
//#####################################################################
template<class TV> TV ELLIPSOID<TV>::
Normal(const TV& location) const   
{
    return orientation.Rotate(sqr(radii).Inverse_Times(orientation.Inverse_Rotate(location-center))).Normalized();
}
//#####################################################################
// Function Inside
//#####################################################################
template<class TV> bool ELLIPSOID<TV>::
Inside(const TV& location,const T thickness_over_two) const
{
    TV scaled_offset=radii.Inverse_Times(orientation.Inverse_Rotate(location-center));
    return scaled_offset.Magnitude_Squared() <= sqr(1-thickness_over_two);
}
//#####################################################################
// Function Outside
//#####################################################################
template<class TV> bool ELLIPSOID<TV>::
Outside(const TV& location,const T thickness_over_two) const
{
    TV scaled_offset=radii.Inverse_Times(orientation.Inverse_Rotate(location-center));
    return scaled_offset.Magnitude_Squared() >= sqr(1+thickness_over_two);
}
//#####################################################################
// Function Boundary
//#####################################################################
template<class TV> bool ELLIPSOID<TV>::
Boundary(const TV& location,const T thickness_over_two) const
{
    return !Inside(location,thickness_over_two) && !Outside(location,thickness_over_two);
}
//#####################################################################
// Function Approximate_Surface
//#####################################################################
// not necessarily the closest point on the surface, but approximates it in some sense
template<class TV> TV ELLIPSOID<TV>::
Approximate_Surface(const TV& location) const 
{
    TV scaled_offset=radii.Inverse_Times(orientation.Inverse_Rotate(location-center));
    return orientation.Rotate(radii*scaled_offset.Normalized())+center;
}
//#####################################################################
// Function Approximate_Surface
//#####################################################################
template<class TV> typename TV::SCALAR ELLIPSOID<TV>::
Approximate_Signed_Distance(const TV& location) const       
{
    TV offset=orientation.Inverse_Rotate(location-center),scaled_offset=radii.Inverse_Times(offset);
    T scaled_magnitude=scaled_offset.Normalize();
    TV closest_offset=radii*scaled_offset;
    T distance=(closest_offset-offset).Magnitude();
    return scaled_magnitude<1?-distance:distance;
}
//#####################################################################
// Function Calculate_Approximate_Signed_Distance
//#####################################################################
// better to reinitialize after this because uses the Approximate_Signed_Distance() function
template<class TV> void ELLIPSOID<TV>::
Calculate_Approximate_Signed_Distance(const GRID<TV>& grid,ARRAY<T,IV>& phi) const
{  
    for(CELL_ITERATOR<TV> iterator(grid);iterator.Valid();iterator.Next())
        phi(iterator.Cell_Index())=Approximate_Signed_Distance(iterator.Location());
}
//#####################################################################
// Function Covariance_Ellipsoid
//#####################################################################
template<class TV> template<class T_ARRAY_TV> ELLIPSOID<TV> ELLIPSOID<TV>::
Covariance_Ellipsoid(const T_ARRAY_TV& points)
{
    TV average=points.Average();
    SYMMETRIC_MATRIX<T,3> covariance;
    for(int p=0;p<points.m;p++)
        covariance+=Outer_Product(points(p)-average);
    covariance/=points.m-1;
    auto [la,ev]=covariance.Solve_Eigenproblem();
    return ELLIPSOID<TV>(average,DIAGONAL_MATRIX<T,d>{sqrt(la.x)},ev);
}
//#####################################################################
// Function Fill_Uniform
//#####################################################################
template<class TV> void ELLIPSOID<TV>::
Fill_Uniform(RANDOM_NUMBERS<T>& rand,const RANGE<TV>& box)
{
    SPHERE<TV> s;
    rand.Fill_Uniform(s,box);
    DIAGONAL_MATRIX<T,TV::m> radii;
    rand.Fill_Uniform(radii.x,0,s.radius);
    ROTATION<TV> rot;
    rand.Fill_Uniform(rot);
    *this=ELLIPSOID<TV>(s.center,radii,rot);
}
//#####################################################################
// Function Scaled
//#####################################################################
template<class T,class TV> ELLIPSOID<TV>
Scaled(const ELLIPSOID<TV>& b,const TV& center,T scale)
{
    auto h(b);
    h.center=(h.center-center)*scale+center;
    h.radii*=scale;
    return h;
}
//#####################################################################
// Function Transformed
//#####################################################################
template<class TV> ELLIPSOID<TV>
Transformed(const ELLIPSOID<TV>& b,const FRAME<TV>& f)
{
    auto h(b);
    h.center=f*h.center;
    h.orientation=f.r*h.orientation;
    return h;
}
//#####################################################################
// Function Intersects
//#####################################################################
template<class TV> RAY_HIT<typename TV::SCALAR> Intersects(const RAY<TV>& ray,
    const ELLIPSOID<TV>& e, typename TV::SCALAR thickness)
{
    typedef typename TV::SCALAR T;
    TV A=e.radii.Inverse_Times(e.orientation.Inverse_Rotate(ray.endpoint-e.center));
    TV B=e.radii.Inverse_Times(e.orientation.Inverse_Rotate(ray.direction));
    T blen=B.Normalize();
    RAY<TV> xform_ray(A,B,ray_normalized);
    RAY_HIT<T> hit=Intersects(xform_ray,SPHERE<TV>(),thickness*blen);
    if(hit.Hit()) hit.dist/=blen;
    return hit;
}
//#####################################################################
template class ELLIPSOID<VECTOR<double,2> >;
template class ELLIPSOID<VECTOR<double,3> >;
template class ELLIPSOID<VECTOR<float,2> >;
template class ELLIPSOID<VECTOR<float,3> >;
template ELLIPSOID<VECTOR<double,3> > ELLIPSOID<VECTOR<double,3> >::Covariance_Ellipsoid<ARRAY<VECTOR<double,3>,int> >(ARRAY<VECTOR<double,3>,int> const&);
template ELLIPSOID<VECTOR<float,3> > ELLIPSOID<VECTOR<float,3> >::Covariance_Ellipsoid<ARRAY<VECTOR<float,3>,int> >(ARRAY<VECTOR<float,3>,int> const&);
template RAY_HIT<float> Intersects(const RAY<VECTOR<float,3> >&,const ELLIPSOID<VECTOR<float,3> >&,const float);
template RAY_HIT<double> Intersects(const RAY<VECTOR<double,3> >&,const ELLIPSOID<VECTOR<double,3> >&,const double);
template RAY_HIT<float> Intersects(const RAY<VECTOR<float,2> >&,const ELLIPSOID<VECTOR<float,2> >&,const float);
template RAY_HIT<double> Intersects(const RAY<VECTOR<double,2> >&,const ELLIPSOID<VECTOR<double,2> >&,const double);
}
