/***************************************************************************
 *   Copyright (c) 2015 Werner Mayer <wmayer[at]users.sourceforge.net>     *
 *                                                                         *
 *   This file is part of the FreeCAD CAx development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 of the License, or (at your option) any later version.      *
 *                                                                         *
 *   This library  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 Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/


#include "PreCompiled.h"
#if defined(HAVE_PCL_OPENNURBS)
#ifndef _PreComp_
# include <Geom_BSplineSurface.hxx>
# include <TColgp_Array2OfPnt.hxx>
# include <TColStd_Array2OfReal.hxx>
# include <TColStd_Array1OfInteger.hxx>
# include <TColStd_Array1OfReal.hxx>
# include <map>
#endif

#include <Mod/Points/App/PointsPy.h>

#include "BSplineFitting.h"

#include <pcl/pcl_config.h>
#if PCL_VERSION_COMPARE(>=,1,7,0)
# include <pcl/point_cloud.h>
# include <pcl/point_types.h>
# include <pcl/io/pcd_io.h>
# include <pcl/surface/on_nurbs/fitting_surface_tdm.h>
# include <pcl/surface/on_nurbs/fitting_curve_2d_asdm.h>
#endif

using namespace Reen;


BSplineFitting::BSplineFitting(const std::vector<Base::Vector3f>& pts)
  : myPoints(pts)
  , myIterations(10)
  , myOrder(3)
  , myRefinement(4)
  , myInteriorSmoothness(0.2)
  , myInteriorWeight(1.0)
  , myBoundarySmoothness(0.2)
  , myBoundaryWeight(0.0)
{
}

void BSplineFitting::setIterations(unsigned value)
{
    myIterations = value;
}

void BSplineFitting::setOrder(unsigned value)
{
    myOrder = value;
}

void BSplineFitting::setRefinement(unsigned value)
{
    myRefinement = value;
}

void BSplineFitting::setInteriorSmoothness(double value)
{
    myInteriorSmoothness = value;
}

void BSplineFitting::setInteriorWeight(double value)
{
    myInteriorWeight = value;
}

void BSplineFitting::setBoundarySmoothness(double value)
{
    myBoundarySmoothness = value;
}

void BSplineFitting::setBoundaryWeight(double value)
{
    myBoundaryWeight = value;
}

Handle(Geom_BSplineSurface) BSplineFitting::perform()
{
#if PCL_VERSION_COMPARE(>=,1,7,0)
    pcl::on_nurbs::NurbsDataSurface data;
    for (std::vector<Base::Vector3f>::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
        if (!pcl_isnan (it->x) && !pcl_isnan (it->y) && !pcl_isnan (it->z))
            data.interior.push_back (Eigen::Vector3d (it->x, it->y, it->z));
    }


    // fit B-spline surface
    //

    pcl::on_nurbs::FittingSurface::Parameter params;
    params.interior_smoothness = myInteriorSmoothness;
    params.interior_weight = myInteriorWeight;
    params.boundary_smoothness = myBoundarySmoothness;
    params.boundary_weight = myBoundaryWeight;

    // initialize
    ON_NurbsSurface nurbs = pcl::on_nurbs::FittingSurface::initNurbsPCABoundingBox (myOrder, &data);
    pcl::on_nurbs::FittingSurface fit (&data, nurbs);
    //  fit.setQuiet (false); // enable/disable debug output

    // surface refinement
    for (unsigned i = 0; i < myRefinement; i++) {
        fit.refine(0);
        fit.refine(1);
        fit.assemble(params);
        fit.solve();
    }

    // surface fitting with final refinement level
    for (unsigned i = 0; i < myIterations; i++) {
        fit.assemble(params);
        fit.solve();
    }

    // fit B-spline curve
#if 0
    // parameters
    pcl::on_nurbs::FittingCurve2dAPDM::FitParameter curve_params;
    curve_params.addCPsAccuracy = 5e-2;
    curve_params.addCPsIteration = 3;
    curve_params.maxCPs = 200;
    curve_params.accuracy = 1e-3;
    curve_params.iterations = 100;

    curve_params.param.closest_point_resolution = 0;
    curve_params.param.closest_point_weight = 1.0;
    curve_params.param.closest_point_sigma2 = 0.1;
    curve_params.param.interior_sigma2 = 0.00001;
    curve_params.param.smooth_concavity = 1.0;
    curve_params.param.smoothness = 1.0;

    // initialisation (circular)
    pcl::on_nurbs::NurbsDataCurve2d curve_data;
    curve_data.interior = data.interior_param;
    curve_data.interior_weight_function.push_back(true);
    ON_NurbsCurve curve_nurbs = pcl::on_nurbs::FittingCurve2dAPDM::initNurbsCurve2D(order, curve_data.interior);

    // curve fitting
    pcl::on_nurbs::FittingCurve2dASDM curve_fit (&curve_data, curve_nurbs);
    // curve_fit.setQuiet (false); // enable/disable debug output
    curve_fit.fitting (curve_params);
#endif

    // u parameters
    int numUKnots = fit.m_nurbs.KnotCount(0);
    int numUPoles = fit.m_nurbs.CVCount(0);
    int uDegree = fit.m_nurbs.Degree(0);
    bool uPeriodic = fit.m_nurbs.IsPeriodic(0) ? true : false;
    std::map<Standard_Real, int> uKnots;

    // v parameters
    int numVKnots = fit.m_nurbs.KnotCount(1);
    int numVPoles = fit.m_nurbs.CVCount(1);
    int vDegree = fit.m_nurbs.Degree(1);
    bool vPeriodic = fit.m_nurbs.IsPeriodic(1) ? true : false;
    std::map<Standard_Real, int> vKnots;

    TColgp_Array2OfPnt poles(1, numUPoles, 1, numVPoles);
    TColStd_Array2OfReal weights(1, numUPoles, 1, numVPoles);

    for (int i=0; i<numUPoles; i++) {
        for (int j=0; j<numVPoles; j++) {
            ON_3dPoint cv;
            fit.m_nurbs.GetCV(i, j, cv);
            poles.SetValue(i+1, j+1, gp_Pnt(cv.x,cv.y,cv.z));

            Standard_Real weight = fit.m_nurbs.Weight(i, j);
            weights.SetValue(i+1, j+1, weight);
        }
    }

    uKnots[fit.m_nurbs.SuperfluousKnot(0,0)] = 1;
    uKnots[fit.m_nurbs.SuperfluousKnot(0,1)] = 1;
    for (int i=0; i<numUKnots; i++) {
        Standard_Real value = fit.m_nurbs.Knot(0, i);
        std::map<Standard_Real, int>::iterator it = uKnots.find(value);
        if (it == uKnots.end())
            uKnots[value] = 1;
        else
            it->second++;
    }

    vKnots[fit.m_nurbs.SuperfluousKnot(1,0)] = 1;
    vKnots[fit.m_nurbs.SuperfluousKnot(1,1)] = 1;
    for (int i=0; i<numVKnots; i++) {
        Standard_Real value = fit.m_nurbs.Knot(1, i);
        std::map<Standard_Real, int>::iterator it = vKnots.find(value);
        if (it == vKnots.end())
            vKnots[value] = 1;
        else
            it->second++;
    }

    TColStd_Array1OfReal uKnotArray(1,uKnots.size());
    TColStd_Array1OfInteger uMultArray(1,uKnots.size());
    int index = 1;
    for (std::map<Standard_Real, int>::iterator it = uKnots.begin(); it != uKnots.end(); ++it, index++) {
        uKnotArray.SetValue(index, it->first);
        uMultArray.SetValue(index, it->second);
    }

    TColStd_Array1OfReal vKnotArray(1,vKnots.size());
    TColStd_Array1OfInteger vMultArray(1,vKnots.size());
    index = 1;
    for (std::map<Standard_Real, int>::iterator it = vKnots.begin(); it != vKnots.end(); ++it, index++) {
        vKnotArray.SetValue(index, it->first);
        vMultArray.SetValue(index, it->second);
    }

    Handle_Geom_BSplineSurface spline = new Geom_BSplineSurface(poles,weights,
        uKnotArray, vKnotArray, uMultArray, vMultArray, uDegree, vDegree,
        uPeriodic, vPeriodic);
    return spline;
#else
    return Handle_Geom_BSplineSurface();
#endif
}
#endif // HAVE_PCL_OPENNURBS