#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/mat_ZZ.h>
#include <gmp.h>

#include "Algebra.h"
#include "params.h"
#include "FFT.h"
#include "Random.h"

using namespace std;
using namespace NTL;

ZZX FastMod(const ZZX& f)
{
    return (trunc(f,N0) - (f>>N0));
}

ZZX Cyclo()
{
    ZZX phi0;
    phi0.SetLength(N0+1);
    phi0[0] = 1;
    phi0[N0] = 1;
    return phi0;
}

const ZZX phi = Cyclo();

//==============================================================================
//Computes the squared norm of a polynomial f
//==============================================================================
ZZ SquaredNorm(const ZZX& f, const unsigned int degree)
{
    unsigned int i;
    ZZ somme;
    for(i=0; i<=degree; i++)
    {
        somme += sqr(f[i]);
    }
    return somme;
}

//==============================================================================
//Verifies that for a parameter N, polynomials f, g are a valid semi-basis for building a NTRU lattice.
//If PGCD!=1, then (f,g) isn't a valid pair
//==============================================================================
void ValidPair(ZZ& PGCD, ZZ& Alpha, ZZ& Beta, ZZX& rho_f, ZZX& rho_g, const ZZX& f, const ZZX& g)
{
    ZZX Res_fx, Res_gx, iphi;
    ZZ Res_f, Res_g;

    XGCD(Res_f, rho_f, iphi, f, phi, 0);
    if(GCD(Res_f, q1)!=1)
    {
        PGCD = 0;
    }
    else
    {    XGCD(Res_g, rho_g, iphi, g, phi, 0);
         XGCD(PGCD, Alpha, Beta, Res_f, Res_g);
    }
}

//==============================================================================
//Computes f(1/x) mod (x^N + 1)
//If f = a0 + a1*x + ... + a_{N-1}*x^{N-1}, then
//Reverse(f) = a0 + a_{N-1}*x + ... + a1*x^{N-1}
//==============================================================================
ZZX Reverse(const ZZX& f)
{
    assert(deg(f)>=0);
    assert(deg(f)<N0);

    ZZX fb;
    unsigned int i;
    fb.SetLength(N0);
    fb[0] = f[0];
    fb.SetLength(N0);
    for(i=N0-deg(f); i<N0; i++)
    {
        fb[i] = -f[N0-i];
    }
    fb[0] = f[0];
    return fb;
}

//==============================================================================
//Computes the polynomial k such that (F,G) <-- (F,G) - k*(f,g) minimizes the size of (F,G)
//==============================================================================
ZZX ReductionCoefficient(const ZZX& f, const ZZX& g, const ZZX& F, const ZZX& G, unsigned int & mb)
{
    unsigned int i;
    ZZ a;
    ZZX fb, gb, num, den, iden, iphi, k;

    fb = Reverse(f);
    gb = Reverse(g);
    num = FastMod(fb*F + gb*G);
    den = FastMod(f*fb + g*gb);
    mb = MaxBits(num);

    XGCD(a, iden, iphi, den, phi);
    k = FastMod(num*iden);

    k.SetLength(N0);
    for(i=0; i<N0; i++)
    {
        k[i] /= a;
    }

    return k;
}

//==============================================================================
//Computes the polynomial k such that (F,G) <-- (F,G) - k*(f,g) minimizes the size of (F,G)
//==============================================================================
ZZX FastReductionCoefficient(const ZZX& f, const ZZX& g, const ZZX& F, const ZZX& G)
{
    unsigned int i;
    ZZX k;
    CC_t f_FFT[N0], g_FFT[N0], F_FFT[N0], G_FFT[N0], num_FFT[N0], den_FFT[N0], k_FFT[N0];

    assert(MaxBits(f)<900);
    ZZXToFFT(f_FFT, f);

    assert(MaxBits(g)<900);
    ZZXToFFT(g_FFT, g);

    assert(MaxBits(F)<900);
    ZZXToFFT(F_FFT, F);

    assert(MaxBits(G)<900);
    ZZXToFFT(G_FFT, G);

    for(i=0; i<N0; i++)
    {
        num_FFT[i] = f_FFT[N0-1-i]*F_FFT[i] + g_FFT[N0-1-i]*G_FFT[i];
        den_FFT[i] = f_FFT[N0-1-i]*f_FFT[i] + g_FFT[N0-1-i]*g_FFT[i];
        k_FFT[i] = num_FFT[i]/den_FFT[i];
    }

    FFTToZZX(k, k_FFT);
    return k;
}

//==============================================================================
//Returns the anticircular matrix associated to integral polynomial f and integer N
//==============================================================================
mat_ZZ AnticircularMatrix(const ZZX& f)
{
    unsigned int i,j;
    int df;
    mat_ZZ M;
    M.SetDims(N0, N0);
    df = deg(f);
    if(df==-1)
    {
        return M;
    }
    unsigned dfu;
    dfu = ((unsigned) df);
    if(dfu>=N0)
    {
        cout << "df = " << dfu << endl;
        cout << "f = " << f << endl;
    }
    assert(dfu<N0);

    for(i=0; i<N0; i++)
    {
        for(j=i; ((j<=dfu+i)&&(j<N0)); j++)
        {
            M[i][j] = f[j-i];
        }
        for(j=0; (j+N0)<=(dfu+i); j++)
        {
            M[i][j] = -f[j-i+N0];
        }
    }
    return M;
}

//==============================================================================
//Generates a basis from the double pair (f,g), (F,G) and N
//This basis has the form :
//    |f g|
//M = |F G|
//==============================================================================
mat_ZZ BasisFromPolynomials(const ZZX& f, const ZZX& g, const ZZX& F, const ZZX& G)
{
    unsigned int i,j;
    mat_ZZ A,M;
    M.SetDims(2*N0, 2*N0);
    A = AnticircularMatrix(f);
    for(i=0; i<N0; i++){
    for(j=0; j<N0; j++){
        M[i][j] = A[i][j];
    }}

    A = AnticircularMatrix(g);
    for(i=0; i<N0; i++){
    for(j=0; j<N0; j++){
        M[i][j+N0] = A[i][j];
    }}

    A = AnticircularMatrix(F);
    for(i=0; i<N0; i++){
    for(j=0; j<N0; j++){
        M[i+N0][j] = A[i][j];
    }}

    A = AnticircularMatrix(G);
    for(i=0; i<N0; i++){
    for(j=0; j<N0; j++){
        M[i+N0][j+N0] = A[i][j];
    }}

    return M;
}

//==============================================================================
//Computes the Inverse of f (mod phi) (mod q)
//==============================================================================
ZZ_pX Inverse(const ZZX& f)
{
    ZZ_p::init(q1);
    ZZX rho_f, iphi;
    ZZ Res_f;
    ZZ_p Res_f_1;
    XGCD(Res_f, rho_f, iphi, f, phi, 0);
    inv(Res_f_1, conv<ZZ_p>(Res_f));
    assert(Res_f_1*conv<ZZ_p>(Res_f) == 1);

    return ( Res_f_1 * conv<ZZ_pX>(rho_f) );
}

//==============================================================================
//Computes h = g/f (mod phi) (mod q)
//==============================================================================
ZZ_pX Quotient(const ZZX& f, const ZZX& g)
{
    ZZ_pX f_1, g0, h0, phi0;
    f_1 = Inverse(f);
    g0 = conv<ZZ_pX>(g);
    phi0 = conv<ZZ_pX>(phi);
    h0 = (f_1*g0)%phi0;
    return h0;
}

//==============================================================================
//Computes the Gram-Schmidt norm of the basis B generated from f,g
//==============================================================================
void GS_Norm(const ZZX fx, const ZZX gx, int& flag)
{
    unsigned int i;

    double acc, acc3, Fred[N0], Gred[N0];
    CC_t f[N0], g[N0], F[N0], G[N0];

    acc = 0;
    for(i=0; i<N0; i++)
    {
        acc += conv<double>(fx[i]*fx[i] + gx[i]*gx[i]);
    }
    acc = sqrt(acc);

    ZZXToFFT(f, fx);
    ZZXToFFT(g, gx);

    for(i=0; i<N0; i++)
    {
        F[i] = f[i]/(f[i]*f[N0-1-i]+g[i]*g[N0-1-i]);
        G[i] = g[i]/(f[i]*f[N0-1-i]+g[i]*g[N0-1-i]);
    }
    MyRealReverseFFT(Fred, F);
    MyRealReverseFFT(Gred, G);

    acc3 = 0;
    for(i=0; i<N0; i++)
    {
        acc3 += Fred[i]*Fred[i] + Gred[i]*Gred[i];
    }
    acc3 = q0*sqrt(acc3);
    if(acc3<acc)
    {
        flag = 1;
    }
}

//==============================================================================
//Generates a secret basis (f,g),(F,G) from the parameters N,q,Norme
//This bases generates a NTRU lattice
//==============================================================================
void GenerateBasis(ZZX& f, ZZX& g, ZZX& F, ZZX& G, const ZZ& Norme)
{
    int i;
    ZZX rho_f, rho_g, k, aux, fb, gb, num;
    ZZ PGCD, Alpha, Beta;

    int flag = 0;

    while( (PGCD!=1) || (flag==0) )
    {
        flag = 1;
        f = RandomPolyFixedSqNorm(Norme,N0-1);
        g = RandomPolyFixedSqNorm(Norme,N0-1);
        GS_Norm(f, g, flag);
        ValidPair(PGCD, Alpha, Beta, rho_f, rho_g, f, g);
    }
    F = -q1*Beta*rho_g;
    G = q1*Alpha*rho_f;

    f.SetLength(N0);
    g.SetLength(N0);

    unsigned int mb;
    k = ReductionCoefficient(f, g, F, G, mb);
    while(deg(k)>=0)
    {
        i++;

        F = FastMod(F - k*f);
        G = FastMod(G - k*g);

        fb = Reverse(f);
        gb = Reverse(g);

        num = FastMod(fb*F + gb*G);
        mb = MaxBits(num);

        k = ReductionCoefficient(f, g, F, G, mb);
        k.normalize();
    }

    aux = FastMod(f*G - g*F);

    assert(aux[0]==q1);
    assert(deg(aux)==0);
    aux.SetLength(N0);
}

RR_t DotProduct(const RR_t * x1, const RR_t * x2)
{
    unsigned int i;
    RR_t rep = 0;
    for(i=0; i<2*N0; i++)
    {
        rep += x1[i]*x2[i];
    }
    return rep;
}

void Rotate(RR_t * const dest, RR_t const * const src)
{
    unsigned int i;
    for(i=0; i<N0-1; i++)
    {
        dest[i+1] = src[i];
        dest[N0+i+1] = src[N0+i];
    }
    dest[0] = -src[N0-1];
    dest[N0] = -src[2*N0-1];
}

void ClassicMGS(RR_t Bstar[2*N0][2*N0], const RR_t B[2*N0][2*N0])
{
    RR_t SquareNorm[2*N0], aux[2*N0];
    unsigned int i,j,k;

    SquareNorm[0] = DotProduct(B[0], B[0]);
    for(j=0; j<2*N0; j++)
    {

        Bstar[0][j] = B[0][j];
    }

    for(i=1; i<2*N0; i++)
    {
        for(k=0; k<2*N0; k++)
        {
            Bstar[i][k] = B[i][k];
        }
        for(j=0; j<i; j++)
        {
            aux[j]= DotProduct(Bstar[i], Bstar[j]) / SquareNorm[j];
        }
        for(k=0; k<2*N0; k++)
        {
            for(j=0; j<i; j++)
            {
                Bstar[i][k] -= aux[j]*Bstar[j][k];
            }
        }
        SquareNorm[i] = DotProduct(Bstar[i], Bstar[i]);
    }
}

void FastMGS(RR_t Bst[2*N0][2*N0], const RR_t B[2*N0][2*N0])
{
    RR_t v[2*N0], v1[2*N0], C_k, D_k, C_ko, D_ko, aux;
    //RR_t C[2*N0], D[2*N0];
    unsigned int j, k;

    cout << endl;
    //Reducing first vector (obvious)
    for(j=0; j<2*N0; j++)
    {    Bst[0][j] = B[0][j];    }

    //Initialising the vector v = b_N - Proj(b_N, (b_1...b_k-2) )
    for(j=0; j<N0-1; j++)
    {    v[j] = Bst[0][j+1];
         v[j+N0] = Bst[0][j+1+N0];    }
    v[N0-1] = -Bst[0][0];
    v[2*N0-1] = -Bst[0][N0];

    for(j=0; j<2*N0; j++)
    {    v1[j] = v[j];    }

    //Initialising recurring variables
    C_k = DotProduct(Bst[0], v);
    D_k = DotProduct(v, v);

    //C[0] = C_k;
    //D[0] = D_k;
    //CD[0] = C[0]/D[0];

    //Reducing b_2 to b_N and updating v at the same time
    for(k=1; k<N0; k++)
    {
        //b~k <-- r(b~_{k-1}) - <b~_{k-1},b_N>/<v_{k-1},b_N> r(v)
        aux = C_k/D_k;
        Bst[k][0] = -Bst[k-1][N0-1] + aux*v[N0-1];
        Bst[k][N0] = -Bst[k-1][2*N0-1] + aux*v[2*N0-1];
        for(j=1; j<N0; j++)
        {
            Bst[k][j] = Bst[k-1][j-1] - aux*v[j-1];
            Bst[k][j+N0] = Bst[k-1][j+N0-1] - aux*v[j+N0-1];
        }

        //v <-- v - Proj(v, b~_{k-1} )
        for(j=0; j<2*N0; j++)
        {
            v[j] -= aux*Bst[k-1][j];
        }
        //sqnorm_v -= aux*aux*SquareNorm[k-1];

        C_ko = C_k;
        D_ko = D_k;

        C_k = DotProduct(Bst[k], v1);
        D_k = D_ko - C_ko*C_ko/D_ko;

        //C[k] = C_k;
        //D[k] = D_k;
        //CD[k] = C[k]/D[k];
        //printf ("C[%d]= %Lf		", k, C_k);
        //printf ("D[%d]= %Lf\n", k, D_k);
    }

    //Reducing second half!
    //cout << "aux = " << (1<<10)/D[N0-1] << endl;
    for(j=0; j<N0; j++)
    {    Bst[N0][N0+j] = Bst[N0-1][N0-1-j]*q0/D_k;
         Bst[N0][j] = -Bst[N0-1][2*N0-1-j]*q0/D_k;    }

    //Initialising the vector v = b_N - Proj(b_N, (b_1...b_k-2) )
    for(j=0; j<N0-1; j++)
    {    v[j] = Bst[N0][j+1];
         v[j+N0] = Bst[N0][j+1+N0];    }
    v[N0-1] = -Bst[N0][0];
    v[2*N0-1] = -Bst[N0][N0];

    for(j=0; j<2*N0; j++)
    {    v1[j] = v[j];    }

    //Initialising recursive variables
    C_k = DotProduct(Bst[N0], v1);
    D_k = DotProduct(Bst[N0], Bst[N0]);

    //C[N0] = C_k;
    //D[N0] = D_k;
    //CD[N0] = C[N0]/D[N0];

    //Reducing b_2 to b_N and updating v at the same time
    for(k=N0+1; k<2*N0; k++)
    {
        //b~k <-- r(b~_{k-1}) - <b~_{k-1},b_N>/<v_{k-1},b_N> r(v)
        aux = C_k/D_k;
        Bst[k][0] = -Bst[k-1][N0-1] + aux*v[N0-1];
        Bst[k][N0] = -Bst[k-1][2*N0-1] + aux*v[2*N0-1];
        for(j=1; j<N0; j++)
        {
            Bst[k][j] = Bst[k-1][j-1] - aux*v[j-1];
            Bst[k][j+N0] = Bst[k-1][j+N0-1] - aux*v[j+N0-1];
        }
        //SquareNorm[k] = SquareNorm[k-1] - aux*aux*sqnorm_v;

        //v <-- v - Proj(v, b~_{k-1} )
        for(j=0; j<2*N0; j++)
        {
            v[j] -= aux*Bst[k-1][j];
        }
        //sqnorm_v -= aux*aux*SquareNorm[k-1];

        C_ko = C_k;
        D_ko = D_k;

        C_k = DotProduct(Bst[k], v1);
        D_k = D_ko - C_ko*C_ko/D_ko;

        //C[k] = C_k;
        //D[k] = D_k;
        //CD[k] = C[k]/D[k];
    }
}


#include <iostream>
#include <cstdlib>
#include <getopt.h>
#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>
#include <boost/math/interpolators/cubic_b_spline.hpp>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

using namespace std;
using namespace boost::numeric::odeint;

/* Set parameters */
const double alpha = 0.0025, //priming rate
             delta = 0.019;    // death rate immune cells

const double hI = 571, // Michaelis constant
             hT = 571; // Michaelis constant

      double xi = 0.005, // default killing rate immune cells
             R = 2; // growth rate tumor

         int STOCHASTIC_KILLING = 0, // default flag stochastic killing (= off)
             STOCHASTIC_GROWTH = 0; // default flag stochastic growth (= off)

      double DIAGNOSIS_THRESHOLD = 65*1e8,
             DIAGNOSED_AT = numeric_limits<double>::infinity(),
             RAISE_KILLING = 1.0, // multiplication factor of killing rate by imm. therapy
             TREATMENT_DURATION = numeric_limits<double>::infinity();

      double DRIFT_XI = 0, // drift terms
             DRIFT_R = 0;

      double t_max = 1825.0; // simulation duration

      double seed = time(0)+getpid();

typedef boost::array< double , 4 > state_type;

/* Make interpolation function */
vector<double> f(500);
vector<double> g(500);
boost::math::cubic_b_spline<double> spline_f, spline_g;

// Function to generate time-dep. killing rate
double xi_t( double t ){
    return ( xi * ( 1 + DRIFT_XI * ( t / t_max ) ) * spline_f(t)); // / ((t + 365) / 365);
}

// Function for time-depedent growth rate
double R_t( double t ){
    return (R * (1 + DRIFT_R * (t / t_max ) ) * spline_g(t)); // / ((t + 365) / 365);
}

/* TUMOR MODEL */
void tumormodel( const state_type &x , state_type &dxdt , double t )
{
    const double T = x[0], N = x[3], S = x[2], I = x[1];

    const double t_cell_activation = alpha * ( T / (1e7 + T) ) * N;

    double killing, growth;

    // Set time of diagnosis
    if( T > DIAGNOSIS_THRESHOLD ){
        if( t < DIAGNOSED_AT )
            DIAGNOSED_AT = t;
    }

    if( STOCHASTIC_KILLING )
        killing = xi_t(t) * I * T / (1 + I/hI + T / hT); // time-dependent killing
    else
         killing = xi * I * T / (1 + I/hI + T / hT);      // time-independent killing

    if( STOCHASTIC_GROWTH )
        growth = R_t(t); // time-dependent growth
    else
        growth = R;

    // Start immunotherapy if: one day after diagnosis, a treatment effect is set (RAISE_KILLING),
    // and time is within treatment duration
    if( (t > (DIAGNOSED_AT+1)) && (RAISE_KILLING != 1.0) && (t <= (DIAGNOSED_AT + TREATMENT_DURATION)) )
        killing *= RAISE_KILLING;

    if ( T < 1 )
        dxdt[0] = -T;                                  // tumor cells
    else
        dxdt[0] = growth * pow( T , 4./5. ) - killing; // tumor cells

    dxdt[1] = S - delta*I;                             // TILs
    dxdt[2] = t_cell_activation;                       // specific T cells
    dxdt[3] = - t_cell_activation;                     // naive T cells
}

/* Print output */
void write_solution( const state_type &x , const double t)
{
    if( STOCHASTIC_GROWTH ){
         cout << t << "\t"<< R_t(t);
    } else {
         cout << t << "\t" << R;
    }
    if( STOCHASTIC_KILLING )
        cout << "\t" << xi_t(t);
    else
        cout << "\t" << xi;
    for( int i = 0 ; i < 4 ; i ++ )
        cout << "\t" << x[i];
  cout << endl;
}

#define XI_OPT 1000
#define R_OPT 1001
#define STOCHASTIC_KILLING_OPT 1002
#define RAISE_KILLING_OPT 1003
#define TREATMENT_DURATION_OPT 1004
#define STOCHASTIC_GROWTH_OPT 1005
#define DRIFT_XI_OPT 1006
#define DRIFT_R_OPT 1007
#define SEED_OPT 1008

static struct option command_line_options[] =
{
    {"xi", required_argument, NULL, XI_OPT},
    {"R", required_argument, NULL, R_OPT},
    {"stochastic-killing", required_argument, NULL, STOCHASTIC_KILLING_OPT},
    {"raise-killing", required_argument, NULL, RAISE_KILLING_OPT},
    {"treatment-duration", required_argument, NULL, TREATMENT_DURATION_OPT},
    {"stochastic-growth", required_argument, NULL, STOCHASTIC_GROWTH_OPT},
    {"drift-xi", required_argument, NULL, DRIFT_XI_OPT},
    {"drift-R", required_argument, NULL, DRIFT_R_OPT},
    {"seed", required_argument, NULL, SEED_OPT}
};

int main(int argc, char **argv)
{
    // Parse command line options
    int c;
    double KILLING_SD = 0;
    double GROWTH_SD = 0;
    while ((c = getopt_long(argc, argv, "", command_line_options, NULL)) != -1){
        switch(c){
            case XI_OPT:
                xi = atof(optarg);
                break;
            case R_OPT:
                R = atof(optarg);
                break;
            case STOCHASTIC_KILLING_OPT:
                STOCHASTIC_KILLING = 1;
                KILLING_SD = atof(optarg);
                break;
            case STOCHASTIC_GROWTH_OPT:
                STOCHASTIC_GROWTH = 1;
                GROWTH_SD = atof(optarg);
                break;
            case RAISE_KILLING_OPT:
                RAISE_KILLING = atof(optarg);
                break;
            case TREATMENT_DURATION_OPT:
                TREATMENT_DURATION = atof(optarg);
                break;
            case DRIFT_XI_OPT:
                DRIFT_XI = atof(optarg);
                break;
            case DRIFT_R_OPT:
                DRIFT_R = atof(optarg);
                break;
            case SEED_OPT:
                if(atof(optarg) != 0){
                    seed = atof(optarg);
                }
            break;
        }
    }

    /*Function to generate a random number*/
    boost::mt19937 rng(seed);
    boost::normal_distribution<> nd(0.0, 1.0);
    boost::variate_generator<boost::mt19937&,
                           boost::normal_distribution<> > rnorm(rng, nd);

    // Fill vector f with i, returns a vector (size=500) with scaled random numbers
    for (auto& i: f)
        i = 1 + rnorm()*KILLING_SD;
    f[0] = 1;

    // Create object cubic_b_spline (constructor);
    spline_f = boost::math::cubic_b_spline<double>(f.begin(), f.end(), 0.0, 30.5);

    for (auto& i: g)
        i = 1 + rnorm()*GROWTH_SD;
    g[0] = 1;

    spline_g = boost::math::cubic_b_spline<double>(g.begin(), g.end(), 0.0, 30.5);

    state_type x = {{ 1.0, 0., 0., 1e6 }};
    //integrate(tumormodel, x, 0.0, 2500.0, 1.0, write_solution );
    controlled_runge_kutta<runge_kutta_dopri5 < state_type >> stepper;

    integrate_const( stepper, tumormodel, x, 0.0, t_max, 1.0, write_solution );
}


// Based on the Boost histogram example
// https://www.boost.org/doc/libs/1_70_0/libs/histogram/doc/html/histogram/getting_started.html
#include <algorithm>
#include <boost/format.hpp>
#include <boost/histogram.hpp>
#include <functional>
#include <iostream>
#include <sstream>
#include <vector>

namespace {
#include "axes_compile_time.inc"
}

int main(int argc, char* argv[]) {
    using namespace boost::histogram;

    auto h = make_histogram(axis::regular<>(48, -12.0, 12.0, "x"));
    std::for_each(data.begin(), data.end(), std::ref(h));

    std::ostringstream oss;
    for (auto x : indexed(h, coverage::all)) {
        oss << boost::format("bin %2i [%5.1f, %5.1f): %i\n") %
            x.index() % x.bin().lower() % x.bin().upper() % *x;
    }

    std::cout << oss.str() << std::flush;
    return 0;
}

/*
Local Variables:
mode: c++
coding: utf-8-dos
tab-width: nil
c-file-style: "stroustrup"
End:
*/


/**
 * linalg.cpp
 *
 * 2021 Gabriel Moreira
 *
 * https://github.com/gabmoreira/maks
 *
 * This software and the related documents  are provided as  is,  with no express
 * or implied  warranties,  other  than those  that are  expressly stated  in the
 * License.
 *
 * Copyright © 2021 Gabriel Moreira. All rights reserved.
 */

#include <cmath>
#include <numeric>
#include <iostream>
#include <algorithm>

#include <Eigen/Eigenvalues>

#include "err.hpp"
#include "linalg.hpp"

using std::min;
using std::max;
using std::iota;
using std::fill;
using std::sort;
using std::vector;
using std::string;
using std::accumulate;

using Eigen::Ref;
using Eigen::VectorXd;
using Eigen::MatrixXd;
using Eigen::Matrix3d;
using Eigen::VectorXd;
using Eigen::VectorXi;

typedef Eigen::Triplet<double>      Triplet;
typedef Eigen::SparseMatrix<double> Sparse;
typedef Eigen::PardisoLDLT<Sparse>  PardisoLDLT;

/* Parameters for the Krylov-Schur eigensolver */
#define KRYLOV_MAXITER           300
#define KRYLOV_TOL               1e-15
#define KRYLOV_RES_TOL           2.22e-16
#define KRYLOV_SUBSPACE_MAX_SIZE 20

namespace alg {

/**
 * Converts two vectors i and j to matrix indices (in-place).
 *
 * @param ei (input) Eigen::VectorXi - row indices.
 * @param ej (input) Eigen::VectorXi - column indices.
 * @param num_nodes (output) int - number of nodes.
 */
void convertToIdx(Ref<VectorXi> ei, Ref<VectorXi> ej, int& num_nodes) {

    int num_edges = (int) ei.rows();

    // Obtain node list from edges
    vector<int> nodes;

    nodes.insert(nodes.end(), ei.data(), ei.data() + num_edges);
    nodes.insert(nodes.end(), ej.data(), ej.data() + num_edges);

    sort(nodes.begin(), nodes.end());
    auto last = unique(nodes.begin(), nodes.end());

    num_nodes = (int) (last - nodes.begin());

    // Build edge map
    vector<int> idx_map(num_nodes);

    for (int i = 0; i < num_nodes ; ++i)
        idx_map[nodes[i]] = i;

    // Store matrix indices
    for (int i = 0; i < num_edges; ++i) {
        ei(i) = idx_map[ei(i)];
        ej(i) = idx_map[ej(i)];
    };
};

/**
 * Builds sparse graph adjacency matrix.
 *
 * @param ei (input) Eigen::VectorXi - row indices.
 * @param ej (input) Eigen::VectorXi - column indices.
 * @param num_edges (input) int - number of edges.
 * @param sym (input) bool - whether to make the matrix symmetric.
 * @param dst (output) Eigen::SparseMatrix<double> - output adjacency matrix.
 */
void adjacency(const Ref<VectorXi> ei, const Ref<VectorXi> ej, int num_edges, bool sym, Sparse& dst) {

    int buffer_size = num_edges;

    if (sym)
        buffer_size *= 2;

    // Allocate buffer
    vector<Triplet> buffer(buffer_size);

    // Fill in the data
    for (int i = 0; i < num_edges; ++i)
        buffer[i] = Triplet( ei(i), ej(i), 1.0f );

    // Duplicate entries if symmetric flag is true
    if (sym)
        for (int i = 0; i < num_edges; ++i)
            buffer[num_edges + i] = Triplet( ej(i), ei(i),  1.0f );

    dst.setFromTriplets(buffer.begin(), buffer.end());
};

/**
 * Builds sparse block matrix.
 *
 * @param ei (input) Eigen::VectorXi - row indices.
 * @param ej (input) Eigen::VectorXi - column indices.
 * @param blocks (input) Eigen::MatrixXd - row block matrix containing contiguous blocks.
 * @param block_height (input) int - block height.
 * @param block_width (input) int - block width.
 * @param num_blocks (input) int - number of contiguous blocks.
 * @param sym (input) bool - whether to make the matrix symmetric.
 * @param dst (output) Eigen::SparseMatrix<double> - output adjacency matrix.
 */
void sparseBlocks(const Ref<VectorXi> ei,
                  const Ref<VectorXi> ej,
                  const Ref<MatrixXd> blocks,
                  int block_height,
                  int block_width,
                  int num_blocks,
                  bool sym,
                  Sparse& dst) {

    int buffer_size = num_blocks * block_height * block_width;

    if (sym)
        buffer_size *= 2;

    // Allocate buffer
    vector<Triplet> buffer(buffer_size);

    // Fill in the data
    int k = 0;
    for (int i = 0; i < num_blocks; ++i) {
        for (int jj = 0; jj < block_width; ++jj) {
            for (int ii = 0; ii < block_height; ++ii) {
                buffer[k] = Triplet( ei(i)*block_height + ii, ej(i)*block_width + jj, blocks(ii, i*block_width + jj) );
                k++;
            };
        };
    };

    // Duplicate entries if symmetric flag is true
    if (sym)
        for (int i = 0; i < num_blocks; ++i) {
            for (int jj = 0; jj < block_width; ++jj) {
                for (int ii = 0; ii < block_height; ++ii) {
                    buffer[k] = Triplet( ej(i)*block_width + jj, ei(i)*block_height + ii,  blocks(ii, i*block_width + jj) );
                    k++;
                };
            };
        };

    dst.setFromTriplets(buffer.begin(), buffer.end());
};

/**
 * Solves the orthogonal Procrustes problem in SO(3) (in-place).
 *
 * @param mat (input/output) Eigen::MatrixXd - 3x3 matrix
 */
void orthoProcrustesSO3(Ref<Matrix3d> mat) {
    Eigen::JacobiSVD<Matrix3d> svd(mat, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Matrix3d U = svd.matrixU();
    Matrix3d R = U * svd.matrixV().transpose();
    U.col(2) *= R.determinant();
    mat = U * svd.matrixV().transpose();
};

/**
 * Solves the orthogonal Procrustes problem in O(3) (in-place).
 *
 * @param mat (input/output) Eigen::MatrixXd - 3x3 matrix.
 */
void orthoProcrustesO3(Ref<Matrix3d> mat) {
    Eigen::JacobiSVD<Matrix3d> svd(mat, Eigen::ComputeFullU | Eigen::ComputeFullV);
    mat = svd.matrixU() * svd.matrixV().transpose();
};

/**
 * Finds the projection of a vector on a subpace spanned by the columns of a matrix
 *
 * @param subspace_basis (input) Eigen::MatrixXd - Dense matrix whose columns span a subspace where we want to project the vector v.
 * @param idx (input) unsigned int - Sub-selects of the first idx columns of A only.
 * @param v (input) Eigen::VectorXd - Vector to project.
 * @return Eigen::VectorXd - The component of the vector in the subspace spanned by the first
 *         idx columns of the input matrix A
 */
VectorXd projectionOnto(const Ref<MatrixXd> subspace_basis, unsigned int idx, const Ref<VectorXd> v) {
    VectorXd w = subspace_basis.leftCols(idx).transpose() * v;
    VectorXd proj = subspace_basis.leftCols(idx) * w;
    return proj;
};

/**
 * Projects a vector onto a subpace spanned by the columns of a matrix (in-place)
 *
 * @param subspace_basis (input) Eigen::MatrixXd - Dense matrix whose columns span a subspace where we want to project the vector v.
 * @param idx (input) unsigned int - Sub-selects of the first idx columns of A only.
 * @param v (input/output) Eigen::VectorXd - Vector to project.
 */
void projectOnto(const Ref<MatrixXd> subspace_basis, unsigned int idx, Ref<VectorXd> v) {
    VectorXd w = subspace_basis.leftCols(idx).transpose() * v;
    v -= subspace_basis.leftCols(idx) * w;
};

/**
 * Symmetric sparse solver via LDLT decomposition.
 *
 * Solves sparse system Ax = b via LDLt decomposition.
 *
 * @param solver (input) Eigen::PardisoLDLT<Sparse> - LDLt Intel PARDISO solver
 * @param mat (input) Eigen::MatrixXd - matrix to factorize.
 * @param rhs (input) Eigen::VectorXd - right hand side.
 * @param x (output) Eigen::VectorXd - solution of the system.
 */
void sparseSolverLDL(PardisoLDLT* solver, const Sparse& mat, const Ref<MatrixXd> rhs, Ref<MatrixXd> x) {
    solver->factorize(mat);
    x = solver->solve(rhs);
};

/**
 * Robust reorthogonalization function. For an input matrix V and an input
 * vector r, tries to iteratively orthogonalize r against the j first columns
 * of V. This prevents numerical errors and ensures orthogonality. If, by any
 * chance, and after multiple iterations of subtracting projections, the vector
 * has a norm smaller than 1/sqrt(2) of its starting norm, it cannot be
 * reorthogonalized. This method then attempts to find another orthogonal vector
 * through random restarts. The flag stop is activate if everything fails.
 * (See G. W. Stewart 2001)
 *
 * @param V Dense matrix of doubles whose columns contain orthogonal basis vectors
 * @param r Vector to orthogonalize
 * @param residual_norm Residual norm or r after it has been orthogonalized against the first j columns of V.
 * @param idx Index specifying the number of columns of V used in the orthogonalization.
 * @param stop Stop the algorithm flag. Indicates that it is impossible to find, to machine precision, a vector r orthogonal to the first j cols of V.
 */
void reorthogonalize(const Ref<MatrixXd> V, Ref<VectorXd> r, double& residual_norm, unsigned int idx, int& stop) {
    stop = 0;
    double normr0 = r.norm();

    projectOnto(V, idx, r);

    residual_norm = r.norm();

    // Iteratively tries to reorthogonalize r against the columns of V
    unsigned int num_reorths = 1;
    while((residual_norm <= (1.0 / sqrt(2)) * normr0) && num_reorths < 5) {
        projectOnto(V, idx, r);
        normr0 = residual_norm;
        residual_norm = r.norm();
        num_reorths++;
    };

    /* Cannot reorthogonalize: Invariant subspace found. Restart with
     * a new random vector and try another 3 times */
    if (residual_norm <= (1.0 / sqrt(2)) * normr0) {
        residual_norm = 0;
        stop = 1;
        // Try another 3 times with random restarts
        for (int j = 0; j < 3; ++j) {
            r = VectorXd::Random(r.rows(),1);
            projectOnto(V, idx, r);
            r.normalize();

            // Reorthogonalize if necessary
            for (unsigned int k = 0; k < 5; ++k) {
                VectorXd Mr = r;
                VectorXd proj = projectionOnto(V, idx, Mr);
                double rMr = sqrt(abs(r.transpose() * Mr));

                if (abs(rMr - 1.0f) <= 1e-10) {
                    stop = 0;
                    break;
                };

                // Reorthogonalize
                r -= proj;
                r.normalize();
            };
            if (!stop)
                break;
        };
    } else {
        r /= residual_norm;
    };
};

/**
 * Symmetric Krylov-Schur LDLt eigensolver.
 *
 * For a real sparse symmetric matrix A, this function computes k real eigenvalues
 * near a real target sigma using the Krylov-Schur method (G. W. Stewart 2001).
 *
 * @param solver (input) Eigen::PardisoLDLT<Sparse> - LDLt Intel PARDISO solver.
 * @param mat (input) Eigen::SparseMatrix<double> - sparse matrix to compute eigenvalues and eigenvectors.
 * @param eigenvalues (output) Eigen::VectorXd - k x 1 matrix to store the computed eigenvalues.
 * @param eigenvectors (output) Eigen::MatrixXd - n x k matrix to store normalized eigenvectors.
 * @param k (input) int - number of eigenvalues and eigenvectors to be computed.
 * @param sigma (input) double - spectral shift / eigenvalue target.
*/
void symKrylovSchurLDL(PardisoLDLT*  solver,
                       const Sparse& mat,
                       Ref<VectorXd> eigenvalues,
                       Ref<MatrixXd> eigenvectors,
                       unsigned int  k,
                       double        sigma) {

    long n = mat.rows();

    // Sparse identity matrix
    Sparse speye(n,n);
    speye.setIdentity();

    Sparse shiftInvert = mat;
    shiftInvert -= sigma * speye;

    // Get LDLt factorization of A - sigma I
    solver->factorize(shiftInvert);

    MKS_ASSERT(solver->info()==Eigen::Success, mksGetStatusString(mksEigenPardisoLDLErr));

    // Matrix to hold the orthogonal Krylov basis
    MatrixXd krylov_basis = MatrixXd::Zero(n, KRYLOV_SUBSPACE_MAX_SIZE);

    // Size of the V matrix (not actual size but size in use)
    unsigned int size_krylov_basis = 0;

    // Stop the algorithm flag (reorthogonalization gone bad)
    int stop = 0;

    // Number of selected eigenvalues
    unsigned int k0 = k;

    // Store projection data
    vector<double> Alpha;
    Alpha.reserve(KRYLOV_SUBSPACE_MAX_SIZE);

    // Store projection data
    vector<double> Beta;
    Beta.reserve(KRYLOV_SUBSPACE_MAX_SIZE);

    MatrixXd c;

    double residual_norm = 0;
    bool restarted = false;
    unsigned int nconv = 0;
    vector<int> converged(k0);

    MatrixXd ritz_vectors;
    MatrixXd ritz_values;

    // Used to compute argsorts
    vector<int> idx_vec(KRYLOV_SUBSPACE_MAX_SIZE);

    // H matrix
    MatrixXd H(KRYLOV_SUBSPACE_MAX_SIZE, KRYLOV_SUBSPACE_MAX_SIZE);

    // Starting vector for the Krylov iterations
    VectorXd v0 = VectorXd::Random(n,1);
    v0.normalize();

    // One step of the power iteration
    VectorXd v = solver->solve(v0);
    v.normalize();

    // Main loop of the Krylov-Schur method
    for (unsigned int i = 0; i < KRYLOV_MAXITER; ++i) {
        // Loop to build the invariant subspace in V, H
        for (unsigned int j = size_krylov_basis; j < KRYLOV_SUBSPACE_MAX_SIZE; ++j) {

            // Store the Krylov vector v from the previous iteration in the Krylov matrix V
            krylov_basis.col(j) = v;
            // Compute Krylov power using Cholesky solver: r = (A-sigma I)^-1 * v
            VectorXd r = solver->solve(v);
            // Component of the new vector r in the direction of the previous vector v
            double alpha = v.transpose() * r;

            // Orthogonalization
            if (j == 0) {
                // 1st iteration: just subtract the projection on the previous vector
                r.noalias() -= alpha * v;
            } else if (restarted) {
                VectorXd w = krylov_basis.leftCols(j+1).transpose() * r;
                r.noalias() -= krylov_basis.leftCols(j+1) * w;
                restarted = false;
            } else {
                // Subtract projection on the previous vector and residuals before that
                r.noalias() -= alpha * v;
                r.noalias() -= residual_norm * krylov_basis.col(j-1);
            };

            /* Robust reorthogonalization of r against the columns of V.
             * This helps prevent numerical errors */
            reorthogonalize(krylov_basis, r, residual_norm, j+1, stop);

            /* Check if it was possible to reorthogonalize. If not, we cannot find the
             * invariant subspace and must exit the program */
            MKS_ASSERT(!stop, mksGetStatusString(mksKrylovReorthogonalizationErr));

            // Save projection of r on the the previous vector v (alpha)
            Alpha.push_back(alpha);
            // Save projection of r on the previous previous vector (beta)
            Beta.push_back(residual_norm);
            // Set the current vector v as the orthogonalized vector r
            v = r;
        };

        // Build matrix H
        Eigen::Map<VectorXd> Alpha_vec(Alpha.data(), Alpha.size());
        Eigen::Map<VectorXd> Beta_vec(Beta.data(), Beta.size()-1);

        MatrixXd H1 = MatrixXd::Zero(Alpha.size(), Alpha.size());
        H1.diagonal(-1).array() += Beta_vec.array();
        H1.diagonal(0).array()  += Alpha_vec.array();
        H1.diagonal(1).array()  += Beta_vec.array();

        // H matrix
        H = MatrixXd::Zero(KRYLOV_SUBSPACE_MAX_SIZE, KRYLOV_SUBSPACE_MAX_SIZE);
        H.bottomRightCorner(H1.rows(), H1.cols()) += H1;

        if (ritz_values.rows() > 0)
            H.topLeftCorner(ritz_values.rows(),ritz_values.rows()) += ritz_values.asDiagonal();

        if (ritz_values.rows() > 0) {
            for (unsigned int l = 0; l < k; ++l) {
                H(l,k) = c(0,l);
                H(k,l) = c(0,l);
            };
        };

        Alpha.resize(0);
        Beta.resize(0);

        // Compute Ritz pairs (eigenpairs of the H matrix)
        Eigen::SelfAdjointEigenSolver<MatrixXd> eig(H);
        ritz_vectors = eig.eigenvectors();
        ritz_values  = eig.eigenvalues();

        // Compute residuals to find out what has converged
        vector<double> res(ritz_vectors.cols());
        for (unsigned int l = 0; l < ritz_vectors.cols(); ++l)
            res[l] = abs(residual_norm * ritz_vectors(ritz_vectors.rows()-1, l));

        // Argsort eigenvalues descending order
        iota(idx_vec.begin(), idx_vec.end(), 0);
        sort(idx_vec.begin(), idx_vec.end(), [&](size_t a, size_t b) { return abs(ritz_values(a)) > abs(ritz_values(b)); });

        // Check which ones have converged
        for (unsigned int l = 0; l < k0; ++l)
            converged[l] = (int) ( res[idx_vec[l]] < KRYLOV_TOL * max(KRYLOV_RES_TOL, abs(ritz_values(idx_vec[l]))) );

        // Number of eigenvalues which have converged
        nconv = accumulate(converged.begin(), converged.end(), 0);

        // More than k0 eigenvalues converged: We're good to go
        if (nconv >= k0) {
            break;
        } else {
            // Adjust k to prevent stagnation
            k = k0 + min( (float) nconv, (float) floor(((float) (KRYLOV_SUBSPACE_MAX_SIZE-k0)) / 2.0f) );
            if (k == 1)
                k = floor(KRYLOV_SUBSPACE_MAX_SIZE / 2.0f);
        };

        // Use previous eigenvalue argsort to sort eigenvalues and eigenvectors
        MatrixXd ritz_vectors_ = MatrixXd::Zero(KRYLOV_SUBSPACE_MAX_SIZE, k);
        VectorXd ritz_values_  = VectorXd::Zero(KRYLOV_SUBSPACE_MAX_SIZE, 1);

        for (unsigned int i = 0; i < k; ++i) {
            ritz_vectors_.col(i) += ritz_vectors.col(idx_vec[i]);
            ritz_values_(i) += ritz_values(idx_vec[i]);
        };

        ritz_vectors = ritz_vectors_;
        ritz_values = ritz_values_;

        // Store variables for the next iteration
        krylov_basis.leftCols(k) = krylov_basis * ritz_vectors;
        c = residual_norm * ritz_vectors.bottomRows<1>();

        restarted = true;
        size_krylov_basis = k;
    };

    MatrixXd ritz_vectors_ = MatrixXd::Zero(ritz_vectors.rows(), k0);
    eigenvalues = VectorXd::Zero(k0);

    for (unsigned int i = 0; i < k0; ++i) {
        eigenvalues(i) += ritz_values(idx_vec[i]);
        ritz_vectors_.col(i) += ritz_vectors.col(idx_vec[i]);
    };

    c = residual_norm * ritz_vectors_.bottomRows<1>();

    eigenvectors = krylov_basis * ritz_vectors_;
    eigenvectors = eigenvectors.array().rowwise() * eigenvalues.transpose().array();
    eigenvectors.noalias() += v * c;

    // Eigenvector normalization
    VectorXd norms = eigenvectors.colwise().norm();
    eigenvectors.array().rowwise() /= norms.array().transpose();

    // Eigenvalue reverse the shift and invert
    eigenvalues.array() = 1.0f / eigenvalues.array() + sigma;
};

};


/****************************************************************************
**
** This file is part of the LibreCAD project, a 2D CAD program
**
** Copyright (C) 2010 R. van Twisk (librecad@rvt.dds.nl)
** Copyright (C) 2001-2003 RibbonSoft. All rights reserved.
**
**
** This file may be distributed and/or modified under the terms of the
** GNU General Public License version 2 as published by the Free Software
** Foundation and appearing in the file gpl-2.0.txt included in the
** packaging of this file.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program; if not, write to the Free Software
** Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**
** This copyright notice MUST APPEAR in all copies of the script!
**
**********************************************************************/
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/math/special_functions/ellint_2.hpp>

#include <cmath>
//#include <muParser.h>
#include <QString>
#include <QDebug>

#include "rs_settings.h"
#include "rs_units.h"
#include "rs_math.h"
#include "rs_vector.h"
#include "rs_debug.h"

namespace {
constexpr double m_piX2 = M_PI*2; //2*PI
}

/**
 * Rounds the given double to the closest int.
 */
int RS_Math::round(double v) {
    return (int) lrint(v);
}

/**
 * Save pow function
 */
double RS_Math::pow(double x, double y) {
    errno = 0;
    double ret = ::pow(x, y);
    if (errno==EDOM) {
        RS_DEBUG->print(RS_Debug::D_ERROR,
                        "RS_Math::pow: EDOM in pow");
        ret = 0.0;
    }
    else if (errno==ERANGE) {
        RS_DEBUG->print(RS_Debug::D_WARNING,
                        "RS_Math::pow: ERANGE in pow");
        ret = 0.0;
    }
    return ret;
}

/* pow of vector components */
RS_Vector RS_Math::pow(RS_Vector vp, double y) {
    return RS_Vector(pow(vp.x,y),pow(vp.y,y));
}

/**
 * Save equal function for real types
 */
bool RS_Math::equal(const double d1, const double d2)
{
    return fabs(d1 - d2) < RS_TOLERANCE;
}

/**
 * Converts radians to degrees.
 */
double RS_Math::rad2deg(double a) {
	return 180./M_PI*a;
}

/**
 * Converts degrees to radians.
 */
double RS_Math::deg2rad(double a) {
	return M_PI/180.0*a;
}

/**
 * Converts radians to gradians.
 */
double RS_Math::rad2gra(double a) {
	return 200./M_PI*a;
}

double RS_Math::gra2rad(double a) {
	return M_PI/200.*a;
}

/**
 * Finds greatest common divider using Euclid's algorithm.
 */
unsigned RS_Math::findGCD(unsigned a, unsigned b) {

	while (b) {
		unsigned rem = a % b;
        a = b;
        b = rem;
    }

    return a;
}

/**
 * Tests if angle a is between a1 and a2. a, a1 and a2 must be in the
 * range between 0 and 2*PI.
 * All angles in rad.
 *
 * @param reversed true for clockwise testing. false for ccw testing.
 * @return true if the angle a is between a1 and a2.
 */
bool RS_Math::isAngleBetween(double a,
                             double a1, double a2,
                             bool reversed) {

	if (reversed) std::swap(a1,a2);
	if(getAngleDifferenceU(a2, a1 ) < RS_TOLERANCE_ANGLE) return true;
	const double tol=0.5*RS_TOLERANCE_ANGLE;
	const double diff0=correctAngle(a2 -a1) + tol;

	return diff0 >= correctAngle(a - a1) || diff0 >= correctAngle(a2 - a);
}

/**
 * Corrects the given angle to the range of 0 to +PI*2.0.
 */
double RS_Math::correctAngle(double a) {
    return fmod(M_PI + remainder(a - M_PI, m_piX2), m_piX2);
}

/**
 * Corrects the given angle to the range of -PI to +PI.
 */
double RS_Math::correctAngle2(double a) {
    return remainder(a, m_piX2);
}

/**
 * Returns the given angle as an Unsigned Angle in the range of 0 to +PI.
 */
double RS_Math::correctAngleU(double a) {
    return fabs(remainder(a, m_piX2));
}

/**
 * @return The angle that needs to be added to a1 to reach a2.
 *         Always positive and less than 2*pi.
 */
double RS_Math::getAngleDifference(double a1, double a2, bool reversed) {
	if(reversed) std::swap(a1, a2);
	return correctAngle(a2 - a1);
}

double RS_Math::getAngleDifferenceU(double a1, double a2)
{
	return correctAngleU(a1 - a2);
}

/**
* Makes a text constructed with the given angle readable. Used
* for dimension texts and for mirroring texts.
*
* @param readable true: make angle readable, false: unreadable
* @param corrected Will point to true if the given angle was
*   corrected, false otherwise.
*
 * @return The given angle or the given angle+PI, depending which on
 * is readable from the bottom or right.
 */
double RS_Math::makeAngleReadable(double angle, bool readable,
                                  bool* corrected) {

    double ret=correctAngle(angle);

    bool cor = isAngleReadable(ret) ^ readable;

    // quadrant 1 & 4
    if (cor) {
        //        ret = angle;
        //    }
        // quadrant 2 & 3
        //    else {
        ret = correctAngle(angle+M_PI);
    }

    if (corrected) {
        *corrected = cor;
    }

    return ret;
}

/**
 * @return true: if the given angle is in a range that is readable
 * for texts created with that angle.
 */
bool RS_Math::isAngleReadable(double angle) {
	const double tolerance=0.001;
    if (angle>M_PI_2)
        return fabs(remainder(angle, m_piX2)) < (M_PI_2 - tolerance);
    else
        return fabs(remainder(angle, m_piX2)) < (M_PI_2 + tolerance);
}

/**
 * @param tol Tolerance in rad.
 * @retval true The two angles point in the same direction.
 */
bool RS_Math::isSameDirection(double dir1, double dir2, double tol) {
	return getAngleDifferenceU(dir1, dir2) < tol;
}

/**
 * Evaluates a mathematical expression and returns the result.
 * If an error occurred, the given default value 'def' will be returned.
 */
//double RS_Math::eval(const QString& expr, double def) {
//
//    bool ok;
//    double res = RS_Math::eval(expr, &ok);
//
//    if (!ok) {
//        //std::cerr << "RS_Math::evaluate: Parse error at col "
//        //<< ret << ": " << fp.ErrorMsg() << "\n";
//        return def;
//    }
//
//    return res;
//}

/**
 * generic replaceAll will allow substitution of one string for another
 * as many times as it exists within a given string.
 */
void RS_Math::replaceAll(QString& str, const std::string& from, const std::string& to) {

    QString qfrom = QString::fromStdString(from);
    QString qto = QString::fromStdString(to);
    if(qfrom.isEmpty())
        return;
    int start_pos = 0;
    while((start_pos = str.indexOf(qfrom, start_pos)) != -1) {
        str.replace(start_pos, qfrom.length(), qto);
        start_pos += qto.length(); // In case 'to' contains 'from', like replacing 'x' with 'yx'
    }
}

/**
 * Translate imperial shortform to inch equivalent math statements
 * this only holds true for simple +,- operators *,/ require manual braces..
 * which is probably a good thing.
 */
void RS_Math::imperialTranslate(QString& str) {

    if (str.isEmpty())
       return;
    // put brackets around everything first
    str = "(" + str + ")";
    replaceAll(str,"+",")+(");
    replaceAll(str,"-",")-(");
    // convert foot shortform
    replaceAll(str,"\'","*12+");
    // convert inch shortform
    replaceAll(str,"\"","+");
    // fix for inch with no fraction component
    replaceAll(str,"+)",")");  // -- cleanup
}

/**
 * Evaluates a mathematical expression and returns the result.
 * If an error occurred, ok will be set to false (if ok isn't NULL).
 */
//double RS_Math::eval(const QString& expr, bool* ok) {
//    bool okTmp(false);
//	if(!ok) ok=&okTmp;
//    if (expr.isEmpty()) {
//        *ok = false;
//        return 0.0;
//    }
//    double ret(0.);
//    // create a local copy of expr
//    QString expr_copy = expr;
//    // main drawing unit:
//    int insunits = RS_Units::stringToUnit(RS_SETTINGS->readEntry("/Unit", "None"));
//    // only apply imperial shorthand conversion if current units are 'inch'
//    if (insunits==RS2::Inch) {
//        // translate imperial shorthand before you eval
//        imperialTranslate(expr_copy);
//    }
//    try{
//        mu::Parser p;
//        //p.DefineConst("pi",M_PI);
//        //p.SetExpr(expr_copy.toStdString());
//        ret=p.Eval();
//        *ok=true;
//    }
//    catch (mu::Parser::exception_type &e)
//    {
//        //std::cout << e.GetMsg() << std::endl;
//        *ok=false;
//    }
//    return ret;
//}

/**
 * Converts a double into a string which is as short as possible
 *
 * @param value The double value
 * @param prec Precision e.g. a precision of 1 would mean that a
 *     value of 2.12030 will be converted to "2.1". 2.000 is always just "2").
 */
QString RS_Math::doubleToString(double value, double prec) {
    if (prec< RS_TOLERANCE ) {
		RS_DEBUG->print(RS_Debug::D_ERROR,
						"RS_Math::doubleToString: invalid precision");
		return QString().setNum(value, prec);
    }

	double const num = RS_Math::round(value / prec)*prec;

	QString exaStr = RS_Math::doubleToString(1./prec, 10);
	int const dotPos = exaStr.indexOf('.');

    if (dotPos==-1) {
		//big numbers for the precision
		return QString().setNum(RS_Math::round(num));
    } else {
		//number of digits after the point
		int digits = dotPos - 1;
		return RS_Math::doubleToString(num, digits);
    }
}

/**
 * Converts a double into a string which is as short as possible.
 *
 * @param value The double value
 * @param prec Precision
 */
QString RS_Math::doubleToString(double value, int prec) {
    QString valStr;

    valStr.setNum(value, 'f', prec);

    if(valStr.contains('.')) {
        // Remove tailing point and zeros:
//        valStr.replace(QRegExp("0*$"), "");
//        valStr.replace(QRegExp(R"(\.$)"), "");
//        while (valStr.at(valStr.length()-1)=='0') {
//            valStr.truncate(valStr.length()-1);
//        }

        if(valStr.at(valStr.length()-1)=='.') {
            valStr.truncate(valStr.length()-1);
        }

    }

    return valStr;
}

/**
 * Performs some testing for the math class.
 */
void RS_Math::test() {
	{
		std::cout<<"testing quadratic solver"<<std::endl;
		//equations x^2 + v[0] x + v[1] = 0
		std::vector<std::vector<double>> const eqns{
			{-1., -1.},
			{-101., -1.},
			{-1., -100.},
			{2., 1.},
			{-2., 1.}
		};
		//expected roots
		std::vector<std::vector<double>> roots{
			{-0.6180339887498948, 1.6180339887498948},
			{-0.0099000196991084878, 101.009900019699108},
			{-9.5124921972503929, 10.5124921972503929},
			{-1.},
			{1.}
		};

		for(size_t i=0; i < eqns.size(); i++) {
			std::cout<<"Test quadratic solver, test case: x^2 + ("
					<<eqns[i].front()<<") x + ("
				   <<eqns[i].back()<<") = 0"<<std::endl;
			auto sol = quadraticSolver(eqns[i]);
			assert(sol.size()==roots[i].size());
			if (sol.front() > sol.back())
				std::swap(sol[0], sol[1]);
			auto expected=roots[i];
			if (expected.front() > expected.back())
				std::swap(expected[0], expected[1]);
			for (size_t j=0; j < sol.size(); j++) {
				double x0 = sol[j];
				double x1 = expected[j];
				double const prec = (x0 - x1)/(fabs(x0 + x1) + RS_TOLERANCE2);
				std::cout<<"root "<<j<<" : precision level = "<<prec<<std::endl;
				std::cout<<std::setprecision(17)<<"found: "<<x0<<"\texpected: "<<x1<<std::endl;
				assert(prec < RS_TOLERANCE);
			}
			std::cout<<std::endl;
		}
		return;
	}
	QString s;
    double v;

    std::cout << "RS_Math::test: doubleToString:\n";

    v = 0.1;
    s = RS_Math::doubleToString(v, 0.1);
	assert(s=="0.1");
    s = RS_Math::doubleToString(v, 0.01);
	assert(s=="0.10");

    v = 0.01;
    s = RS_Math::doubleToString(v, 0.1);
	assert(s=="0.0");
    s = RS_Math::doubleToString(v, 0.01);
	assert(s=="0.01");
	s = RS_Math::doubleToString(v, 0.001);
	assert(s=="0.010");

    v = 0.001;
    s = RS_Math::doubleToString(v, 0.1);
	assert(s=="0.0");
    s = RS_Math::doubleToString(v, 0.01);
	assert(s=="0.00");
    s = RS_Math::doubleToString(v, 0.001);
	assert(s=="0.001");

    std::cout << "RS_Math::test: imperialTranslate:\n";

    s = "20'2\"+10'11\"3/4";
    RS_Math::imperialTranslate(s);
    assert(s=="(20*12+2)+(10*12+11+3/4)");

    s = "20'2\"-10'11\"3/4";
    RS_Math::imperialTranslate(s);
    assert(s=="(20*12+2)-(10*12+11+3/4)");

    s = "-10'11\"3/4";
    RS_Math::imperialTranslate(s);
    assert(s=="()-(10*12+11+3/4)");

	std::cout << "RS_Math::test: complete"<<std::endl;
}

//Equation solvers

// quadratic, cubic, and quartic equation solver
// @ ce[] contains coefficient of the cubic equation:
// @ returns a vector contains real roots
//
// solvers assume arguments are valid, and there's no attempt to verify validity of the argument pointers
//
// @author Dongxu Li <dongxuli2011@gmail.com>
std::vector<double> RS_Math::quadraticSolver(const std::vector<double>& ce)
//quadratic solver for
// x^2 + ce[0] x + ce[1] =0
{
    std::vector<double> ans(0,0.);
	if (ce.size() != 2) return ans;
	using LDouble = long double;
	LDouble const b = -0.5L * ce[0];
	LDouble const c = ce[1];
	// x^2 -2 b x + c=0
	// (x - b)^2 = b^2 - c
	// b^2 >= fabs(c)
	// x = b \pm b sqrt(1. - c/(b^2))
	LDouble const b2= b * b;
	LDouble const discriminant= b2 - c;
	LDouble const fc = std::abs(c);

	//TODO, fine tune to tolerance level
	LDouble const TOL = 1e-24L;

	if (discriminant < 0.L)
		//negative discriminant, no real root
		return ans;

	//find the radical
	LDouble r;

	// given |p| >= |q|
	// sqrt(p^2 \pm q^2) = p sqrt(1 \pm q^2/p^2)
	if (b2 >= fc)
		r = std::abs(b) * std::sqrt(1.L - c/b2);
	else
		// c is negative, because b2 - c is non-negative
		r = std::sqrt(fc) * std::sqrt(1.L + b2/fc);

	if (r >= TOL*std::abs(b)) {
		//two roots
		if (b >= 0.L)
			//since both (b,r)>=0, avoid (b - r) loss of significance
			ans.push_back(b + r);
		else
			//since b<0, r>=0, avoid (b + r) loss of significance
			ans.push_back(b - r);

		//Vieta's formulas for the second root
		ans.push_back(c/ans.front());
	} else
		//multiple roots
		ans.push_back(b);
	return ans;
}

std::vector<double> RS_Math::cubicSolver(const std::vector<double>& ce)
//cubic equation solver
// x^3 + ce[0] x^2 + ce[1] x + ce[2] = 0
{
//    std::cout<<"x^3 + ("<<ce[0]<<")*x^2+("<<ce[1]<<")*x+("<<ce[2]<<")==0"<<std::endl;
    std::vector<double> ans(0,0.);
	if (ce.size() != 3) return ans;
    // depressed cubic, Tschirnhaus transformation, x= t - b/(3a)
    // t^3 + p t +q =0
    double shift=(1./3)*ce[0];
    double p=ce[1] -shift*ce[0];
    double q=ce[0]*( (2./27)*ce[0]*ce[0]-(1./3)*ce[1])+ce[2];
    //Cardano's method,
    //	t=u+v
    //	u^3 + v^3 + ( 3 uv + p ) (u+v) + q =0
    //	select 3uv + p =0, then,
    //	u^3 + v^3 = -q
    //	u^3 v^3 = - p^3/27
    //	so, u^3 and v^3 are roots of equation,
    //	z^2 + q z - p^3/27 = 0
    //	and u^3,v^3 are,
    //		-q/2 \pm sqrt(q^2/4 + p^3/27)
    //	discriminant= q^2/4 + p^3/27
    //std::cout<<"p="<<p<<"\tq="<<q<<std::endl;
    double discriminant= (1./27)*p*p*p+(1./4)*q*q;
    if ( fabs(p)< 1.0e-75) {
        ans.push_back((q>0)?-pow(q,(1./3)):pow(-q,(1./3)));
        ans[0] -= shift;
//        DEBUG_HEADER
//        std::cout<<"cubic: one root: "<<ans[0]<<std::endl;
        return ans;
    }
    //std::cout<<"discriminant="<<discriminant<<std::endl;
    if(discriminant>0) {
        std::vector<double> ce2(2,0.);
        ce2[0]=q;
        ce2[1]=-1./27*p*p*p;
		auto r=quadraticSolver(ce2);
        if ( r.size()==0 ) { //should not happen
			std::cerr<<__FILE__<<" : "<<__func__<<" : line"<<__LINE__<<" :cubicSolver()::Error cubicSolver("<<ce[0]<<' '<<ce[1]<<' '<<ce[2]<<")\n";
        }
        double u,v;
        u= (q<=0) ? pow(r[0], 1./3): -pow(-r[1],1./3);
        //u=(q<=0)?pow(-0.5*q+sqrt(discriminant),1./3):-pow(0.5*q+sqrt(discriminant),1./3);
        v=(-1./3)*p/u;
        //std::cout<<"u="<<u<<"\tv="<<v<<std::endl;
        //std::cout<<"u^3="<<u*u*u<<"\tv^3="<<v*v*v<<std::endl;
        ans.push_back(u+v - shift);

//        DEBUG_HEADER
//        std::cout<<"cubic: one root: "<<ans[0]<<std::endl;
	}else{
		std::complex<double> u(q,0),rt[3];
		u=std::pow(-0.5*u-sqrt(0.25*u*u+p*p*p/27),1./3);
		rt[0]=u-p/(3.*u)-shift;
		std::complex<double> w(-0.5,sqrt(3.)/2);
		rt[1]=u*w-p/(3.*u*w)-shift;
		rt[2]=u/w-p*w/(3.*u)-shift;
		//        DEBUG_HEADER
		//        std::cout<<"Roots:\n";
		//        std::cout<<rt[0]<<std::endl;
		//        std::cout<<rt[1]<<std::endl;
		//        std::cout<<rt[2]<<std::endl;
		ans.push_back(rt[0].real());
		ans.push_back(rt[1].real());
		ans.push_back(rt[2].real());
	}
	// newton-raphson
	for(double& x0: ans){
		double dx=0.;
		for(size_t i=0; i<20; ++i){
			double f=( (x0 + ce[0])*x0 + ce[1])*x0 +ce[2];
			double df=(3.*x0+2.*ce[0])*x0 +ce[1];
			if(fabs(df)>fabs(f)+RS_TOLERANCE){
				dx=f/df;
				x0 -= dx;
			}else
				break;
		}
	}

    return ans;
}

/** quartic solver
* x^4 + ce[0] x^3 + ce[1] x^2 + ce[2] x + ce[3] = 0
@ce, a vector of size 4 contains the coefficient in order
@return, a vector contains real roots
**/
std::vector<double> RS_Math::quarticSolver(const std::vector<double>& ce)
{
    std::vector<double> ans(0,0.);
    if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
		DEBUG_HEADER
        std::cout<<"expected array size=4, got "<<ce.size()<<std::endl;
    }
    if(ce.size() != 4) return ans;
    if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
        std::cout<<"x^4+("<<ce[0]<<")*x^3+("<<ce[1]<<")*x^2+("<<ce[2]<<")*x+("<<ce[3]<<")==0"<<std::endl;
    }

    // x^4 + a x^3 + b x^2 +c x + d = 0
    // depressed quartic, x= t - a/4
    // t^4 + ( b - 3/8 a^2 ) t^2 + (c - a b/2 + a^3/8) t + d - a c /4 + a^2 b/16 - 3 a^4/256 =0
    // t^4 + p t^2 + q t + r =0
    // p= b - (3./8)*a*a;
    // q= c - 0.5*a*b+(1./8)*a*a*a;
    // r= d - 0.25*a*c+(1./16)*a*a*b-(3./256)*a^4
    double shift=0.25*ce[0];
    double shift2=shift*shift;
    double a2=ce[0]*ce[0];
    double p= ce[1] - (3./8)*a2;
    double q= ce[2] + ce[0]*((1./8)*a2 - 0.5*ce[1]);
    double r= ce[3] - shift*ce[2] + (ce[1] - 3.*shift2)*shift2;
    if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
		DEBUG_HEADER
        std::cout<<"x^4+("<<p<<")*x^2+("<<q<<")*x+("<<r<<")==0"<<std::endl;
    }
    if (q*q <= 1.e-4*RS_TOLERANCE*fabs(p*r)) {// Biquadratic equations
        double discriminant= 0.25*p*p -r;
        if (discriminant < -1.e3*RS_TOLERANCE) {

//            DEBUG_HEADER
//            std::cout<<"discriminant="<<discriminant<<"\tno root"<<std::endl;
            return ans;
        }
        double t2[2];
        t2[0]=-0.5*p-sqrt(fabs(discriminant));
        t2[1]= -p - t2[0];
        //        std::cout<<"t2[0]="<<t2[0]<<std::endl;
        //        std::cout<<"t2[1]="<<t2[1]<<std::endl;
        if ( t2[1] >= 0.) { // two real roots
            ans.push_back(sqrt(t2[1])-shift);
            ans.push_back(-sqrt(t2[1])-shift);
        }
        if ( t2[0] >= 0. ) {// four real roots
            ans.push_back(sqrt(t2[0])-shift);
            ans.push_back(-sqrt(t2[0])-shift);
        }
//        DEBUG_HEADER
//        for(int i=0;i<ans.size();i++){
//            std::cout<<"root x: "<<ans[i]<<std::endl;
//        }
        return ans;
    }
    if ( fabs(r)< 1.0e-75 ) {
        std::vector<double> cubic(3,0.);
        cubic[1]=p;
        cubic[2]=q;
        ans.push_back(0.);
		auto r=cubicSolver(cubic);
		std::copy(r.begin(),r.end(), std::back_inserter(ans));
        for(size_t i=0; i<ans.size(); i++) ans[i] -= shift;
        return ans;
    }
    // depressed quartic to two quadratic equations
    // t^4 + p t^2 + q t + r = ( t^2 + u t + v) ( t^2 - u t + w)
    // so,
    // 	p + u^2= w+v
    // 	q/u= w-v
    // 	r= wv
    // so,
    //  (p+u^2)^2 - (q/u)^2 = 4 r
    //  y=u^2,
    //  y^3 + 2 p y^2 + ( p^2 - 4 r) y - q^2 =0
    //
    std::vector<double> cubic(3,0.);
    cubic[0]=2.*p;
    cubic[1]=p*p-4.*r;
    cubic[2]=-q*q;
	auto r3= cubicSolver(cubic);
    //std::cout<<"quartic_solver:: real roots from cubic: "<<ret<<std::endl;
    //for(unsigned int i=0; i<ret; i++)
    //   std::cout<<"cubic["<<i<<"]="<<cubic[i]<<" x= "<<croots[i]<<std::endl;
	//newton-raphson
    if (r3.size()==1) { //one real root from cubic
        if (r3[0]< 0.) {//this should not happen
			DEBUG_HEADER
			qDebug()<<"Quartic Error:: Found one real root for cubic, but negative\n";
            return ans;
        }
        double sqrtz0=sqrt(r3[0]);
        std::vector<double> ce2(2,0.);
        ce2[0]=	-sqrtz0;
        ce2[1]=0.5*(p+r3[0])+0.5*q/sqrtz0;
        auto r1=quadraticSolver(ce2);
        if (r1.size()==0 ) {
            ce2[0]=	sqrtz0;
            ce2[1]=0.5*(p+r3[0])-0.5*q/sqrtz0;
            r1=quadraticSolver(ce2);
        }
		for(auto& x: r1){
			x -= shift;
		}
        return r1;
    }
    if ( r3[0]> 0. && r3[1] > 0. ) {
        double sqrtz0=sqrt(r3[0]);
        std::vector<double> ce2(2,0.);
        ce2[0]=	-sqrtz0;
        ce2[1]=0.5*(p+r3[0])+0.5*q/sqrtz0;
        ans=quadraticSolver(ce2);
        ce2[0]=	sqrtz0;
        ce2[1]=0.5*(p+r3[0])-0.5*q/sqrtz0;
		auto r1=quadraticSolver(ce2);
		std::copy(r1.begin(),r1.end(),std::back_inserter(ans));
		for(auto& x: ans){
			x -= shift;
		}
    }
	// newton-raphson
	for(double& x0: ans){
		double dx=0.;
		for(size_t i=0; i<20; ++i){
			double f=(( (x0 + ce[0])*x0 + ce[1])*x0 +ce[2])*x0 + ce[3] ;
			double df=((4.*x0+3.*ce[0])*x0 +2.*ce[1])*x0+ce[2];
//			DEBUG_HEADER
//			qDebug()<<"i="<<i<<"\tx0="<<x0<<"\tf="<<f<<"\tdf="<<df;
			if(fabs(df)>RS_TOLERANCE2){
				dx=f/df;
				x0 -= dx;
			}else
				break;
		}
	}

    return ans;
}

/** quartic solver
* ce[4] x^4 + ce[3] x^3 + ce[2] x^2 + ce[1] x + ce[0] = 0
@ce, a vector of size 5 contains the coefficient in order
@return, a vector contains real roots
*ToDo, need a robust algorithm to locate zero terms, better handling of tolerances
**/
std::vector<double> RS_Math::quarticSolverFull(const std::vector<double>& ce)
{
    if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
		DEBUG_HEADER
        std::cout<<ce[4]<<"*y^4+("<<ce[3]<<")*y^3+("<<ce[2]<<"*y^2+("<<ce[1]<<")*y+("<<ce[0]<<")==0"<<std::endl;
    }

    std::vector<double> roots(0,0.);
    if(ce.size()!=5) return roots;
    std::vector<double> ce2(4,0.);

    if ( fabs(ce[4]) < 1.0e-14) { // this should not happen
        if ( fabs(ce[3]) < 1.0e-14) { // this should not happen
            if ( fabs(ce[2]) < 1.0e-14) { // this should not happen
                if( fabs(ce[1]) > 1.0e-14) {
                    roots.push_back(-ce[0]/ce[1]);
                } else { // can not determine y. this means overlapped, but overlap should have been detected before, therefore return empty set
                    return roots;
                }
            } else {
                ce2.resize(2);
                ce2[0]=ce[1]/ce[2];
                ce2[1]=ce[0]/ce[2];
                //std::cout<<"ce2[2]={ "<<ce2[0]<<' '<<ce2[1]<<" }\n";
                roots=RS_Math::quadraticSolver(ce2);
            }
        } else {
            ce2.resize(3);
            ce2[0]=ce[2]/ce[3];
            ce2[1]=ce[1]/ce[3];
            ce2[2]=ce[0]/ce[3];
            //std::cout<<"ce2[3]={ "<<ce2[0]<<' '<<ce2[1]<<' '<<ce2[2]<<" }\n";
            roots=RS_Math::cubicSolver(ce2);
        }
    } else {
        ce2[0]=ce[3]/ce[4];
        ce2[1]=ce[2]/ce[4];
        ce2[2]=ce[1]/ce[4];
        ce2[3]=ce[0]/ce[4];
        if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
			DEBUG_HEADER
            std::cout<<"ce2[4]={ "<<ce2[0]<<' '<<ce2[1]<<' '<<ce2[2]<<' '<<ce2[3]<<" }\n";
        }
        if(fabs(ce2[3])<= RS_TOLERANCE15) {
            //constant term is zero, factor 0 out, solve a cubic equation
            ce2.resize(3);
            roots=RS_Math::cubicSolver(ce2);
            roots.push_back(0.);
        }else
            roots=RS_Math::quarticSolver(ce2);
    }
    return roots;
}

//linear Equation solver by Gauss-Jordan
/**
  * Solve linear equation set
  *@ mt holds the augmented matrix
  *@ sn holds the solution
  *@ return true, if the equation set has a unique solution, return false otherwise
  *
  *@Author: Dongxu Li
  */

bool RS_Math::linearSolver(const std::vector<std::vector<double> >& mt, std::vector<double>& sn){
    //verify the matrix size
	size_t mSize(mt.size()); //rows
	size_t aSize(mSize+1); //columns of augmented matrix
	if(std::any_of(mt.begin(), mt.end(), [&aSize](const std::vector<double>& v)->bool{
				   return v.size() != aSize;
}))
		return false;
    sn.resize(mSize);//to hold the solution
#if false
    boost::numeric::ublas::matrix<double> bm (mSize, mSize);
    boost::numeric::ublas::vector<double> bs(mSize);

    for(int i=0;i<mSize;i++) {
        for(int j=0;j<mSize;j++) {
            bm(i,j)=mt[i][j];
        }
        bs(i)=mt[i][mSize];
    }
    //solve the linear equation set by LU decomposition in boost ublas

    if ( boost::numeric::ublas::lu_factorize<boost::numeric::ublas::matrix<double> >(bm) ) {
		std::cout<<__FILE__<<" : "<<__func__<<" : line "<<__LINE__<<std::endl;
        std::cout<<" linear solver failed"<<std::endl;
        //        RS_DEBUG->print(RS_Debug::D_WARNING, "linear solver failed");
        return false;
    }

    boost::numeric::ublas:: triangular_matrix<double, boost::numeric::ublas::unit_lower>
            lm = boost::numeric::ublas::triangular_adaptor< boost::numeric::ublas::matrix<double>,  boost::numeric::ublas::unit_lower>(bm);
    boost::numeric::ublas:: triangular_matrix<double,  boost::numeric::ublas::upper>
            um =  boost::numeric::ublas::triangular_adaptor< boost::numeric::ublas::matrix<double>,  boost::numeric::ublas::upper>(bm);
    ;
    boost::numeric::ublas::inplace_solve(lm,bs, boost::numeric::ublas::lower_tag());
    boost::numeric::ublas::inplace_solve(um,bs, boost::numeric::ublas::upper_tag());
    for(int i=0;i<mSize;i++){
        sn[i]=bs(i);
    }
    //    std::cout<<"dn="<<dn<<std::endl;
    //    data.center.set(-0.5*dn(1)/dn(0),-0.5*dn(3)/dn(2)); // center
    //    double d(1.+0.25*(dn(1)*dn(1)/dn(0)+dn(3)*dn(3)/dn(2)));
    //    if(fabs(dn(0))<RS_TOLERANCE2
    //            ||fabs(dn(2))<RS_TOLERANCE2
    //            ||d/dn(0)<RS_TOLERANCE2
    //            ||d/dn(2)<RS_TOLERANCE2
    //            ) {
    //        //ellipse not defined
    //        return false;
    //    }
    //    d=sqrt(d/dn(0));
    //    data.majorP.set(d,0.);
    //    data.ratio=sqrt(dn(0)/dn(2));
#else
    // solve the linear equation by Gauss-Jordan elimination
	std::vector<std::vector<double> > mt0(mt); //copy the matrix;
	for(size_t i=0;i<mSize;++i){
		size_t imax(i);
        double cmax(fabs(mt0[i][i]));
		for(size_t j=i+1;j<mSize;++j) {
            if(fabs(mt0[j][i]) > cmax ) {
                imax=j;
                cmax=fabs(mt0[j][i]);
            }
        }
        if(cmax<RS_TOLERANCE2) return false; //singular matrix
        if(imax != i) {//move the line with largest absolute value at column i to row i, to avoid division by zero
            std::swap(mt0[i],mt0[imax]);
		}
		for(size_t k=i+1;k<=mSize;++k) { //normalize the i-th row
            mt0[i][k] /= mt0[i][i];
        }
		mt0[i][i]=1.;
		for(size_t j=0;j<mSize;++j) {//Gauss-Jordan
            if(j != i ) {
				double& a = mt0[j][i];
				for(size_t k=i+1;k<=mSize;++k) {
					mt0[j][k] -= mt0[i][k]*a;
                }
				a=0.;
            }
		}
		//output gauss-jordan results for debugging
//		std::cout<<"========"<<i<<"==========\n";
//		for(auto v0: mt0){
//			for(auto v1:v0)
//				std::cout<<v1<<'\t';
//			std::cout<<std::endl;
//		}
    }
	for(size_t i=0;i<mSize;++i) {
        sn[i]=mt0[i][mSize];
    }
#endif

    return true;
}

/**
 * wrapper of elliptic integral of the second type, Legendre form
 * @param k the elliptic modulus or eccentricity
 * @param phi elliptic angle, must be within range of [0, M_PI]
 *
 * @author: Dongxu Li
 */
double RS_Math::ellipticIntegral_2(const double& k, const double& phi)
{
    double a= remainder(phi-M_PI_2,M_PI);
    if(a>0.) {
        return boost::math::ellint_2<double,double>(k,a);
    } else {
        return - boost::math::ellint_2<double,double>(k,fabs(a));
    }
}

/** solver quadratic simultaneous equations of set two **/
/* solve the following quadratic simultaneous equations,
  *  ma000 x^2 + ma011 y^2 - 1 =0
  * ma100 x^2 + 2 ma101 xy + ma111 y^2 + mb10 x + mb11 y +mc1 =0
  *
  *@m, a vector of size 8 contains coefficients in the strict order of:
  ma000 ma011 ma100 ma101 ma111 mb10 mb11 mc1
  * m[0] m[1] must be positive
  *@return a vector contains real roots
  */
RS_VectorSolutions RS_Math::simultaneousQuadraticSolver(const std::vector<double>& m)
{
    RS_VectorSolutions ret(0);
    if(m.size() != 8 ) return ret; // valid m should contain exact 8 elements
    std::vector< double> c1(0,0.);
    std::vector< std::vector<double> > m1(0,c1);
    c1.resize(6);
    c1[0]=m[0];
    c1[1]=0.;
    c1[2]=m[1];
    c1[3]=0.;
    c1[4]=0.;
    c1[5]=-1.;
    m1.push_back(c1);
    c1[0]=m[2];
    c1[1]=2.*m[3];
    c1[2]=m[4];
    c1[3]=m[5];
    c1[4]=m[6];
    c1[5]=m[7];
    m1.push_back(c1);

    return simultaneousQuadraticSolverFull(m1);
}

/** solver quadratic simultaneous equations of a set of two **/
/* solve the following quadratic simultaneous equations,
  * ma000 x^2 + ma001 xy + ma011 y^2 + mb00 x + mb01 y + mc0 =0
  * ma100 x^2 + ma101 xy + ma111 y^2 + mb10 x + mb11 y + mc1 =0
  *
  *@m, a vector of size 2 each contains a vector of size 6 coefficients in the strict order of:
  ma000 ma001 ma011 mb00 mb01 mc0
  ma100 ma101 ma111 mb10 mb11 mc1
  *@return a RS_VectorSolutions contains real roots (x,y)
  */
RS_VectorSolutions RS_Math::simultaneousQuadraticSolverFull(const std::vector<std::vector<double> >& m)
{
    RS_VectorSolutions ret;
    if(m.size()!=2)  return ret;
    if( m[0].size() ==3 || m[1].size()==3 ){
        return simultaneousQuadraticSolverMixed(m);
    }
    if(m[0].size()!=6 || m[1].size()!=6) return ret;
    /** eliminate x, quartic equation of y **/
    auto& a=m[0][0];
    auto& b=m[0][1];
    auto& c=m[0][2];
    auto& d=m[0][3];
    auto& e=m[0][4];
    auto& f=m[0][5];

    auto& g=m[1][0];
    auto& h=m[1][1];
    auto& i=m[1][2];
    auto& j=m[1][3];
    auto& k=m[1][4];
    auto& l=m[1][5];
    /**
      Collect[Eliminate[{ a*x^2 + b*x*y+c*y^2+d*x+e*y+f==0,g*x^2+h*x*y+i*y^2+j*x+k*y+l==0},x],y]
      **/
    /*
     f^2 g^2 - d f g j + a f j^2 - 2 a f g l + (2 e f g^2 - d f g h - b f g j + 2 a f h j - 2 a f g k) y + (2 c f g^2 - b f g h + a f h^2 - 2 a f g i) y^2
 ==
 -(d^2 g l) + a d j l - a^2 l^2
+
 (d e g j - a e j^2 - d^2 g k + a d j k - 2 b d g l + 2 a e g l + a d h l + a b j l - 2 a^2 k l) y
+
 (-(e^2 g^2) + d e g h - d^2 g i + c d g j + b e g j - 2 a e h j + a d i j - a c j^2 - 2 b d g k + 2 a e g k + a d h k + a b j k - a^2 k^2 - b^2 g l + 2 a c g l + a b h l - 2 a^2 i l) y^2
 +
(-2 c e g^2 + c d g h + b e g h - a e h^2 - 2 b d g i + 2 a e g i + a d h i + b c g j - 2 a c h j + a b i j - b^2 g k + 2 a c g k + a b h k - 2 a^2 i k) y^3
+
 (-(c^2 g^2) + b c g h - a c h^2 - b^2 g i + 2 a c g i + a b h i - a^2 i^2) y^4

      */
    double a2=a*a;
    double b2=b*b;
    double c2=c*c;
    double d2=d*d;
    double e2=e*e;
    double f2=f*f;

    double g2=g*g;
    double  h2=h*h;
    double  i2=i*i;
    double  j2=j*j;
    double  k2=k*k;
    double  l2=l*l;
    std::vector<double> qy(5,0.);
    //y^4
    qy[4]=-c2*g2 + b*c*g*h - a*c*h2 - b2*g*i + 2.*a*c*g*i + a*b*h*i - a2*i2;
    //y^3
    qy[3]=-2.*c*e*g2 + c*d*g*h + b*e*g*h - a*e*h2 - 2.*b*d*g*i + 2.*a*e*g*i + a*d*h*i +
            b*c*g*j - 2.*a*c*h*j + a*b*i*j - b2*g*k + 2.*a*c*g*k + a*b*h*k - 2.*a2*i*k;
    //y^2
    qy[2]=(-e2*g2 + d*e*g*h - d2*g*i + c*d*g*j + b*e*g*j - 2.*a*e*h*j + a*d*i*j - a*c*j2 -
           2.*b*d*g*k + 2.*a*e*g*k + a*d*h*k + a*b*j*k - a2*k2 - b2*g*l + 2.*a*c*g*l + a*b*h*l - 2.*a2*i*l)
            - (2.*c*f*g2 - b*f*g*h + a*f*h2 - 2.*a*f*g*i);
    //y
    qy[1]=(d*e*g*j - a*e*j2 - d2*g*k + a*d*j*k - 2.*b*d*g*l + 2.*a*e*g*l + a*d*h*l + a*b*j*l - 2.*a2*k*l)
            -(2.*e*f*g2 - d*f*g*h - b*f*g*j + 2.*a*f*h*j - 2.*a*f*g*k);
    //y^0
    qy[0]=-d2*g*l + a*d*j*l - a2*l2
            - ( f2*g2 - d*f*g*j + a*f*j2 - 2.*a*f*g*l);
	if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
		DEBUG_HEADER
        std::cout<<qy[4]<<"*y^4 +("<<qy[3]<<")*y^3+("<<qy[2]<<")*y^2+("<<qy[1]<<")*y+("<<qy[0]<<")==0"<<std::endl;
	}
    //quarticSolver
	auto roots=quarticSolverFull(qy);
    if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
        std::cout<<"roots.size()= "<<roots.size()<<std::endl;
    }

    if (roots.size()==0 ) { // no intersection found
        return ret;
    }
    std::vector<double> ce(0,0.);

    for(size_t i0=0;i0<roots.size();i0++){
        if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
			DEBUG_HEADER
            std::cout<<"y="<<roots[i0]<<std::endl;
        }
        /*
          Collect[Eliminate[{ a*x^2 + b*x*y+c*y^2+d*x+e*y+f==0,g*x^2+h*x*y+i*y^2+j*x+k*y+l==0},x],y]
          */
        ce.resize(3);
        ce[0]=a;
        ce[1]=b*roots[i0]+d;
        ce[2]=c*roots[i0]*roots[i0]+e*roots[i0]+f;
//    DEBUG_HEADER
//                std::cout<<"("<<ce[0]<<")*x^2 + ("<<ce[1]<<")*x + ("<<ce[2]<<") == 0"<<std::endl;
        if(fabs(ce[0])<1e-75 && fabs(ce[1])<1e-75) {
            ce[0]=g;
            ce[1]=h*roots[i0]+j;
            ce[2]=i*roots[i0]*roots[i0]+k*roots[i0]+f;
//            DEBUG_HEADER
//            std::cout<<"("<<ce[0]<<")*x^2 + ("<<ce[1]<<")*x + ("<<ce[2]<<") == 0"<<std::endl;

        }
        if(fabs(ce[0])<1e-75 && fabs(ce[1])<1e-75) continue;

        if(fabs(a)>1e-75){
            std::vector<double> ce2(2,0.);
            ce2[0]=ce[1]/ce[0];
            ce2[1]=ce[2]/ce[0];
//                DEBUG_HEADER
//                        std::cout<<"x^2 +("<<ce2[0]<<")*x+("<<ce2[1]<<")==0"<<std::endl;
			auto xRoots=quadraticSolver(ce2);
            for(size_t j0=0;j0<xRoots.size();j0++){
//                DEBUG_HEADER
//                std::cout<<"x="<<xRoots[j0]<<std::endl;
                RS_Vector vp(xRoots[j0],roots[i0]);
                if(simultaneousQuadraticVerify(m,vp)) ret.push_back(vp);
            }
            continue;
        }
        RS_Vector vp(-ce[2]/ce[1],roots[i0]);
        if(simultaneousQuadraticVerify(m,vp)) ret.push_back(vp);
    }
	if(RS_DEBUG->getLevel()>=RS_Debug::D_INFORMATIONAL){
		DEBUG_HEADER
        std::cout<<"ret="<<ret<<std::endl;
	}
    return ret;
}

RS_VectorSolutions RS_Math::simultaneousQuadraticSolverMixed(const std::vector<std::vector<double> >& m)
{
    RS_VectorSolutions ret;
    auto p0=& (m[0]);
    auto p1=& (m[1]);
    if(p1->size()==3){
        std::swap(p0,p1);
    }
    if(p1->size()==3) {
            //linear
			std::vector<double> sn(2,0.);
			std::vector<std::vector<double> > ce;
			ce.push_back(m[0]);
			ce.push_back(m[1]);
            ce[0][2]=-ce[0][2];
            ce[1][2]=-ce[1][2];
            if( RS_Math::linearSolver(ce,sn)) ret.push_back(RS_Vector(sn[0],sn[1]));
            return ret;
    }
//    DEBUG_HEADER
//    std::cout<<"p0: size="<<p0->size()<<"\n Solve[{("<< p0->at(0)<<")*x + ("<<p0->at(1)<<")*y + ("<<p0->at(2)<<")==0,";
//    std::cout<<"("<< p1->at(0)<<")*x^2 + ("<<p1->at(1)<<")*x*y + ("<<p1->at(2)<<")*y^2 + ("<<p1->at(3)<<")*x +("<<p1->at(4)<<")*y+("
//            <<p1->at(5)<<")==0},{x,y}]"<<std::endl;
    const double& a=p0->at(0);
    const double& b=p0->at(1);
    const double& c=p0->at(2);
    const double& d=p1->at(0);
    const double& e=p1->at(1);
    const double& f=p1->at(2);
    const double& g=p1->at(3);
    const double& h=p1->at(4);
    const double& i=p1->at(5);
    /**
      y (2 b c d-a c e)-a c g+c^2 d = y^2 (a^2 (-f)+a b e-b^2 d)+y (a b g-a^2 h)+a^2 (-i)
      */
    std::vector<double> ce(3,0.);
	const double& a2=a*a;
	const double& b2=b*b;
	const double& c2=c*c;
    ce[0]= -f*a2+a*b*e-b2*d;
    ce[1]=a*b*g-a2*h- (2*b*c*d-a*c*e);
    ce[2]=a*c*g-c2*d-a2*i;
//    DEBUG_HEADER
//    std::cout<<"("<<ce[0]<<") y^2 + ("<<ce[1]<<") y + ("<<ce[2]<<")==0"<<std::endl;
    std::vector<double> roots(0,0.);
    if( fabs(ce[1])>RS_TOLERANCE15 && fabs(ce[0]/ce[1])<RS_TOLERANCE15){
        roots.push_back( - ce[2]/ce[1]);
    }else{
        std::vector<double> ce2(2,0.);
        ce2[0]=ce[1]/ce[0];
        ce2[1]=ce[2]/ce[0];
        roots=quadraticSolver(ce2);
    }
//    for(size_t i=0;i<roots.size();i++){
//    std::cout<<"x="<<roots.at(i)<<std::endl;
//    }

    if(roots.size()==0)  {
        return RS_VectorSolutions();
    }
    for(size_t i=0;i<roots.size();i++){
        ret.push_back(RS_Vector(-(b*roots.at(i)+c)/a,roots.at(i)));
//        std::cout<<ret.at(ret.size()-1).x<<", "<<ret.at(ret.size()-1).y<<std::endl;
    }

    return ret;

}

/** verify a solution for simultaneousQuadratic
  *@m the coefficient matrix
  *@v, a candidate to verify
  *@return true, for a valid solution
  **/
bool RS_Math::simultaneousQuadraticVerify(const std::vector<std::vector<double> >& m, RS_Vector& v)
{
	RS_Vector v0=v;
	auto& a=m[0][0];
	auto& b=m[0][1];
	auto& c=m[0][2];
	auto& d=m[0][3];
	auto& e=m[0][4];
	auto& f=m[0][5];

	auto& g=m[1][0];
	auto& h=m[1][1];
	auto& i=m[1][2];
	auto& j=m[1][3];
	auto& k=m[1][4];
	auto& l=m[1][5];
    /**
      * tolerance test for bug#3606099
      * verifying the equations to floating point tolerance by terms
      */
	double sum0=0., sum1=0.;
	double f00=0.,f01=0.;
	double amax0, amax1;
	for(size_t i0=0; i0<20; ++i0){
		double& x=v.x;
		double& y=v.y;
		double x2=x*x;
		double y2=y*y;
		double const terms0[12]={ a*x2, b*x*y, c*y2, d*x, e*y, f, g*x2, h*x*y, i*y2, j*x, k*y, l};
		amax0=fabs(terms0[0]), amax1=fabs(terms0[6]);
		double px=2.*a*x+b*y+d;
		double py=b*x+2.*c*y+e;
		sum0=0.;
		for(int i=0; i<6; i++) {
			if(amax0<fabs(terms0[i])) amax0=fabs(terms0[i]);
			sum0 += terms0[i];
		}
		std::vector<std::vector<double>> nrCe;
		nrCe.push_back(std::vector<double>{px, py, sum0});
		px=2.*g*x+h*y+j;
		py=h*x+2.*i*y+k;
		sum1=0.;
		for(int i=6; i<12; i++) {
			if(amax1<fabs(terms0[i])) amax1=fabs(terms0[i]);
			sum1 += terms0[i];
		}
		nrCe.push_back(std::vector<double>{px, py, sum1});
		std::vector<double> dn;
		bool ret=linearSolver(nrCe, dn);
//		DEBUG_HEADER
//		qDebug()<<"i0="<<i0<<"\tf=("<<sum0<<','<<sum1<<")\tdn=("<<dn[0]<<","<<dn[1]<<")";
		if(!i0){
			f00=sum0;
			f01=sum1;
		}
		if(!ret) break;
		v -= RS_Vector(dn[0], dn[1]);
	}
	if( fabs(sum0)> fabs(f00) && fabs(sum1)>fabs(f01)){
		v=v0;
		sum0=f00;
		sum1=f01;
	}

//    DEBUG_HEADER
//    std::cout<<"verifying: x="<<x<<"\ty="<<y<<std::endl;
//    std::cout<<"0: maxterm: "<<amax0<<std::endl;
//    std::cout<<"verifying: fabs(a*x2 + b*x*y+c*y2+d*x+e*y+f)/maxterm="<<fabs(sum0)/amax0<<" required to be smaller than "<<sqrt(6.)*sqrt(DBL_EPSILON)<<std::endl;
//    std::cout<<"1: maxterm: "<<amax1<<std::endl;
//    std::cout<<"verifying: fabs(g*x2+h*x*y+i*y2+j*x+k*y+l)/maxterm="<< fabs(sum1)/amax1<<std::endl;
    const double tols=2.*sqrt(6.)*sqrt(DBL_EPSILON); //experimental tolerances to verify simultaneous quadratic

    return (amax0<=tols || fabs(sum0)/amax0<tols) &&  (amax1<=tols || fabs(sum1)/amax1<tols);
}
//EOF


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ASIND_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ASIND_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the inverse sine in degree.

    @par Header <boost/simd/function/asind.hpp>

    @par Note

      For every parameter of floating type `asind(x)`
      returns the arc @c r in the interval  \f$[-90, 90[\f$ such that
      <tt>sin(r) == x</tt>.  If @c x is outside \f$[-1, 1[\f$ the result is Nan.

    @see asin,  asinpi

    @par Example:

      @snippet asind.cpp asind

    @par Possible output:

      @snippet asind.txt asind

  **/
  IEEEValue asind(IEEEValue const & x);
} }
#endif

#include <boost/simd/function/scalar/asind.hpp>
#include <boost/simd/function/simd/asind.hpp>

#endif


#ifndef __fovis_initial_homography_estimator_hpp__
#define __fovis_initial_homography_estimator_hpp__

#include <vector>
#include <stdint.h>
#include <Eigen/Dense>

namespace fovis
{

/**
 * \brief Estimates a rough 2D homography registering two images.
 */
class InitialHomographyEstimator {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
  /**
   * Set the template image to the passed in arguments.
   * assumes the image data is row-major.
   * The image will be downsampled by \f$ 1/2^{downsampleFactor} \f$
   */
  void setTemplateImage(const uint8_t * grayData, int width, int height, int stride, int downsampleFactor);

  /**
   * Set the test image accordingly. The opimization will warp this image to
   * match the template assumes the image data is row-major.
   * The image will be downsampled by \f$ 1/2^{downsampleFactor} \f$
   */
  void setTestImage(const uint8_t * grayData, int width, int height, int stride, int downsampleFactor);

  /**
   * Run ESM to find the homography between the template and test images. These
   * should have already been passed in using the methods setTemplateImage(), and
   * setTestImage().
   */
  Eigen::Matrix3f track(const Eigen::Matrix3f &init_H, int nIters, double *finalRMS);

private:
  int template_rows, template_cols; //size of the template
  Eigen::MatrixXf templateImage, testImage; //the images
  Eigen::MatrixXf warpedTestImage; //storage for the warped testImage on the current iteration
  Eigen::MatrixXf errorIm; //the difference between the templateImage and the warpedTestImage
  Eigen::ArrayXf templateDxRow, templateDyRow; //the gradient of the template flattened into a row vector
  Eigen::MatrixXf templatePoints; //nx3 matrix storing the x,y,1 for each pixel
  Eigen::ArrayXf xx, yy; //convenience array storing the original x/y for each pixel in the flattened image

  //internal functions

  /**
   * Compute the x/y gradient of the passed in image
   */
  static void computeGradient(const Eigen::MatrixXf &image, Eigen::MatrixXf * dxp, Eigen::MatrixXf *dyp);

  /**
   * Compute the error (RMS) for the passed in difference image
   */
  static double computeError(const Eigen::MatrixXf &error);

  /**
   * form the Jacobian
   */
  Eigen::MatrixXf computeJacobian(const Eigen::ArrayXf &dx, const Eigen::ArrayXf &dy) const;

  /**
   * Compute the homography from the lie parameterization
   */
  Eigen::Matrix3f lieToH(const Eigen::VectorXf &lie) const;

  /**
   * Warp srcImage according to the warped points
   */
  Eigen::MatrixXf constructWarpedImage(const Eigen::MatrixXf &srcImage, const Eigen::MatrixXf &warpedPoints) const;

  /**
   * Flatten an image stored as a matrix down into a row vector
   */
  static Eigen::ArrayXf flattenMatrix(Eigen::MatrixXf &m);

};

}

#endif


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ACSCH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACSCH_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-hyperbolic
    This function object returns the hyperbolic cosecant argument
    \f$\mathop{\textrm{asinh}}(1/x)\f$

    @par Header <boost/simd/function/acsch.hpp>

    @see cosh, sinh,  acosh, asinh, atanh, asech, acoth, atanh

    @par Example:

      @snippet acsch.cpp acsch

    @par Possible output:

      @snippet acsch.txt acsch

  **/
  IEEEValue acsch(IEEEValue const & x);
} }
#endif

#include <boost/simd/function/scalar/acsch.hpp>
#include <boost/simd/function/simd/acsch.hpp>

#endif


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/back.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/front.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/tail.hpp>
namespace hana = boost::hana;

BOOST_HANA_CONSTANT_CHECK(hana::front(hana::range_c<int, 0, 5>) == hana::int_c<0>);
BOOST_HANA_CONSTANT_CHECK(hana::back(hana::range_c<unsigned long, 0, 5>) == hana::ulong_c<4>);
BOOST_HANA_CONSTANT_CHECK(hana::tail(hana::range_c<int, 0, 5>) == hana::make_range(hana::int_c<1>, hana::int_c<5>));

int main() { }


#define BOOST_TEST_MODULE "test_uniform_lennard_jones_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif
#include <mjolnir/forcefield/global/UniformLennardJonesPotential.hpp>
#include <mjolnir/util/make_unique.hpp>

BOOST_AUTO_TEST_CASE(UniformLennardJones_double)
{
    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type   = typename traits_type::real_type;
    using molecule_id_type = mjolnir::Topology::molecule_id_type;
    using group_id_type    = mjolnir::Topology::group_id_type;
    constexpr std::size_t N = 10000;
    constexpr real_type   h = 1e-6;

    constexpr real_type sigma   = 3.0;
    constexpr real_type epsilon = 1.0;
    mjolnir::UniformLennardJonesPotential<traits_type> lj{
        sigma, epsilon,
        mjolnir::UniformLennardJonesPotential<traits_type>::default_cutoff(), {}, {},
        mjolnir::IgnoreMolecule<molecule_id_type>("Nothing"),
        mjolnir::IgnoreGroup   <group_id_type   >({})
    };
    const real_type cutoff = lj.cutoff_ratio();

    const real_type x_min = 0.8 * sigma;
    const real_type x_max = cutoff * sigma;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + i * dx;
        const real_type pot1 = lj.potential(0, 1, x + h);
        const real_type pot2 = lj.potential(0, 1, x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = lj.derivative(0, 1, x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
    }
}

BOOST_AUTO_TEST_CASE(UniformLennardJones_float)
{
    using traits_type = mjolnir::SimulatorTraits<float, mjolnir::UnlimitedBoundary>;
    using real_type   = typename traits_type::real_type;
    using molecule_id_type = mjolnir::Topology::molecule_id_type;
    using group_id_type    = mjolnir::Topology::group_id_type;
    constexpr std::size_t N = 1000;
    constexpr real_type   h = 0.002;
    constexpr real_type tol = 0.005;

    constexpr real_type sigma   = 3.0;
    constexpr real_type epsilon = 1.0;
    mjolnir::UniformLennardJonesPotential<traits_type> lj{
        sigma, epsilon,
        mjolnir::UniformLennardJonesPotential<traits_type>::default_cutoff(), {}, {},
        mjolnir::IgnoreMolecule<molecule_id_type>("Nothing"),
        mjolnir::IgnoreGroup   <group_id_type   >({})
    };
    const real_type cutoff = lj.cutoff_ratio();

    const real_type x_min = 0.8 * sigma;
    const real_type x_max = cutoff * sigma;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + i * dx;
        const real_type pot1 = lj.potential(0, 1, x + h);
        const real_type pot2 = lj.potential(0, 1, x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = lj.derivative(0, 1, x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(tol));
    }
}


#include <cnoid/Plugin>
#include <cnoid/ItemTreeView>
#include <cnoid/BodyItem>
#include <cnoid/ToolBar>
#include <cnoid/Body>
#include <cnoid/JointPath>
#include <cnoid/Jacobian>
#include <cnoid/BodyLoader>
#include <cnoid/Link>

#include <Eigen/Core>

using namespace std;
using namespace cnoid;
using namespace Eigen;

class ComJacobianPlugin : public Plugin
{
public:

    ComJacobianPlugin() : Plugin("ComJacobianTest")
    {;
        require("Body");
    }

    virtual bool initialize()
    {
        ToolBar* bar = new ToolBar("ComJacobianTest");
        bar->addButton("Increment")
            ->sigClicked().connect(bind(&ComJacobianPlugin::onButtonClicked, this, 0.02));
        bar->addButton("Decrement")
                ->sigClicked().connect(bind(&ComJacobianPlugin::onButtonClicked, this, -0.02));
        addToolBar(bar);

        return true;
    }

    void onButtonClicked(double ref_com_pos)
    {
        ItemList<BodyItem> bodyItems =
            ItemTreeView::mainInstance()->selectedItems<BodyItem>();

		const double ik_gain = 0.5;
		const int iteration = 100;
		const double erreps = 1e-06;
		ColPivHouseholderQR<MatrixXd> QR;
		const double ik_lambda = 1.0e-12;

		BodyPtr robot = bodyItems[0]->body();

		JointPathPtr leg = getCustomJointPath(robot, robot->link("RLEG_ANKLE_R"), robot->link("WAIST"));

		// 順運動学でリンクの位置姿勢更新
		robot->calcForwardKinematics();

		// ワールド座標系における重心位置を更新
		Vector3d cur_com(robot->calcCenterOfMass());
		Vector3d ref_com(cur_com);
		ref_com.y() += ref_com_pos;

		// 目標関節角度の差分
		VectorXd dq(robot->numJoints());
		// 収束ループ
		for(int n=0;n<iteration;n++)
		{
			if(ref_com.dot(ref_com) < erreps) break;
			// 重心ヤコビアン
			MatrixXd J_com(3, robot->numJoints());

			// 重心ヤコビアンを計算
			calcCMJacobian(robot, leg->baseLink(), J_com);

			MatrixXd JJ = J_com * J_com.transpose() + ik_lambda*MatrixXd::Identity(J_com.rows(), J_com.rows());
			dq = J_com.transpose() * QR.compute(JJ).solve(ref_com - cur_com) * ik_gain;
		}

        for(size_t i=0; i < bodyItems.size(); ++i){
            for(int j=0; j < robot->numJoints(); ++j)
                robot->joint(j)->q() += dq[j];
            bodyItems[i]->notifyKinematicStateChange(true);
        }
    }
};

CNOID_IMPLEMENT_PLUGIN_ENTRY(ComJacobianPlugin)


#include <iostream>
#include <array>
#include <bitset>

#include <boost/multiprecision/cpp_int.hpp>

using namespace boost::multiprecision::literals;
using boost::multiprecision::number;
using boost::multiprecision::backends::cpp_int_backend;
using boost::multiprecision::cpp_integer_type;
using boost::multiprecision::cpp_int_check_type;

using std::cout;

#define BLS12_381_MODULUS_LEN 255
#define GRAIN_LFSR_STATE_LEN 80

BOOST_MP_DEFINE_SIZED_CPP_INT_LITERAL(BLS12_381_MODULUS_LEN);
BOOST_MP_DEFINE_SIZED_CPP_INT_LITERAL(GRAIN_LFSR_STATE_LEN);

template<std::size_t t, std::size_t full_rounds, std::size_t part_rounds>
struct round_constants_generator {
    constexpr static std::size_t modulus_bits = BLS12_381_MODULUS_LEN;
    constexpr static std::size_t state_bits = GRAIN_LFSR_STATE_LEN;

    typedef number<cpp_int_backend<modulus_bits, modulus_bits, cpp_integer_type::unsigned_magnitude, cpp_int_check_type::unchecked, void>>
        integral_type;
    typedef number<cpp_int_backend<state_bits, state_bits, cpp_integer_type::unsigned_magnitude, cpp_int_check_type::unchecked, void>>
        state_type;

    constexpr static integral_type mod = 0x73eda753299d7d483339d80809a1d80553bda402fffe5bfeffffffff00000001_cppui255;

    constexpr void generate_round_constants() {
        integral_type constant = 0x0_cppui255;
        state_type lfsr_state = get_lfsr_init_state();

        for (std::size_t i = 0; i < (full_rounds + part_rounds) * t; i++) {
            while (true) {
                constant = 0x0_cppui255;
                for (std::size_t i = 0; i < modulus_bits; i++) {
                    lfsr_state = update_state(lfsr_state);
                    constant = set_new_bit<integral_type>(constant, get_state_bit(lfsr_state, state_bits - 1));
                }
                if (constant < 0x73eda753299d7d483339d80809a1d80553bda402fffe5bfeffffffff00000001_cppui255) {
                    constants[i] = constant;
                    break;
                }
            }
        }
    }

    constexpr void generate_round_constants_unfolded() {
        integral_type constant = 0x0_cppui255;
        bool new_bit = false;
        state_type lfsr_state = get_lfsr_init_state();

        for (std::size_t i = 0; i < (full_rounds + part_rounds) * t; i++) {
            while (true) {
                constant = 0x0_cppui255;
                for (std::size_t i = 0; i < modulus_bits; i++) {
                    while (true) {
                        new_bit = ((lfsr_state & (0x1_cppui80 << (state_bits - 1))) != 0) !=
                                  ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 13))) != 0) !=
                                  ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 23))) != 0) !=
                                  ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 38))) != 0) !=
                                  ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 51))) != 0) !=
                                  ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 62))) != 0);
                        lfsr_state = (lfsr_state << 1) | (new_bit ? 1 : 0);
                        if (new_bit)
                            break;
                        else {
                            new_bit = ((lfsr_state & (0x1_cppui80 << (state_bits - 1))) != 0) !=
                                      ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 13))) != 0) !=
                                      ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 23))) != 0) !=
                                      ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 38))) != 0) !=
                                      ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 51))) != 0) !=
                                      ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 62))) != 0);
                            lfsr_state = (lfsr_state << 1) | (new_bit ? 1 : 0);
                        }
                    }
                    new_bit = ((lfsr_state & (0x1_cppui80 << (state_bits - 1))) != 0) !=
                              ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 13))) != 0) !=
                              ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 23))) != 0) !=
                              ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 38))) != 0) !=
                              ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 51))) != 0) !=
                              ((lfsr_state & (0x1_cppui80 << (state_bits - 1 - 62))) != 0);
                    lfsr_state = (lfsr_state << 1) | (new_bit ? 1 : 0);
                    constant = (constant << 1) | (lfsr_state & 1);
                }
                if (constant < 0x73eda753299d7d483339d80809a1d80553bda402fffe5bfeffffffff00000001_cppui255) {
                    constants[i] = constant;
                    break;
                }
            }
        }
    }

    constexpr static integral_type get_round_constant(std::size_t constant_number) {
        integral_type constant = 0x0_cppui255;

        state_type lfsr_state = get_lfsr_init_state();

        // previous constants
        for (std::size_t i = 0; i < constant_number; i++) {
            constant = 0x0_cppui255;
            while (true) {
                constant = 0x0_cppui255;
                for (std::size_t i = 0; i < modulus_bits; i++) {
                    lfsr_state = update_state(lfsr_state);
                    constant = set_new_bit<integral_type>(constant, get_state_bit(lfsr_state, state_bits - 1));
                }
                if (constant < 0x73eda753299d7d483339d80809a1d80553bda402fffe5bfeffffffff00000001_cppui255)
                    break;
            }
        }

        // requested constant
        while (true) {
            constant = 0x0_cppui255;
            for (std::size_t i = 0; i < modulus_bits; i++) {
                lfsr_state = update_state(lfsr_state);
                constant = set_new_bit<integral_type>(constant, get_state_bit(lfsr_state, state_bits - 1));
            }
            if (constant < 0x73eda753299d7d483339d80809a1d80553bda402fffe5bfeffffffff00000001_cppui255)
                break;
        }

        return constant;
    }

    constexpr static state_type get_lfsr_init_state() {
        state_type state = 0x0_cppui80;
        int i = 0;
        for (i = 1; i >= 0; i--)
            state = set_new_bit(state, (1 >> i) & 1); // field - as in filecoin
        for (i = 3; i >= 0; i--)
            state = set_new_bit(state, (1 >> i) & 1); // s-box - as in filecoin
        for (i = 11; i >= 0; i--)
            state = set_new_bit(state, (modulus_bits >> i) & 1);
        for (i = 11; i >= 0; i--)
            state = set_new_bit(state, (t >> i) & 1);
        for (i = 9; i >= 0; i--)
            state = set_new_bit(state, (full_rounds >> i) & 1);
        for (i = 9; i >= 0; i--)
            state = set_new_bit(state, (part_rounds >> i) & 1);
        for (i = 29; i >= 0; i--)
            state = set_new_bit(state, 1);
        // idling
        for (i = 0; i < 160; i++)
            state = update_state_raw(state);
        return state;
    }

    constexpr static state_type update_state(state_type state) {
        while (true) {
            state = update_state_raw(state);
            if (get_state_bit(state, state_bits - 1))
                break;
            else
                state = update_state_raw(state);
        }
        return update_state_raw(state);
    }

    constexpr static state_type update_state_raw(state_type state) {
        bool new_bit = get_state_bit(state, 0) != get_state_bit(state, 13) != get_state_bit(state, 23) !=
                       get_state_bit(state, 38) != get_state_bit(state, 51) != get_state_bit(state, 62);
        return set_new_bit(state, new_bit);
    }

    constexpr static bool get_state_bit(state_type state, std::size_t pos) {
        state_type bit_getter = 0x1_cppui80;
        bit_getter <<= (state_bits - 1 - pos);
        return (state & bit_getter) ? true : false;
    }

    template<typename T>
    constexpr static T set_new_bit(T var, bool new_bit) {
        return (var << 1) | (new_bit ? 1 : 0);
    }

    constexpr round_constants_generator() : constants() {
        // generate_round_constants();
        generate_round_constants_unfolded();

    }

    integral_type constants[(full_rounds + part_rounds) * t];

};

int main() {
    constexpr std::size_t width = 4;
    constexpr std::size_t full_rounds = 8;
    constexpr std::size_t part_rounds = 56;
    typedef round_constants_generator<width, full_rounds, part_rounds> rcg;

    // Add option -fconstexpr-ops-limit=4294967296 to compiler
    constexpr rcg gen;
    for (std::size_t i = 0; i < (8 + 56) * 4; i++)
        cout << gen.constants[i] << '\n';

    return 0;
}


#define BOOST_TEST_MODULE binary_number_test

#include <boost/test/included/unit_test.hpp>

#include "genetic_algorithm/genetic_algorithm.h"

BOOST_AUTO_TEST_CASE(test) {

  using namespace gaus;

  const int target = 27;

  const std::function<double(arma::uvec)> fitness_function =
      [&target](const arma::uvec & genes) {

        const auto powers_of_two = arma::uvec{32, 16, 8, 4, 2, 1};

        const int strength = arma::accu(powers_of_two % genes);

        std::cout << "strength " << strength << std::endl;
        genes.t().print("genes");

        return std::abs(strength - target);
      };

  const auto solution =
      genetic_algorithm::find_solution(fitness_function, 20, arma::SizeMat{6, 1}, 0.8, 0.1);

  const arma::uvec answer = {0, 1, 1, 0, 1, 1};

  BOOST_TEST(arma::approx_equal(solution.genes, answer, "absdiff", 0));

}