// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement.  The various license agreements may be found at
// the Magic Software web site.  This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf

#include "SM_MathPCH.h"
#include "MgcLinearSystem.h"
#include "MgcNaturalSpline3.h"
#include "MgcPolynomial.h"
#include "MgcRTLib.h"

//---------------------------------------------------------------------------
MgcNaturalSpline3::MgcNaturalSpline3 (BoundaryType eType, int iSegments,
    float* afTime, Vector3D* akPoint)
    :
    MgcMultipleCurve3(iSegments,afTime)
{
    m_akA = akPoint;

    switch ( eType )
    {
        case BT_FREE:
        {
            CreateFreeSpline();
            break;
        }
        case BT_CLAMPED:
        {
            CreateClampedSpline();
            break;
        }
        case BT_CLOSED:
        {
            CreateClosedSpline();
            break;
        }
    }
}
//---------------------------------------------------------------------------
MgcNaturalSpline3::~MgcNaturalSpline3 ()
{
    delete[] m_akA;
    delete[] m_akB;
    delete[] m_akC;
    delete[] m_akD;
}
//---------------------------------------------------------------------------
void MgcNaturalSpline3::CreateFreeSpline ()
{
    float* afDt = new float[m_iSegments];
    int i;
    for (i = 0; i < m_iSegments; i++)
        afDt[i] = m_afTime[i+1] - m_afTime[i];

    float* afD2t = new float[m_iSegments];
    for (i = 1; i < m_iSegments; i++)
        afD2t[i] = m_afTime[i+1] - m_afTime[i-1];

    Vector3D* akAlpha = new Vector3D[m_iSegments];
    for (i = 1; i < m_iSegments; i++)
    {
        Vector3D kNumer = 3.0f*(afDt[i-1]*m_akA[i+1] - afD2t[i]*m_akA[i] +
            afDt[i]*m_akA[i-1]);
        float fInvDenom = 1.0f/(afDt[i-1]*afDt[i]);
        akAlpha[i] = fInvDenom*kNumer;
    }

    float* afEll = new float[m_iSegments+1];
    float* afMu = new float[m_iSegments];
    Vector3D* akZ = new Vector3D[m_iSegments+1];
    float fInv;

    afEll[0] = 1.0;
    afMu[0] = 0.0;
    akZ[0] = Vector3D::ZERO;
    for (i = 1; i < m_iSegments; i++)
    {
        afEll[i] = 2.0f*afD2t[i] - afDt[i-1]*afMu[i-1];
        fInv = 1.0f/afEll[i];
        afMu[i] = fInv*afDt[i];
        akZ[i] = fInv*(akAlpha[i] - afDt[i-1]*akZ[i-1]);
    }
    afEll[m_iSegments] = 1.0f;
    akZ[m_iSegments] = Vector3D::ZERO;

    m_akB = new Vector3D[m_iSegments];
    m_akC = new Vector3D[m_iSegments+1];
    m_akD = new Vector3D[m_iSegments];

    m_akC[m_iSegments] = Vector3D::ZERO;

    for (i = m_iSegments-1; i >= 0; i--)
    {
        const float fOneThird = 1.0f/3.0f;
        m_akC[i] = akZ[i] - afMu[i]*m_akC[i+1];
        fInv = 1.0f/afDt[i];
        m_akB[i] = fInv*(m_akA[i+1] - m_akA[i]) - fOneThird*afDt[i]*(
            m_akC[i+1] + 2.0f*m_akC[i]);
        m_akD[i] = fOneThird*fInv*(m_akC[i+1] - m_akC[i]);
    }

    delete[] afDt;
    delete[] afD2t;
    delete[] akAlpha;
    delete[] afEll;
    delete[] afMu;
    delete[] akZ;
}
//-----------------------------------------------------------------------------
void MgcNaturalSpline3::CreateClampedSpline ()
{
    float* afDt = new float[m_iSegments];
    int i;
    for (i = 0; i < m_iSegments; i++)
        afDt[i] = m_afTime[i+1] - m_afTime[i];

    float* afD2t = new float[m_iSegments];
    for (i = 1; i < m_iSegments; i++)
        afD2t[i] = m_afTime[i+1] - m_afTime[i-1];

    Vector3D* akAlpha = new Vector3D[m_iSegments+1];
    float fInv = 1.0f/afDt[0];
    Vector3D kDiff = m_akA[1] - m_akA[0];
    akAlpha[0] = 3.0f*(fInv - 1.0f)*(m_akA[1] - m_akA[0]);
    fInv = 1.0f/afDt[m_iSegments-1];
    akAlpha[m_iSegments] = 3.0f*(1.0f - fInv)*(m_akA[m_iSegments] -
        m_akA[m_iSegments-1]);
    for (i = 1; i < m_iSegments; i++)
    {
        Vector3D kNumer = 3.0f*(afDt[i-1]*m_akA[i+1] - afD2t[i]*m_akA[i] +
            afDt[i]*m_akA[i-1]);
        float fInvDenom = 1.0f/(afDt[i-1]*afDt[i]);
        akAlpha[i] = fInvDenom*kNumer;
    }

    float* afEll = new float[m_iSegments+1];
    float* afMu = new float[m_iSegments];
    Vector3D* akZ = new Vector3D[m_iSegments+1];

    afEll[0] = 2.0f*afDt[0];
    afMu[0] = 0.5;
    fInv = 1.0f/afEll[0];
    akZ[0] = fInv*akAlpha[0];

    for (i = 1; i < m_iSegments; i++)
    {
        afEll[i] = 2.0f*afD2t[i] - afDt[i-1]*afMu[i-1];
        fInv = 1.0f/afEll[i];
        afMu[i] = fInv*afDt[i];
        akZ[i] = fInv*(akAlpha[i] - afDt[i-1]*akZ[i-1]);
    }
    afEll[m_iSegments] = afDt[m_iSegments-1]*(2.0f - afMu[m_iSegments-1]);
    fInv = 1.0f/afEll[m_iSegments];
    akZ[m_iSegments] = fInv*(akAlpha[m_iSegments] - afDt[m_iSegments-1]*
        akZ[m_iSegments-1]);

    m_akB = new Vector3D[m_iSegments];
    m_akC = new Vector3D[m_iSegments+1];
    m_akD = new Vector3D[m_iSegments];

    m_akC[m_iSegments] = akZ[m_iSegments];

    for (i = m_iSegments-1; i >= 0; i--)
    {
        float fOneThird = 1.0f/3.0f;
        m_akC[i] = akZ[i] - afMu[i]*m_akC[i+1];
        fInv = 1.0f/afDt[i];
        m_akB[i] = fInv*(m_akA[i+1] - m_akA[i]) - fOneThird*afDt[i]*(
            m_akC[i+1] + 2.0f*m_akC[i]);
        m_akD[i] = fOneThird*fInv*(m_akC[i+1] - m_akC[i]);
    }

    delete[] afDt;
    delete[] afD2t;
    delete[] akAlpha;
    delete[] afEll;
    delete[] afMu;
    delete[] akZ;
}
//-----------------------------------------------------------------------------
void MgcNaturalSpline3::CreateClosedSpline ()
{
    float* afDt = new float[m_iSegments];
    int i;
    for (i = 0; i < m_iSegments; i++)
        afDt[i] = m_afTime[i+1] - m_afTime[i];

    // TO DO.  Add ability to solve AX = B for B nxm (not just m=1) and
    // remove resetting of aafMat that occurs for each component of m_afC.
    MgcLinearSystem kSys;

    // construct matrix of system
    float** aafMat = kSys.NewMatrix(m_iSegments+1);
    aafMat[0][0] = 1.0f;
    aafMat[0][m_iSegments] = -1.0f;
    for (i = 1; i <= m_iSegments-1; i++)
    {
        aafMat[i][i-1] = afDt[i-1];
        aafMat[i][i  ] = 2.0f*(afDt[i-1] + afDt[i]);
        aafMat[i][i+1] = afDt[i];
    }
    aafMat[m_iSegments][m_iSegments-1] = afDt[m_iSegments-1];
    aafMat[m_iSegments][0] = 2.0f*(afDt[m_iSegments-1] + afDt[0]);
    aafMat[m_iSegments][1] = afDt[0];

    // construct right-hand side of system
    m_akC = new Vector3D[m_iSegments+1];
    m_akC[0] = Vector3D::ZERO;
    float fInv0, fInv1;
    for (i = 1; i <= m_iSegments-1; i++)
    {
        fInv0 = 1.0f/afDt[i];
        fInv1 = 1.0f/afDt[i-1];
        m_akC[i] = 3.0f*(fInv0*(m_akA[i+1] - m_akA[i]) - fInv1*(m_akA[i] -
            m_akA[i-1]));
    }
    fInv0 = 1.0f/afDt[0];
    fInv1 = 1.0f/afDt[m_iSegments-1];
    m_akC[m_iSegments] = 3.0f*(fInv0*(m_akA[1] - m_akA[0]) - fInv1*(m_akA[0] -
        m_akA[m_iSegments-1]));

    float* afCx = kSys.NewVector(m_iSegments+1);
    float* afCy = kSys.NewVector(m_iSegments+1);
    float* afCz = kSys.NewVector(m_iSegments+1);
    for (i = 0; i <= m_iSegments; i++)
    {
        afCx[i] = m_akC[i].x;
        afCy[i] = m_akC[i].y;
        afCz[i] = m_akC[i].z;
    }
    kSys.Solve(m_iSegments+1,aafMat,afCx);

    // reset matrix for next system
    aafMat[0][0] = 1.0f;
    aafMat[0][m_iSegments] = -1.0f;
    for (i = 1; i <= m_iSegments-1; i++)
    {
        aafMat[i][i-1] = afDt[i-1];
        aafMat[i][i  ] = 2.0f*(afDt[i-1] + afDt[i]);
        aafMat[i][i+1] = afDt[i];
    }
    aafMat[m_iSegments][m_iSegments-1] = afDt[m_iSegments-1];
    aafMat[m_iSegments][0] = 2.0f*(afDt[m_iSegments-1] + afDt[0]);
    aafMat[m_iSegments][1] = afDt[0];

    kSys.Solve(m_iSegments+1,aafMat,afCy);

    // reset matrix for next system
    aafMat[0][0] = 1.0f;
    aafMat[0][m_iSegments] = -1.0f;
    for (i = 1; i <= m_iSegments-1; i++)
    {
        aafMat[i][i-1] = afDt[i-1];
        aafMat[i][i  ] = 2.0f*(afDt[i-1] + afDt[i]);
        aafMat[i][i+1] = afDt[i];
    }
    aafMat[m_iSegments][m_iSegments-1] = afDt[m_iSegments-1];
    aafMat[m_iSegments][0] = 2.0f*(afDt[m_iSegments-1] + afDt[0]);
    aafMat[m_iSegments][1] = afDt[0];

    kSys.Solve(m_iSegments+1,aafMat,afCz);

    for (i = 0; i <= m_iSegments; i++)
    {
        m_akC[i].x = afCx[i];
        m_akC[i].y = afCy[i];
        m_akC[i].z = afCz[i];
    }

    const float fOneThird = 1.0f/3.0f;
    m_akB = new Vector3D[m_iSegments];
    m_akD = new Vector3D[m_iSegments];
    for (i = 0; i < m_iSegments; i++)
    {
        fInv0 = 1.0f/afDt[i];
        m_akB[i] = fInv0*(m_akA[i+1] - m_akA[i]) - fOneThird*(m_akC[i+1] +
            2.0f*m_akC[i])*afDt[i];
        m_akD[i] = fOneThird*fInv0*(m_akC[i+1] - m_akC[i]);
    }

    kSys.DeleteMatrix(m_iSegments+1,aafMat);
    delete[] afDt;
    delete[] afCx;
    delete[] afCy;
    delete[] afCz;
}
//-----------------------------------------------------------------------------
Vector3D MgcNaturalSpline3::GetPosition (float fTime) const
{
    int iKey;
    float fDt;
    GetKeyInfo(fTime,iKey,fDt);

    Vector3D kResult = m_akA[iKey] + fDt*(m_akB[iKey] + fDt*(
        m_akC[iKey] + fDt*m_akD[iKey]));

    return kResult;
}
//---------------------------------------------------------------------------
Vector3D MgcNaturalSpline3::GetFirstDerivative (float fTime) const
{
    int iKey;
    float fDt;
    GetKeyInfo(fTime,iKey,fDt);

    Vector3D kResult = m_akB[iKey] + fDt*(2.0f*m_akC[iKey] + 3.0f*fDt*
        m_akD[iKey]);

    return kResult;
}
//---------------------------------------------------------------------------
Vector3D MgcNaturalSpline3::GetSecondDerivative (float fTime) const
{
    int iKey;
    float fDt;
    GetKeyInfo(fTime,iKey,fDt);

    Vector3D kResult = 2.0f*m_akC[iKey] + 6.0f*fDt*m_akD[iKey];

    return kResult;
}
//---------------------------------------------------------------------------
Vector3D MgcNaturalSpline3::GetThirdDerivative (float fTime) const
{
    int iKey;
    float fDt;
    GetKeyInfo(fTime,iKey,fDt);

    Vector3D kResult = 6.0f*m_akD[iKey];

    return kResult;
}
//---------------------------------------------------------------------------
float MgcNaturalSpline3::GetSpeed (int iKey, float fTime) const
{
    Vector3D kVelocity = m_akB[iKey] + fTime*(2.0f*m_akC[iKey] +
        3.0f*fTime*m_akD[iKey]);
    return kVelocity.Length();
}
//-----------------------------------------------------------------------------
float MgcNaturalSpline3::GetLength (int iKey, float fT0, float fT1) const
{
    class ThisPlusKey
    {
    public:
        ThisPlusKey (const MgcNaturalSpline3* pkThis, int iKey)
            : m_pkThis(pkThis), m_iKey(iKey) { /**/ }

        const MgcNaturalSpline3* m_pkThis;
        int m_iKey;
    };

    ThisPlusKey kData(this,iKey);

    /*
    return MgcIntegrate::RombergIntegral(fT0,fT1,GetSpeedWithData,
        (void*)&kData);
        */
    assert(!"REMOVED");
    return 0.0f;
}
//-----------------------------------------------------------------------------
float MgcNaturalSpline3::GetVariation (int iKey, float fT0,
    float fT1, const Vector3D& rkA, const Vector3D& rkB) const
{
    MgcPolynomial kXPoly(3);
    kXPoly[0] = m_akA[iKey].x;
    kXPoly[1] = m_akB[iKey].x;
    kXPoly[2] = m_akC[iKey].x;
    kXPoly[3] = m_akD[iKey].x;

    MgcPolynomial kYPoly(3);
    kYPoly[0] = m_akA[iKey].y;
    kYPoly[1] = m_akB[iKey].y;
    kYPoly[2] = m_akC[iKey].y;
    kYPoly[3] = m_akD[iKey].y;

    MgcPolynomial kZPoly(3);
    kZPoly[0] = m_akA[iKey].z;
    kZPoly[1] = m_akB[iKey].z;
    kZPoly[2] = m_akC[iKey].z;
    kZPoly[3] = m_akD[iKey].z;

    // construct line segment A + t*B
    MgcPolynomial kLx(1), kLy(1), kLz(1);
    kLx[0] = rkA.x;
    kLx[1] = rkB.x;
    kLy[0] = rkA.y;
    kLy[1] = rkB.y;
    kLz[0] = rkA.z;
    kLz[1] = rkB.z;

    // compute |X(t) - L(t)|^2
    MgcPolynomial kDx = kXPoly - kLx;
    MgcPolynomial kDy = kYPoly - kLy;
    MgcPolynomial kDz = kZPoly - kLz;
    MgcPolynomial kNormSqr = kDx*kDx + kDy*kDy + kDz*kDz;

    // compute indefinite integral of |X(t)-L(t)|^2
    MgcPolynomial kIntegral(kNormSqr.GetDegree()+1);
    kIntegral[0] = 0.0f;
    for (int i = 1; i <= kIntegral.GetDegree(); i++)
        kIntegral[i] = kNormSqr[i-1]/i;

    // compute definite Integral(t0,t1,|X(t)-L(t)|^2)
    float fResult = kIntegral(fT1) - kIntegral(fT0);
    return fResult;
}
//---------------------------------------------------------------------------
