/***************************************************************************
 *   Copyright (c) 2020 Graeme van der Vlugt                               *
 *                                                                         *
 *   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                                *
 *                                                                         *
 ***************************************************************************/


#ifndef MESH_SPHERE_FIT_H
#define MESH_SPHERE_FIT_H

#include "Approximation.h"
#include <Eigen/Eigenvalues>

// -------------------------------------------------------------------------------
namespace MeshCoreFit {

typedef Eigen::Matrix<double,4,4,Eigen::RowMajor> Matrix4x4;

/**
 * Best-fit sphere for a given set of points. 
 */
class MeshExport SphereFit : public MeshCore::Approximation
{
public:
    /**
     * Construction
     */
    SphereFit();
    /**
     * Destruction
     */
    virtual ~SphereFit();

    /**
     * Set approximations before calling Fit()
     */
    void SetApproximations(double radius, const Base::Vector3d &center);
    /**
	 * Set iteration convergence criteria for the fit if special values are needed.
	 * The default values set in the constructor are suitable for most uses
     */
	void SetConvergenceCriteria(double posConvLimit, double vConvLimit, int maxIter);
    /**
     * Returns the radius of the fitted sphere. If Fit() has not been called then zero is returned.
     */
    double GetRadius() const;
    /**
     * Returns the center of the fitted sphere. If Fit() has not been called the null vector is returned.
     */
    Base::Vector3d GetCenter() const;
    /**
     * Returns the number of iterations that Fit() needed to converge. If Fit() has not been called then zero is returned.
     */
	int GetNumIterations() const;
    /**
	 * Compute approximations for the parameters using all points
     */
	void ComputeApproximations();
    /**
     * Fit a sphere onto the given points. If the fit fails FLOAT_MAX is returned.
     */
    float Fit();
    /**
     * Returns the distance from the point \a rcPoint to the fitted sphere. If Fit() has not been
     * called FLOAT_MAX is returned.
     */
    float GetDistanceToSphere(const Base::Vector3f &rcPoint) const;
    /**
     * Returns the standard deviation from the points to the fitted sphere. If Fit() has not been
     * called FLOAT_MAX is returned.
     */
    float GetStdDeviation() const;
    /**
     * Projects the points onto the fitted sphere.
     */
    void ProjectToSphere();

protected:
   /**
	 * Set up the normal equations
     */
	void setupNormalEquationMatrices(const std::vector< Base::Vector3d > &residuals, Matrix4x4 &atpa, Eigen::VectorXd &atpl) const;
	/**
	 * Sets up contributions of given observation to the normal equation matrices.
     */
	void setupObservation(const Base::Vector3f &point, const Base::Vector3d &residual, double a[4], double &f0, double &qw, double b[3]) const;
   /**
	 * Computes contribution of the given observation equation on the normal equation matrices
     */
	void addObservationU(double a[4], double li, double pi, Matrix4x4 &atpa, Eigen::VectorXd &atpl) const;
   /**
	 * Set the lower part of the normal matrix equal to the upper part
     */
	void setLowerPart(Matrix4x4 &atpa) const;

   /**
	 * Compute the residuals and sigma0 and check the residual convergence
     */
	bool computeResiduals(const Eigen::VectorXd &x, std::vector< Base::Vector3d > &residuals, double &sigma0, double vConvLimit, bool &vConverged) const;

protected:
    Base::Vector3d _vCenter;/**< Center of sphere. */
    double _dRadius;		/**< Radius of the sphere. */
	int _numIter;			/**< Number of iterations for solution to converge. */
	double _posConvLimit;	/**< Position and radius parameter convergence threshold. */
	double _vConvLimit;		/**< Residual convergence threshold. */
	int _maxIter;			/**< Maximum number of iterations. */

};


} // namespace MeshCore

#endif // MESH_SPHERE_FIT_H