// Geometric Tools, LLC
// Copyright (c) 1998-2014
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
//
// File Version: 5.0.1 (2010/10/01)

#include "Wm5MathematicsPCH.h"
#include "Wm5ApprEllipsoidFit3.h"
#include "Wm5ContBox3.h"
#include "Wm5DistPoint3Ellipsoid3.h"
#include "Wm5MinimizeN.h"

namespace Wm5
{
//----------------------------------------------------------------------------
template <typename Real>
EllipsoidFit3<Real>::EllipsoidFit3 (int numPoints,
    const Vector3<Real>* points, Vector3<Real>& center,
    Matrix3<Real>& rotate, Real diag[3])
    :
    mNumPoints(numPoints),
    mPoints(points)
{
    // Energy function is E : R^9 -> R where
    // V = (V0,V1,V2,V3,V4,V5,V6,V7,V8)
    //   = (D[0],D[1],D[2],U.X(),U,y,U.Z(),A0,A1,A2). 
    // For really scattered data, you might need a search function

    mTemp = new1<Vector3<Real> >(numPoints);

    MinimizeN<Real> minimizer(9, Energy, 8, 8, 32, this);

    InitialGuess(numPoints, points, center, rotate, diag);

    Real angle[3];
    MatrixToAngles(rotate, angle);

    Real extent[3] =
    {
        diag[0]*Math<Real>::FAbs(rotate[0][0]) +
            diag[1]*Math<Real>::FAbs(rotate[0][1]) +
            diag[2]*Math<Real>::FAbs(rotate[0][2]),

        diag[0]*Math<Real>::FAbs(rotate[1][0]) +
            diag[1]*Math<Real>::FAbs(rotate[1][1]) +
            diag[2]*Math<Real>::FAbs(rotate[1][2]),

        diag[0]*Math<Real>::FAbs(rotate[2][0]) +
            diag[1]*Math<Real>::FAbs(rotate[2][1]) +
            diag[2]*Math<Real>::FAbs(rotate[2][2])
    };

    Real v0[9] =
    {
        ((Real)0.5)*diag[0],
        ((Real)0.5)*diag[1],
        ((Real)0.5)*diag[2],
        center.X() - extent[0],
        center.Y() - extent[1],
        center.Z() - extent[2],
        -Math<Real>::PI,
        (Real)0,
        (Real)0
    };

    Real v1[9] =
    {
        ((Real)2)*diag[0],
        ((Real)2)*diag[1],
        ((Real)2)*diag[2],
        center.X() + extent[0],
        center.Y() + extent[1],
        center.Z() + extent[2],
        Math<Real>::PI,
        Math<Real>::PI,
        Math<Real>::PI
    };

    Real vInitial[9] =
    {
        diag[0],
        diag[1],
        diag[2],
        center.X(),
        center.Y(),
        center.Z(),
        angle[0],
        angle[1],
        angle[2]
    };

    Real vMin[9];
    minimizer.GetMinimum(v0, v1, vInitial, vMin, mError);

    diag[0] = vMin[0];
    diag[1] = vMin[1];
    diag[2] = vMin[2];
    center.X() = vMin[3];
    center.Y() = vMin[4];
    center.Z() = vMin[5];

    AnglesToMatrix(&vMin[6], rotate);

    delete1(mTemp);
}
//----------------------------------------------------------------------------
template <typename Real>
EllipsoidFit3<Real>::operator Real ()
{
    return mError;
}
//----------------------------------------------------------------------------
template <typename Real>
void EllipsoidFit3<Real>::InitialGuess (int numPoints,
    const Vector3<Real>* points, Vector3<Real>& center,
    Matrix3<Real>& rotate, Real diag[3])
{
    Box3<Real> box = ContOrientedBox(numPoints, points);

    center = box.Center;
    rotate[0][0] = box.Axis[0].X();
    rotate[0][1] = box.Axis[0].Y();
    rotate[0][2] = box.Axis[0].Z();
    rotate[1][0] = box.Axis[1].X();
    rotate[1][1] = box.Axis[1].Y();
    rotate[1][2] = box.Axis[1].Z();
    rotate[2][0] = box.Axis[2].X();
    rotate[2][1] = box.Axis[2].Y();
    rotate[2][2] = box.Axis[2].Z();
    diag[0] = box.Extent[0];
    diag[1] = box.Extent[1];
    diag[2] = box.Extent[2];
}
//----------------------------------------------------------------------------
template <typename Real>
Real EllipsoidFit3<Real>::Energy (const Real* input, void* userData)
{
    EllipsoidFit3& self = *(EllipsoidFit3*)userData;

    // Build rotation matrix.
    Matrix3<Real> rotate;
    AnglesToMatrix(&input[6], rotate);

    // Uniformly scale the extents to keep reasonable floating point values
    // in the distance calculations.
    Real maxValue = input[0];
    if (input[1] > maxValue)
    {
        maxValue = input[1];
    }
    if (input[2] > maxValue)
    {
        maxValue = input[2];
    }

    Real invMax = ((Real)1)/maxValue;
    Ellipsoid3<Real> ellipsoid(Vector3<Real>::ZERO, Vector3<Real>::UNIT_X,
        Vector3<Real>::UNIT_Y, Vector3<Real>::UNIT_Z, invMax*input[0],
        invMax*input[1], invMax*input[2]);

    // Transform the points to the coordinate system of center C and columns
    // of rotation R.
    Real energy = (Real)0;
    for (int i = 0; i < self.mNumPoints; ++i)
    {
        Vector3<Real> diff(
            self.mPoints[i].X() - input[3],
            self.mPoints[i].Y() - input[4],
            self.mPoints[i].Z() - input[5]);

        self.mTemp[i] = invMax*(diff*rotate);

        Real dist =
            DistPoint3Ellipsoid3<Real>(self.mTemp[i], ellipsoid).Get();
        energy += maxValue*dist;
    }

    return energy;
}
//----------------------------------------------------------------------------
template <typename Real>
void EllipsoidFit3<Real>::MatrixToAngles (const Matrix3<Real>& rotate,
    Real* angle)
{
    // rotation axis = (cos(a0)sin(a1),sin(a0)sin(a1),cos(a1))
    // a0 in [-pi,pi], a1 in [0,pi], a2 in [0,pi]

    Vector3<Real> axis;
    rotate.ExtractAxisAngle(axis, angle[2]);

    if (-(Real)1 < axis.Z())
    {
        if (axis.Z() < (Real)1)
        {
            angle[0] = Math<Real>::ATan2(axis.Y(), axis.X());
            angle[1] = Math<Real>::ACos(axis.Z());
        }
        else
        {
            angle[0] = (Real)0;
            angle[1] = (Real)0;
        }
    }
    else
    {
        angle[0] = (Real)0;
        angle[1] = Math<Real>::PI;
    }
}
//----------------------------------------------------------------------------
template <typename Real>
void EllipsoidFit3<Real>::AnglesToMatrix (const Real* angle,
    Matrix3<Real>& rotate)
{
    // rotation axis = (cos(a0)sin(a1),sin(a0)sin(a1),cos(a1))
    // a0 in [-pi,pi], a1 in [0,pi], a2 in [0,pi]

    Real cs0 = Math<Real>::Cos(angle[0]);
    Real sn0 = Math<Real>::Sin(angle[0]);
    Real cs1 = Math<Real>::Cos(angle[1]);
    Real sn1 = Math<Real>::Sin(angle[1]);
    Vector3<Real> axis(cs0*sn1, sn0*sn1, cs1);
    rotate.MakeRotation(axis, angle[2]);
}
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------
// Explicit instantiation.
//----------------------------------------------------------------------------
template WM5_MATHEMATICS_ITEM
class EllipsoidFit3<float>;

template WM5_MATHEMATICS_ITEM
class EllipsoidFit3<double>;
//----------------------------------------------------------------------------
}
