// Wild Magic Source Code
// David Eberly
// http://www.geometrictools.com
// Copyright (c) 1998-2007
//
// This library is free software; you can redistribute it and/or modify it
// under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation; either version 2.1 of the License, or (at
// your option) any later version.  The license is available for reading at
// either of the locations:
//     http://www.gnu.org/copyleft/lgpl.html
//     http://www.geometrictools.com/License/WildMagicLicense.pdf
//
// Version: 4.0.1 (2007/01/25)

#include "Wm4FoundationPCH.h"
#include "Wm4MultipleCurve3.h"

namespace Wm4
{
//----------------------------------------------------------------------------
template <class Real>
MultipleCurve3<Real>::MultipleCurve3 (int iSegments, Real* afTime)
    :
    Curve3<Real>(afTime[0],afTime[iSegments])
{
    m_iSegments = iSegments;
    m_afTime = afTime;
    m_afLength = 0;
    m_afAccumLength = 0;
}
//----------------------------------------------------------------------------
template <class Real>
MultipleCurve3<Real>::~MultipleCurve3 ()
{
    WM4_DELETE[] m_afTime;
    WM4_DELETE[] m_afLength;
    WM4_DELETE[] m_afAccumLength;
}
//----------------------------------------------------------------------------
template <class Real>
int MultipleCurve3<Real>::GetSegments () const
{
    return m_iSegments;
}
//----------------------------------------------------------------------------
template <class Real>
const Real* MultipleCurve3<Real>::GetTimes () const
{
    return m_afTime;
}
//----------------------------------------------------------------------------
template <class Real>
void MultipleCurve3<Real>::GetKeyInfo (Real fTime, int& riKey, Real& rfDt)
    const
{
    if (fTime <= m_afTime[0])
    {
        riKey = 0;
        rfDt = (Real)0.0;
    }
    else if (fTime >= m_afTime[m_iSegments])
    {
        riKey = m_iSegments-1;
        rfDt = m_afTime[m_iSegments] - m_afTime[m_iSegments-1];
    }
    else
    {
        for (int i = 0; i < m_iSegments; i++)
        {
            if (fTime < m_afTime[i+1])
            {
                riKey = i;
                rfDt = fTime - m_afTime[i];
                break;
            }
        }
    }
}
//----------------------------------------------------------------------------
template <class Real>
Real MultipleCurve3<Real>::GetSpeedWithData (Real fTime, void* pvData)
{
    MultipleCurve3* pvThis = *(MultipleCurve3**) pvData;
    int iKey = *(int*)((char*)pvData + sizeof(pvThis));
    return pvThis->GetSpeedKey(iKey,fTime);
}
//----------------------------------------------------------------------------
template <class Real>
void MultipleCurve3<Real>::InitializeLength () const
{
    m_afLength = WM4_NEW Real[m_iSegments];
    m_afAccumLength = WM4_NEW Real[m_iSegments];

    // arc lengths of the segments
    int iKey;
    for (iKey = 0; iKey < m_iSegments; iKey++)
    {
        m_afLength[iKey] = GetLengthKey(iKey,(Real)0.0,
            m_afTime[iKey+1]-m_afTime[iKey]);
    }

    // accumulative arc length
    m_afAccumLength[0] = m_afLength[0];
    for (iKey = 1; iKey < m_iSegments; iKey++)
    {
        m_afAccumLength[iKey] = m_afAccumLength[iKey-1] + m_afLength[iKey];
    }
}
//----------------------------------------------------------------------------
template <class Real>
Real MultipleCurve3<Real>::GetLength (Real fT0, Real fT1) const
{
    assert(m_fTMin <= fT0 && fT0 <= m_fTMax);
    assert(m_fTMin <= fT1 && fT1 <= m_fTMax);
    assert(fT0 <= fT1);

    if (!m_afLength)
    {
        InitializeLength();
    }

    int iKey0, iKey1;
    Real fDt0, fDt1;
    GetKeyInfo(fT0,iKey0,fDt0);
    GetKeyInfo(fT1,iKey1,fDt1);

    Real fLength;
    if (iKey0 < iKey1)
    {
        // accumulate full-segment lengths
        fLength = (Real)0.0;
        for (int i = iKey0+1; i < iKey1; i++)
            fLength += m_afLength[i];
        
        // add on partial first segment
        fLength += GetLengthKey(iKey0,fDt0,m_afTime[iKey0+1]-m_afTime[iKey0]);
        
        // add on partial last segment
        fLength += GetLengthKey(iKey1,(Real)0.0,fDt1);
    }
    else
    {
        fLength = GetLengthKey(iKey0,fDt0,fDt1);
    }

    return fLength;
}
//----------------------------------------------------------------------------
template <class Real>
Real MultipleCurve3<Real>::GetTime (Real fLength, int iIterations,
    Real fTolerance) const
{
    if (!m_afLength)
    {
        InitializeLength();
    }

    if (fLength <= (Real)0.0)
    {
        return m_fTMin;
    }

    if (fLength >= m_afAccumLength[m_iSegments-1])
    {
        return m_fTMax;
    }

    int iKey;
    for (iKey = 0; iKey < m_iSegments; iKey++)
    {
        if (fLength < m_afAccumLength[iKey])
        {
            break;
        }
    }
    if (iKey >= m_iSegments)
    {
        return m_afTime[m_iSegments];
    }

    // try Newton's method first for rapid convergence
    Real fL0, fL1;
    if (iKey == 0)
    {
        fL0 = fLength;
        fL1 = m_afAccumLength[0];
    }
    else
    {
        fL0 = fLength - m_afAccumLength[iKey-1];
        fL1 = m_afAccumLength[iKey] - m_afAccumLength[iKey-1];
    }

    // use Newton's method to invert the arc length integral
    Real fDt1 = m_afTime[iKey+1] - m_afTime[iKey];
    Real fDt0 = fDt1*fL0/fL1;
    for (int i = 0; i < iIterations; i++)
    {
        Real fDifference = GetLengthKey(iKey,(Real)0.0,fDt0) - fL0;
        if (Math<Real>::FAbs(fDifference) <= fTolerance)
        {
            return m_afTime[iKey] + fDt0;
        }

        fDt0 -= fDifference/GetSpeedKey(iKey,fDt0);
    }

    // Newton's method failed.  You might want to increase iterations or
    // tolerance or integration accuracy.  However, in this application it
    // is likely that the time values are oscillating, due to the limited
    // numerical precision of 32-bit floats.  It is safe to use the last
    // computed time.
    return m_afTime[iKey] + fDt0;
}
//----------------------------------------------------------------------------
template <class Real>
Real MultipleCurve3<Real>::GetVariation (Real fT0, Real fT1,
    const Vector3<Real>* pkP0, const Vector3<Real>* pkP1) const
{
    assert(m_fTMin <= fT0 && fT0 <= m_fTMax);
    assert(m_fTMin <= fT1 && fT1 <= m_fTMax);
    assert(fT0 <= fT1);

    // construct line segment, A + (t-t0)*B
    Vector3<Real> kP0, kP1;
    if (!pkP0)
    {
        kP0 = GetPosition(fT0);
        pkP0 = &kP0;
    }
    if (!pkP1)
    {
        kP1 = GetPosition(fT1);
        pkP1 = &kP1;
    }
    Real fInvDT = ((Real)1.0)/(fT1 - fT0);
    Vector3<Real> kA, kB = fInvDT*(*pkP1 - *pkP0);

    int iKey0, iKey1;
    Real fDt0, fDt1;
    GetKeyInfo(fT0,iKey0,fDt0);
    GetKeyInfo(fT1,iKey1,fDt1);

    Real fVariation;
    if (iKey0 < iKey1)
    {
        // accumulate full-segment variations
        fVariation = (Real)0.0;
        for (int i = iKey0+1; i < iKey1; i++)
        {
            kA = *pkP0 + (m_afTime[i] - fT0)*kB;
            fVariation += GetVariationKey(i,(Real)0.0,
                m_afTime[i+1]-m_afTime[i],kA,kB);
        }
        
        // add on partial first segment
        kA = *pkP0 + (m_afTime[iKey0] - fT0)*kB;
        fVariation += GetVariationKey(iKey0,fDt0,
            m_afTime[iKey0+1]-m_afTime[iKey0],kA,kB);
        
        // add on partial last segment
        kA = *pkP0 + (m_afTime[iKey1] - fT0)*kB;
        fVariation += GetVariationKey(iKey1,0.0f,fDt1,kA,kB);
    }
    else
    {
        kA = *pkP0 + (m_afTime[iKey0] - fT0)*kB;
        fVariation = GetVariationKey(iKey0,fDt0,fDt1,kA,kB);
    }

    return fVariation;
}
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------
// explicit instantiation
//----------------------------------------------------------------------------
template WM4_FOUNDATION_ITEM
class MultipleCurve3<float>;

template WM4_FOUNDATION_ITEM
class MultipleCurve3<double>;
//----------------------------------------------------------------------------
}
