// 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 "Wm5ApprGreatCircleFit3.h"
#include "Wm5EigenDecomposition.h"

namespace Wm5
{
//----------------------------------------------------------------------------
template <typename Real>
Vector3<Real> GreatCircleFit3 (int numPoints, const Vector3<Real>* points)
{
    // Compute the covariance matrix of the vectors.
    Real sumXX = (Real)0, sumXY = (Real)0, sumXZ = (Real)0;
    Real sumYY = (Real)0, sumYZ = (Real)0, sumZZ = (Real)0;
    for (int i = 0; i < numPoints; i++) 
    {
        Vector3<Real> diff = points[i];
        sumXX += diff[0]*diff[0];
        sumXY += diff[0]*diff[1];
        sumXZ += diff[0]*diff[2];
        sumYY += diff[1]*diff[1];
        sumYZ += diff[1]*diff[2];
        sumZZ += diff[2]*diff[2];
    }

    Real invNumPoints = ((Real)1)/numPoints;
    sumXX *= invNumPoints;
    sumXY *= invNumPoints;
    sumXZ *= invNumPoints;
    sumYY *= invNumPoints;
    sumYZ *= invNumPoints;
    sumZZ *= invNumPoints;

    // Set up the eigensolver.
    EigenDecomposition<Real> esystem(3);
    esystem(0,0) = sumXX;
    esystem(0,1) = sumXY;
    esystem(0,2) = sumXZ;
    esystem(1,0) = esystem(0,1);
    esystem(1,1) = sumYY;
    esystem(1,2) = sumYZ;
    esystem(2,0) = esystem(0,2);
    esystem(2,1) = esystem(1,2);
    esystem(2,2) = sumZZ;

    // Compute eigenstuff; the smallest eigenvalue is in last position.
    esystem.Solve(false);

    // Unit-length direction for best-fit great circle.
    Vector3<Real> normal = esystem.GetEigenvector3(2);
    return normal;
}
//----------------------------------------------------------------------------
template <typename Real>
GreatArcFit3<Real>::GreatArcFit3 (int numPoints,
    const Vector3<Real>* points, Vector3<Real>& normal,
    Vector3<Real>& arcEnd0, Vector3<Real>& arcEnd1)
{
    // Get the least-squares great circle for the vectors.  The circle is on
    // the plane Dot(N,X) = 0.
    normal = GreatCircleFit3<Real>(numPoints, points);

    // Compute a coordinate system to allow projection of the vectors onto
    // the great circle.  The coordinates axes have directions U, V, and N.
    Vector3<Real> U, V;
    Vector3<Real>::GenerateComplementBasis(U, V, normal);

    // The vectors are X[i] = u[i]*U + v[i]*V + w[i]*N.  The projections
    // are P[i] = (u[i]*U + v[i]*V)/sqrt(u[i]*u[i] + v[i]*v[i]).  The great
    // circle is parameterized by C(t) = cos(t)*U + sin(t)*V.  Compute the
    // angles t in [-pi,pi] for the projections onto the great circle.  It
    // is not necesarily to normalize (u[i],v[i]), instead computing
    // t = atan2(v[i],u[i]).
    std::vector<Item> items(numPoints);
    int i;
    for (i = 0; i < numPoints; ++i)
    {
        Item& item = items[i];
        item.U = U.Dot(points[i]);
        item.V = V.Dot(points[i]);
        item.Angle = Math<Real>::ATan2(item.V, item.U);
    }
    std::sort(items.begin(), items.end());

    // Locate the pair of consecutive angles whose difference is a maximum.
    // Effectively, we are constructing a cone of minimum angle that contains
    // the unit-length vectors.
    int numPointsM1 = numPoints - 1;
    Real maxDiff = Math<Real>::TWO_PI + items[0].Angle -
        items[numPointsM1].Angle;
    int end0 = 0, end1 = numPointsM1;
    for (int i0 = 0, i1 = 1; i0 < numPointsM1; i0 = i1++)
    {
        Real diff = items[i1].Angle - items[i0].Angle;
        if (diff > maxDiff)
        {
            maxDiff = diff;
            end0 = i1;
            end1 = i0;
        }
    }

    arcEnd0 = items[end0].U*U + items[end0].V*V;
    arcEnd1 = items[end1].U*U + items[end1].V*V;
    arcEnd0.Normalize();
    arcEnd1.Normalize();
}
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------
// GreatArcFit3::Item
//----------------------------------------------------------------------------
template <typename Real>
bool GreatArcFit3<Real>::Item::operator< (const Item& item) const
{
    return Angle < item.Angle;
}
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------
// Explicit instantiation.
//----------------------------------------------------------------------------
template WM5_MATHEMATICS_ITEM
Vector3<float> GreatCircleFit3<float> (int, const Vector3<float>*);

template WM5_MATHEMATICS_ITEM
class GreatArcFit3<float>;

template WM5_MATHEMATICS_ITEM
Vector3<double> GreatCircleFit3<double> (int, const Vector3<double>*);

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