#ifndef OPTIMIZATION_H
#define OPTIMIZATION_H

/*!
    \defgroup optimization Optimization
    \brief Global optimization for autocalibration

    \bug Problem with the function non-homogenious !!!
*/
///@{
///

#include <math.h>
#include "matcell.h"
#include "transformations.h"
#include "utils.h"
#include <iostream>
#include <iomanip>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <nlopt.hpp>

#ifndef PI
#define PI       3.141592653589793238462643383279502884L
#endif

using namespace std;

class Optimization
{
public:
    Optimization(){}

    ~Optimization();

    static double wrap(const std::vector<double> &x, std::vector<double> &grad, void *data) {
        return (*reinterpret_cast<Optimization*>(data))(x, grad);
    }
    double deg2rad(double angDeg);
    void setMeasurementMatrix(cv::Mat inW);
    void init();
    bool isInit() const { return ! W.empty(); }

    void copyVectorToMatElements(const std::vector<double> &vec, const cv::Mat &idx, cv::Mat &mat);
    int _objFuncMethod = OBJFUNC_Rectification;

    enum ObjFuncMethod{
        OBJFUNC_DistanceSquared,
        OBJFUNC_Distance,
        OBJFUNC_PseudoInverse,
        OBJFUNC_Rectification
    };

    double _minf;
    double getMinf() const{ return _minf;}

    int _maxTimeStep1 = 15;
    int _maxTimeStep2 = 60;

    void setMaxTimeStep1(int maxTime){ _maxTimeStep1 = maxTime; }
    void setMaxTimeStep2(int maxTime){ _maxTimeStep2 = maxTime; }

    void run();

    double operator() (const std::vector<double> &x, std::vector<double> &grad);
    double testFunction (const std::vector<double> &x);
    double distanceSquared       (const std::vector<double> &x);
    double distancePseudoInverse (const std::vector<double> &x);
    double distanceWminusMMW     (const std::vector<double> &x);

    int getParametersNumber();
    void getInitialConditionsAndBounds(std::vector<double> & _x0, std::vector<double> & _lb, std::vector<double> & _ub);
    void setObjectiveFunction(int objFuncMethod) { _objFuncMethod = objFuncMethod; _iterCnt = 0;}

    bool isFeaturerScaled = true;
    void useFeatureScaling(bool b) { isFeaturerScaled = b; }

    double _bestObjValue = HUGE_VAL;
    int _iterCnt = 0;

    int nbCams, nbPts;
    cv::Mat W;
    cv::Mat Win;
    cv::Mat paramConst; ///< matrix defining if optimization parameter is constant (=0). Size = number of images \f$\times\f$ number of parameters
    /** matrix defining optimization bounds. Size = number of images \f$\times\f$ number of parameters.
    *  lower bounds = -paramBounds;
    *  upper bounds = +paramBounds;
    */
    cv::Mat paramBounds;
    /** Offset for all parameters. Size = number of images \f$\times\f$ number of parameters.
     *  Actual value is calculated as offset + initial
     */
    cv::Mat paramOffset;

    cv::Mat paramInitial; ///< Initial values of parameters. Size = number of images \f$\times\f$ number of parameters
    cv::Mat paramResult; ///< Resulting values of parameters. Offset + optimization results
    cv::Mat paramVarIdx; ///< indices of varying parameters in the table of parameters paramConst, paramOffset etc.
    int nbParams;


    ///< indices of each parameter
    enum{
        K = 0, ///< scale factor
        ALPHA, ///< fx/fy, misscaling of axis
        S, ///< skew
        RT1, ///< \f$ \theta_1 \f$
        RR, ///< \f$ \rho \f$
        RT2, ///< \f$ \theta_2 \f$
        PARAMS_PER_CAM ///< number of parameters for one camera
    };

    std::vector<cv::Mat> getCameraMatrices() const;
    cv::Mat getCalibrationMatrix() const;
    cv::Mat getRotationAnglesDeg() const;
    cv::Mat getPoints3D(const std::vector<double> &x) { return getPoints3DfromParams(x);}

   

private:
    std::vector<cv::Mat> tm;
    void computeWmean(const cv::Mat & Win, cv::Mat & Wmean, std::vector<cv::Mat> & tm);
    MatCell getMfromParams(const std::vector<double> &x);
    cv::Mat getPoints3DfromParams(const std::vector<double> &x);
    cv::Mat getCalibrationMatrixFromParamTable(const cv::Mat &paramTable) const;
};


#endif // OPTIMIZATIONPROBLEM_H

/*nlopt methods
 *  nlopt::opt opt(nlopt::GN_CRS2_LM, optim->getParametersNumber()); // 1
    //nlopt::opt opt(nlopt::GN_MLSL, paramsNumber);                  // 2
    //nlopt::opt opt(nlopt::GN_MLSL_LDS, paramsNumber);              // 2
    //nlopt::opt opt(nlopt::GN_ESCH, paramsNumber);                  // 3
    //nlopt::opt opt(nlopt::GN_DIRECT_L, paramsNumber);              // 4
    //nlopt::opt opt(nlopt::GN_DIRECT, paramsNumber);                // 4
    //nlopt::opt opt(nlopt::GN_ORIG_DIRECT, paramsNumber);           // 99
    //nlopt::opt opt(nlopt::GN_ISRES, paramsNumber);                 // 99
*/