//=============================================================================================================
/**
 * @file     interpolation.cpp
 * @author   Lorenz Esch <lesch@mgh.harvard.edu>
 * @since    2.0.0
 * @date     May, 2017
 *
 * @section  LICENSE
 *
 * Copyright (C) 2017, Lorenz Esch. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 * the following conditions are met:
 *     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *       following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
 *       the following disclaimer in the documentation and/or other materials provided with the distribution.
 *     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
 *       to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * @brief    Interpolation class definition.
 *
 */

//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "interpolation.h"

//=============================================================================================================
// QT INCLUDES
//=============================================================================================================

#include <QSet>
#include <QDebug>

//=============================================================================================================
// STL INCLUDES
//=============================================================================================================

#include <unordered_set>

//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace DISP3DRHILIB;
using namespace Eigen;

//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

QSharedPointer<SparseMatrix<float> > Interpolation::createInterpolationMat(const VectorXi &vecProjectedSensors,
                                                                             const QSharedPointer<MatrixXd> matDistanceTable,
                                                                             double (*interpolationFunction) (double),
                                                                             const double dCancelDist,
                                                                             const VectorXi &vecExcludeIndex)
{
    if(matDistanceTable->rows() == 0 && matDistanceTable->cols() == 0) {
        qDebug() << "[WARNING] Interpolation::createInterpolationMat - received an empty distance table.";
        return QSharedPointer<SparseMatrix<float> >::create();
    }

    QSharedPointer<Eigen::SparseMatrix<float> > matInterpolationMatrix = QSharedPointer<SparseMatrix<float> >::create(matDistanceTable->rows(), static_cast<int>(vecProjectedSensors.size()));

    QVector<Triplet<float> > vecNonZeroEntries;
    const qint32 iRows = matInterpolationMatrix->rows();
    const qint32 iCols = matInterpolationMatrix->cols();

    // Build exclude set for O(1) lookup
    std::unordered_set<int> excludeSet;
    for(Eigen::Index i = 0; i < vecExcludeIndex.size(); ++i) {
        excludeSet.insert(vecExcludeIndex[i]);
    }

    QSet<qint32> sensorLookup;
    for(Eigen::Index idx = 0; idx < vecProjectedSensors.size(); ++idx){
        if(excludeSet.count(static_cast<int>(idx)) == 0){
            sensorLookup.insert(vecProjectedSensors[idx]);
        }
    }

    for (qint32 r = 0; r < iRows; ++r) {
        if (sensorLookup.contains(r) == false) {
            QVector<QPair<qint32, float> > vecBelowThresh;
            float dWeightsSum = 0.0;
            const RowVectorXd& rowVec = matDistanceTable->row(r);

            for (qint32 c = 0; c < iCols; ++c) {
                const float dDist = rowVec[c];

                if (dDist < dCancelDist) {
                    const float dValueWeight = std::fabs(1.0 / interpolationFunction(dDist));
                    dWeightsSum += dValueWeight;
                    vecBelowThresh.push_back(qMakePair(c, dValueWeight));
                }
            }

            for (const QPair<qint32, float> &qp : vecBelowThresh) {
                vecNonZeroEntries.push_back(Eigen::Triplet<float> (r, qp.first, qp.second / dWeightsSum));
            }
        } else {
            // Find index of r in vecProjectedSensors
            int iIndexInSubset = 0;
            for(Eigen::Index k = 0; k < vecProjectedSensors.size(); ++k) {
                if(vecProjectedSensors[k] == r) {
                    iIndexInSubset = static_cast<int>(k);
                    break;
                }
            }
            vecNonZeroEntries.push_back(Eigen::Triplet<float> (r, iIndexInSubset, 1));
        }
    }

    matInterpolationMatrix->setFromTriplets(vecNonZeroEntries.begin(), vecNonZeroEntries.end());

    return matInterpolationMatrix;
}

//=============================================================================================================

VectorXf Interpolation::interpolateSignal(const QSharedPointer<SparseMatrix<float> > matInterpolationMatrix,
                                          const QSharedPointer<VectorXf> &vecMeasurementData)
{
    if (matInterpolationMatrix->cols() != vecMeasurementData->rows()) {
        qDebug() << "[WARNING] Interpolation::interpolateSignal - Dimension mismatch. Return null pointer...";
        return VectorXf();
    }

    VectorXf pOutVec = *matInterpolationMatrix * (*vecMeasurementData);
    return pOutVec;
}

//=============================================================================================================

VectorXf Interpolation::interpolateSignal(const SparseMatrix<float> &matInterpolationMatrix,
                                          const VectorXf &vecMeasurementData)
{
    if (matInterpolationMatrix.cols() != vecMeasurementData.rows()) {
        qDebug() << "[WARNING] Interpolation::interpolateSignal - Dimension mismatch. Return null pointer...";
        return VectorXf();
    }

    VectorXf pOutVec = matInterpolationMatrix * vecMeasurementData;
    return pOutVec;
}

//=============================================================================================================

double Interpolation::linear(const double dIn)
{
    return dIn;
}

//=============================================================================================================

double Interpolation::gaussian(const double dIn)
{
    return exp(-((dIn * dIn) / 2.0));
}

//=============================================================================================================

double Interpolation::square(const double dIn)
{
    return std::max((-(1.0f / 9.0f) * (dIn * dIn) + 1), 0.0);
}

//=============================================================================================================

double Interpolation::cubic(const double dIn)
{
    return dIn * dIn * dIn;
}
