/*
===========================================================================
Copyright (C) 2015 the OpenMoHAA team

This file is part of OpenMoHAA source code.

OpenMoHAA source code is free software; you can redistribute it
and/or modify it under the terms of the GNU General Public License as
published by the Free Software Foundation; either version 2 of the License,
or (at your option) any later version.

OpenMoHAA source code is distributed in the hope that it will be
useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with OpenMoHAA source code; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
===========================================================================
*/

// spline.h: Spline paths.
//

#pragma once

#include "g_local.h"
#include <class.h>

template<unsigned int cGrids, unsigned int cPoints>
class cSpline : public Class
{
public:
    int   m_iPoints;
    float m_vPoints[cPoints][cGrids];
    int   m_iPointFlags[cPoints];

    cSpline();

    void   Reset(void);
    int    Add(float *fAdd, int flags);
    void   Modify(int, float *, int);
    void   UniformAdd(float *pos);
    void   Remove(class cSpline<cGrids, cPoints> *, int);
    void   RemoveRange(int, int);
    int    Right(float x);
    float *Get(float x, int *flags);
    float *GetByNode(float x, int *flags);
    int    Append(cSpline<cGrids, cPoints> *pNew);

    void Archive(Archiver& arc);
};

template<unsigned int cGrids, unsigned int cPoints>
cSpline<cGrids, cPoints>::cSpline()
{
    m_iPoints = 0;
}

template<unsigned int cGrids, unsigned int cPoints>
void cSpline<cGrids, cPoints>::Archive(Archiver& arc)
{
    arc.ArchiveInteger(&m_iPoints);

    for (int i = 0; i < cPoints; i++) {
        for (int j = 0; j < cGrids; j++) {
            arc.ArchiveFloat(&m_vPoints[i][j]);
        }

        arc.ArchiveInteger(&m_iPointFlags[i]);
    }
}

template<unsigned int cGrids, unsigned int cPoints>
void cSpline<cGrids, cPoints>::Reset(void)
{
    m_iPoints = 0;
}

template<unsigned int cGrids, unsigned int cPoints>
int cSpline<cGrids, cPoints>::Add(float *fAdd, int flags)
{
    int i;
    int ii;
    int insertIndex;

    if (m_iPoints + 1 > cPoints) {
        return -1;
    }

    insertIndex = Right(*fAdd);

    for (i = m_iPoints; i > insertIndex; i--) {
        for (ii = 0; ii < cGrids; ii++) {
            m_vPoints[i][ii] = m_vPoints[i - 1][ii];
        }

        m_iPointFlags[i] = m_iPointFlags[i - 1];
    }

    for (i = 0; i < cGrids; i++) {
        m_vPoints[insertIndex][i] = fAdd[i];
    }

    m_iPointFlags[insertIndex] = flags;
    m_iPoints++;

    return insertIndex;
}

template<unsigned int cGrids, unsigned int cPoints>
void cSpline<cGrids, cPoints>::UniformAdd(float *pos)
{
    int i;
    int ii;

    for (i = 0; i < m_iPoints; i++) {
        for (ii = 0; ii < cGrids; ii++) {
            m_vPoints[i][ii] += pos[ii];
        }
    }
}

template<unsigned int cGrids, unsigned int cPoints>
int cSpline<cGrids, cPoints>::Right(float x)
{
    int i;

    for (i = 0; i < m_iPoints; i++) {
        if (m_vPoints[i][0] > x) {
            break;
        }
    }

    return i;
}

template<unsigned int cGrids, unsigned int cPoints>
float *cSpline<cGrids, cPoints>::Get(float x, int *flags)
{
    if (!m_iPoints) {
        return NULL;
    }

    int          rp;
    int          i;
    static float r[cGrids];
    double       delta[cGrids];

    rp = Right(x);

    if (rp) {
        if (rp == m_iPoints) {
            if (flags) {
                *flags = m_iPointFlags[rp - 1];
            }

            for (i = 0; i < cGrids; i++) {
                r[i] = m_vPoints[rp - 1][i];
            }
        } else {
            if (flags) {
                *flags = m_iPointFlags[rp - 1];
            }

            for (i = 0; i < cGrids; i++) {
                delta[i] = m_vPoints[rp][i] - m_vPoints[rp - 1][i];
            }

            for (i = 0; i < cGrids; i++) {
                r[i] = (x - m_vPoints[rp - 1][0]) / delta[0] * delta[i] + m_vPoints[rp - 1][i];
            }
        }
    } else {
        if (flags) {
            *flags = m_iPointFlags[0];
        }

        for (i = 0; i < cGrids; i++) {
            r[i] = m_vPoints[0][i];
        }
    }

    return r;
}

template<unsigned int cGrids, unsigned int cPoints>
float *cSpline<cGrids, cPoints>::GetByNode(float x, int *flags)
{
    if (!m_iPoints) {
        return NULL;
    }

    int          rp;
    int          i;
    static float r[cGrids];
    double       delta[cGrids];

    rp = (int)(floor(x) + 1.0);

    if (rp <= 0) {
        if (flags) {
            *flags = m_iPointFlags[0];
        }

        for (i = 0; i < cGrids; i++) {
            r[i] = m_vPoints[0][i];
        }
    } else if (rp < m_iPoints) {
        if (flags) {
            *flags = m_iPointFlags[rp - 1];
        }

        for (i = 0; i < cGrids; i++) {
            delta[i] = m_vPoints[rp][i] - m_vPoints[rp - 1][i];
        }

        for (i = 0; i < cGrids; i++) {
            r[i] = (x - (rp - 1)) * delta[i] + m_vPoints[rp - 1][i];
        }
    } else {
        if (flags) {
            *flags = m_iPointFlags[m_iPoints - 1];
        }

        for (i = 0; i < cGrids; i++) {
            r[i] = m_vPoints[m_iPoints - 1][i];
        }
    }

    return r;
}

template<unsigned int cGrids, unsigned int cPoints>
int cSpline<cGrids, cPoints>::Append(cSpline<cGrids, cPoints> *pNew)
{
    float *i_fTmp;
    float  o_fTmp[4];
    float  fIndexAdd;
    int    i;
    int    ii;
    int    iFlags = 0;

    if (!pNew || pNew->m_iPoints == 0) {
        return -1;
    }

    if (m_iPoints) {
        int points;

        points = m_iPoints;
        fIndexAdd = *GetByNode(m_iPoints, NULL);

        for (i = 0; i < pNew->m_iPoints; i++) {
            i_fTmp = pNew->GetByNode(i, &iFlags);

            for (ii = 0; ii < cGrids; ii++) {
                o_fTmp[ii] = i_fTmp[ii];
            }

            o_fTmp[0] += fIndexAdd;
            Add(o_fTmp, iFlags);
        }

        return points;
    } else {
        for (i = 0; i < pNew->m_iPoints; i++) {
            i_fTmp = pNew->GetByNode(i, &iFlags);

            for (ii = 0; ii < cGrids; ii++) {
                o_fTmp[ii] = i_fTmp[ii];
            }

            Add(o_fTmp, iFlags);
        }

        return 0;
    }
}