/****************************************************************************
* MeshLab                                                           o o     *
* A versatile mesh processing toolbox                             o     o   *
*                                                                _   O  _   *
* Copyright(C) 2005                                                \/)\/    *
* Visual Computing Lab                                            /\/|      *
* ISTI - Italian National Research Council                           |      *
*                                                                    \      *
* All rights reserved.                                                      *
*                                                                           *
* This program 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.                                       *
*                                                                           *
* This program 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 (http://www.gnu.org/licenses/gpl.txt)          *
* for more details.                                                         *
*                                                                           *
****************************************************************************/

#include "mlssurface.h"
#include <vcg/space/index/kdtree/kdtree.h>
#include <iostream>
#include <limits>
#include <vcg/space/index/octree.h>

namespace GaelMls {

// template<typename _Scalar>
// MlsSurface<_Scalar>::MlsSurface(const MeshModel& m)
// 	: mMesh(m)
// {
// 	mCachedQueryPointIsOK = false;
//
// 	mPoints.resize(m.cm.vert.size());
// 	mNormals.resize(m.cm.vert.size());
// 	mRadii.resize(m.cm.vert.size());
// 	mAABB.Set(mMesh.cm.vert[0].cP());
// 	for (uint i = 0; i< m.cm.vert.size(); i++)
// 	{
// 		mPoints[i] = /*vcg::vector_cast<Scalar>*/(mMesh.cm.vert[i].cP());
// 		mNormals[i] = /*vcg::vector_cast<Scalar>*/(mMesh.cm.vert[i].cN());
// 		mAABB.Add(mMesh.cm.vert[i].cP());
// 	}
//
// 	// compute radii using a basic meshless density estimator
// 	computeVertexRaddi();
//
// 	mFilterScale = 4.0;
// 	mMaxNofProjectionIterations = 20;
// 	mProjectionAccuracy = 1e-4;
// 	mBallTree = 0;
// }

template<typename _MeshType>
void MlsSurface<_MeshType>::setFilterScale(Scalar v)
{
    mFilterScale = v;
    mCachedQueryPointIsOK = false;
    if (mBallTree)
        mBallTree->setRadiusScale(mFilterScale);
}

template<typename _MeshType>
void MlsSurface<_MeshType>::setMaxProjectionIters(int n)
{
    mMaxNofProjectionIterations = n;
    mCachedQueryPointIsOK = false;
}

template<typename _MeshType>
void MlsSurface<_MeshType>::setProjectionAccuracy(Scalar v)
{
    mProjectionAccuracy = v;
    mCachedQueryPointIsOK = false;
}

template<typename _MeshType>
void MlsSurface<_MeshType>::setGradientHint(int h)
{
    mGradientHint = h;
    mCachedQueryPointIsOK = false;
}

template<typename _MeshType>
void MlsSurface<_MeshType>::setHessianHint(int h)
{
    mHessianHint = h;
    mCachedQueryPointIsOK = false;
}

template<typename _MeshType>
void MlsSurface<_MeshType>::computeVertexRaddi(const int nbNeighbors)
{
    #if 0
    int nbNeighbors = 16;
    vcg::Octree<VectorType, Scalar> knn;
    knn.Set(mPoints.begin(), mPoints.end());
    std::vector<VectorType*> nearest_objects;
    std::vector<VectorType> nearest_points;
    std::vector<Scalar> sqDistances;
    mAveragePointSpacing = 0;
    for (uint i = 0; i< mPoints.size(); i++)
    {
        DummyObjectMarker dom;
        PointToPointSqDist dfunc;
        Scalar max_dist2 = 1e9;//std::numeric_limits<Scalar>::max();
        knn.GetKClosest(dfunc, dom, nbNeighbors, mPoints[i],
                                        max_dist2, nearest_objects, sqDistances, nearest_points);
// 		for (int j=0; i<sqDistances.size(); ++j)
// 			std::cout << sqDistances[j] << " ";
// 		std::cout << "$\n";
        mRadii[i] = 2. * sqrt(sqDistances.at(0)/nearest_objects.size());
        mAveragePointSpacing += mRadii[i];
    }
    mAveragePointSpacing /= Scalar(mPoints.size());

    #else

//	int nbNeighbors = 16;

    assert(mPoints.size()>=2);
    vcg::KdTree<Scalar> knn(positions());
    typename vcg::KdTree<Scalar>::PriorityQueue pq;
//    knn.setMaxNofNeighbors(nbNeighbors);
    mAveragePointSpacing = 0;
    for (size_t i = 0; i< mPoints.size(); i++)
    {
        knn.doQueryK(mPoints[i].cP(),nbNeighbors,pq);
        const_cast<PointsType&>(mPoints)[i].R() = 2. * sqrt(pq.getTopWeight()/Scalar(pq.getNofElements()));
        mAveragePointSpacing += mPoints[i].cR();
    }
    mAveragePointSpacing /= Scalar(mPoints.size());

    #endif
}

template<typename _MeshType>
void MlsSurface<_MeshType>::computeNeighborhood(const VectorType& x, bool computeDerivatives) const
{
    if (!mBallTree)
    {
        const_cast<BallTree<Scalar>*&>(mBallTree) = new BallTree<Scalar>(positions(), radii());
        const_cast<BallTree<Scalar>*>(mBallTree)->setRadiusScale(mFilterScale);
    }
    mBallTree->computeNeighbors(x, &mNeighborhood);
    size_t nofSamples = mNeighborhood.size();

    // compute spatial weights and partial derivatives
    mCachedWeights.resize(nofSamples);
    if (computeDerivatives)
    {
        mCachedWeightDerivatives.resize(nofSamples);
        mCachedWeightGradients.resize(nofSamples);
    }
    else
        mCachedWeightGradients.clear();

    for (size_t i=0; i<nofSamples; i++)
    {
        int id = mNeighborhood.index(i);
        Scalar s = 1./(mPoints[id].cR()*mFilterScale);
        s = s*s;
        Scalar w = Scalar(1) - mNeighborhood.squaredDistance(i) * s;
        if (w<0)
            w = 0;
        Scalar aux = w;
        w = w * w;
        w = w * w;
        mCachedWeights[i] = w;

        if (computeDerivatives)
        {
            mCachedWeightDerivatives[i] = (-2. * s) * (4. * aux * aux * aux);
            mCachedWeightGradients[i]  = (x - mPoints[id].cP()) * mCachedWeightDerivatives[i];
        }
    }
}

template<typename _MeshType>
void MlsSurface<_MeshType>::requestSecondDerivatives() const
{
    //if (!mSecondDerivativeUptodate)
    {
        size_t nofSamples = mNeighborhood.size();
        if (nofSamples>mCachedWeightSecondDerivatives.size())
            mCachedWeightSecondDerivatives.resize(nofSamples+10);

        {
            for (size_t i=0 ; i<nofSamples ; ++i)
            {
                int id = mNeighborhood.index(i);
                Scalar s = 1./(mPoints[id].cR()*mFilterScale);
                s = s*s;
                Scalar x2 = s * mNeighborhood.squaredDistance(i);
                x2 = 1.0 - x2;
                if (x2<0)
                    x2 = 0.;
                mCachedWeightSecondDerivatives[i] = (4.0*s*s) * (12.0 * x2 * x2);
            }
        }
        //mSecondDerivativeUptodate = true;
    }
}

template<typename _MeshType>
typename MlsSurface<_MeshType>::Scalar
MlsSurface<_MeshType>::meanCurvature(const VectorType& gradient, const MatrixType& hessian) const
{
    Scalar gl = gradient.Norm();
//	return (gl*gl*hessian.Trace() - vcg::Dot(gradient, VectorType(hessian * gradient))) / (2.*gl*gl*gl);
    return (gl*gl*hessian.Trace() - (gradient * VectorType(hessian * gradient))) / (2.*gl*gl*gl);
}

template<typename _MeshType>
bool MlsSurface<_MeshType>::isInDomain(const VectorType& x) const
{
    if ((!mCachedQueryPointIsOK) || mCachedQueryPoint!=x)
    {
        computeNeighborhood(x, false);
    }
    int nb = mNeighborhood.size();
    if (nb<mDomainMinNofNeighbors)
        return false;

    int i=0;
    bool out = true;
    bool hasNormal = true;
    if ((mDomainNormalScale==1.f) || (!hasNormal))
    {
        while (out && i<nb)
        {
            int id = mNeighborhood.index(i);
            Scalar rs2 = mPoints[id].cR() * mDomainRadiusScale;
            rs2 = rs2*rs2;
            out = mNeighborhood.squaredDistance(i) > rs2;
            ++i;
        }
    }
    else
    {
        Scalar s = 1./(mDomainNormalScale*mDomainNormalScale) - 1.f;
        while (out && i<nb)
        {
            int id = mNeighborhood.index(i);
            Scalar rs2 = mPoints[id].cR() * mDomainRadiusScale;
            rs2 = rs2*rs2;
            Scalar dn = mPoints[id].cN().dot(x-mPoints[id].cP());
            out = (mNeighborhood.squaredDistance(i) + s*dn*dn) > rs2;
            ++i;
        }
    }
    return !out;
}

// template class MlsSurface<float>;
// template class MlsSurface<double>;

}
