/*

By downloading, copying, installing or using the software you agree to this license.
If you do not agree to this license, do not download, install,
copy or use the software.

                          License Agreement
               For Open Source Computer Vision Library
                       (3-clause BSD License)

Copyright (C) 2015,
      Guillaume Lemaitre (g.lemaitre58@gmail.com),
      Johan Massich (mailsik@gmail.com),
      Gerard Bahi (zomeck@gmail.com),
      Yohan Fougerolle (Yohan.Fougerolle@u-bourgogne.fr).
Third party copyrights are property of their respective owners.

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 names of the copyright holders nor the names of the 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 copyright holders 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.
*/

#include "math_utils.h"
#include "SuperFormula.h"
#include "random-standalone.h"
#include "timer.h"

#include <iostream>
#include <fstream>
#include <cmath>
#include <sstream>
#include <algorithm>

#include <iomanip>

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/Cholesky>
#include <Eigen/SVD>
#include <limits>
//#include <GL/glut.h>

using namespace Eigen;

extern Random R1;

//---------------------------------------------------------------------
//
// Rational 2D Gielis Curves
//
//---------------------------------------------------------------------
void RationalSuperShape2D :: Init( double a, double b, double n1,double n2,double n3,double p, double q , double thtoffset, double phioffset, double xoffset, double yoffset, double zoffset){
    Parameters.clear();
    Parameters.push_back(a);
    Parameters.push_back(b);
    Parameters.push_back(n1);
    Parameters.push_back(n2);
    Parameters.push_back(n3);
    Parameters.push_back(p);
    Parameters.push_back(q);
    Parameters.push_back(thtoffset);
    Parameters.push_back(phioffset);
    Parameters.push_back(xoffset);
    Parameters.push_back(yoffset);
    Parameters.push_back(zoffset);
}
void RationalSuperShape2D :: Init( double a, double b, double n1,double n2,double n3)
{
    Init(
                a, b, //scale
                n1, n2, n3, //shape
                Get_p(), Get_q(), //sym
                0,0, //rota
                0,0,0); //trans
}
RationalSuperShape2D :: RationalSuperShape2D(){
    Init(
                1, 1, //scale
                2, 2, 2, //shape
                4, 1, //sym
                0,0, //rota
                0,0,0); //trans
}
RationalSuperShape2D :: RationalSuperShape2D(double a, double b, double n1,double n2,double n3,double p, double q, double thtoffset, double phioffset, double xoffset, double yoffset, double zoffset){
    Init(a, b, n1, n2, n3, p, q, thtoffset, phioffset,xoffset, yoffset, zoffset);
}
RationalSuperShape2D :: ~RationalSuperShape2D() {
    Parameters.clear();
    PointList.clear();
    FaceList.clear();
    NormalList.clear();
}
double RationalSuperShape2D :: radius ( const double angle ){
    double tmp_angle = Get_p() * angle * 0.25 / Get_q() ;
    double tmp1( cos(tmp_angle) );
    double tmp2( sin(tmp_angle) );
    //if( tmp1 != 0)
    tmp1 = pow(fabs(tmp1),Get_n2()) / Get_a();
    //if( tmp2 != 0)
    tmp2 = pow(fabs(tmp2),Get_n3()) / Get_b();
    if( tmp1 + tmp2 !=0 ) return( pow( (tmp1 + tmp2), -1.0/Get_n1() ) );
    else	{std::cout<<"ERROR RADIUS NULL"<<std::endl;return 0;}
}
// void RationalSuperShape2D :: Aff(){
// for (unsigned int i=0; i<Parameters.size(); i++)
// {
// std::cout << setiosflags(ios::fixed) << setprecision(6)<<Parameters[i] << " ";
// }
// std::cout << std::endl;
// }
//Potential fields
double RationalSuperShape2D :: ImplicitFunction1( const Vector2d P, std::vector<double> &Dffinal) {
    Dffinal.clear();
    // nothing computable, return zero values, zero partial derivatives
    // the point will have no effect on the ongoing computations
    if ( P[0] == 0 && P[1] == 0)
    {
        // Df/Dx, Df/Dy, Df/Dr set to zero...
        for (int i=0; i<3; i++) Dffinal.push_back(0);
        return 0;
    }
    std::vector<double> f, Ddum;
    double x(P[0]), y(P[1]), PSL(P.squaredNorm()), PL(sqrt(PSL)), dthtdx (-y/PSL), dthtdy (x/PSL), R,drdth;
    std::vector< std::vector<double> > Df;
    //assert angular values between [0, 2q*Pi]
    double tht (atan2(y,x)), thtbase(tht);
    if (tht<0) thtbase += 2.*M_PI;
    //compute all intersections and associated partial derivatives
    for (int i=0; i<Get_q(); i++)
    {
        tht = thtbase + i*2.*M_PI;
        R = radius(tht);
        f.push_back(R - PL); //store function
        // store partial derivatives
        std::vector<double> rowi;
        drdth = DrDtheta(tht);
        rowi.push_back( drdth*dthtdx - cos(tht)); //df/dx
        rowi.push_back( drdth*dthtdy - sin(tht)); //df/dy
        rowi.push_back( 1. ); //df/dr
        Df.push_back(rowi);
    }
    //bubble sort, not really efficient but acceptable for such small arrays
    for(int i=0; i<Get_q()-1; i++)
        for (int j=i+1; j<Get_q(); j++)
            if (f[i]<f[j])
            {
                //std::swap values of f[i] and f[j]
                std::swap(f[i],f[j]);
                //std::swap rows Df[i] and Df[j]
                Df[i].swap(Df[j]);
            }
    //Compute resulting Rfunction
    std::vector<double> Df1; //std::vectorfor df/dxi
    //iterative evaluation of:
    // -the resulting R-functions
    // -the associated partial derivatives
    double f1,fdum;
    f1 = f[0]; // first value of f
    Df1 = Df[0]; // first associated row with partial derivatives
    //combine functions as (...((F1 v F2) v F3 ) v F4) v ...)
    for(int i=1; i<Get_q(); i++) // for all intersections
    {
        //compute R-function, sets all partial derivatives
        //fdum and Ddum temporary results of the union from F1 to Fi
        RpUnion(f1, f[i], Df1, Df[i], fdum, Ddum);
        //update results in f1 and Df1, and iterate
        f1 = fdum;
        Df1 = Ddum;
    }
    //final partial derivatives df/dxi after R-functions
    Dffinal = Df1;
    //clear arrays
    f.clear(); Df.clear(); Ddum.clear(); Df1.clear();
    //return results
    return f1;
}
double RationalSuperShape2D :: ImplicitFunction2( const Vector2d P, std::vector<double> &Dffinal){
    Dffinal.clear();
    // nothing computable, return zero values, zero partial derivatives
    // the point will have no effect on the ongoing computations
    if ( P[0] == 0 && P[1] == 0)
    {
        // Df/Dx, Df/Dy, Df/Dr set to zero...
        for (int i=0; i<3; i++) Dffinal.push_back(0);
        return 0;
    }
    std::vector<double> f, Ddum;
    double x(P[0]), y(P[1]), PSL(P.squaredNorm()), PL(sqrt(PSL)), dthtdx (-y/PSL), dthtdy (x/PSL), R,drdth;
    std::vector<std::vector<double> > Df;
    //assert angular values between [0, 2q*Pi]
    double tht (atan2(y,x)), thtbase(tht);
    if (tht<0) thtbase += 2.*M_PI;
    //compute all intersections and associated gradient values
    for (int i=0; i<Get_q(); i++)
    {
        tht = thtbase + i*2.*M_PI;
        R = radius(tht);
        drdth = DrDtheta(tht);
        f.push_back(1. - PL/R); //store function
        // store partial derivatives
        std::vector<double> rowi;
        drdth = DrDtheta(tht);
        rowi.push_back( - ( x*R/PL - drdth*dthtdx*PL )/(R*R) ); //df/dx
        rowi.push_back( - ( y*R/PL - drdth*dthtdy*PL )/(R*R) ); //df/dy
        rowi.push_back( PL/(R*R) ); //df/dr
        Df.push_back(rowi);
    }
    //bubble sort, not really efficient but acceptable for such small arrays
    for(int i=0; i<Get_q()-1; i++)
        for (int j=i+1; j<Get_q(); j++)
            if (f[i]<f[j])
            {
                //std::swap values of f[i] and f[j]
                std::swap(f[i],f[j]);
                //std::swap rows Df[i] and Df[j]
                Df[i].swap(Df[j]);
            }
    //Compute resulting Rfunction
    std::vector<double> Df1; //std::vectorfor df/dxi
    //iterative evaluation of:
    // -the resulting R-functions
    // -the associated partial derivatives
    double f1,fdum;
    f1 = f[0]; // first value of f
    Df1 = Df[0]; // first associated row with partial derivatives
    //combine functions as (...((F1 v F2) v F3 ) v F4) v ...)
    for(int i=1; i<Get_q(); i++) // for all intersections
    {
        //compute R-function, sets all partial derivatives
        //fdum and Ddum temporary results of the union from F1 to Fi
        RpUnion(f1, f[i], Df1, Df[i], fdum, Ddum);
        //update results in f1 and Df1, and iterate
        f1 = fdum;
        Df1 = Ddum;
    }
    //final partial derivatives df/dxi after R-functions
    Dffinal = Df1;
    //clear arrays
    f.clear(); Df.clear(); Ddum.clear(); Df1.clear();
    //return results
    return f1;
}
double RationalSuperShape2D :: ImplicitFunction3( const Vector2d P, std::vector<double> &Dffinal){
    Dffinal.clear();
    // nothing computable, return zero values, zero partial derivatives
    // the point will have no effect on the ongoing computations
    if ( P[0] == 0 && P[1] == 0)
    {
        // Df/Dx, Df/Dy, Df/Dr set to zero...
        for (int i=0; i<3; i++) Dffinal.push_back(0);
        return 0;
    }
    std::vector<double> f, Ddum;
    double x(P[0]), y(P[1]), PSL(P.squaredNorm()), dthtdx (-y/PSL), dthtdy (x/PSL), R,drdth;
    std::vector< std::vector<double> > Df;
    //assert angular values between [0, 2q*Pi]
    double tht (atan2(y,x)), thtbase(tht);
    if (tht<0) thtbase += 2.*M_PI;
    //compute all intersections and associated gradient values
    for (int i=0; i<Get_q(); i++)
    {
        tht = thtbase + i*2.*M_PI;
        R = radius(tht);
        drdth = DrDtheta(tht);
        f.push_back( log( R*R / PSL)); //store function
        // store partial derivatives
        drdth = DrDtheta(tht);
        std::vector<double> rowi;
        rowi.push_back( -2.*(x*R - PSL * drdth*dthtdx)/(R*PSL) ); //df/dx
        rowi.push_back( -2.*(y*R - PSL * drdth*dthtdy)/(R*PSL) ); //df/dy
        rowi.push_back( 2./R ); //df/dr
        Df.push_back(rowi);
    }
    //bubble sort, not really efficient but acceptable for such small arrays
    for(int i=0; i<Get_q()-1; i++)
        for (int j=i+1; j<Get_q(); j++)
            if (f[i]<f[j])
            {
                //std::swap values of f[i] and f[j]
                std::swap(f[i],f[j]);
                //std::swap rows Df[i] and Df[j]
                Df[i].swap(Df[j]);
            }
    //Compute resulting Rfunction
    std::vector<double> Df1; //std::vectorfor df/dxi
    //iterative evaluation of:
    // -the resulting R-functions
    // -the associated partial derivatives
    double f1,fdum;
    f1 = f[0]; // first value of f
    Df1 = Df[0]; // first associated row with partial derivatives
    //combine functions as (...((F1 v F2) v F3 ) v F4) v ...)
    for(int i=1; i<Get_q(); i++) // for all intersections
    {
        //compute R-function, sets all partial derivatives
        //fdum and Ddum temporary results of the union from F1 to Fi
        RpUnion(f1, f[i], Df1, Df[i], fdum, Ddum);
        //update results in f1 and Df1, and iterate
        f1 = fdum;
        Df1 = Ddum;
    }
    //final partial derivatives df/dxi after R-functions
    Dffinal = Df1;
    //clear arrays
    f.clear(); Df.clear(); Ddum.clear(); Df1.clear();
    //return results
    return f1;
}
double RationalSuperShape2D :: DrDtheta(double tht)
{
    // analytic
    /*
double n1(Get_n1()), n2(Get_n2()), n3(Get_n3()), m(Get_p()),q(Get_q()),a(Get_a()),b(Get_b());
double c ( cos( m*0.25*tht / q)), C( fabs(c)), s( sin( m*0.25*tht / q)), S( fabs(s));
if(C<EPSILON || S<EPSILON) return 0;
double DC (-0.25*m*s/q), DS (0.25*m*c/q);
return
(-1./n1) *
pow( pow(C,n2) / a + pow(S,n3)/b , -1./n1 - 1.)*
(
-0.25*m*pow(C,n2)*(n2*tan(m*tht*0.25/q)) / (a*q)
+
0.25*m*pow(S,n3)*(n3/tan(m*tht*0.25/q)) / (b*q)
);
*/
    double r0,r1,r2,r3;
    double delta(1e-3);
    r0 = radius(tht - 2*delta);
    r1 = radius(tht - delta);
    r2 = radius(tht + delta);
    r3 = radius(tht + 2*delta);
    Vector4d V(
                r0,
                -8*r1,
                8*r2,
                -r3
                );
    return V.sum() / (12.*delta);
}
double RationalSuperShape2D :: DrDa(const double tht)
{
    //analytic version
    /*
double C = fabs( cos( Get_p() * tht * 0.25 / Get_q())) ;
double S = fabs( sin( Get_p() * tht * 0.25 / Get_q())) ;
double aCn2 = pow(C, Get_n2())/Get_a();
double bSn3 = pow(S, Get_n3())/Get_b();
return aCn2 * pow( aCn2 + bSn3, -1./Get_n1() - 1.) / (Get_n1()*Get_a());
*/
    //discrete approximation
    double olda(Get_a());
    double r0,r1,r2,r3;
    double delta(1e-3);
    Set_a(olda - 2*delta); r0 = radius(tht);
    Set_a(olda - delta); r1 = radius(tht);
    Set_a(olda + delta); r2 = radius(tht);
    Set_a(olda + 2*delta); r3 = radius(tht);
    Set_a(olda);
    Vector4d V(
                r0,
                -8*r1,
                8*r2,
                -r3
                );
    return V.sum() / (12.*delta);
}
double RationalSuperShape2D :: DrDb(const double tht)
{
    //analytic
    /*
double C = fabs( cos( Get_p() * tht * 0.25 / Get_q())) ;
double S = fabs( sin( Get_p() * tht * 0.25 / Get_q())) ;
double aCn2 = pow(C, Get_n2())/Get_a();
double bSn3 = pow(S, Get_n3())/Get_b();
return bSn3 * pow( aCn2 + bSn3, -1./Get_n1() - 1.) / (Get_n1()*Get_b());
*/
    //discrete approximation
    double oldb(Get_b());
    double r0,r1,r2,r3;
    double delta(1e-3);
    Set_b(oldb - 2*delta); r0 = radius(tht);
    Set_b(oldb - delta); r1 = radius(tht);
    Set_b(oldb + delta); r2 = radius(tht);
    Set_b(oldb + 2*delta); r3 = radius(tht);
    Set_b(oldb);
    Vector4d V(
                r0,
                -8*r1,
                8*r2,
                -r3
                );
    return V.sum() / (12.*delta);
}
double RationalSuperShape2D :: DrDn1(const double tht)
{
    //analytic
    /*
double R(radius(tht));
double rn1 = pow(R, -Get_n1());
if (R>EPSILON){
//return log ( pow ( R, -1./Get_n1()))*R;
return log(rn1) * R /(Get_n1()*Get_n1());
}
//std::cout <<"rhaaaa"<<std::endl;
return 0;
*/
    //discrete approximation
    double oldn1(Get_n1());
    double r0,r1,r2,r3;
    double delta(1e-3);
    Set_n1(oldn1 - 2*delta); r0 = radius(tht);
    Set_n1(oldn1 - delta); r1 = radius(tht);
    Set_n1(oldn1 + delta); r2 = radius(tht);
    Set_n1(oldn1 + 2*delta); r3 = radius(tht);
    Set_n1(oldn1);
    Vector4d V(
                r0,
                -8*r1,
                8*r2,
                -r3
                );
    return V.sum() / (12.*delta);
}
double RationalSuperShape2D :: DrDn2(const double tht)
{
    //analytic
    /*
double C = fabs( cos( Get_p() * tht * 0.25 / Get_q())) ;
double S = fabs( sin( Get_p() * tht * 0.25 / Get_q())) ;
double aCn2 = pow(C, Get_n2())/Get_a();
double bSn3 = pow(S, Get_n3())/Get_b();
if(C <EPSILON) return 0;
return
- pow( aCn2 + bSn3, -1./Get_n1() - 1.)
*
log(C)*aCn2 / Get_n1();
*/
    //discrete approximation
    double oldn2(Get_n2());
    double r0,r1,r2,r3;
    double delta(1e-3);
    Set_n2(oldn2 - 2*delta); r0 = radius(tht);
    Set_n2(oldn2 - delta); r1 = radius(tht);
    Set_n2(oldn2 + delta); r2 = radius(tht);
    Set_n2(oldn2 + 2*delta); r3 = radius(tht);
    Set_n2(oldn2);
    Vector4d V(
                r0,
                -8*r1,
                8*r2,
                -r3
                );
    return V.sum() / (12.*delta);
}
double RationalSuperShape2D :: DrDn3(const double tht)
{
    //analytic
    /*
double C = fabs( cos( Get_p() * tht * 0.25 / Get_q())) ;
double S = fabs( sin( Get_p() * tht * 0.25 / Get_q())) ;
double aCn2 = pow(C, Get_n2())/Get_a();
double bSn3 = pow(S, Get_n3())/Get_b();
if(S < EPSILON) return 0;
return
- pow( aCn2 + bSn3, -1./Get_n1() - 1.)
*
log(S)*bSn3 / Get_n1();
*/
    //discrete approx
    double oldn3(Get_n3());
    double r0,r1,r2,r3;
    double delta(1e-3);
    Set_n3(oldn3 - 2*delta); r0 = radius(tht);
    Set_n3(oldn3 - delta); r1 = radius(tht);
    Set_n3(oldn3 + delta); r2 = radius(tht);
    Set_n3(oldn3 + 2*delta); r3 = radius(tht);
    Set_n3(oldn3);
    Vector4d V(
                r0,
                -8*r1,
                8*r2,
                -r3
                );
    return V.sum() / (12.*delta);
}
void RpUnion(double f1, double f2, std::vector<double> Df1, std::vector<double> Df2, double &f, std::vector<double> &Df)
{
    assert(Df1.size() == Df2.size());
    Df.clear();
    f = f1+f2+sqrt(f1*f1+f2*f2);
    if(f1 != 0 || f2 != 0) // function differentiable
        for(unsigned int i=0; i<Df1.size(); i++)
            Df.push_back( Df1[i] + Df2[i] + (f1*Df1[i]+f2*Df2[i])/sqrt(f1*f1+f2*f2) );
    else //function not differentiable, set everything to zero
        for(unsigned int i=0; i<Df1.size(); i++)
            Df.push_back( 0 );
}
void RpIntersection(double f1, double f2, std::vector<double> Df1, std::vector<double> Df2, double &f, std::vector<double> &Df)
{
    assert(Df1.size() == Df2.size());
    Df.clear();
    f = f1+f2-sqrt(f1*f1+f2*f2);
    if(f1 != 0 || f2 != 0) // function differentiable
        for(unsigned int i=0; i<Df.size(); i++)
            Df.push_back ( Df1[i] + Df2[i] - (f1*Df1[i]+f2*Df2[i])/sqrt(f1*f1+f2*f2) ) ;
    else //function not differentiable, set everything to zero
        for(unsigned int i=0; i<Df1.size(); i++)
            Df.push_back( 0 );
}
void RationalSuperShape2D :: Optimize5D(
        std::string outfilename,
        std::vector< Vector2d, aligned_allocator< Vector2d> > Data,
        double &err ,
        int functionused
        )
{
    double NewChiSquare, ChiSquare(1e15), OldChiSquare(1e15);
    std::ofstream logfile;
    logfile.open(outfilename.c_str());
    bool STOP(false);
    double oldparams[] ={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; //dj[16];
    double LAMBDA_INCR(10), lambda(pow(LAMBDA_INCR, -6));
    MatrixXd alpha, alpha2;
    alpha = MatrixXd::Zero(5,5);
    alpha2 = MatrixXd::Zero(5,5);
    VectorXd beta, beta2, dj, oldbeta, sigma;
    beta = VectorXd::Zero(5);
    beta2 = VectorXd::Zero(5);
    dj = VectorXd::Zero(5);
    oldbeta = VectorXd::Zero(5);
    // trial = VectorXd::Zero(5);
    // sigma = VectorXd::Zero(5);
    logfile << *this;

    std::vector<double> Df;
    int itnum = 0;
    for(itnum=0; itnum<1000 && STOP==false; itnum++) {
        //store oldparams
        for(size_t i=0; i<Parameters.size(); i++) oldparams[i] = Parameters[i];
        alpha.setZero();
        beta.setZero();
        alpha2.setZero();
        beta2.setZero();
        //std::cout <<"TRIAL:"<<*this<<std::endl;
        bool outofbounds(false);
        //std::cout <<"Norm on in Opt2 : "<<Normalization<<std::endl;
        ChiSquare = XiSquare5D(Data,
                               alpha,
                               beta,
                               functionused, //implicit function1
                               true); //update vectors
        //
        // add Lambda to diagonla elements and solve the matrix
        //
        //Linearization of Hessian, cf Numerical Recepies
        for(int k=0; k<5; k++)
        {
            alpha(k,k) *= 1. + lambda; //multiplicative factor to make diagonal dominant
            alpha(k,k) += lambda; //additive factor to avoid rank deficient matrix
        }
        //solve system
        alpha.ldlt().solveInPlace(beta);
        //coefficients a and b in [0.01, 100] to preserve numerical accuracy
        outofbounds = Parameters[0] + beta[0] < 0.01 || Parameters[0] + beta[0] > 1000 ||
                Parameters[1] + beta[1] < 0.01 || Parameters[1] + beta[1] > 1000 ||
                Parameters[2] + beta[2] < 0.1 || Parameters[2] + beta[2]> 1000 ||
                Parameters[3] + beta[3] < 0.1 || Parameters[3] + beta[3]> 1000 ||
                Parameters[4] + beta[4] < 0.1 || Parameters[4] + beta[4]> 1000;
        if( !outofbounds )
        {
            Set_a(Parameters[0] + beta[0]);
            Set_b(Parameters[1] + beta[1]);
            // coefficients n1 in [1., 1000]
            // setting n1<1. leads to strong numerical instabilities
            Set_n1( Parameters[2] + beta[2] );
            // coefficients n2,n3 in [0.001, 1000]
            Set_n2( Parameters[3] + beta[3]);
            Set_n3( Parameters[4] + beta[4]);
        }
        //
        // Evaluate chisquare with new values
        //
        OldChiSquare=ChiSquare;
        NewChiSquare=XiSquare5D(Data,
                                alpha2,
                                beta2,
                                functionused, //implicit function
                                false); // update alpha and beta or not
        //
        // check if better result
        //
        if( NewChiSquare>0.999*OldChiSquare ) // new result sucks-->restore old params and try with lambda 10 times bigger
        {
            lambda *=LAMBDA_INCR;
            for(size_t i=0; i<Parameters.size(); i++) Parameters[i]=oldparams[i];
        }
        else //successful iteration
        {
            // huge improvement, something may have been wrong
            // this may arise during the first iterations
            // n1 may literally explode, or tend to 0...
            // in such case, the next iteration is successful but leads to a local minimum
            // ==> it is better to verify the result with a smaller step
            // if indeed it was a correct iteration, then it will pass the next time
            if (NewChiSquare <= 0.01*OldChiSquare) //99% improvement, impossible
            {
                lambda *=LAMBDA_INCR; // reduce the step within the search direction
                for(size_t i=0; i<Parameters.size(); i++) Parameters[i]=oldparams[i]; // restore old parameters
            }
            else
            {
                //correct and realistic improvement
                logfile << *this;
                lambda /=LAMBDA_INCR;
            }
        }
        STOP = lambda > 1e15 || NewChiSquare < 1e-5; // very small displacement ==> local convergence
    } //end for(...
    err = ChiSquare;
    logfile << *this;
    logfile.close();
}
double RationalSuperShape2D :: XiSquare5D(
        const std::vector< Vector2d, aligned_allocator< Vector2d> > Data,
        MatrixXd &alpha,
        VectorXd &beta,
        int functionused,
        bool update) {
    //five dimensional optimization: a, b, n1, n2, n3 are optimized
    VectorXd dj; dj = VectorXd::Zero(5);
    Matrix3d Tr,Rot;
    //functions pointer
    double (RationalSuperShape2D ::*pt2ConstMember)(const Vector2d P, std::vector<double> &Dffinal) = NULL;
    switch (functionused){
    case 1 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction1; }break;
    case 2 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction2; }break;
    case 3 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction3; }break;
    default : pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction1;
    }
    std::vector<double> Df;
    double x, y, tht, ChiSquare(1e15), f(0),
            x0(Get_xoffset()),y0(Get_yoffset()),tht0(Get_thtoffset());
    //clean memory
    if ( update ) {
        alpha.setZero();
        beta.setZero();
    }
    //evaluate ChiSquare, components for the beta and matrix alpha
    ChiSquare=0;
    //First define inverse translation T-1
    Tr.setZero();
    Rot.setZero();
    Tr << 1 , 0 , -x0 ,
            0 , 1 , -y0 ,
            0 , 0 , 1;
    //Then define inverse rotation, i.e. transposed rotation
    Rot << cos(tht0) , sin(tht0) , 0 ,
            -sin(tht0) , cos(tht0) , 0 ,
            0 , 0 , 1;
    //summation of squared potentials
    for(size_t i=0; i<Data.size(); i++ ){
        double DfDr;
        //global inverse transform is T * R
        Vector2d dum(Data[i]);
        Vector3d dum2(dum[0],dum[1],1);
        //apply inverse transform
        Vector3d dum3 ( Rot * (Tr * dum2));
        Vector2d P( dum3[0], dum3[1]); //2D point in canonical referential
        //get partial derivatives for canonical point
        x = P[0]; y = P[1];
        // avoid division by 0 ==> numerical stability
        if (P.norm()<EPSILON) continue; // avoids division by zero
        tht = atan2(y,x); if( tht<0) tht+=2.*M_PI;
        //theta = Arctan(Y/X)

        //avoid non differentiable cases
        f = (*this.*pt2ConstMember)(P, Df); // call to the implicit function
        //
        //compute elements beta[i][0] and alpha[i][j]
        //
        //==> requires partial derivatives!!
        DfDr = Df[2]; //Df/Dr stored at index 2 in array Df during the call to ImplicitFunction1-2-3
        // F1 = R-PL ==> DfDr = 1. ;
        // F2 = 1-PL/R ==> DfDr = PL/R\B2 ;
        // F3 = log ( R\B2/PSL) ==> DfDr = 2/R
        dj.setZero();
        //df/da = df/dr * dr/da
        dj[0] = DfDr * DrDa(tht) ;
        //df/db = df/dr * dr/db
        dj[1] = DfDr * DrDb(tht) ;
        //df/dn1 = df/dr * dr/dn1
        dj[2] = DfDr * DrDn1(tht);
        //df/dn2 = df/dr * dr/dn2
        dj[3] = DfDr * DrDn2(tht);
        //df/dn3 = df/dr * dr/dn3
        dj[4] = DfDr * DrDn3(tht);
        ChiSquare += f*f;
        if(update){
            //gradient beta
            for(int k=0;k<5; k++)
            {
                beta[k] -= f * dj[k];
            }
            //compute approximation of Hessian matrix
            for(int k=0; k<5; k++)
                for(int j=0; j<5; j++)
                    alpha(k,j) += dj[k]*dj[j];
        }
    }//for all vertices
    return ChiSquare;
}
void RationalSuperShape2D :: Optimize7D(
        std::string outfilename,
        std::vector< Vector2d, aligned_allocator< Vector2d> > Data,
        double &err ,
        int functionused
        )
{
    double NewChiSquare, ChiSquare(1e15), OldChiSquare(1e15);
    std::ofstream logfile;
    logfile.open(outfilename.c_str());
    bool STOP(false);
    double oldparams[] ={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};//, dj[16];
    double LAMBDA_INCR(10), lambda(pow(LAMBDA_INCR, -6));
    MatrixXd alpha, alpha2;
    alpha = MatrixXd::Zero(7,7);
    alpha2 = MatrixXd::Zero(7,7);
    VectorXd beta, beta2, dj, oldbeta, trial, sigma;
    beta = VectorXd::Zero(7);
    beta2 = VectorXd::Zero(7);
    dj = VectorXd::Zero(7);
    oldbeta = VectorXd::Zero(7);
    trial = VectorXd::Zero(7);
    sigma = VectorXd::Zero(7);
    logfile << *this;

    int itnum = 0;
    for(itnum=0; itnum<1000 && STOP==false; itnum++) {
        //store oldparams
        for(size_t i=0; i<Parameters.size(); i++) oldparams[i]=Parameters[i];
        alpha.setZero();
        beta.setZero();
        alpha2.setZero();
        beta2.setZero();
        //std::cout <<"TRIAL:"<<*this<<std::endl;
        bool outofbounds(false);
        //std::cout <<"Norm on in Opt2 : "<<Normalization<<std::endl;
        ChiSquare = XiSquare7D(Data,
                               alpha,
                               beta,
                               functionused, //implicitf cuntion1
                               true); //update vectors
        //
        // add Lambda to diagonla elements and solve the matrix
        //
        //Linearization of Hessian, cf Numerical Recepies

        for(int k=0; k<7; k++)
        {
            alpha(k,k) *= 1. + lambda; //multiplicative factor to make diagonal dominant
            alpha(k,k) += lambda; //additive factor to avoid rank deficient matrix
        }
        //std::cout << alpha << std::endl<<std::endl;
        //std::cout << beta << std::endl<<std::endl;
        //solve system
        alpha.ldlt().solveInPlace(beta);
        //coefficients a and b in [0.01, 100]
        outofbounds = Parameters[0] + beta[0] < 0.01 || Parameters[0] + beta[0] > 1000 ||
                Parameters[1] + beta[1] < 0.01 || Parameters[1] + beta[1] > 1000 ||
                Parameters[2] + beta[2] < 0.1 || Parameters[2] + beta[2]> 1000 ||
                Parameters[3] + beta[3] < 0.1 || Parameters[3] + beta[3]> 1000 ||
                Parameters[4] + beta[4] < 0.1 || Parameters[4] + beta[4]> 1000;
        if( !outofbounds )
        {
            Set_a( Parameters[0] + beta[0]);
            Set_b( Parameters[1] + beta[1]);
            // coefficients n1 in [1., 1000]
            // setting n1<1. leads to strong numerical instabilities
            Set_n1( Parameters[2] + beta[2] );
            // coefficients n2,n3 in [0.001, 1000]
            Set_n2( Parameters[3] + beta[3]);
            Set_n3( Parameters[4] + beta[4]);
            // coefficients x0 and y0
            //truncate translation to avoid huge gaps
            beta[5] = std::min(0.05, std::max(-0.05, beta[5]));
            beta[6] = std::min(0.05, std::max(-0.05, beta[6]));
            Parameters[9] += beta[5];
            Parameters[10] += beta[6];
        }
        //
        // Evaluate chisquare with new values
        //
        OldChiSquare = ChiSquare;
        NewChiSquare = XiSquare7D(Data,
                                  alpha2,
                                  beta2,
                                  functionused, //implicitf cuntion1
                                  false);
        if( NewChiSquare>0.999*OldChiSquare ) // new result sucks-->restore old params and try with lambda 10 times bigger
        {
            lambda *= LAMBDA_INCR;
            for(size_t i=0; i<Parameters.size(); i++) Parameters[i]=oldparams[i];
        }
        else //successful iteration
        {
            // huge improvement, something may have been wrong
            // this may arise during the first iterations
            // n1 may literally explode, or tend to 0...
            // in such case, the next iteration is successful but leads to a local minimum
            // ==> it is better to verify the result with a smaller step
            // if indeed it was a correct iteration, then it will pass the next time
            if (NewChiSquare <= 0.01*OldChiSquare) //99% improvement, impossible
            {
                lambda *= LAMBDA_INCR; // reduce the step within the search direction
                for(size_t i=0; i<Parameters.size(); i++) Parameters[i]=oldparams[i]; // restore old parameters
            }
            else
            {
                //correct and realistic improvement
                logfile << *this;
                lambda /=LAMBDA_INCR;
            }
        }
        STOP = lambda > 1e15 || NewChiSquare < 1e-5;
    } //end for(...
    err = ChiSquare;
    logfile << *this;
    logfile.close();
}
double RationalSuperShape2D :: XiSquare7D(
        const std::vector< Vector2d, aligned_allocator< Vector2d> > Data,
        MatrixXd &alpha,
        VectorXd &beta,
        int functionused,
        bool update) {
    VectorXd dj; dj = VectorXd::Zero(7);
    Matrix3d Tr,Rot, dTrdx0, dTrdy0;
    //functions pointer
    double (RationalSuperShape2D ::*pt2ConstMember)(const Vector2d P, std::vector<double> &Dffinal) = NULL;
    switch (functionused){
    case 1 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction1; }break;
    case 2 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction2; }break;
    case 3 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction3; }break;
    default : pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction1;
    }
    std::vector<double> Df;
    double x, y, tht, dthtdx, dthtdy,dthtdx0, dthtdy0, drdth,
            ChiSquare(1e15),
            f(0),
            x0(Get_xoffset()),y0(Get_yoffset()),tht0(Get_thtoffset()),
            dxdx0,dxdy0, dydx0,dydy0;
    //clean memory
    if(update) {
        alpha.setZero();
        beta.setZero();
    }
    //evaluate ChiSquare, components for the beta and matrix alpha
    ChiSquare=0;
    //First define inverse translation T-1
    Tr.setZero();
    Rot.setZero();
    Tr << 1 , 0 , -x0 ,
            0 , 1 , -y0 ,
            0 , 0 , 1;
    //derivatives of translation matrix are constant and can be computed once for all
    dTrdx0 << 0 , 0 , -1 , 0 , 0 , 0 , 0 , 0 , 1;
    dTrdy0 << 0 , 0 , 0 , 0 , 0 , -1 , 0 , 0 , 1;
    //Then define inverse rotation, i.e. transposed rotation
    Rot << cos(tht0) , sin(tht0) , 0 ,
            -sin(tht0) , cos(tht0) , 0 ,
            0 , 0 , 1;
    for(size_t i=0; i<Data.size(); i++){
        double DfDr;
        //global inverse transform is T * R
        Vector2d dum(Data[i]);
        Vector3d dum2(dum[0],dum[1],1);
        //apply inverse transform
        Vector3d dum3 ( Rot * (Tr * dum2));
        Vector2d P(dum3[0],dum3[1]); //2D point in canonical ref
        //get partial derivatives for canonical point
        Vector3d dPdx0 ( Rot * (dTrdx0 * dum2) );
        Vector3d dPdy0 ( Rot * (dTrdy0 * dum2) );
        //simplify notations
        dxdx0 = dPdx0[0];
        dxdy0 = dPdy0[0];
        dydx0 = dPdx0[1];
        dydy0 = dPdy0[1];
        x = P[0]; y = P[1];
        // avoid division by 0 ==> numerical stability
        if (P.norm()<EPSILON) continue; // avoids division by zero
        tht = atan2(y,x); if( tht < 0 ) tht+=2.*M_PI;

        //theta = Arctan(Y/X)
        dthtdx = -sin(tht) ;//-y / (x*x+y*y);
        dthtdy = cos(tht); //x / (x*x+y*y);
        //partial derivatives of theta regarding x offset, y offset, and angular offset
        dthtdx0 = dthtdx * dxdx0 + dthtdy * dydx0;
        dthtdy0 = dthtdx * dxdy0 + dthtdy * dydy0;
        //avoid non differentiable cases
        drdth = DrDtheta(tht);
        f = (*this.*pt2ConstMember)(P, Df); // call to the implicit function
        //
        //compute elements beta[i][0] and alpha[i][j]
        //
        //==> requires partial derivatives!!
        DfDr = Df[2]; //Df/Dr stored at index 2 in array Df during the call to ImplicitFunction1-2-3
        // F1 = R-PL ==> DfDr = 1. ;
        // F2 = 1-PL/R ==> DfDr = PL/R\B2 ;
        // F3 = log ( R\B2/PSL) ==> DfDr = 2/R
        dj.setZero();
        //df/da = df/dr * dr/da
        dj[0] = DfDr * DrDa(tht) ;
        //df/db = df/dr * dr/db
        dj[1] = DfDr * DrDb(tht) ;
        //df/dn1 = df/dr * dr/dn1
        dj[2] = DfDr * DrDn1(tht);
        //df/dn2 = df/dr * dr/dn2
        dj[3] = DfDr * DrDn2(tht);
        //df/dn3 = df/dr * dr/dn3
        dj[4] = DfDr * DrDn3(tht);
        //df/dx0 = df/dr * dr/dtht *dtht/dx0
        dj[5]= DfDr * drdth*dthtdx0;
        //df/dy0 = df/dr * dr/dtht *dtht/dy0
        dj[6]= DfDr * drdth*dthtdy0;
        //squared summation
        ChiSquare += f*f;
        if(update){
            for(int k=0; k<7; k++)
            {
                beta[k] -= f*dj[k];
            }
            //compute approximation of Hessian matrix
            for(int k=0; k<7; k++)
                for(int j=0; j<7; j++)
                    alpha(k,j) += dj[k]*dj[j];
        }
    }//for all vertices
    return ChiSquare;
}
void RationalSuperShape2D :: Optimize8D(
        std::vector< Vector2d, aligned_allocator< Vector2d> > Data,
        double &err ,
        int functionused
        )
{
    double NewChiSquare, ChiSquare(1e15), OldChiSquare(1e15);
    // ofstream logfile;
    // logfile.open(outfilename.c_str());
    bool STOP(false);
    double oldparams[] ={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};//, dj[16];
    double LAMBDA_INCR(10);
    double lambda(pow(LAMBDA_INCR, -6));
    MatrixXd alpha, alpha2;
    alpha = MatrixXd::Zero(8,8);
    alpha2 = MatrixXd::Zero(8,8);
    VectorXd beta, beta2, dj, oldbeta, trial, sigma;
    beta = VectorXd::Zero(8);
    beta2 = VectorXd::Zero(8);
    dj = VectorXd::Zero(8);
    oldbeta = VectorXd::Zero(8);
    trial = VectorXd::Zero(8);
    sigma = VectorXd::Zero(8);

    // logfile << *this;
    int itnum = 0;
    for(itnum=0; itnum<1000 && STOP==false; itnum++) {
        //store oldparams
        for(size_t i=0; i<Parameters.size(); i++) oldparams[i]=Parameters[i];
        alpha.setZero();
        beta.setZero();
        alpha2.setZero();
        beta2.setZero();
        //std::cout <<"TRIAL:"<<*this<<std::endl;
        bool outofbounds(false);
        //std::cout <<"Norm on in Opt2 : "<<Normalization<<std::endl;
        ChiSquare = XiSquare8D(Data,
                               alpha,
                               beta,
                               functionused, //implicitf cuntion1
                               true); //update vectors
        //
        // add Lambda to diagonla elements and solve the matrix
        //
        //Linearization of Hessian, cf Numerical Recepies
        for(int k=0; k<8; k++)
        {
            alpha(k,k) *= 1. + lambda; //multiplicative factor to make diagonal dominant
            alpha(k,k) += lambda; //additive factor to avoid rank deficient matrix
        }
        //solve system
        alpha.ldlt().solveInPlace(beta);
        //coefficients a and b in [0.01, 100]
        outofbounds = Parameters[0] + beta[0] < 0.01 || Parameters[0] + beta[0] > 1000 ||
                Parameters[1] + beta[1] < 0.01 || Parameters[1] + beta[1] > 1000 ||
                Parameters[2] + beta[2] < 0.1 || Parameters[2] + beta[2]> 1000 ||
                Parameters[3] + beta[3] < 0.1 || Parameters[3] + beta[3]> 1000 ||
                Parameters[4] + beta[4] < 0.1 || Parameters[4] + beta[4]> 1000;
        if( !outofbounds ) {
            Set_a( Parameters[0] + beta[0]);
            Set_b( Parameters[1] + beta[1]);
            // coefficients n1 in [1., 1000]
            // setting n1<1. leads to strong numerical instabilities
            Set_n1( Parameters[2] + beta[2] );
            // coefficients n2,n3 in [0.001, 1000]
            Set_n2( Parameters[3] + beta[3]);
            Set_n3( Parameters[4] + beta[4]);
            // coefficients x0 and y0
            //truncate translation to avoid huge gaps
            beta[5] = std::min(0.05, std::max(-0.05, beta[5]));
            beta[6] = std::min(0.05, std::max(-0.05, beta[6]));
            Parameters[9] += beta[5];
            Parameters[10] += beta[6];
            //same for rotational offset tht0
            beta[7] = std::min(M_PI/50., std::max(-M_PI/50., beta[7]));
            Parameters[7] += beta[7];
        }
        //
        // Evaluate chisquare with new values
        //
        OldChiSquare = ChiSquare;
        NewChiSquare=XiSquare8D(Data,
                                alpha2,
                                beta2,
                                functionused, //implicitf cuntion1
                                false);
        //
        // check if better result
        //
        if( NewChiSquare>0.999*OldChiSquare ) // new result sucks-->restore old params and try with lambda 10 times bigger
        {
            lambda *=LAMBDA_INCR;
            for(size_t i=0; i<Parameters.size(); i++) Parameters[i]=oldparams[i];
        }
        else //successful iteration
        {
            // huge improvement, something may have been wrong
            // this may arise during the first iterations
            // n1 may literally explode, or tend to 0...
            // in such case, the next iteration is successful but leads to a local minimum
            // ==> it is better to verify the result with a smaller step
            // if indeed it was a correct iteration, then it will pass the next time
            if (NewChiSquare <= 0.01*OldChiSquare) //99% improvement, impossible
            {
                lambda *=LAMBDA_INCR; // reduce the step within the search direction
                for(size_t i=0; i<Parameters.size(); i++) Parameters[i]=oldparams[i]; // restore old parameters
            }
            else
            {
                //correct and realistic improvement
                // logfile << *this;
                lambda /=LAMBDA_INCR;
            }
        }
        STOP = lambda > 1e15 || NewChiSquare < 1e-5; // very small displacement ==> local convergence
    } //end for(...
    err = ChiSquare;
    // logfile << *this;
    // logfile.close();
}
double RationalSuperShape2D :: XiSquare8D(
        const std::vector< Vector2d, aligned_allocator< Vector2d> > Data,
        MatrixXd &alpha,
        VectorXd &beta,
        int functionused,
        bool update) {
    VectorXd dj; dj = VectorXd::Zero(8);
    Matrix3d Tr,Rot, dTrdx0, dTrdy0, dRotdtht0;
    //functions pointer
    double (RationalSuperShape2D ::*pt2ConstMember)(const Vector2d P, std::vector<double> &Dffinal) = NULL;
    switch (functionused){
    case 1 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction1; }break;
    case 2 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction2; }break;
    case 3 :{ pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction3; }break;
    default : pt2ConstMember = &RationalSuperShape2D :: ImplicitFunction1;
    }
    std::vector<double> Df;
    double  x, y, tht, dthtdx, dthtdy,dthtdx0, dthtdy0, drdth,
            ChiSquare(1e15),
            f(0),
            x0(Get_xoffset()),y0(Get_yoffset()),tht0(Get_thtoffset()),
            dxdx0,dxdy0,dxdtht0, dydx0,dydy0,dydtht0, dthtdtht0;
    //clean memory
    if(update)
    {
        alpha.setZero();
        beta.setZero();
    }
    //evaluate ChiSquare, components for the beta and matrix alpha
    ChiSquare=0;
    //First define inverse translation T-1
    Tr.setZero();
    Rot.setZero();
    dRotdtht0.setZero();
    Tr << 1 , 0 , -x0 ,
            0 , 1 , -y0 ,
            0 , 0 , 1;
    //derivatives of translation matrix are constant and can be computed once for all
    dTrdx0 << 0 , 0 , -1 , 0 , 0 , 0 , 0 , 0 , 1;
    dTrdy0 << 0 , 0 , 0 , 0 , 0 , -1 , 0 , 0 , 1;
    //Then define inverse rotation, i.e. transposed rotation
    Rot << cos(tht0) , sin(tht0) , 0 ,
            -sin(tht0) , cos(tht0) , 0 ,
            0 , 0 , 1;
    //get partial derivatives
    dRotdtht0 << -sin(tht0) , cos(tht0) , 0 ,
            -cos(tht0) , -sin(tht0) , 0 ,
            0 , 0 , 1;
    for(size_t i=0; i<Data.size(); i++){
        double DfDr;
        //global inverse transform is T * R
        Vector2d dum(Data[i]);
        Vector3d dum2(dum[0],dum[1],1);
        //apply inverse transform
        Vector3d dum3 ( Rot * (Tr * dum2));
        //get partial derivatives for canonical point
        Vector3d dPdx0 ( Rot * (dTrdx0 * dum2) );
        Vector3d dPdy0 ( Rot * (dTrdy0 * dum2) );
        Vector3d dPdtht0 (dRotdtht0 * (Tr *dum2) );
        Vector2d P(dum3[0],dum3[1]);
        //simplify notations
        dxdx0 = dPdx0[0];
        dxdy0 = dPdy0[0];
        dxdtht0 = dPdtht0[0];
        dydx0 = dPdx0[1];
        dydy0 = dPdy0[1];
        dydtht0 = dPdtht0[1];
        x = P[0]; y = P[1];
        // avoid division by 0 ==> numerical stability
        if (P.norm()<EPSILON) continue; // avoids division by zero
        tht = atan2(y,x); if( tht<0) tht+=2.*M_PI;

        //theta = Arctan(Y/X)
        dthtdx = -sin(tht) ;//-y / (x*x+y*y);
        dthtdy = cos(tht); //x / (x*x+y*y);
        //partial derivatives of theta regarding x offset, y offset, and angular offset
        dthtdx0 = dthtdx * dxdx0 + dthtdy * dydx0;
        dthtdy0 = dthtdx * dxdy0 + dthtdy * dydy0;
        dthtdtht0 = dthtdx * dxdtht0+ dthtdy * dydtht0;
        //avoid non differentiable cases
        drdth = DrDtheta(tht);
        f = (*this.*pt2ConstMember)(P, Df); // call to the implicit function
        //
        //compute elements beta[i][0] and alpha[i][j]
        //
        //==> requires partial derivatives!!
        DfDr = Df[2]; //Df/Dr stored at index 2 in array Df during the call to ImplicitFunction1-2-3
        // F1 = R-PL ==> DfDr = 1. ;
        // F2 = 1-PL/R ==> DfDr = PL/R\B2 ;
        // F3 = log ( R\B2/PSL) ==> DfDr = 2/R
        //GetPartialDerivatives(tht, DrDa, DrDb, DrDn1, DrDn2, DrDn3);
        //for(int toto=0; toto<8; toto++) dj[toto]=0;
        dj.setZero();
        //df/da = df/dr * dr/da
        dj[0] = DfDr * DrDa(tht) ;
        //df/db = df/dr * dr/db
        dj[1] = DfDr * DrDb(tht) ;
        //df/dn1 = df/dr * dr/dn1
        dj[2] = DfDr * DrDn1(tht);
        //df/dn2 = df/dr * dr/dn2
        dj[3] = DfDr * DrDn2(tht);
        //df/dn3 = df/dr * dr/dn3
        dj[4] = DfDr * DrDn3(tht);
        //df/dx0 = df/dr * dr/dtht *dtht/dx0
        dj[5]= DfDr * drdth*dthtdx0;
        //df/dy0 = df/dr * dr/dtht *dtht/dy0
        dj[6]= DfDr * drdth*dthtdy0;
        //df/dth0 = dfdr * dr/dtht * dtht/dtht0
        dj[7]= DfDr * drdth*dthtdtht0;
        ChiSquare += f*f;
        if( update ){

            for(int k=0; k<8; k++)
            {
                beta[k] -= f*dj[k];
            }
            //compute approximation of Hessian matrix

            for(int k=0; k<8; k++)
                for(int j=0; j<8; j++)
                    alpha(k,j) += dj[k]*dj[j];
        }
    }//for all vertices
    return ChiSquare;
}
Vector2d RationalSuperShape2D :: ClosestPoint( Vector2d P, int itmax){
    // P is supposed to be expressed in canonical referential
    double tht = atan2(P[1],P[0]); if (tht<0) tht +=2*M_PI;
    double error(1), change(1);
    int it(0);
    while (error >= 0.001 && fabs(change)>1e-6 && it < itmax){
        // we would like to optimize the distance function
        // change = l' / l'' where l is distance function with argument phi
        // find the change of phi
        // but by somehow we feel like to ignore the sign of f''
        // dl_dphi(parameters, point, phi);
        double f_prime = Deriv1(tht, P);
        //parameters, point, phi, delta);
        // d2l_dphi2(parameters, point, phi, delta);
        double f_2prime = Deriv2(tht, P);
        change = f_prime / fabs(f_2prime);
        error = fabs(change);
        change = change / 10; // slowly move on
        tht = tht-change;
        it++;
    }
    // std::cout <<"PPoint found"<<std::endl;
    Vector2d H (Point(tht));
    //H = Rot.transpose()*H + Vector2d(Get_xoffset(), Get_yoffset());
    return H;
}
// void RationalSuperShape2D :: writeFile(std::string fileName){
// ofstream logfile;
// logfile.open(fileName.c_str());
// for ( int i=0; i<Parameters.size(); i++)
// {
// logfile << setiosflags(ios::fixed) << setprecision(6)<<Parameters[i] << " ";
// }
// logfile << std::endl;
// logfile.close();
// }
bool RationalSuperShape2D :: ErrorMetric (std::vector< Vector2d, aligned_allocator< Vector2d> > Data, Vector4d &Mean, Vector4d &Var)
{
    //Bring back data into canonical referential
    double x0(Get_xoffset()), y0(Get_yoffset()), tht0(Get_thtoffset());
    //allocate matrices (homogenous coordinate, so 3x3 matrix)
    Matrix3d Tr,Rot;
    //First define inverse translation T-1
    Tr.setZero();
    Tr << 1 , 0 , -x0 ,
            0 , 1 , -y0 ,
            0 , 0 , 1;
    //Now define inverse rotation, i.e. transposed rotation
    Rot.setZero();
    Rot << cos(tht0) , sin(tht0) , 0 ,
            -sin(tht0) , cos(tht0) , 0 ,
            0 , 0 , 1;
    //now process all the data and store the point in a local array
    std::vector< Vector2d, aligned_allocator< Vector2d> > CanonicalData;
    // use 1 array of std::vector4d to reduce computational load
    std::vector< Vector4d, aligned_allocator< Vector4d> > dumarray;
    for( size_t i=0; i<Data.size(); i++ ){
        //global inverse transform is T * R
        Vector3d dum(Data[i][0], Data[i][1], 1.);
        //apply inverse transform
        Vector3d dum2 ( Rot * (Tr * dum));
        CanonicalData.push_back( Vector2d (dum2[0], dum2[1]) ); //2D point in canonical referential
    }
    /*
glColor3f(0,0,0);
glPointSize(2);
glBegin(GL_POINTS);
for( int i=0; i<Data.size(); i++ ){
//global inverse transform is T * R
Vector2d dum(CanonicalData[i]);
glVertex3f(dum[0],dum[1],0);
}
glEnd();
*/
    //Init Mean and Var
    Mean = Vector4d(0,0,0,0);
    Var = Mean;
    std::vector<double> Dffinal;//dummy local variable to store partial derivatives, unused in this function
    //compute Mean
    /*
glColor3f(1,0,0);
glLineWidth(2);
glBegin(GL_LINES);
*/
    for (size_t i=0; i< CanonicalData.size(); i++)
    {
        //get point
        Vector2d P ( CanonicalData[i] );
        //compute shortest point
        Vector2d H ( ClosestPoint(P) );
        /*
glVertex3f( P[0], P[1], 0);
glVertex3f( H[0], H[1], 0);
*/
        //compute everything for this 2d point)
        Vector4d dumvector4d(
                    pow ( ImplicitFunction1(P, Dffinal), 2) ,
                    pow ( ImplicitFunction2(P, Dffinal), 2) ,
                    pow ( ImplicitFunction3(P, Dffinal), 2) ,
                    (P-H).norm() //euclidean distance
                    );
        //compute directly the 3 implicit functions implemented
        Mean += dumvector4d;
        dumarray.push_back(dumvector4d); //store results for variance computation without recomputing everything
    }
    // glEnd();
    Mean /= CanonicalData.size();
    //Compute Var
    for (size_t i=0; i< CanonicalData.size(); i++)
    {
        //compute variance from previously saved results
        Var[0] += pow( Mean[0] - dumarray[i][0], 2);
        Var[1] += pow( Mean[1] - dumarray[i][1], 2);
        Var[2] += pow( Mean[2] - dumarray[i][2], 2);
        Var[3] += pow( Mean[3] - dumarray[i][3], 2);
    }
    Var /= CanonicalData.size()- 1;
    return true;
}


/* This file is part of PyMesh. Copyright (c) 2018 by Qingnan Zhou */
#include "SparseSolver.h"
#include <sstream>

#include <Eigen/IterativeLinearSolvers>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseLU>
#include <Eigen/SparseQR>
#ifdef WITH_UMFPACK
#include <Eigen/UmfPackSupport>
#endif
#ifdef WITH_CHOLMOD
#include <Eigen/CholmodSupport>
#endif
#ifdef WITH_MKL
#include <Eigen/PardisoSupport>
#endif

#include <Core/Exception.h>

#include "SparseSolverImplementation.h"

using namespace PyMesh;

SparseSolver::Ptr SparseSolver::create(const std::string& solver_type) {
    if (solver_type == "LDLT") {
        using LDLT = Eigen::SimplicialLDLT<ZSparseMatrix::ParentType>;
        return SparseSolver::Ptr(new SparseSolverImplementation<LDLT>);
    } else if (solver_type == "LLT") {
        using LLT = Eigen::SimplicialLLT<ZSparseMatrix::ParentType>;
        return SparseSolver::Ptr(new SparseSolverImplementation<LLT>);
    } else if (solver_type == "CG") {
        using CG = Eigen::ConjugateGradient<ZSparseMatrix::ParentType,
              Eigen::Lower|Eigen::Upper>;
        return SparseSolver::Ptr(
                new IterativeSparseSolverImplementation<CG>);
    } else if (solver_type == "LSCG") {
        using LSCG = Eigen::LeastSquaresConjugateGradient<ZSparseMatrix::ParentType>;
        return SparseSolver::Ptr(
                new IterativeSparseSolverImplementation<LSCG>);
    } else if (solver_type == "BiCG") {
        using BiCGSTAB = Eigen::BiCGSTAB<ZSparseMatrix::ParentType>;
        return SparseSolver::Ptr(
                new IterativeSparseSolverImplementation<BiCGSTAB>);
    } else if (solver_type == "SparseLU") {
        using SparseLU = Eigen::SparseLU<ZSparseMatrix::ParentType,
              Eigen::COLAMDOrdering<ZSparseMatrix::ParentType::StorageIndex> >;
        return SparseSolver::Ptr(new SparseSolverImplementation<SparseLU>);
    } else if (solver_type == "SparseQR") {
        using SparseQR = Eigen::SparseQR<ZSparseMatrix::ParentType,
              Eigen::COLAMDOrdering<ZSparseMatrix::ParentType::StorageIndex> >;
        return SparseSolver::Ptr(new SparseSolverImplementation<SparseQR>);
    }
#ifdef WITH_UMFPACK
    if (solver_type == "UmfPackLU") {
        using UmfPackLU = Eigen::UmfPackLU<ZSparseMatrix::ParentType>;
        return SparseSolver::Ptr(
                new SparseSolverImplementationWithLocalCopy<UmfPackLU>);
    }
#endif
#ifdef WITH_CHOLMOD
    if (solver_type == "Cholmod") {
        using CholmodSupernodalLLT =
            Eigen::CholmodSupernodalLLT<ZSparseMatrix::ParentType>;
        return SparseSolver::Ptr(
                new SparseSolverImplementation<CholmodSupernodalLLT>);
    }
#endif
#ifdef WITH_MKL
    if (solver_type == "PardisoLLT") {
        using PardisoLLT = Eigen::PardisoLLT<ZSparseMatrix::ParentType>;
        return SparseSolver::Ptr(
                new SparseSolverImplementation<PardisoLLT>);
    } else if (solver_type == "PardisoLDLT") {
        using PardisoLDLT = Eigen::PardisoLDLT<ZSparseMatrix::ParentType>;
        return SparseSolver::Ptr(
                new SparseSolverImplementation<PardisoLDLT>);
    } else if (solver_type == "PardisoLU") {
        using PardisoLU = Eigen::PardisoLU<ZSparseMatrix::ParentType>;
        return SparseSolver::Ptr(
                new SparseSolverImplementation<PardisoLU>);
    }
#endif

    std::stringstream err_msg;
    err_msg << "Unsupported solver type " << solver_type;
    throw NotImplementedError(err_msg.str());
}

std::vector<std::string> SparseSolver::get_supported_solvers() {
    std::vector<std::string> solver_names{
        "LDLT",
        "LLT",
        "CG",
        "LSCG",
        "BiCG",
        "SparseLU",
        "SparseQR"
    };
#ifdef WITH_UMFPACK
    solver_names.push_back("UmfPackLU");
#endif
#ifdef WITH_CHOLMOD
    solver_names.push_back("Cholmod");
#endif
#ifdef WITH_MKL
    solver_names.push_back("PardisoLLT");
    solver_names.push_back("PardisoLDLT");
    solver_names.push_back("PardisoLU");
#endif
    return solver_names;
}


/* boosting_training_tool.cc                                       -*- C++ -*-
   Jeremy Barnes, 12 June 2003
   Copyright (c) 2003 Jeremy Barnes.  All rights reserved.
   $Source$

   Tool to use to train boosting with.
*/

#include "jml/boosting/boosted_stumps.h"
#include "jml/boosting/training_data.h"
#include "jml/boosting/dense_features.h"
#include "jml/boosting/sparse_features.h"
#include "jml/boosting/probabilizer.h"
#include "jml/boosting/decoded_classifier.h"
#include "jml/boosting/boosting_tool_common.h"
#include "jml/boosting/weighted_training.h"
#include "jml/boosting/training_index.h"
#include "jml/boosting/boosted_stumps_generator.h"
#include "jml/utils/vector_utils.h"
#include "jml/utils/command_line.h"
#include <boost/progress.hpp>
#include <boost/timer.hpp>
#include "jml/stats/moments.h"
#include "datasets.h"
#include "jml/utils/info.h"

#include <iterator>
#include <iostream>
#include <set>

using namespace std;

using namespace ML;
using namespace Math;
using namespace Stats;

void print_weights(const boost::multi_array<float, 2> & weights,
                   const Training_Data & training_set, int num_print_weights)
{
    if (num_print_weights == 0) return;

    int nx = weights.shape()[0];
    int nl = weights.shape()[1];

    vector<pair<int, float> > sorted(nx);
    for (unsigned x = 0;  x < nx;  ++x) {
        sorted[x].first = x;
        for (unsigned l = 0;  l < nl;  ++l) {
            sorted[x].second += weights[x][l];
        }
        sorted[x].second *= nx;
    }

    sort_on_second_descending(sorted);

    cerr << "Hardest examples: " << endl;
    for (unsigned i = 0;  i < num_print_weights;  ++i) {
        if (i >= sorted.size()) break;
        int ex = sorted[i].first;
        cerr << "  " << i << ": example " << ex << " with weight "
             << sorted[i].second << endl;
        cerr << training_set.feature_space()->print(training_set[ex]);
        cerr << endl;
    }
}

string print_pred(const Stump & stump)
{
    const Feature_Space & fs = *stump.feature_space();

    const char * op = (fs.info(stump.feature).categorical() ? "=" : "<");

    string result
        = format("%10s %s %-6s",
                 fs.print(stump.feature, stump.arg).c_str(), op,
                 fs.print(stump.feature).c_str());

    return result;
}

Boosted_Stumps
train_boosting(const Training_Data & training_set,
               const Training_Data & validation_set,
               unsigned max_iter, unsigned min_iter, int verbosity,
               vector<Feature> features,
               const Feature & predicted,
               bool true_only,
               int num_print_weights, bool profile, bool fair,
               float feature_prop, int committee_size, int trace,
               int cost_function, int output_function, int update_alg,
               float ignore_highest, float sample_prop, int weight_function,
               const std::vector<Weight_Spec> & weight_spec)
{
    Boosted_Stumps_Generator generator;
    generator.init(training_set.feature_space(), predicted);
    generator.defaults();

    generator.max_iter = max_iter;
    generator.min_iter = min_iter;
    generator.verbosity = verbosity;
    generator.profile = profile;
    generator.fair = fair;
    generator.weak_learner.feature_prop = feature_prop;
    generator.weak_learner.committee_size = committee_size;
    generator.weak_learner.trace = trace;
    generator.cost_function = (Cost_Function)cost_function;
    generator.output_function = (Boosted_Stumps::Output)output_function;
    generator.weak_learner.update_alg = (Stump::Update)update_alg;

    distribution<float> training_ex_weights
        = apply_weight_spec(training_set, weight_spec);
    distribution<float> validate_ex_weights
        = apply_weight_spec(validation_set, weight_spec);

    return generator.generate_stumps(training_set, validation_set,
                                     training_ex_weights,
                                     validate_ex_weights, features, -1);
}

int main(int argc, char ** argv)
try
{
    ios::sync_with_stdio(false);

    int max_iter            = 1000;
    int min_iter            = 5;
    float validation_split  = 0.0;
    float testing_split     = 0.0;
    bool randomize_order    = false;
    int probabilize_mode    = 1;
    int probabilize_data    = 0;
    bool probabilize_weighted = false;
    bool data_is_sparse     = false;
    float equalize_beta     = 0.0;
    string weight_spec      = "";
    int draw_graphs         = false;
    //Stop_At stop_at        = BEST_RECOG;
    string scale_output;
    string scale_input;
    string output_file;
    string variable_name_file;
    string equalize_name = "LABEL";
    int verbosity           = 1;
    vector<string> ignore_features;
    vector<string> optional_features;
    int cross_validate      = 1;
    int repeat_trials       = 1;
    bool dump_testing       = false;
    bool true_only          = false;
    int num_print_weights   = 0;
    int min_feature_count   = 1;
    bool remove_aliased     = false;
    bool profile            = false;
    bool fair               = false;
    int committee_size      = 1;
    float feature_prop      = 1.0;
    int print_confusion     = false;
    int trace               = 0;
    string probabilize_link = "logit";
    int cost_function       = 0;
    int output_function     = 0;
    int update_alg          = 0;
    int weight_function     = 0;
    float ignore_highest    = 0.0;
    float sample_prop       = 1.0;
    bool is_regression      = false;  // Set to be a regression?
    bool is_regression_set  = false;  // Value of is_regression has been set?
    int num_buckets         = DEFAULT_NUM_BUCKETS;
    string group_feature_name = "";
    bool eval_by_group      = false;  // Evaluate a group at a time?
    bool eval_by_group_set  = false;  // Value of eval_by_group has been set?
    string predicted_name   = "LABEL";
    vector<string> type_overrides;

    vector<string> extra;
    {
        using namespace CmdLine;

        static const Option data_options[] = {
            { "validation-split", 'V', validation_split, validation_split,
              false, "split X% of training data for validation", "0-100" },
            { "testing-split", 'T', testing_split, testing_split,
              false, "split X% of training data for testing", "0-100" },
            { "randomize-order", 'R', NO_ARG, flag(randomize_order),
              false, "randomize the order of data before splitting" },
            { "variable-names", 0, variable_name_file, variable_name_file,
              false, "read variables names from FILE", "FILE" },
            { "ignore-var", 'z', string(), push_back(ignore_features),
              false,"ignore variable with name matching REGEX","REGEX|@FILE" },
            { "optional-feature", 'O', string(), push_back(optional_features),
              false, "feature with name REGEX is optional", "REGEX|@FILE" },
            { "sparse-data", 'S', NO_ARG, flag(data_is_sparse),
              false, "dataset is in sparse format" },
            { "min-feature-count", 'c', min_feature_count, min_feature_count,
              true, "don't consider features seen < NUM times", "NUM" },
            { "remove-aliased", 'A', NO_ARG, flag(remove_aliased),
              false, "remove aliased training rows from the training data" },
            { "regression", 'N', NO_ARG, flag(is_regression, is_regression_set),
              false, "force dataset to be a regression problem" },
            { "num-buckets", '1', num_buckets, num_buckets,
              true, "number of buckets to discretize into (0=off)", "INT" },
            { "group-feature", 'g', group_feature_name, group_feature_name,
              false, "use FEATURE to group examples in dataset", "FEATURE" },
            { "type-override", 'Y', string(), push_back(type_overrides),
              false, "override feature types for matching", "REGEX=TYPE,..." },
            Last_Option
        };

        static const Option training_options[] = {
            { "max-iterations", 'm', max_iter, max_iter,
              true, "run no more than NUM iterations", "NUM" },
            { "min-iterations", 'n', min_iter, min_iter,
              true, "output must be trained on at least N iterations", "NUM" },
            { "cross-validate", 'X', cross_validate, cross_validate,
              false, "run N-fold cross validation", "INT" },
            { "repeat-trials", 'r', repeat_trials, repeat_trials,
              false, "repeat experiment N times", "INT" },
            { "true-only", '9', NO_ARG, flag(true_only),
              false, "don't false or missing predicates to infer labels" },
            { "fair", 'f', NO_ARG, flag(fair),
              false, "treat mutiple equal features in a fair manner" },
            { "feature-proportion", '3', feature_prop, feature_prop,
              false, "lazy train on this proportion of features", "0.0-1.0" },
            { "committee-size", '4', committee_size, committee_size,
              true, "(fair=1 only): learn committees of N classifiers", "INT"},
            { "cost-function", '5', cost_function, cost_function,
              true, "cost function: 0 = boosting, 1 = logitboost", "0-1" },
            { "output-function", '6', output_function, output_function,
              true, "output fn: 0 = raw, 1 = prob, 2 = probnorm", "0-2" },
            { "update-alg", '7', update_alg, update_alg,
              true, "update alg: 0 = boosting, 1 = gentle", "0-1" },
            { "ignore-highest", '8', ignore_highest, ignore_highest,
              true, "ignore highest x proportion of weights", "0.0-1.0" },
            { "sample-proportion", 0, sample_prop, sample_prop,
              true, "use only X proportion of samples each iter", "0.0-1.0" },
            { "weight-function", 'a', weight_function, weight_function,
              true, "weight function: 0 = normal, 1 = conf", "0,1" },
            { "predict-feature", 'L', predicted_name, predicted_name,
              true, "train classifier to predict FEATURE", "FEATURE" },
            Last_Option
        };

        static const Option weight_options[] = {
            { "equalize-beta", 'E', equalize_beta, equalize_beta,
              true, "equalize labels (b in w=freq^(-b); 0.0=off)", "0.0-1.0" },
            { "equalize-feature", 'F', equalize_name, equalize_name,
              true, "equalize based upon feature FEATURE","FEATURE" },
            { "weight-spec", 'W', weight_spec, weight_spec,
              false, "use SPEC for weights", "VAR1(BETA1),..." },
            Last_Option
        };

        static const Option probabilize_options[] = {
            { "probabilize-mode", 'p', probabilize_mode, probabilize_mode,
              true, "prob mode: 0=matrix, 1=pervar, 2=oneonly, 3=off", "0-3" },
            { "probabilize-link", 'K', probabilize_link, probabilize_link,
              true, "prob link function", "logit|log" },
            { "probabilize-data", 'P', probabilize_data, probabilize_data,
              true, "data for prob: 0 = train, 1 = validate", "0|1" },
            { "probabilize-weighted", 'Q', NO_ARG, flag(probabilize_weighted),
              false, "train probabilizer using weights also" },
            Last_Option
        };

        static const Option output_options[] = {
            { "output-file", 'o', output_file, output_file,
              false, "write output network to FILE", "FILE" },
            { "quiet", 'q', NO_ARG, assign(verbosity, 0),
              false, "don't write any non-fatal output" },
            { "verbosity", 'v', optional(2), verbosity,
              false, "set verbosity to LEVEL (0-3)", "LEVEL" },
            { "profile", 'l', NO_ARG, flag(profile),
              false, "profile execution time" },
            { "draw-graphs", 'G', NO_ARG, increment(draw_graphs),
              false, "draw graphs for two-class predictor" },
            { "dump-testing", 'D', NO_ARG, flag(dump_testing),
              false, "dump output of classifier on testing sets" },
            { "print-weights", 0, num_print_weights, num_print_weights,
              false, "print NUM higest weighted training examples", "NUM" },
            { "print-confusion", 'C', NO_ARG, increment(print_confusion),
              false, "print confusion matrix" },
            { "trace", 't', trace, trace,
              false, "set trace level to LEVEL (0 off)", "LEVEL" },
            { "eval-by-group", 0, NO_ARG, flag(eval_by_group, eval_by_group_set),
              false, "evaluate by group rather than by example" },
            Last_Option
        };

        static const Option options[] = {
            group("Data options", data_options),
            group("Weight options", weight_options),
            group("Training options", training_options),
            group("Probabilizer options", probabilize_options),
            group("Output options",   output_options),
            Help_Options,
            Last_Option };

        Command_Line_Parser parser("boosting_training_tool", argc, argv,
                                   options);

        bool res = parser.parse();
        if (res == false) exit(1);

        extra.insert(extra.end(), parser.extra_begin(), parser.extra_end());
    }

    if (extra.empty()) {
        cerr << "error: need to specify (at least) training data" << endl;
        exit(1);
    }

    cerr << all_info() << endl;

    Datasets datasets;
    datasets.init(extra, verbosity, profile);

    std::shared_ptr<Mutable_Feature_Space> feature_space = datasets.feature_space;

    vector<Feature> features;
    map<string, Feature> feature_index;
    Feature predicted;

    do_features(*datasets.data[0], feature_space, predicted_name,
                ignore_features, optional_features,
                min_feature_count, verbosity, features,
                predicted, feature_index, type_overrides);

    Feature group_feature = MISSING_FEATURE;

    if (group_feature_name != "") {
        if (!feature_index.count(group_feature_name)) {
            cerr << "feature_index.size() = " << feature_index.size()
                 << endl;
            throw Exception("grouping feature " + group_feature_name
                            + " not found in data");
        }

        group_feature = feature_index[group_feature_name];

        if (!eval_by_group_set) {
            if (verbosity > 0)
                cerr << "note: overriding eval-by-group=1, use "
                     << "--no-eval-by-group to avoid" << endl;
            eval_by_group = true;
        }
    }

    float training_split = 100.0 - validation_split - testing_split;
    datasets.split(training_split, validation_split, testing_split,
                   randomize_order, group_feature);

    if (remove_aliased)
        remove_aliased_examples(*datasets.training, predicted, verbosity, profile);

    /* Write a null classifier, if there is no training data or we can't
       train for some reason. */
    if (datasets.training->example_count() == 0) {
        write_null_classifier(output_file, predicted, feature_space, verbosity);
        return 0;
    }

    vector<Weight_Spec> untrained_weight_spec
        = parse_weight_spec(*feature_space, equalize_name, equalize_beta,
                            weight_spec, group_feature);

    vector<Weight_Spec> trained_weight_spec
        = train_weight_spec(*datasets.training, untrained_weight_spec);

    if (verbosity > 4)
        print_weight_spec(trained_weight_spec, feature_space);

    /* Now for the training. */
        vector<distribution<float> > accum_acc(datasets.testing.size());

    for (unsigned i = 0;  i < repeat_trials;  ++i) {
        if (repeat_trials > 1) cerr << "trial " << (i + 1) << endl;

        Boosted_Stumps current
            = train_boosting(*datasets.training, *datasets.validation,
                             max_iter, min_iter,
                             verbosity, features, predicted,
                             true_only, num_print_weights,
                             profile, fair, feature_prop, committee_size, trace,
                             cost_function, output_function, update_alg,
                             ignore_highest, sample_prop, weight_function,
                             trained_weight_spec);

        if (current.stumps.size() == 0) {
            write_null_classifier(output_file, predicted, feature_space,
                                  verbosity);
            continue;
        }

        GLZ_Probabilizer prob;
        if (probabilize_data < 0 || probabilize_data > 1)
            throw Exception("probabilize-data must be 0 or 1 (currently "
                            + ostream_format(probabilize_data) + ")");

        std::shared_ptr<const Training_Data> prob_set
            = (probabilize_data == 0 ? datasets.training : datasets.validation);

        distribution<float> pr_weights(prob_set->example_count(), 1.0);

        if (probabilize_weighted)
            pr_weights
                = apply_weight_spec(*prob_set, trained_weight_spec);

        prob.train(*prob_set, current, pr_weights,
                   probabilize_mode, probabilize_link);

        if (repeat_trials == 1 || verbosity > 2) {
            boost::timer t;
            cerr << "Stats over training set: " << endl;
            calc_stats(current, prob, *datasets.training, draw_graphs, dump_testing,
                       print_confusion, eval_by_group, group_feature);

            cerr << "Stats over validation set: " << endl;
            calc_stats(current, prob, *datasets.validation, draw_graphs,
                       dump_testing, print_confusion, eval_by_group,
                       group_feature);

            if (profile)
                cerr << "   [accuracy: " << t.elapsed() << "s]" << endl;
        }

        Classifier output_wrapped(current);
        Decoded_Classifier output(output_wrapped, Decoder(prob));
        Classifier cl(output);

        if (output_file != "") {
            if (verbosity > 0)
                cerr << "writing to \'" << output_file << "\'... ";
            cl.save(output_file);
            if (verbosity > 0) cerr << "done." << endl;
        }

        /* Test all of the testing datasets. */
        for (unsigned j = 0;  j < datasets.testing.size();  ++j) {
            cerr << "Stats over testing set " << j << ":" << endl;

            calc_stats(current, prob, *datasets.testing[j], draw_graphs,
                       dump_testing, print_confusion, eval_by_group,
                       group_feature);

            if (repeat_trials > 1) {
                float acc = current.accuracy(*datasets.testing[j]);
                accum_acc[j].push_back(acc);
                cerr << "trial " << i + 1 << ": accuracy " << acc * 100.0
                     << "%, average over all trials = "
                     << accum_acc[j].total() * 100.0 / (i + 1)
                     << "%" << endl;
            }
        }

        if (i != repeat_trials - 1)
            datasets.reshuffle();
    }

    if (repeat_trials > 1) {
        for (unsigned j = 0;  j < accum_acc.size();  ++j) {
            float mean = Stats::mean(accum_acc[j].begin(), accum_acc[j].end(),
                                     0.0);
            float std_dev
                = Stats::std_dev(accum_acc[j].begin(), accum_acc[j].end(),
                                 mean);
            cerr << "testing set " << j << ": " << repeat_trials
                 << " trials accuracy: mean "
                 << mean * 100.0 << "% std " << std_dev * 100.0 << "%."
                 << endl;
        }
    }
}
catch (const std::exception & exc) {
    cerr << "error: " << exc.what() << endl;
    exit(1);
}


#include <math.h>
#include <Eigen/Core>
#include <algorithm>
#include <functional>

#include "crf_op.h"

namespace caffe2 {

namespace {

template <typename T>
void bilinear_interpolation(const float* input, float* output,
                            const int batch_size, const int num_channels,
                            const int input_height, const int input_width,
                            const int output_height, const int output_width) {
  int channels = num_channels * batch_size;

  const float rheight = (output_height > 1)
                            ? (float)(input_height - 1) / (output_height - 1)
                            : 0.f;
  const float rwidth =
      (output_width > 1) ? (float)(input_width - 1) / (output_width - 1) : 0.f;
  for (int h2 = 0; h2 < output_height; ++h2) {
    const float h1r = rheight * h2;
    const int h1 = h1r;
    const int h1p = (h1 < input_height - 1) ? 1 : 0;
    const float h1lambda = h1r - h1;
    const float h0lambda = (float)1. - h1lambda;
    for (int w2 = 0; w2 < output_width; ++w2) {
      const float w1r = rwidth * w2;
      const int w1 = w1r;
      const int w1p = (w1 < input_width - 1) ? 1 : 0;
      const float w1lambda = w1r - w1;
      const float w0lambda = (float)1. - w1lambda;
      const float* Xdata = &input[h1 * input_width + w1];
      float* Ydata = &output[h2 * output_width + w2];
      for (int c = 0; c < channels; ++c) {
        Ydata[0] = h0lambda * (w0lambda * Xdata[0] + w1lambda * Xdata[w1p]) +
                   h1lambda * (w0lambda * Xdata[h1p * input_width] +
                               w1lambda * Xdata[h1p * input_width + w1p]);
        Xdata += input_width * input_height;
        Ydata += output_width * output_height;
      }
    }
  }
}

template <typename T>
void image_process(const float* input, unsigned char* output,
                   const int batch_size, const int height, const int width) {
  // TODO(YH): add argument
  float mean[] = {102.9801, 115.9465, 122.7717};
  for (int b = 0; b < batch_size; b++) {
    for (int c = 0; c < 3; c++) {
      for (int h = 0; h < height; h++) {
        for (int w = 0; w < width; w++) {
          int idx_i = ((b * 3 + c) * height + h) * width + w;
          int idx_o = ((b * height + h) * width + w) * 3 + c;
          output[idx_o] = (unsigned char)(input[idx_i] + mean[c]);
        }
      }
    }
  }
}

template <typename T>
void unary_process(const float* input, float* output, const int batch_size,
                   const int num_classes, const int height, const int width) {
  const float min_prob = 0.0001;
  for (int b = 0; b < batch_size; b++) {
    for (int c = 0; c < num_classes; c++) {
      for (int h = 0; h < height; h++) {
        for (int w = 0; w < width; w++) {
          int idx_i = ((b * num_classes + c) * height + h) * width + w;
          int idx_o = ((b * height + h) * width + w) * num_classes + c;
          output[idx_o] = std::max(input[idx_i], min_prob);
          // output[idx_o] = -1. * std::max(input[idx_i], min_prob);
          // output[idx_o] = -1. * input[idx_i];
        }
      }
    }
  }
}

template <typename T>
void result_process(const float* input, float* output, const int batch_size,
                    const int num_classes, const int height, const int width) {
  const float min_prob = 0.0001;

  Tensor N(caffe2::CPU);
  N.Resize(batch_size, height, width);
  float* Nmdata = N.mutable_data<float>();

  for (int b = 0; b < batch_size; b++) {
    for (int h = 0; h < height; h++) {
      for (int w = 0; w < width; w++) {
        Nmdata[0] = 0;
        for (int c = 0; c < num_classes; c++) {
          int idx_i = ((b * height + h) * width + w) * num_classes + c;
          int idx_o = ((b * num_classes + c) * height + h) * width + w;
          output[idx_o] = std::max(input[idx_i], min_prob);
          Nmdata[0] += output[idx_o];
        }
        Nmdata += 1;
      }
    }
  }

  const float* Ndata = N.data<float>();

  for (int b = 0; b < batch_size; b++) {
    for (int c = 0; c < num_classes; c++) {
      for (int h = 0; h < height; h++) {
        for (int w = 0; w < width; w++) {
          int idx_o = ((b * num_classes + c) * height + h) * width + w;
          float norm = *(Ndata + (b * height + h) * width + w);
          // output[idx_o] = log(output[idx_o] / norm);
          output[idx_o] = output[idx_o] / norm;
        }
      }
    }
  }
}

}  // namespace

template <>
int DenseCRFOp<float, CPUContext>::npixels() {
  return W * H;
}

template <>
int DenseCRFOp<float, CPUContext>::nlabels() {
  return m_nlabels;
}

template <>
void DenseCRFOp<float, CPUContext>::add_pairwise_energy(
    float w1, float theta_alpha_1, float theta_alpha_2, float theta_betta_1,
    float theta_betta_2, float theta_betta_3, float w2, float theta_gamma_1,
    float theta_gamma_2, const unsigned char* im) {
  m_crf->addPairwiseGaussian(theta_gamma_1, theta_gamma_2,
                             new PottsCompatibility(w2));
  m_crf->addPairwiseBilateral(theta_alpha_1, theta_alpha_2, theta_betta_1,
                              theta_betta_2, theta_betta_3, im,
                              new PottsCompatibility(w1));
  // m_crf->addPairwiseGaussian(3, 3, new PottsCompatibility(3));
  // m_crf->addPairwiseBilateral(80, 80, 13, 13, 13, im,
  // new PottsCompatibility(10));
}

template <>
void DenseCRFOp<float, CPUContext>::set_unary_energy(
    const float* unary_costs_ptr) {
  m_crf->setUnaryEnergy(
      Eigen::Map<const Eigen::MatrixXf>(unary_costs_ptr, m_nlabels, W * H));
}

template <>
void DenseCRFOp<float, CPUContext>::map(int n_iters, int* labels) {
  VectorXs labels_vec = m_crf->map(n_iters);
  for (int i = 0; i < (W * H); ++i) labels[i] = labels_vec(i);
}

template <>
void DenseCRFOp<float, CPUContext>::inference(int n_iters, float* probs_out) {
  MatrixXf probs = m_crf->inference(n_iters);
  for (int i = 0; i < npixels(); ++i)
    for (int j = 0; j < nlabels(); ++j)
      probs_out[i * nlabels() + j] = probs(j, i);
}

template <>
void DenseCRFOp<float, CPUContext>::dense_crf(const unsigned char* image,
                                              const float* unary,
                                              float* probs_out) {
  // set unary potentials
  set_unary_energy(unary);

  // set pairwise potentials
  // add_pairwise_energy(10, 80 / scale_factor_, 80 / scale_factor_,
  // color_factor_, color_factor_, color_factor_, 3, 3 / scale_factor_, 3 /
  // scale_factor_, image);
  add_pairwise_energy(BI_W, BI_X_STD / scale_factor_, BI_Y_STD / scale_factor_,
                      BI_R_STD, BI_G_STD, BI_B_STD, POS_W,
                      POS_X_STD / scale_factor_, POS_Y_STD / scale_factor_,
                      image);

  // run inference
  inference(max_iter_, probs_out);
}

template <>
bool DenseCRFOp<float, CPUContext>::RunOnDevice() {
  const auto& U = Input(0);
  const auto& I = Input(1);

  CAFFE_ENFORCE_EQ(U.dim(), 4);
  CAFFE_ENFORCE_EQ(I.dim(), 4);
  CAFFE_ENFORCE_EQ(U.dim32(0), I.dim32(0));
  CAFFE_ENFORCE_EQ(I.dim32(1), 3);

  const int batch_size = U.dim32(0);
  const int num_classes = U.dim32(1);
  const int height = U.dim32(2);
  const int width = U.dim32(3);
  const int height_im = I.dim32(2);
  const int width_im = I.dim32(3);
  H = height;
  W = width;
  m_nlabels = num_classes;

  m_crf = new DenseCRF2D(W, H, m_nlabels);

  auto* M = Output(0);
  M->Resize(batch_size, num_classes, height, width);

  Tensor MT(caffe2::CPU);
  MT.Resize(batch_size, height, width, num_classes);

  Tensor IT(caffe2::CPU);
  IT.Resize(batch_size, height, width, 3);

  if (height != height_im || width != width_im) {
    Tensor IB(caffe2::CPU);
    IB.Resize(batch_size, 3, height, width);
    bilinear_interpolation<float>(I.data<float>(), IB.mutable_data<float>(),
                                  batch_size, 3, height_im, width_im, height,
                                  width);

    image_process<float>(IB.data<float>(), IT.mutable_data<unsigned char>(),
                         batch_size, height, width);
  } else {
    image_process<float>(I.data<float>(), IT.mutable_data<unsigned char>(),
                         batch_size, height, width);
  }

  Tensor UT(caffe2::CPU);
  UT.Resize(batch_size, height, width, num_classes);
  unary_process<float>(U.data<float>(), UT.mutable_data<float>(), batch_size,
                       num_classes, height, width);

  for (int b = 0; b < batch_size; b++) {
    const unsigned char* image =
        IT.data<unsigned char>() + b * height * width * 3;
    const float* unary = UT.data<float>() + b * height * width * num_classes;
    float* probs_out =
        MT.mutable_data<float>() + b * height * width * num_classes;

    // auto adjust scale_factor_
    scale_factor_ = 1.0 * SIZE_STD / std::max(height, width);

    dense_crf(image, unary, probs_out);
  }

  result_process<float>(MT.data<float>(), M->mutable_data<float>(), batch_size,
                        num_classes, height, width);

  delete m_crf;
  return true;
}

REGISTER_CPU_OPERATOR(DenseCRF, DenseCRFOp<float, CPUContext>);

namespace {}  // namespace

using namespace std::placeholders;

OPERATOR_SCHEMA(DenseCRF)
    .NumInputs(2)
    .NumOutputs(1)
    .SetDoc(R"DOC(
)DOC")
    .Arg("max_iter", "(int) default to 0")
    .Arg("debug_info", "(bool) default to false")
    .Input(0, "U", "input tensor of size (BxCxH1xW1)")
    .Input(1, "I", "input tensor of size (Bx3xH2xW2)")
    .Output(0, "M", "output tensor of size (BxCxH1xW1)");

namespace {

NO_GRADIENT(DenseCRF);

}  // namespace

}  // namespace caffe2


/*
    Copyright 2013 Adobe
    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*/
/**************************************************************************************************/

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/test/unit_test.hpp>

#include <stlab/concurrency/default_executor.hpp>
#include <stlab/concurrency/serial_queue.hpp>

#include <array>
#include <cmath>
#include <condition_variable>
#include <iostream>
#include <thread>

using namespace stlab;
using namespace std;

namespace mp = boost::multiprecision;

namespace
{
void rest() { std::this_thread::sleep_for(std::chrono::milliseconds(1)); }

template <typename T, typename N, typename O>
T power(T x, N n, O op)
{
  if (n == 0) return identity_element(op);

  while ((n & 1) == 0) {
    n >>= 1;
    x = op(x, x);
  }

  T result = x;
  n >>= 1;
  while (n != 0) {
    x = op(x, x);
    if ((n & 1) != 0) result = op(result, x);
    n >>= 1;
  }
  return result;
}

template <typename N>
struct multiply_2x2 {
  std::array<N, 4> operator()(const std::array<N, 4>& x, const std::array<N, 4>& y)
  {
    return { x[0] * y[0] + x[1] * y[2], x[0] * y[1] + x[1] * y[3],
    x[2] * y[0] + x[3] * y[2], x[2] * y[1] + x[3] * y[3] };
  }
};
template <typename N>
std::array<N, 4> identity_element(const multiply_2x2<N>&) { return { N(1), N(0), N(0), N(1) }; }

template <typename R, typename N>
R fibonacci(N n) {
  if (n == 0) return R(0);
  return power(std::array<R, 4>{ 1, 1, 1, 0 }, N(n - 1), multiply_2x2<R>())[0];
}

}

BOOST_AUTO_TEST_CASE(all_low_prio_tasks_are_executed) {
    BOOST_TEST_MESSAGE("All low priority tasks are executed");

    serial_queue_t queue(low_executor);
    mutex m;
    vector<int> results;
    atomic_bool done{false};

    for (auto i = 0; i < 10; ++i) {
        queue.executor()([_i = i, &m, &results] {
            unique_lock<mutex> block{m};
            results.push_back(_i);
        });
    }
    queue.executor()([&done] { done = true; });

    while (!done) {
        rest();
    }

    for (auto i = 0; i < 10; ++i) {
        BOOST_REQUIRE_EQUAL(i, results[i]);
    }
}

BOOST_AUTO_TEST_CASE(all_default_prio_tasks_get_executed) {
    BOOST_TEST_MESSAGE("All default priority tasks are executed");

    serial_queue_t queue(default_executor);
    mutex m;
    vector<int> results;
    atomic_bool done{false};

    for (auto i = 0; i < 10; ++i) {
        queue.executor()([_i = i, &m, &results] {
            unique_lock<mutex> block{m};
            results.push_back(_i);
        });
    }
    queue.executor()([&done] { done = true; });

    while (!done) {
        rest();
    }

    for (auto i = 0; i < 10; ++i) {
        BOOST_REQUIRE_EQUAL(i, results[i]);
    }
}

BOOST_AUTO_TEST_CASE(all_high_prio_tasks_get_executed) {
    BOOST_TEST_MESSAGE("All high priority tasks are executed");

    serial_queue_t queue(high_executor);
    mutex m;
    vector<int> results;
    atomic_bool done{false};

    for (auto i = 0; i < 10; ++i) {
        queue.executor()([_i = i, &m, &results] {
            unique_lock<mutex> block{m};
            results.push_back(_i);
        });
    }
    queue.executor()([&done] { done = true; });

    while (!done) {
        rest();
    }

    for (auto i = 0; i < 10; ++i) {
        BOOST_REQUIRE_EQUAL(i, results[i]);
    }
}

BOOST_AUTO_TEST_CASE(task_system_restarts_after_it_went_pending) {
    BOOST_TEST_MESSAGE("The task system restarts after it went to pending");

    atomic_bool done{false};
    mutex m;
    condition_variable cv;

    default_executor([&]{
        rest();
        {
            unique_lock<mutex> block{m};
            done = true;
        }
        cv.notify_one();
    });

    {
        unique_lock<mutex> block{m};
        while (!done) {
            cv.wait(block);
        }
    }

    default_executor([&] {
        rest();
        {
            unique_lock<mutex> block{m};
            done = false;
            cv.notify_one();
        }
    });

    {
        unique_lock<mutex> block{m};
        while (done) {
            cv.wait(block);
        }
    }

    BOOST_REQUIRE(!done);
}

namespace
{
    auto fiboN{ 1000 };
    const auto iterations = 100'000;
    const auto startCount = 100;
    atomic_int workToDo{(iterations - startCount) * 3};
    const auto expectedWork = startCount * 3 + workToDo;
    atomic_int highCount{0};
    atomic_int defaultCount{0};
    atomic_int lowCount{0};

    atomic_int taskRunning{0};
    atomic_int done{0};

    atomic_int correctLow{ 0 };
    atomic_int correctDefault{ 0 };
    atomic_int correctHigh{ 0 };

    template<stlab::detail::executor_priority P>
    struct check_task
    {
        atomic_int &_correctScheduleCount;
        atomic_int &_currentPrioCount;

        check_task(atomic_int &correctCount, atomic_int& prioCount)
            : _correctScheduleCount(correctCount)
            , _currentPrioCount(prioCount)
        {
            ++_currentPrioCount;
        }

        template <stlab::detail::executor_priority Q>
        auto schedule_increment() -> std::enable_if_t<Q == stlab::detail::executor_priority::low, int> {
            return static_cast<int>(highCount <= taskRunning && defaultCount <= taskRunning);
        }

        template <stlab::detail::executor_priority Q>
        auto schedule_increment() -> std::enable_if_t<Q == stlab::detail::executor_priority::medium, int> {
            return static_cast<int>(highCount <= taskRunning);
        }

        template <stlab::detail::executor_priority Q>
        auto schedule_increment() -> std::enable_if_t<Q == stlab::detail::executor_priority::high, int> {
            return 0;
        }

        void operator()() {
            --_currentPrioCount;

            ++taskRunning;

            _correctScheduleCount += schedule_increment<P>();

            fibonacci<mp::cpp_int>(fiboN);

            switch (workToDo % 3)
            {
            case 0:
              default_executor(check_task<stlab::detail::executor_priority::medium>{correctDefault, defaultCount});
              break;

            case 1:
              high_executor(check_task<stlab::detail::executor_priority::high>{correctHigh, highCount});
              break;

            case 2:
              low_executor(check_task<stlab::detail::executor_priority::low>{correctLow, lowCount});
              break;
            }
            --workToDo;

            ++done;
            --taskRunning;
        }
    };
}

BOOST_AUTO_TEST_CASE(all_tasks_will_be_executed_according_to_their_prio) {
    BOOST_TEST_MESSAGE("All tasks will be executed according to their prio");

    auto start = chrono::high_resolution_clock::now();

    for (auto i = 0; i < startCount; ++i) {
        low_executor(check_task<stlab::detail::executor_priority::low>{correctLow, lowCount});
        high_executor(check_task<stlab::detail::executor_priority::high>{correctHigh, highCount});
        default_executor(check_task<stlab::detail::executor_priority::medium>{correctDefault, defaultCount});
    }
    while (done < expectedWork) {
        rest();
    }

    auto stop = std::chrono::high_resolution_clock::now();
    std::cout << "\nPerformance measuring: " << std::chrono::duration<double>(stop - start).count() << "s\n";

    cout << "Correct default ordering: " << static_cast<double>(correctDefault.load())/iterations <<"%\n";
    cout << "Correct low ordering:     " << static_cast<double>(correctLow.load())/iterations << "%\n";
}

BOOST_AUTO_TEST_CASE(MeasureTiming) {
    std::vector<int> results;
    const auto iterations = 300'000;
    results.resize(iterations * 3);
    atomic_bool done{false};
    condition_variable ready;
    atomic_int counter{0};

    auto start = chrono::high_resolution_clock::now();

    for (auto i = 0; i < iterations; ++i) {
        low_executor([_i = i, &results,&counter] {
            results[_i] = 1;
            fibonacci<mp::cpp_int>(fiboN);
            ++counter;
        });
        default_executor([_i = i + iterations, &results,&counter] {
            results[_i] = 2;
            fibonacci<mp::cpp_int>(fiboN);
            ++counter;
        });
        high_executor([_i = i + iterations * 2, &results,&counter] {
            results[_i] = 3;
            fibonacci<mp::cpp_int>(fiboN);
            ++counter;
        });
    }

    mutex block;
    low_executor([&] {
        {
            unique_lock<mutex> lock{block};
            done = true;

            ready.notify_one();
        }
    });

    unique_lock<mutex> lock{block};
    while (!done) ready.wait(lock);

    auto stop = std::chrono::high_resolution_clock::now();
    std::cout << "\nPerformance measuring: " << std::chrono::duration<double>(stop - start).count() << "s\n";

    while (counter < 3*iterations) {
        rest();
    }
}