// floop23 generated by makeloops.py Thu Jun 30 16:44:56 2011

#include <blitz/vector2.h>
#include <blitz/array.h>
#include <random/uniform.h>
#include <blitz/benchext.h>

#ifdef BZ_HAVE_VALARRAY
 #define BENCHMARK_VALARRAY
#endif

#ifdef BENCHMARK_VALARRAY
#include <valarray>
#endif

BZ_NAMESPACE(blitz)
extern void sink();
BZ_NAMESPACE_END

BZ_USING_NAMESPACE(blitz)
BZ_USING_NAMESPACE(std)

#if defined(BZ_FORTRAN_SYMBOLS_WITH_TRAILING_UNDERSCORES)
 #define floop23_f77 floop23_f77_
 #define floop23_f77overhead floop23_f77overhead_
 #define floop23_f90 floop23_f90_
 #define floop23_f90overhead floop23_f90overhead_
#elif defined(BZ_FORTRAN_SYMBOLS_WITH_DOUBLE_TRAILING_UNDERSCORES)
 #define floop23_f77 floop23_f77__
 #define floop23_f77overhead floop23_f77overhead__
 #define floop23_f90 floop23_f90__
 #define floop23_f90overhead floop23_f90overhead__
#elif defined(BZ_FORTRAN_SYMBOLS_CAPS)
 #define floop23_f77 FLOOP23_F77
 #define floop23_f77overhead FLOOP23_F77OVERHEAD
 #define floop23_f90 FLOOP23_F90
 #define floop23_f90overhead FLOOP23_F90OVERHEAD
#endif

extern "C" {
  void floop23_f77(const int& N, float* x, float* a, float* b, float* c, float* d, float* y);
  void floop23_f77overhead(const int& N, float* x, float* a, float* b, float* c, float* d, float* y);
  void floop23_f90(const int& N, float* x, float* a, float* b, float* c, float* d, float* y);
  void floop23_f90overhead(const int& N, float* x, float* a, float* b, float* c, float* d, float* y);

}

void VectorVersion(BenchmarkExt<int>& bench);
void ArrayVersion(BenchmarkExt<int>& bench);
void ArrayVersion_unaligned(BenchmarkExt<int>& bench);
void ArrayVersion_misaligned(BenchmarkExt<int>& bench);
void ArrayVersion_index(BenchmarkExt<int>& bench);
void doTinyVectorVersion(BenchmarkExt<int>& bench);
void F77Version(BenchmarkExt<int>& bench);
#ifdef FORTRAN_90
void F90Version(BenchmarkExt<int>& bench);
#endif
#ifdef BENCHMARK_VALARRAY
void ValarrayVersion(BenchmarkExt<int>& bench);
#endif

const int numSizes = 80;
const bool runvector=false; // no point as long as Vector is Array<1>

int main()
{
    int numBenchmarks = 5;
    if (runvector) numBenchmarks++;
#ifdef BENCHMARK_VALARRAY
    numBenchmarks++;
#endif
#ifdef FORTRAN_90
    numBenchmarks++;
#endif

    BenchmarkExt<int> bench("floop23: $x = $a*$b + $c*$d; $y = $b+$d", numBenchmarks);

    bench.setNumParameters(numSizes);

    Array<int,1> parameters(numSizes);
    Array<long,1> iters(numSizes);
    Array<double,1> flops(numSizes);

    parameters=pow(pow(2.,0.25),tensor::i)+tensor::i;
    flops = 4 * parameters;
    iters = 100000000L / flops;
    iters = where(iters<2, 2, iters);
    cout << iters << endl;

    bench.setParameterVector(parameters);
    bench.setIterations(iters);
    bench.setOpsPerIteration(flops);
    bench.setDependentVariable("flops");
    bench.beginBenchmarking();

    ArrayVersion(bench);
    ArrayVersion_unaligned(bench);
    ArrayVersion_misaligned(bench);
    ArrayVersion_index(bench);
    //doTinyVectorVersion(bench);
    F77Version(bench);
#ifdef FORTRAN_90
    F90Version(bench);
#endif
#ifdef BENCHMARK_VALARRAY
    ValarrayVersion(bench);
#endif

    if(runvector)
      VectorVersion(bench);

    bench.endBenchmarking();

    bench.saveMatlabGraph("floop23.m");
    return 0;
}

template<class T>
void initializeRandomDouble(T* data, int numElements, int stride = 1)
{
    ranlib::Uniform<T> rnd;

    for (int i=0; i < numElements; ++i)
        data[size_t(i*stride)] = rnd.random();
}

template<class T>
void initializeRandomDouble(valarray<T>& data, int numElements, int stride = 1)
{
    ranlib::Uniform<T> rnd;

    for (int i=0; i < numElements; ++i)
        data[size_t(i*stride)] = rnd.random();
}

void VectorVersion(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Vector<T>");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

        Vector<float> x(N);
        initializeRandomDouble(x.data(), N);
        Vector<float> a(N);
        initializeRandomDouble(a.data(), N);
        Vector<float> b(N);
        initializeRandomDouble(b.data(), N);
        Vector<float> c(N);
        initializeRandomDouble(c.data(), N);
        Vector<float> d(N);
        initializeRandomDouble(d.data(), N);
        Vector<float> y(N);
        initializeRandomDouble(y.data(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x = a*b + c*d; y = b+d;
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

  void ArrayVersion(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Array<T,1>");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

        Array<float,1> x(N);
        initializeRandomDouble(x.dataFirst(), N);
        Array<float,1> a(N);
        initializeRandomDouble(a.dataFirst(), N);
        Array<float,1> b(N);
        initializeRandomDouble(b.dataFirst(), N);
        Array<float,1> c(N);
        initializeRandomDouble(c.dataFirst(), N);
        Array<float,1> d(N);
        initializeRandomDouble(d.dataFirst(), N);
        Array<float,1> y(N);
        initializeRandomDouble(y.dataFirst(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x = a*b + c*d; y = b+d;
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

  void ArrayVersion_index(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Array<T,1> (indexexpr.)");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

        Array<float,1> x(N);
        initializeRandomDouble(x.dataFirst(), N);
        Array<float,1> a(N);
        initializeRandomDouble(a.dataFirst(), N);
        Array<float,1> b(N);
        initializeRandomDouble(b.dataFirst(), N);
        Array<float,1> c(N);
        initializeRandomDouble(c.dataFirst(), N);
        Array<float,1> d(N);
        initializeRandomDouble(d.dataFirst(), N);
        Array<float,1> y(N);
        initializeRandomDouble(y.dataFirst(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x = a(tensor::i)*b(tensor::i) + c(tensor::i)*d(tensor::i); y = b(tensor::i)+d(tensor::i);;
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

  void ArrayVersion_unaligned(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Array<T,1> (unal.)");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

    Array<float,1> xfill(N+1);
    Array<float,1> x(xfill(Range(1,N)));
    initializeRandomDouble(x.dataFirst(), N);

    Array<float,1> afill(N+1);
    Array<float,1> a(afill(Range(1,N)));
    initializeRandomDouble(a.dataFirst(), N);

    Array<float,1> bfill(N+1);
    Array<float,1> b(bfill(Range(1,N)));
    initializeRandomDouble(b.dataFirst(), N);

    Array<float,1> cfill(N+1);
    Array<float,1> c(cfill(Range(1,N)));
    initializeRandomDouble(c.dataFirst(), N);

    Array<float,1> dfill(N+1);
    Array<float,1> d(dfill(Range(1,N)));
    initializeRandomDouble(d.dataFirst(), N);

    Array<float,1> yfill(N+1);
    Array<float,1> y(yfill(Range(1,N)));
    initializeRandomDouble(y.dataFirst(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x = a*b + c*d; y = b+d;
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

  void ArrayVersion_misaligned(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Array<T,1> (misal.)");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

    Array<float,1> xfill(N+6);
    Array<float,1> x(xfill(Range(0,N+0-1)));
    initializeRandomDouble(x.dataFirst(), N);

    Array<float,1> afill(N+6);
    Array<float,1> a(afill(Range(1,N+1-1)));
    initializeRandomDouble(a.dataFirst(), N);

    Array<float,1> bfill(N+6);
    Array<float,1> b(bfill(Range(2,N+2-1)));
    initializeRandomDouble(b.dataFirst(), N);

    Array<float,1> cfill(N+6);
    Array<float,1> c(cfill(Range(3,N+3-1)));
    initializeRandomDouble(c.dataFirst(), N);

    Array<float,1> dfill(N+6);
    Array<float,1> d(dfill(Range(4,N+4-1)));
    initializeRandomDouble(d.dataFirst(), N);

    Array<float,1> yfill(N+6);
    Array<float,1> y(yfill(Range(5,N+5-1)));
    initializeRandomDouble(y.dataFirst(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x = a*b + c*d; y = b+d;
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

#ifdef BENCHMARK_VALARRAY
void ValarrayVersion(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("valarray<T>");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        cout << bench.currentImplementation() << ": N = " << N << endl;

        long iters = bench.getIterations();

        valarray<float> x(N);
        initializeRandomDouble(x, N);
        valarray<float> a(N);
        initializeRandomDouble(a, N);
        valarray<float> b(N);
        initializeRandomDouble(b, N);
        valarray<float> c(N);
        initializeRandomDouble(c, N);
        valarray<float> d(N);
        initializeRandomDouble(d, N);
        valarray<float> y(N);
        initializeRandomDouble(y, N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x = a*b + c*d; y = b+d;
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
	  sink();
	}
        bench.stopOverhead();
    }

    bench.endImplementation();
}
#endif

void F77Version(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Fortran 77");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        cout << bench.currentImplementation() << ": N = " << N << endl;

        int iters = bench.getIterations();

        float* x = new float[N];
        initializeRandomDouble(x, N);
        float* a = new float[N];
        initializeRandomDouble(a, N);
        float* b = new float[N];
        initializeRandomDouble(b, N);
        float* c = new float[N];
        initializeRandomDouble(c, N);
        float* d = new float[N];
        initializeRandomDouble(d, N);
        float* y = new float[N];
        initializeRandomDouble(y, N);

        bench.start();
        for (int iter=0; iter < iters; ++iter)
            floop23_f77(N, x, a, b, c, d, y);
        bench.stop();

        bench.startOverhead();
        for (int iter=0; iter < iters; ++iter)
            floop23_f77overhead(N, x, a, b, c, d, y);

        bench.stopOverhead();

        delete [] x;
        delete [] a;
        delete [] b;
        delete [] c;
        delete [] d;
        delete [] y;

    }

    bench.endImplementation();
}

#ifdef FORTRAN_90
void F90Version(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Fortran 90");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        cout << bench.currentImplementation() << ": N = " << N << endl;

        int iters = bench.getIterations();

        float* x = new float[N];
        initializeRandomDouble(x, N);
        float* a = new float[N];
        initializeRandomDouble(a, N);
        float* b = new float[N];
        initializeRandomDouble(b, N);
        float* c = new float[N];
        initializeRandomDouble(c, N);
        float* d = new float[N];
        initializeRandomDouble(d, N);
        float* y = new float[N];
        initializeRandomDouble(y, N);

        bench.start();
        for (int iter=0; iter < iters; ++iter)
            floop23_f90(N, x, a, b, c, d, y);
        bench.stop();

        bench.startOverhead();
        for (int iter=0; iter < iters; ++iter)
            floop23_f90overhead(N, x, a, b, c, d, y);

        bench.stopOverhead();
        delete [] x;
        delete [] a;
        delete [] b;
        delete [] c;
        delete [] d;
        delete [] y;

    }

    bench.endImplementation();
}
#endif


// This file is part of PolyMPC, a lightweight C++ template library
// for real-time nonlinear optimization and optimal control.
//
// Copyright (C) 2020 Listov Petr <petr.listov@epfl.ch>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "gtest/gtest.h"
#include "solvers/box_admm.hpp"
#include "solvers/qp_preconditioners.hpp"
#include <Eigen/IterativeLinearSolvers>

TEST(ADMMSolverTest, box_admmSimpleQP)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 2,2> H;
    Eigen::Matrix<Scalar, 2,1> h;
    Eigen::Matrix<Scalar, 1,2> A;
    Eigen::Matrix<Scalar, 1,1> Al;
    Eigen::Matrix<Scalar, 1,1> Au;
    Eigen::Matrix<Scalar, 2,1> xl, xu, solution;

    H << 4, 1,
         1, 2;
    h << 1, 1;
    A << 1, 1;
    Al << 1;
    Au << 1;
    xl << 0, 0;
    xu << 0.7, 0.7;
    solution << 0.3, 0.7;

    boxADMM<2, 1, Scalar> prob;
    prob.settings().max_iter = 150;

    prob.solve(H,h,A,Al,Au,xl,xu);
    Eigen::Vector2d sol = prob.primal_solution();

    EXPECT_TRUE(sol.isApprox(solution, 1e-2));
    EXPECT_LT(prob.iter, prob.settings().max_iter);
    EXPECT_EQ(prob.info().status, status_t::SOLVED);
}

TEST(ADMMSolverTest, box_admmRuizEquilibration)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 2,2> H;
    Eigen::Matrix<Scalar, 2,1> h;
    Eigen::Matrix<Scalar, 1,2> A;
    Eigen::Matrix<Scalar, 1,1> Al;
    Eigen::Matrix<Scalar, 1,1> Au;
    Eigen::Matrix<Scalar, 2,1> xl, xu, solution;

    H << 4, 1,
         1, 2;
    h << 1, 1;
    A << 1, 1;
    Al << 1;
    Au << 1;
    xl << 0, 0;
    xu << 0.7, 0.7;
    solution << 0.3, 0.7;

    boxADMM<2, 1, Scalar> prob;
    prob.settings().max_iter = 150;

    polympc::RuizEquilibration<Scalar, 2, 1> preconditioner;
    preconditioner.compute(H, h, A, Al, Au, xl, xu);

    prob.solve(H, h, A, Al, Au, xl, xu);
    Eigen::Vector2d sol      = prob.primal_solution();
    Eigen::Vector3d sol_dual = prob.dual_solution();

    preconditioner.unscale(sol, sol_dual);

    EXPECT_TRUE(sol.isApprox(solution, 1e-2));
    EXPECT_LT(prob.iter, prob.settings().max_iter);
    EXPECT_EQ(prob.info().status, status_t::SOLVED);
}

TEST(ADMMSolverTest, box_admmSinglePrecisionFloat)
{
    using Scalar = float;

    Eigen::Matrix<Scalar, 2,2> H;
    Eigen::Matrix<Scalar, 2,1> h;
    Eigen::Matrix<Scalar, 1,2> A;
    Eigen::Matrix<Scalar, 1,1> Al;
    Eigen::Matrix<Scalar, 1,1> Au;
    Eigen::Matrix<Scalar, 2,1> xl, xu, solution;

    H << 4, 1,
         1, 2;
    h << 1, 1;
    A << 1, 1;
    Al << 1;
    Au << 1;
    xl << 0, 0;
    xu << Scalar(0.7), Scalar(0.7);
    solution << Scalar(0.3), Scalar(0.7);

    boxADMM<2, 1, Scalar> prob;
    prob.settings().max_iter = 150;

    prob.solve(H,h,A,Al,Au,xl,xu);
    Eigen::Vector2f sol = prob.primal_solution();

    EXPECT_TRUE(sol.isApprox(solution, Scalar(1e-2)));
    EXPECT_LT(prob.iter, prob.settings().max_iter);
    EXPECT_EQ(prob.info().status, status_t::SOLVED);
}

TEST(ADMMSolverTest, box_admmConstraintViolation)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 2,2> H;
    Eigen::Matrix<Scalar, 2,1> h;
    Eigen::Matrix<Scalar, 1,2> A;
    Eigen::Matrix<Scalar, 1,1> Al;
    Eigen::Matrix<Scalar, 1,1> Au;
    Eigen::Matrix<Scalar, 2,1> xl, xu, solution;

    H << 4, 1,
         1, 2;
    h << 1, 1;
    A << 1, 1;
    Al << 1;
    Au << 1;
    xl << 0, 0;
    xu << 0.7, 0.7;
    solution << 0.3, 0.7;

    boxADMM<2, 1, Scalar> prob;

    prob.settings().eps_rel = 1e-4;
    prob.settings().eps_abs = 1e-4;

    prob.solve(H,h,A,Al,Au,xl,xu);
    Eigen::Vector2d sol = prob.primal_solution();

    // check feasibility (with some epsilon margin)
    Eigen::Vector3d lower;
    Eigen::Vector3d upper;
    lower.segment<1>(0) = A * sol - Al;
    upper.segment<1>(0) = A * sol - Au;
    lower.segment<2>(1) = sol - xl;
    upper.segment<2>(1) = sol - xu;
    EXPECT_GE(lower.minCoeff(), -1e-3);
    EXPECT_LE(upper.maxCoeff(), 1e-3);
}

TEST(ADMMSolverTest, box_admmAdaptiveRho)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 2,2> H;
    Eigen::Matrix<Scalar, 2,1> h;
    Eigen::Matrix<Scalar, 1,2> A;
    Eigen::Matrix<Scalar, 1,1> Al;
    Eigen::Matrix<Scalar, 1,1> Au;
    Eigen::Matrix<Scalar, 2,1> xl, xu, solution;

    H << 4, 1,
         1, 2;
    h << 1, 1;
    A << 1, 1;
    Al << 1;
    Au << 1;
    xl << 0, 0;
    xu << 0.7, 0.7;
    solution << 0.3, 0.7;

    boxADMM<2, 1, Scalar> prob;

    prob.settings().adaptive_rho = false;
    prob.settings().adaptive_rho_interval = 10;

    prob.solve(H,h,A,Al,Au,xl,xu);
    Eigen::Vector2d sol = prob.primal_solution();

    EXPECT_EQ(prob.info().status, SOLVED);
}

TEST(ADMMSolverTest, box_admmLox)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 2,2> H;
    Eigen::Matrix<Scalar, 2,1> h;
    Eigen::Matrix<Scalar, 1,2> A;
    Eigen::Matrix<Scalar, 1,1> Al;
    Eigen::Matrix<Scalar, 1,1> Au;
    Eigen::Matrix<Scalar, 2,1> xl, xu, solution;

    H << 4, 1,
         1, 2;
    h << 1, 1;
    A << 1, 1;
    Al << 1;
    Au << 1;
    xl << 0, 0;
    xu << 0.7, 0.7;
    solution << 0.3, 0.7;

    boxADMM<2, 1, Scalar> prob;

    prob.settings().warm_start = false;
    prob.settings().max_iter = 1000;
    prob.settings().rho = 0.1;

    // solve whithout adaptive rho
    prob.settings().adaptive_rho = false;
    prob.solve(H,h,A,Al,Au,xl,xu);
    int prev_iter = prob.info().iter;

    // solve with adaptive rho
    prob.settings().adaptive_rho = true;
    prob.settings().adaptive_rho_interval = 10;
    prob.solve(H,h,A,Al,Au,xl,xu);

    auto info = prob.info();
    EXPECT_LT(info.iter, prob.settings().max_iter);
    EXPECT_LT(info.iter, prev_iter); // adaptive rho should improve :)
    EXPECT_EQ(info.status, SOLVED);
}

#ifdef EIGEN_NO_DEBUG
TEST(ADMMSolverTest, box_admmConjugateGradientLinearSolver)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 2,2> H;
    Eigen::Matrix<Scalar, 2,1> h;
    Eigen::Matrix<Scalar, 1,2> A;
    Eigen::Matrix<Scalar, 1,1> Al;
    Eigen::Matrix<Scalar, 1,1> Au;
    Eigen::Matrix<Scalar, 2,1> xl, xu, solution;

    H << 4, 1,
         1, 2;
    h << 1, 1;
    A << 1, 1;
    Al << 1;
    Au << 1;
    xl << 0, 0;
    xu << 0.7, 0.7;
    solution << 0.3, 0.7;

    boxADMM<2,1,double, DENSE, Eigen::ConjugateGradient, Eigen::Lower | Eigen::Upper> prob;
    prob.solve(H,h,A,Al,Au,xl,xu);
    Eigen::Vector2d sol = prob.primal_solution();

    auto info = prob.info();
    EXPECT_TRUE(sol.isApprox(solution, 1e-2));
    EXPECT_EQ(info.status, SOLVED);
    EXPECT_LT(info.iter, prob.settings().max_iter); // convergence test
}
#endif

TEST(ADMMSolverTest, box_admmSimpleLP)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 1,1> H;
    Eigen::Matrix<Scalar, 1,1> h;
    Eigen::Matrix<Scalar, 0,1> A;
    Eigen::Matrix<Scalar, 0,1> al;
    Eigen::Matrix<Scalar, 0,1> au;
    Eigen::Matrix<Scalar, 1,1> xl,xu;
    Eigen::Matrix<Scalar, 1,1> solution;

    H << 0;
    h << 1;
    xl << -1e6;
    xu <<  1e6;
    solution << -1e6;

    boxADMM<1, 0, Scalar> prob;

    prob.settings().max_iter = 200;
    prob.settings().alpha = 1.0;
    prob.settings().adaptive_rho = true;
    prob.settings().check_termination = 10;

    prob.solve(H,h,A,al,au,xl,xu);
    Eigen::VectorXd sol = prob.primal_solution();

    EXPECT_TRUE(sol.isApprox(solution, 1e-2));
    EXPECT_LT(prob.iter, prob.settings().max_iter);
    EXPECT_EQ(prob.info().status, SOLVED);
}

TEST(ADMMSolverTest, box_admmNonConvex)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 1,1> H;
    Eigen::Matrix<Scalar, 1,1> h;
    Eigen::Matrix<Scalar, 0,1> A;
    Eigen::Matrix<Scalar, 0,1> al;
    Eigen::Matrix<Scalar, 0,1> au;
    Eigen::Matrix<Scalar, 1,1> xl,xu;
    Eigen::Matrix<Scalar, 1,1> solution, guess, dual_guess;

    H << -1;
    h << 0;
    xl << -1;
    xu <<  2;
    solution << 2;
    guess << 0.1;
    dual_guess << 0.1;

    boxADMM<1, 0, Scalar> prob;

    prob.settings().max_iter = 200;
    prob.settings().alpha = 1.0;
    prob.settings().adaptive_rho = true;
    prob.settings().rho = 2;
    prob.settings().check_termination = 10;

    prob.solve(H,h,A,al,au,xl,xu, guess, dual_guess);
    Eigen::VectorXd sol = prob.primal_solution();

    EXPECT_TRUE(sol.isApprox(solution, 1e-2));
    EXPECT_LT(prob.iter, prob.settings().max_iter);
    EXPECT_EQ(prob.info().status, SOLVED);
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_POSMAX_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_POSMAX_HPP_INCLUDED

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

 /*!

    @ingroup group-reduction
    Function object implementing posmax capabilities

    Returns the index of the first occurence of greatest element of the SIMD vector

    @par Semantic:

    For every parameter of type T

    @code
    std::size_t r = posmax(x);
    @endcode

    is similar to:

    @code
      scalar_of_t<T> m =  maximum(x);
      std::size_t r;
      for(std::size_t i=0; i < cardinal_of<T>; i++)
        if (m == x[i]) { r = i;  break; }
    @endcode

  **/
  const boost::dispatch::functor<tag::posmax_> posmax = {};
} }
#endif

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

#endif


#define BOOST_TEST_MODULE EffectiveSampleSizeTestSuite

#include <boost/test/included/unit_test.hpp>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <vector>
#include <hops/hops.hpp>

BOOST_AUTO_TEST_SUITE(EffectiveSampleSizeTestSuite)
    BOOST_AUTO_TEST_CASE(ComputeOneDimSingleChain) {
        // evaluated using stan implementation
        std::vector<Eigen::VectorXd> draws = {
            0*Eigen::VectorXd::Ones(1),
            1*Eigen::VectorXd::Ones(1),
            3*Eigen::VectorXd::Ones(1),
            2*Eigen::VectorXd::Ones(1),
            4*Eigen::VectorXd::Ones(1),
            2*Eigen::VectorXd::Ones(1),
            1*Eigen::VectorXd::Ones(1),
            6*Eigen::VectorXd::Ones(1)
        };
        std::vector<std::vector<Eigen::VectorXd>> chains = {draws};

        double expectedResult = 7.22472;
        double actualResult = hops::computeEffectiveSampleSize(chains, 0);

        BOOST_CHECK_CLOSE(expectedResult, actualResult, 0.01);
    }

    BOOST_AUTO_TEST_CASE(ComputeMultiDimSingleChain) {
        // evaluated using stan implementation
        auto statesMatrix = hops::CsvReader::readMatrix<Eigen::MatrixXd>("../../resources/test_states/states0.csv");

        std::vector<Eigen::VectorXd> draws(statesMatrix.rows());

        for (long j = 0; j < statesMatrix.rows(); ++j) {
            draws[j] = statesMatrix.row(j);
        }

        std::vector<std::vector<Eigen::VectorXd>> chains = {draws};

        std::vector<double> expectedResult = {9.66671, 3.70636, 3.06245, 3.13602, 5.25293};
        std::vector<double> actualResult = hops::computeEffectiveSampleSize(chains);

        for (long d = 0; d < draws[0].size(); ++d) {
            BOOST_CHECK_CLOSE(expectedResult[d], actualResult[d], 0.01);
        }
    }

    BOOST_AUTO_TEST_CASE(ComputeMultiDimMultiChain) {
        // evaluated using stan implementation
        std::vector<std::vector<Eigen::VectorXd>> chains;

        for (unsigned i = 0; i < 4; ++i) {
            auto statesMatrix = hops::CsvReader::readMatrix<Eigen::MatrixXd>(
                    std::string("../../resources/test_states/states") + std::to_string(i) + ".csv");

            std::vector<Eigen::VectorXd> draws(statesMatrix.rows());

            for (long j = 0; j < statesMatrix.rows(); ++j) {
                draws[j] = statesMatrix.row(j);
            }

            chains.push_back(draws);
        }

        std::vector<double> expectedResult = {2.5219, 2.45149, 2.5539, 2.4103, 2.50195};
        std::vector<double> actualResult = hops::computeEffectiveSampleSize(chains);

        for (long d = 0; d < chains[0][0].size(); ++d) {
            BOOST_CHECK_CLOSE(expectedResult[d], actualResult[d], 20);
        }
    }

    BOOST_AUTO_TEST_CASE(ComputeMultiDimMultiChainPointerArray) {
        unsigned numChains = 4;
        std::vector<std::vector<Eigen::VectorXd>> chains(numChains);
        std::vector<const std::vector<Eigen::VectorXd>*> chainsPtrArray(numChains);

        for (unsigned i = 0; i < numChains; ++i) {
            auto statesMatrix = hops::CsvReader::readMatrix<Eigen::MatrixXd>(
                    std::string("../../resources/test_states/states") + std::to_string(i) + ".csv");

            std::vector<Eigen::VectorXd> draws(statesMatrix.rows());

            for (long j = 0; j < statesMatrix.rows(); ++j) {
                draws[j] = statesMatrix.row(j);
            }

            chains[i] = draws;
            chainsPtrArray[i] = &chains[i];
        }

        std::vector<double> expectedResult = {2.5219, 2.45149, 2.5539, 2.4103, 2.50195};
        std::vector<double> actualResult = hops::computeEffectiveSampleSize(chainsPtrArray);

        for (long d = 0; d < (*chainsPtrArray[0])[0].size(); ++d) {
            BOOST_CHECK_CLOSE(expectedResult[d], actualResult[d], 20);
        }
    }

    BOOST_AUTO_TEST_CASE(ComputeMultiDimMultiChainLarge) {
        // evaluated using stan implementation
        std::vector<std::vector<Eigen::VectorXd>> chains;

        for (unsigned i = 0; i < 4; ++i) {
            auto statesMatrix = hops::CsvReader::readMatrix<Eigen::MatrixXd>(
                    std::string("../../resources/test_states/chain") + std::to_string(i) + "_states.csv");

            std::vector<Eigen::VectorXd> draws(statesMatrix.rows());

            for (long j = 0; j < statesMatrix.rows(); ++j) {
                draws[j] = statesMatrix.row(j);
            }

            chains.push_back(draws);
        }

        std::vector<double> expectedResult = {10259.3, 10600.8, 10670.2, 10097.2, 10382.7};
        std::vector<double> actualResult = hops::computeEffectiveSampleSize(chains);

        for (long d = 0; d < chains[0][0].size(); ++d) {
            BOOST_CHECK_CLOSE(expectedResult[d], actualResult[d], 1);
        }
    }

    BOOST_AUTO_TEST_CASE(ComputeMultiDimMultiChainLargePointerArray) {
        unsigned numChains = 4;
        std::vector<std::vector<Eigen::VectorXd>> chains(numChains);
        std::vector<const std::vector<Eigen::VectorXd>*> chainsPtrArray(numChains);

        for (unsigned i = 0; i < numChains; ++i) {
            auto statesMatrix = hops::CsvReader::readMatrix<Eigen::MatrixXd>(
                    std::string("../../resources/test_states/chain") + std::to_string(i) + "_states.csv");

            std::vector<Eigen::VectorXd> draws(statesMatrix.rows());

            for (long j = 0; j < statesMatrix.rows(); ++j) {
                draws[j] = statesMatrix.row(j);
            }

            chains[i] = draws;
            chainsPtrArray[i] = &chains[i];
        }

        std::vector<double> expectedResult = {10259.3, 10600.8, 10670.2, 10097.2, 10382.7};
        std::vector<double> actualResult = hops::computeEffectiveSampleSize(chainsPtrArray);

        for (long d = 0; d < (*chainsPtrArray[0])[0].size(); ++d) {
            BOOST_CHECK_CLOSE(expectedResult[d], actualResult[d], 1);
        }
    }
BOOST_AUTO_TEST_SUITE_END()


/**
@file

Copyright John Reid 2007
*/

#include "bio-pch.h"

#include "bio/application.h"
#include "bio/markov_model.h"
#include "bio/fasta.h"
USING_BIO_NS

#include <boost/filesystem/path.hpp>
#include <boost/spirit/include/classic_file_iterator.hpp>
using namespace boost;
using namespace BOOST_SPIRIT_CLASSIC_NS;
namespace po = boost::program_options;
namespace fs = boost::filesystem;

#include <vector>
#include <strstream>
#include <fstream>
using namespace std;

//typedef BOOST_SPIRIT_CLASSIC_NS::file_iterator<> file_it;
typedef file_iterator<> file_it;

/**
 * Learns Markov models from the sequences in a FASTA file.
 */
struct FastaMarkovModelsApp : Application
{
	typedef std::vector< std::string > filename_vec_t;

	bool sorted;
	filename_vec_t input_filenames;

	FastaMarkovModelsApp()
	{
		get_options().add_options()
			("sorted", po::value(&sorted)->default_value(true), "sort markov model counts")
		    ("input-file", po::value(&input_filenames), "input file")
			;

		get_positional_options().add("input-file", -1);
	}

	int task()
	{

		DnaSymbolAlphabet alphabet;
		MarkovModel< 0 > zeroth_order_mm(4);
		MarkovModel< 1 > first_order_mm(4);
		MarkovModel< 2 > second_order_mm(4);
		MarkovModel< 3 > third_order_mm(4);

		for (filename_vec_t::const_iterator f = input_filenames.begin();
			input_filenames.end() != f;
			++f)
		{
			//open the file
			file_it file_start(*f);
			if (! file_start) {
				throw BIO_MAKE_STRING("Could not open " << *f);
			}

			//find the end of the file
			file_it file_end = file_start.make_end();

			//skip first line of fasta format
			file_it seq_start = file_start;
			while (*seq_start != '\n') {
				++seq_start;
			}
			++seq_start;

			std::cout << *f << "\n";

			zeroth_order_mm.add_to_counts(
				file_start,
				file_end,
				alphabet);
			first_order_mm.add_to_counts(
				file_start,
				file_end,
				alphabet);
#if 0
			second_order_mm.add_to_counts(
				file_start,
				file_end,
				alphabet);
			third_order_mm.add_to_counts(
				file_start,
				file_end,
				alphabet);
#endif

		}

		std::cout << "\n";
		zeroth_order_mm.print(alphabet, sorted); std::cout << "\n";
		first_order_mm.print(alphabet, sorted); std::cout << "\n";
		//second_order_mm.print(alphabet, sorted); std::cout << "\n";
		//third_order_mm.print(alphabet, sorted); std::cout << "\n";

		return 0;
	}

};

int
main(int argc, char * argv[])
{
	return FastaMarkovModelsApp().main(argc, argv);
}


/**
 Svi is written by Yue Ariel Wu
 UCLA
*/
#include <fstream>
#include <iostream>
#include <string>
#include <stdlib.h>
#include <vector>
#include <iomanip>

//#include <random>

#include <bits/stdc++.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <Eigen/QR>
#include "time.h"

#include "boost/numeric/ublas/matrix.hpp"
#include "boost/numeric/ublas/io.hpp"
#include "boost/random.hpp"
#include "boost/numeric/ublas/lu.hpp"
#include "boost/numeric/ublas/vector.hpp"
#include "boost/numeric/ublas/vector_proxy.hpp"
#include "boost/numeric/ublas/triangular.hpp"

#include "genotype.h"
#include "mailman.h"
#include "arguments.h"
#include "helper.h"
#include "storage.h"

#if SSE_SUPPORT==1
	#define fastmultiply fastmultiply_sse
	#define fastmultiply_pre fastmultiply_pre_sse
#else
	#define fastmultiply fastmultiply_normal
	#define fastmultiply_pre fastmultiply_pre_normal
#endif
namespace ukblas =boost::numeric::ublas;
using namespace Eigen;
using namespace std;

// Storing in RowMajor Form
typedef Matrix<double, Dynamic, Dynamic, RowMajor> MatrixXdr;

//Intermediate Variables
int blocksize;
double *partialsums;
double *sum_op;
double *yint_e;
double *yint_m;
double **y_e;
double **y_m;
//
//
struct timespec t0;

MatrixXdr covariate;
MatrixXdr pheno;
genotype g;
MatrixXdr geno_matrix; //(p,n)
int MAX_ITER;
int k,p,n;
int k_orig;

MatrixXdr c;
MatrixXdr x;
MatrixXdr v;
MatrixXdr means; //(p,1)
MatrixXdr stds; //(p,1)
MatrixXdr sum2;
MatrixXdr sum;

options command_line_opts;

bool debug = false;
bool check_accuracy = false;
bool var_normalize=true;
int accelerated_em=0;
double convergence_limit;
bool memory_efficient = false;
bool missing=false;
bool fast_mode = true;
bool text_version = false;
bool use_cov=false;
bool reg = true;
bool gwas=false;

vector<string> pheno_name;

std::istream& newline(std::istream& in)
{
    if ((in >> std::ws).peek() != std::char_traits<char>::to_int_type('\n')) {
        in.setstate(std::ios_base::failbit);
    }
    return in.ignore();
}
int read_cov(bool std,int Nind, std::string filename, std::string covname){
	ifstream ifs(filename.c_str(), ios::in);
	std::string line;
	std::istringstream in;
	int covIndex = 0;
	std::getline(ifs,line);
	in.str(line);
	string b;
	vector<vector<int> > missing;
	int covNum=0;
	while(in>>b)
	{
		if(b!="FID" && b !="IID"){
		missing.push_back(vector<int>()); //push an empty row
		if(b==covname && covname!="")
			covIndex=covNum;
		covNum++;
		}
	}
	vector<double> cov_sum(covNum, 0);
	if(covname=="")
	{
		covariate.resize(Nind, covNum);
		cout<< "Read in "<<covNum << " Covariates.. "<<endl;
	}
	else
	{
		covariate.resize(Nind, 1);
		cout<< "Read in covariate "<<covname<<endl;
	}

	int j=0;
	while(std::getline(ifs, line)){
		in.clear();
		in.str(line);
		string temp;
		in>>temp; in>>temp; //FID IID
		for(int k=0; k<covNum; k++){

			in>>temp;
			if(temp=="NA")
			{
				missing[k].push_back(j);
				continue;
			}
			double cur = atof(temp.c_str());
			if(cur==-9)
			{
				missing[k].push_back(j);
				continue;
			}
			if(covname=="")
			{
				cov_sum[k]= cov_sum[k]+ cur;
				covariate(j,k) = cur;
			}
			else
				if(k==covIndex)
				{
					covariate(j, 0) = cur;
					cov_sum[k] = cov_sum[k]+cur;
				}
		}
		j++;
	}
	//compute cov mean and impute
	for (int a=0; a<covNum ; a++)
	{
		int missing_num = missing[a].size();
		cov_sum[a] = cov_sum[a] / (Nind - missing_num);

		for(int b=0; b<missing_num; b++)
		{
                        int index = missing[a][b];
                        if(covname=="")
                                covariate(index, a) = cov_sum[a];
                        else if (a==covIndex)
                                covariate(index, 0) = cov_sum[a];
                }
	}
	if(std)
	{
		MatrixXdr cov_std;
		cov_std.resize(1,covNum);
		MatrixXdr sum = covariate.colwise().sum();
		MatrixXdr sum2 = (covariate.cwiseProduct(covariate)).colwise().sum();
		MatrixXdr temp;
//		temp.resize(Nind, 1);
//		for(int i=0; i<Nind; i++)
//			temp(i,0)=1;
		for(int b=0; b<covNum; b++)
		{
			cov_std(0,b) = sum2(0,b) + Nind*cov_sum[b]*cov_sum[b]- 2*cov_sum[b]*sum(0,b);
			cov_std(0,b) =sqrt((Nind-1)/cov_std(0,b)) ;
			double scalar=cov_std(0,b);
			for(int j=0; j<Nind; j++)
			{
				covariate(j,b) = covariate(j,b)-cov_sum[b];
				covariate(j,b) =covariate(j,b)*scalar;
			}
			//covariate.col(b) = covariate.col(b) -temp*cov_sum[b];

		}
	}
	return covNum;
}
int read_pheno2(int Nind, std::string filename,int pheno_idx){
//	pheno.resize(Nind,1);
	ifstream ifs(filename.c_str(), ios::in);

	std::string line;
	std::istringstream in;
	int phenocount=0;
	vector<vector<int> > missing;
//read header
	std::getline(ifs,line);
	in.str(line);
	string b;
	while(in>>b)
	{
		if(b!="FID" && b !="IID"){
			phenocount++;
			missing.push_back(vector<int>());
			pheno_name.push_back(b);
		}
	}
	if(pheno_idx !=0)
		pheno_name[0] = pheno_name[pheno_idx-1];
	vector<double> pheno_sum(phenocount,0);
	if(pheno_idx !=0)
		pheno.resize(Nind,1);
	else
		pheno.resize(Nind, phenocount);
	int i=0;
	while(std::getline(ifs, line)){
		in.clear();
		in.str(line);
		string temp;
		//fid,iid
		//todo: fid iid mapping;
		//todo: handle missing phenotype
		in>>temp; in>>temp;
		for(int j=0; j<phenocount;j++) {
			in>>temp;
			if(temp=="NA")
			{
				missing[j].push_back(i);
				continue;
			}
			double cur= atof(temp.c_str());
			if(pheno_idx==0){
				pheno(i,j)=cur;
				pheno_sum[j] = pheno_sum[j]+cur;
			}
			else if(j == (pheno_idx-1))
			{
				pheno(i,0)=cur;
				pheno_sum[j]= pheno_sum[j]+cur;
			}
		}
		i++;
	}
	for(int a=0; a<phenocount; a++)
	{
		int missing_num= missing[a].size();
		double	pheno_avg = pheno_sum[a]/(Nind- missing_num);
		//cout<<"pheno "<<a<<" avg: "<<pheno_avg<<endl;
		for(int b=0 ; b<missing_num; b++)
		{
			int index = missing[a][b];
			pheno(index, a)= pheno_avg;
		}
		if(pheno_idx!=0)
			return 1;
	}
	return phenocount;
}

void multiply_y_pre_fast(MatrixXdr &op, int Ncol_op ,MatrixXdr &res,bool subtract_means){

	for(int k_iter=0;k_iter<Ncol_op;k_iter++){
		sum_op[k_iter]=op.col(k_iter).sum();
	}

			//cout << "Nops = " << Ncol_op << "\t" <<g.Nsegments_hori << endl;
	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Starting mailman on premultiply"<<endl;
			cout << "Nops = " << Ncol_op << "\t" <<g.Nsegments_hori << endl;
			cout << "Segment size = " << g.segment_size_hori << endl;
			cout << "Matrix size = " <<g.segment_size_hori<<"\t" <<g.Nindv << endl;
			cout << "op = " <<  op.rows () << "\t" << op.cols () << endl;
		}
	#endif

	//TODO: Memory Effecient SSE FastMultipy

	for(int seg_iter=0;seg_iter<g.Nsegments_hori-1;seg_iter++){
		mailman::fastmultiply(g.segment_size_hori,g.Nindv,Ncol_op,g.p[seg_iter],op,yint_m,partialsums,y_m);
		int p_base = seg_iter*g.segment_size_hori;
		for(int p_iter=p_base; (p_iter<p_base+g.segment_size_hori) && (p_iter<g.Nsnp) ; p_iter++ ){
			for(int k_iter=0;k_iter<Ncol_op;k_iter++)
				res(p_iter,k_iter) = y_m[p_iter-p_base][k_iter];
		}
	}

	int last_seg_size = (g.Nsnp%g.segment_size_hori !=0 ) ? g.Nsnp%g.segment_size_hori : g.segment_size_hori;
	mailman::fastmultiply(last_seg_size,g.Nindv,Ncol_op,g.p[g.Nsegments_hori-1],op,yint_m,partialsums,y_m);
	int p_base = (g.Nsegments_hori-1)*g.segment_size_hori;
	for(int p_iter=p_base; (p_iter<p_base+g.segment_size_hori) && (p_iter<g.Nsnp) ; p_iter++){
		for(int k_iter=0;k_iter<Ncol_op;k_iter++)
			res(p_iter,k_iter) = y_m[p_iter-p_base][k_iter];
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Ending mailman on premultiply"<<endl;
		}
	#endif

	if(!subtract_means)
		return;

	for(int p_iter=0;p_iter<p;p_iter++){
 		for(int k_iter=0;k_iter<Ncol_op;k_iter++){
			res(p_iter,k_iter) = res(p_iter,k_iter) - (g.get_col_mean(p_iter)*sum_op[k_iter]);
			if(var_normalize)
				res(p_iter,k_iter) = res(p_iter,k_iter)/(g.get_col_std(p_iter));
 		}
 	}

}

void multiply_y_post_fast(MatrixXdr &op_orig, int Nrows_op, MatrixXdr &res,bool subtract_means){

	MatrixXdr op;
	op = op_orig.transpose();

	if(var_normalize && subtract_means){
		for(int p_iter=0;p_iter<p;p_iter++){
			for(int k_iter=0;k_iter<Nrows_op;k_iter++)
				op(p_iter,k_iter) = op(p_iter,k_iter) / (g.get_col_std(p_iter));
		}
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Starting mailman on postmultiply"<<endl;
		}
	#endif

	int Ncol_op = Nrows_op;

	//cout << "ncol_op = " << Ncol_op << endl;

	int seg_iter;
	for(seg_iter=0;seg_iter<g.Nsegments_hori-1;seg_iter++){
		mailman::fastmultiply_pre(g.segment_size_hori,g.Nindv,Ncol_op, seg_iter * g.segment_size_hori, g.p[seg_iter],op,yint_e,partialsums,y_e);
	}
	int last_seg_size = (g.Nsnp%g.segment_size_hori !=0 ) ? g.Nsnp%g.segment_size_hori : g.segment_size_hori;
	mailman::fastmultiply_pre(last_seg_size,g.Nindv,Ncol_op, seg_iter * g.segment_size_hori, g.p[seg_iter],op,yint_e,partialsums,y_e);

	for(int n_iter=0; n_iter<n; n_iter++)  {
		for(int k_iter=0;k_iter<Ncol_op;k_iter++) {
			res(k_iter,n_iter) = y_e[n_iter][k_iter];
			y_e[n_iter][k_iter] = 0;
		}
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Ending mailman on postmultiply"<<endl;
		}
	#endif

	if(!subtract_means)
		return;

	double *sums_elements = new double[Ncol_op];
 	memset (sums_elements, 0, Nrows_op * sizeof(int));

 	for(int k_iter=0;k_iter<Ncol_op;k_iter++){
 		double sum_to_calc=0.0;
 		for(int p_iter=0;p_iter<p;p_iter++)
 			sum_to_calc += g.get_col_mean(p_iter)*op(p_iter,k_iter);
 		sums_elements[k_iter] = sum_to_calc;
 	}
 	for(int k_iter=0;k_iter<Ncol_op;k_iter++){
 		for(int n_iter=0;n_iter<n;n_iter++)
 			res(k_iter,n_iter) = res(k_iter,n_iter) - sums_elements[k_iter];
 	}

}

void multiply_y_pre_naive_mem(MatrixXdr &op, int Ncol_op ,MatrixXdr &res){
	for(int p_iter=0;p_iter<p;p_iter++){
		for(int k_iter=0;k_iter<Ncol_op;k_iter++){
			double temp=0;
			for(int n_iter=0;n_iter<n;n_iter++)
				temp+= g.get_geno(p_iter,n_iter,var_normalize)*op(n_iter,k_iter);
			res(p_iter,k_iter)=temp;
		}
	}
}

void multiply_y_post_naive_mem(MatrixXdr &op, int Nrows_op ,MatrixXdr &res){
	for(int n_iter=0;n_iter<n;n_iter++){
		for(int k_iter=0;k_iter<Nrows_op;k_iter++){
			double temp=0;
			for(int p_iter=0;p_iter<p;p_iter++)
				temp+= op(k_iter,p_iter)*(g.get_geno(p_iter,n_iter,var_normalize));
			res(k_iter,n_iter)=temp;
		}
	}
}

void multiply_y_pre_naive(MatrixXdr &op, int Ncol_op ,MatrixXdr &res){
	res = geno_matrix * op;
}

void multiply_y_post_naive(MatrixXdr &op, int Nrows_op ,MatrixXdr &res){
	res = op * geno_matrix;
}

void multiply_y_post(MatrixXdr &op, int Nrows_op ,MatrixXdr &res,bool subtract_means){
    if(fast_mode)
        multiply_y_post_fast(op,Nrows_op,res,subtract_means);
    else{
		if(memory_efficient)
			multiply_y_post_naive_mem(op,Nrows_op,res);
		else
			multiply_y_post_naive(op,Nrows_op,res);
	}
}

void multiply_y_pre(MatrixXdr &op, int Ncol_op ,MatrixXdr &res,bool subtract_means){
    if(fast_mode)
        multiply_y_pre_fast(op,Ncol_op,res,subtract_means);
    else{
		if(memory_efficient)
			multiply_y_pre_naive_mem(op,Ncol_op,res);
		else
			multiply_y_pre_naive(op,Ncol_op,res);
	}
}

int main(int argc, char const *argv[]){

	pair<double,double> prev_error = make_pair(0.0,0.0);
	double prevnll=0.0;

	parse_args(argc,argv);

	//TODO: Memory effecient Version of Mailman

	memory_efficient = command_line_opts.memory_efficient;
	text_version = command_line_opts.text_version;
	fast_mode = command_line_opts.fast_mode;
	missing = command_line_opts.missing;
	reg = command_line_opts.reg;
	gwas=command_line_opts.gwas;
	if(gwas)
		fast_mode=false; //for now, gwas need the genotype matrix, and compute kinship constructed with one chrom leave out
	if(!reg)
		fast_mode=false; //force save whole genome if non randomized
	if(text_version){
		if(fast_mode)
			g.read_txt_mailman(command_line_opts.GENOTYPE_FILE_PATH,missing);
		else
			g.read_txt_naive(command_line_opts.GENOTYPE_FILE_PATH,missing);
	}
	else{
		g.read_plink(command_line_opts.GENOTYPE_FILE_PATH,missing,fast_mode);

	}

	//TODO: Implement these codes.
	if(missing && !fast_mode){
		cout<<"Missing version works only with mailman i.e. fast mode\n EXITING..."<<endl;
		exit(-1);
	}
	if(fast_mode && memory_efficient){
		cout<<"Memory effecient version for mailman EM not yet implemented"<<endl;
		cout<<"Ignoring Memory effecient Flag"<<endl;
	}
	if(missing && var_normalize){
		cout<<"Missing version works only without variance normalization\n EXITING..."<<endl;
		exit(-1);
	}

	MAX_ITER=100;
    //	MAX_ITER =  command_line_opts.max_iterations ;
	int B = command_line_opts.batchNum;
	int pheno_idx = command_line_opts.pheno_idx;
	k_orig = command_line_opts.num_of_evec ;
	debug = command_line_opts.debugmode ;
	float tr2= command_line_opts.tr2;
	check_accuracy = command_line_opts.getaccuracy;
	var_normalize = true;
	accelerated_em = command_line_opts.accelerated_em;
	k = k_orig + command_line_opts.l;
	k = (int)ceil(k/10.0)*10;
	command_line_opts.l = k - k_orig;
	p = g.Nsnp;
	n = g.Nindv;
	bool toStop=false;
		toStop=true;
	srand((unsigned int) time(0));

	means.resize(p,1);
	stds.resize(p,1);
	sum2.resize(p,1);
	sum.resize(p,1);

//	geno_matrix.resize(p,n);
//	g.generate_eigen_geno(geno_matrix, var_normalize);

	if(!fast_mode && !memory_efficient){
		geno_matrix.resize(p,n);
//		g.generate_eigen_geno(geno_matrix,var_normalize);
		g.generate_eigen_geno(geno_matrix, false);
		cout<<geno_matrix.data()<<endl;
		cout<<geno_matrix.rows();
		cout<<geno_matrix.cols();

	}

	for(int i=0;i<p;i++){
		means(i,0) = g.get_col_mean(i);
		stds(i,0) =1/ g.get_col_std(i);
		sum2(i,0) =g.get_col_sum2(i);
		sum(i,0)= g.get_col_sum(i);
	}

//	cout<<"printing means: "<<endl<<means<<endl;
//	cout<<"printing std: "<<endl<<stds<<endl;
	ofstream c_file;

	cout<<"Running on Dataset of "<<g.Nsnp<<" SNPs and "<<g.Nindv<<" Individuals"<<endl;

	#if SSE_SUPPORT==1
		if(fast_mode)
			cout<<"Using Optimized SSE FastMultiply"<<endl;
	#endif

	//get geno
	//cout<<g.get_geno(0,0,false);
	//read phenotype
	//
	//
	std::string filename=command_line_opts.PHENOTYPE_FILE_PATH;
	int pheno_num= read_pheno2(g.Nindv, filename, pheno_idx);
	int cov_num=0 ;
	if(filename=="")
	{
		cout<<"No Phenotype File Specified"<<endl;
		return 0 ;
	}
	cout<< "Read in "<<pheno_num << " phenotypes"<<endl;
	if(pheno_idx!=0)
		cout<<"Using phenotype "<<pheno_name[pheno_idx-1]<<endl;

	MatrixXdr y_sum=pheno.colwise().sum();
	MatrixXdr y_mean = y_sum/g.Nindv;
	for(int i=0; i<g.Nindv; i++)
		pheno.block(i,0,1,pheno_num) =pheno.block(i,0,1,pheno_num) - y_mean; //center phenotype
	y_sum=pheno.colwise().sum();

//model
	float hyper[4];
	for(int i=0; i<4; i++)
		hyper[i]=1e-3;
	float tol = 1e-6;
	MatrixXdr beta = MatrixXdr::Constant(g.Nsnp, 1, 0);
	MatrixXdr betaSigma = MatrixXdr::Constant(g.Nsnp, g.Nsnp, 1);

	//exponential distribution
	boost::mt19937 gen;
	boost::exponential_distribution<> dist(1);
	boost::variate_generator<boost::mt19937&, boost::exponential_distribution<> > init(gen,dist);

	float a=init();
	float l=init();
	a=2.575836; l=0.1306393;
	cout<<"Initializing prarameters..."<<endl<<"a: "<<a <<endl<<"l: "<<l <<endl;

	float rho;
	int iter=0;

	float ll_det=1;
	float prevll=0 ;
//	while(iter <= MAX_ITER){
	while(iter<= MAX_ITER && ll_det > tol){
		rho = a/l;
		MatrixXdr temp = MatrixXdr::Identity(g.Nsnp,g.Nsnp);
		MatrixXdr betaSigmaInv = geno_matrix * geno_matrix.transpose() + rho* temp;
		betaSigma =betaSigmaInv.inverse();

		beta = geno_matrix* pheno;
		beta = betaSigma * beta;
		betaSigma = betaSigma / l;

		MatrixXdr A = betaSigma * geno_matrix;
		MatrixXdr B = geno_matrix.transpose()*A;

		MatrixXdr temp_vec = geno_matrix.transpose() * beta;
		temp_vec = pheno - temp_vec;
		double temp_l = temp_vec.squaredNorm(); //TODO: check with R code if doing desired operation
		l = 0.5 * (temp_l + B.trace() )+ hyper[3];
		l = l/ (0.5 * g.Nindv + hyper[2] -1);
		l = 1/l;

		double temp_a = beta.squaredNorm();
		a=0.5*(betaSigma.trace()+ temp_a) + hyper[1];
		a=a/(0.5*g.Nsnp + hyper[0]-1);
		a = 1/a;

		//compute ELBO

		float ll = -(0.5 * g.Nindv + hyper[2]-1) + (0.5 *g.Nindv + hyper[2]-1)*std::log(l) - (0.5* g.Nsnp + hyper[0]-1) + (0.5*g.Nsnp + hyper[0]-1)*std::log(a) + 0.5*std::log(betaSigma.trace());
		if(iter>0)
			ll_det = ll - prevll;
		prevll = ll;
		cout<<iter<< ": ELBO: "<<ll<<"   ";

		float s2g = g.Nsnp/a;
		float s2e = 1/l;
		float h2g = s2g/ (s2g + s2e);
//		cout<<"V(G): "<<s2g<<endl;
//	        cout<<"V(E): "<<s2e<<endl;
        	cout<<"h2g: "<<h2g<<endl;
		iter++;

}
	return 0;
}


#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

#include "../secp256k1/include/secp256k1_scalar.hpp"
#include "../secp256k1/include/secp256k1_group.hpp"

BOOST_AUTO_TEST_SUITE(sigma_primitive_types)

BOOST_AUTO_TEST_CASE(scalar_test)
{
    // Create a large scalar value.
    secp_primitives::Scalar s(123456789);
    s *= s;
    s *= s;
    s *= s;
    s *= s;
    s *= s;
    secp_primitives::Scalar s2(s);

    // Make sure that copy constructor works correctly.
    BOOST_CHECK(s == s2);
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/shared_ptr.hpp>
#include <lcm/lcm-cpp.hpp>

#include <ConciseArgs>
#include <zlib.h>
#include <lcmtypes/multisense.hpp>

#include <multisense_image_utils/multisense_image_utils.hpp>

#include <chrono>

using namespace std;

struct CommandLineConfig
{
    float depthThresh;
    int sizeThresh;
    std::string input;
    std::string output;
};

class App{
  public:
    App(boost::shared_ptr<lcm::LCM> &lcm_, const CommandLineConfig& cl_cfg_);

    ~App(){
    }
  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    const CommandLineConfig cl_cfg_;

    void msHandler(const lcm::ReceiveBuffer* rbuf,
                           const std::string& channel, const  multisense::images_t* msg);

    multisense_image_utils miu_;
};

App::App(boost::shared_ptr<lcm::LCM> &lcm_, const CommandLineConfig& cl_cfg_):
    lcm_(lcm_), cl_cfg_(cl_cfg_){
  lcm_->subscribe( cl_cfg_.input ,&App::msHandler,this);
}

void App::msHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  multisense::images_t* msg){
  int n_bytes =2;
  int width =msg->images[0].width;
  int height = msg->images[0].height;

  // 1. Uncompress Depth Image:
  std::vector<uint8_t> buf(width*height*n_bytes);
  unsigned long len = buf.size();
  uncompress(buf.data(), &len, msg->images[1].data.data(), msg->images[1].data.size());
  cv::Mat disparityMat = cv::Mat( height, width, CV_16UC1, buf.data()).clone();

  // 2. Remove small components (parameters used here only for easy mapping)
  float k00 = 591.909423828125; // focal length x;
  float baseline = 0.0700931567882511;
  float mDisparityFactor = 1/k00/baseline;
  auto t1 = std::chrono::high_resolution_clock::now();
  //uint16_t thresh = 16/mDisparityFactor/cl_cfg_.depthThresh;
  //miu_.removeSmall(disparityMat, thresh, cl_cfg_.sizeThresh);
  double thresh =
    std::abs(16*k00*baseline*(1.0/2 - 1.0/(2+cl_cfg_.depthThresh)));
  miu_.removeSpeckles(disparityMat, thresh, cl_cfg_.sizeThresh);
  auto t2 = std::chrono::high_resolution_clock::now();
  auto dt = std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1);
  std::cout << "processed frame in " << dt.count() << " ms" << std::endl;

  // 3. Recompress:
  int uncompressed_size = width*height*n_bytes;
  unsigned long compressed_size = width* height* sizeof(int16_t) * 4;;
  std::vector<uint8_t> buf2(width*height*4);
  compress2(buf2.data() , &compressed_size, (const Bytef*) disparityMat.data, uncompressed_size,
            Z_BEST_SPEED);

  // 4. Publish
  multisense::image_t image;
  image.utime =msg->utime;
  image.width =width;
  image.height=height;
  image.row_stride =n_bytes*width; // this is useless if doing zip
  image.pixelformat = multisense::image_t::PIXEL_FORMAT_RGB;// This label will be invalid...
  image.size = (int) compressed_size;
  image.data = buf2;
  image.nmetadata =0;
  //image.metadata = NULL;

  multisense::images_t out;
  out.utime =msg->utime;
  out.n_images = 2;
  out.image_types = msg->image_types;
  out.images.push_back( msg->images[0] );
  out.images.push_back( image );

  lcm_->publish( cl_cfg_.output, &out);
}

int main(int argc, char ** argv) {
  CommandLineConfig cl_cfg;
  cl_cfg.depthThresh = 100.0f; //m
  cl_cfg.sizeThresh = 4000; // pixels
  cl_cfg.input = "CAMERA";
  cl_cfg.output = "CAMERA_OUT";
  ConciseArgs opt(argc, (char**)argv);
  opt.add(cl_cfg.depthThresh, "d", "depthThresh","depth in m from camera");
  opt.add(cl_cfg.sizeThresh, "s", "sizeThresh","pixel size of cluster to remove");
  opt.add(cl_cfg.input, "i", "input","Camera input channel");
  opt.add(cl_cfg.output, "o", "output","Camera output channel");
  opt.parse();

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good()){
    std::cerr <<"ERROR: lcm is not good()" <<std::endl;
  }

  App app(lcm, cl_cfg);
  cout << "Tool ready" << endl << "============================" << endl;
  while(0 == lcm->handle());
  return 0;
}


#include "LidarImageProjectorPNG.h"

#include <cfloat>
#include <iostream>
#include <boost/filesystem.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include "PngDistanceImage.hpp"
#include "CsvReader.h"

using namespace std;
//using namespace matrixTools;
using namespace boost::filesystem;

typedef boost::numeric::ublas::vector<double>   DVector;
typedef boost::numeric::ublas::matrix<double>   DMatrix;

//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
////////////////////         PNGImageProjector         ///////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
/*!
 * \brief constructor
 * \param cfgFile file holding all needed configuration settings.
 *                either: imgHSize; imgVSize; horizStartAngle(grad); horizStopAngle(grad); vertStartAngle(grad); vertStopAngle(grad)
 *                or: imgHSize; imgVSize; horizStartAngle(grad); horizStopAngle(grad); vertAngle_1; vertAngle_2; ...; vertAngle_n
 * \throws runtime_error if the config file is invalid
 */
PNGImageProjector::PNGImageProjector(string cfgFile)
{
	CsvReader config(cfgFile, ";", "#");
	if (config.lineCount() < 1)
		throw runtime_error("PNGImageProjector: configuration file has no valid line");
	unsigned int nbEntries = config.fieldCount(0);
	if (nbEntries < 6)
		throw runtime_error("PNGImageProjector: configuration file has not enough valid entries");
	double toRAD = M_PI/180.0;
	imgHSize = config.get<unsigned int>(0,0);
	imgVSize = config.get<unsigned int>(0,1);
	hSpacing = NULL;
	vSpacing = NULL;
//	cout << "entries: " << nbEntries;
//	for (unsigned int i=0; i<nbEntries; ++i) cout << " " << i << ":" << config.get(0,i) << flush;
	if (nbEntries == 2+2+2) {
		double horizStartAngleRAD_ = config.get<double>(0,2)*toRAD;
		double horizStopAngleRAD_ = config.get<double>(0,3)*toRAD;
		if (horizStopAngleRAD_ >= horizStartAngleRAD_) throw invalid_argument("PNGImageProjector: horizStopAngleRAD_ >= horizStartAngleRAD_");
		hSpacing = new PNGImageProjector::RegularSpacing(horizStartAngleRAD_, horizStopAngleRAD_, imgHSize);
		double vertStartAngleRAD_ = config.get<double>(0,4)*toRAD;
		double vertStopAngleRAD_ = config.get<double>(0,5)*toRAD;
		if (vertStopAngleRAD_ <= vertStartAngleRAD_) throw invalid_argument("PNGImageProjector: vertStopAngleRAD_ <= vertStartAngleRAD_");
		vSpacing = new PNGImageProjector::RegularSpacing(vertStartAngleRAD_, vertStopAngleRAD_, imgVSize);
	}
	if (nbEntries == 2+2+imgVSize) {
		double horizStartAngleRAD_ = config.get<double>(0,2)*toRAD;
		double horizStopAngleRAD_ = config.get<double>(0,3)*toRAD;
		if (horizStopAngleRAD_ >= horizStartAngleRAD_)
			throw invalid_argument("PNGImageProjector: horizStopAngleRAD_ >= horizStartAngleRAD_");
		horizOpeningAngleRAD = horizStartAngleRAD_ - horizStopAngleRAD_;
		hSpacing = new PNGImageProjector::RegularSpacing(horizStartAngleRAD_, horizStopAngleRAD_, imgHSize);
		vector<double> vertAngles; vertAngles.reserve(imgVSize);
		for (unsigned int i=4; i<nbEntries; ++i)
			vertAngles.push_back(config.get<double>(0,i)*toRAD);
		vertOpeningAngleRAD = vertAngles[0] - vertAngles[nbEntries-1];
		vSpacing = new PNGImageProjector::IrregularSpacing(vertAngles.begin(), vertAngles.end(), imgVSize);
	}
	if (hSpacing == NULL || vSpacing == NULL)
		throw runtime_error("PNGImageProjector: configuration file has unexpected number of entries");

	setupSphere();
//	horizStartAngleRAD = config.get<double>(0,2)*toRAD;
	//horizStartAngle(grad); horizStopAngle(grad); vertStartAngle(grad); vertStopAngle(grad)
}

/*!
 * \brief constructor for a regular lattice.

		horizontal angle increases to the left, i.e. NOT according to horizontal indexing => horizStopAngleRAD_ < horizStartAngleRAD
		vertical angle increases downward, i.e. according to vertical indexing => vertStopAngleRAD_ > vertStartAngleRAD

 * \param horizStartAngleRAD_ yaw-angle (mathematical ccw angle in radian) of the left border of the first=leftmost image column.
 * \param horizStopAngleRAD_ yaw-angle (mathematical ccw angle in radian) of the right border of the last=rightmost image column.
 * \param vertStartAngleRAD_ pitch-angle (mathematical ccw angle in radian) of the upper border of the first=uppermost image row.
 * \param vertStopAngleRAD_ pitch-angle (mathematical ccw angle in radian) of the lower border of the last=lowermost image row.
 * \param imgHSize_ horizontal image size in pixel
 * \param imgVSize_ vertical image size in pixel
 * \throws invalid_argument if (horizStopAngleRAD_ >= horizStartAngleRAD_) or (vertStopAngleRAD_ <= vertStartAngleRAD_)
 */
PNGImageProjector::PNGImageProjector(double horizStartAngleRAD_, double horizStopAngleRAD_, double vertStartAngleRAD_, double vertStopAngleRAD_, unsigned int imgHSize_, unsigned int imgVSize_)
	: imgHSize(imgHSize_)
	 ,imgVSize(imgVSize_)
{
	if (horizStopAngleRAD_ >= horizStartAngleRAD_) throw invalid_argument("PNGImageProjector: horizStopAngleRAD_ >= horizStartAngleRAD_");
	if (vertStopAngleRAD_ <= vertStartAngleRAD_) throw invalid_argument("PNGImageProjector: vertStopAngleRAD_ <= vertStartAngleRAD_");

	horizOpeningAngleRAD = horizStartAngleRAD_ - horizStopAngleRAD_;
	vertOpeningAngleRAD = vertStartAngleRAD_ - vertStopAngleRAD_;
	hSpacing = new PNGImageProjector::RegularSpacing(horizStartAngleRAD_, horizStopAngleRAD_, imgHSize);
	vSpacing = new PNGImageProjector::RegularSpacing(vertStartAngleRAD_, vertStopAngleRAD_, imgVSize);
	setupSphere();
}

void YawPitch_2_Rot( const double yaw, const double pitch, DMatrix &R)
{
  double c_a = cos( yaw );
  double s_a = sin( yaw );
  double c_b = cos( pitch );
  double s_b = sin( pitch );

  R.resize( 3, 3 ); //(row,col)

  R(0, 0) = c_a * c_b;
  R(1, 0) = s_a * c_b;
  R(2, 0) = -s_b;

  R(0, 1) = -s_a;
  R(1, 1) = c_a;
  R(2, 1) = 0.0;

  R(0, 2) = c_a * s_b;
  R(1, 2) = s_a * s_b;
  R(2, 2) = c_b;
}

void PNGImageProjector::setupSphere()
{
	cout << "  allocating 3x" << imgHSize*imgVSize*sizeof(double)/1024 << "kB..." << flush;
	xUnitSphere = new double[imgHSize*imgVSize];
	yUnitSphere = new double[imgHSize*imgVSize];
	zUnitSphere = new double[imgHSize*imgVSize];
	rotMat = new double[imgHSize*imgVSize*9];

	cout << "initializing..." << flush;
	DVector unity(3,0.); unity[0] = 1.;
	DMatrix Rot;
	DVector pt;
	double yaw,pitch;
	for (unsigned int row=0; row<imgVSize; ++row) {
		for (unsigned int col=0; col<imgHSize; ++col) {
			yaw = hSpacing->getAngleRAD(col);
			pitch = vSpacing->getAngleRAD(row);
			YawPitch_2_Rot(yaw,pitch,Rot);
			pt = boost::numeric::ublas::prod(Rot, unity);
			unsigned int idx = row*imgHSize + col;
			xUnitSphere[idx] = pt[0];
			yUnitSphere[idx] = pt[1];
			zUnitSphere[idx] = pt[2];
			memcpy( &(rotMat[9*idx]), &(Rot.data()[0]), 9*sizeof(double) );
		}
	}
	cout << "done" << endl;
}

PNGImageProjector::~PNGImageProjector()
{
  delete [] rotMat;
	delete [] zUnitSphere;
	delete [] yUnitSphere;
	delete [] xUnitSphere;
	delete vSpacing;
	delete hSpacing;
}

unsigned int PNGImageProjector::getImgHorizSize() const
{
  return imgHSize;
}

unsigned int PNGImageProjector::getImgVertSize() const
{
  return imgVSize;
}

double PNGImageProjector::getImgHorizAngleRAD() const
{
	return horizOpeningAngleRAD;
}

double PNGImageProjector::getImgVertAngleRAD() const
{
	return vertOpeningAngleRAD;
}

bool PNGImageProjector::getImageIndexYP(double yawRAD, double pitchRAD, int &hi, int &vi) const
{
	return (hSpacing->getImageIndex(yawRAD, hi) && hSpacing->getImageIndex(pitchRAD, vi));
}

void PNGImageProjector::get3DCoordRel(int hi, int vi, double dist, double &x, double &y, double &z) const
{
  unsigned int idx = vi*imgHSize + hi;
  x = dist * xUnitSphere[idx];
  y = dist * yUnitSphere[idx];
  z = dist * zUnitSphere[idx];
}

//////////////////////////////////////////////////////////////////////////////
//////////////////  Inner Class: Regular Spacing  ////////////////////////////
//////////////////////////////////////////////////////////////////////////////

PNGImageProjector::RegularSpacing::RegularSpacing(double startAngleRAD_, double stopAngleRAD_, unsigned int nbPix_)
{
	cout << "  set up regular spacing with " << startAngleRAD_ << ".." << stopAngleRAD_ << ", pix " << nbPix_ << endl;
	// normalize starting angle to [0,2pi)
	while (startAngleRAD_ >= 2*M_PI) { startAngleRAD_ -= 2*M_PI; stopAngleRAD_ -= 2*M_PI;}
	while (startAngleRAD_ < 0.0) { startAngleRAD_ += 2*M_PI; stopAngleRAD_ += 2*M_PI;}
	double rangeRAD_ = stopAngleRAD_-startAngleRAD_;

	startAngleRAD = startAngleRAD_;
	rangeRAD = fabs(rangeRAD_);
	rangeSign = (rangeRAD_ < 0) ? -1.0 : 1.0;
	pixSizeRAD = rangeRAD_/(double)nbPix_;
}

bool PNGImageProjector::RegularSpacing::getImageIndex(double angleRAD, int &idx)
{
	// 1) normalize angle
	while (angleRAD < 0) angleRAD += 2*M_PI;
	while (angleRAD >= 2*M_PI) angleRAD -= 2*M_PI;
	// 2) make angle relative
	angleRAD -= startAngleRAD;
	// 3) check angle
	if (angleRAD*rangeSign < 0) return false;
	if (angleRAD*rangeSign > rangeRAD) return false;
	// 4) calculate index
	idx = angleRAD / pixSizeRAD;
	return true;
}

double PNGImageProjector::RegularSpacing::getAngleRAD(int idx)
{
	return startAngleRAD + pixSizeRAD*((double)idx+0.5); // center of pixel
}

//////////////////////////////////////////////////////////////////////////////
//////////////////  Inner Class: Irregular Spacing  //////////////////////////
//////////////////////////////////////////////////////////////////////////////

bool PNGImageProjector::IrregularSpacing::getImageIndex(double angleRAD, int &idx)
{
	if (angleRAD*pixSizeSign < (vAnglesRAD.front()-avgPixSizeRAD)*pixSizeSign) return false;
	if (angleRAD*pixSizeSign > (vAnglesRAD.back()+avgPixSizeRAD)*pixSizeSign) return false;
	idx = 0;
	double oldDst = DBL_MAX;
	while (idx < (int)vAnglesRAD.size()) {
		double cDst = fabs(angleRAD-vAnglesRAD[idx]);
		if (cDst < oldDst) {
			oldDst = cDst;
			++idx;
		}	else
			break;
	}
	idx--;
	return true;
}

double PNGImageProjector::IrregularSpacing::getAngleRAD(int idx)
{
	return vAnglesRAD[idx];
}


#include <string>
#include <unordered_map>

#include <boost/lexical_cast.hpp>

#include <frovedis.hpp>
#include <frovedis/ml/tree/tree.hpp>

double parse(const std::string& line) {
  return boost::lexical_cast<double>(line);
}

int main(int argc, char** argv) {
  frovedis::use_frovedis use(argc, argv);

  auto dataset = frovedis::make_rowmajor_matrix_load<double>("./train.mat");
  auto labels = frovedis::make_dvector_loadline("./train.label").map(parse);

  const size_t num_classes = 3;
  // iris dataset has no categorical feature, so leave this map empty
  std::unordered_map<size_t, size_t> categorical_features_info;
  // frovedis::tree::impurity_type::Entropy is also available
  const auto impurity = frovedis::tree::impurity_type::Gini;
  const size_t max_depth = 5;
  const size_t max_bins = 32;
  const size_t min_instances_per_node = 1;
  const double min_info_gain = 0.0;

  auto model = frovedis::decision_tree::train_classifier<double>(
    dataset, labels,
    num_classes, categorical_features_info,
    impurity, max_depth, max_bins,
    min_instances_per_node, min_info_gain
  );

  std::cout << model << std::endl;
  model.save("./model");

  auto model2 = frovedis::make_tree_by_load<double>("./model");

  auto testers = frovedis::make_rowmajor_matrix_local_load<double>("./test.mat");
  auto results = model2.predict_with_probability(testers);

  std::cout << "-------- Prediction Results:: --------" << std::endl;
  for (const auto result: results) {
    std::cout <<
      result.get_predict() << " (" <<
      result.get_probability() * 100 << "%)" <<
    std::endl;
  }

  return 0;
}


#pragma once
#include "criteria.hpp"
#include <boost/heap/pairing_heap.hpp>
#include <cfloat>
#include <vector>

namespace sssp {

class traff_bridge : public criteria {
  public:
    traff_bridge(const sssp::graph* graph, size_t start_node);
    virtual void relaxable_nodes(todo_output& output) const override;
    virtual void changed_predecessor(size_t node, size_t predecessor, double distance) override;
    virtual void relaxed_node(size_t node) override;
    virtual bool is_complete() const override { return false; }

  private:
    struct node_info;

    struct node_info_compare_distance {
        bool operator()(const node_info* a, const node_info* b) const;
    };

    // tentative(n) - min{ cost(p, n) + min{ cost(p*, p) : p* predecessor of p } : p predecessor of n in U }
    struct node_info_compare_threshold {
        bool operator()(const node_info* a, const node_info* b) const;
    };

    using distance_queue = boost::heap::pairing_heap<node_info*, boost::heap::compare<node_info_compare_distance>>;
    using threshold_queue = boost::heap::pairing_heap<node_info*, boost::heap::compare<node_info_compare_threshold>>;

    struct pred_info {
        pred_info(size_t pred, double cost) : pred(pred), cost(cost) {}
        size_t pred;
        double cost; // cost(p, n) + min{ cost(p*, p) : p* predecessor of p }
    };

    struct node_info {
        node_info(const sssp::graph& graph, size_t index);
        size_t index;
        double tentative = INFINITY;
        bool settled = false;
        std::vector<pred_info> predecessors;
        size_t fringe_predecessors = 0;
        distance_queue::handle_type distance_queue_handle;
        threshold_queue::handle_type threshold_queue_handle;

        double threshold() const;
    };

    node_map<node_info> m_info;
    distance_queue m_distance_queue;
    threshold_queue m_threshold_queue;
};

} // namespace sssp


#include "stdafx.h"
#include <cmath>
#include <iostream>
#include <fmt/format.h>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include "contest_types.h"
#include "solver_registry.h"
#include "visual_editor.h"
#include "solver_util.h"
#include "judge.h"

namespace HopGridAnnealingSolver2 {

namespace bg = boost::geometry;
using BoostPoint = bg::model::d2::point_xy<double>;
using BoostPolygon = bg::model::polygon<BoostPoint>;
using BoostLinestring = bg::model::linestring<BoostPoint>;

template <typename T>
void shrink(T& point_a, T& point_b) {
  constexpr double eps = 1e-6;
  auto shrink_a = point_a * (1.0 - eps) + point_b * eps;
  auto shrink_b = point_b * (1.0 - eps) + point_a * eps;
  point_a = shrink_a;
  point_b = shrink_b;
}

template <typename T>
BoostPoint ToBoostPoint(const T& point) {
  const auto [x, y] = point;
  return BoostPoint(x, y);
}

template <typename T>
BoostPolygon ToBoostPolygon(const std::vector<T>& points) {
  BoostPolygon polygon;
  for (std::size_t i = 0; i <= points.size(); ++i) {
    polygon.outer().push_back(ToBoostPoint(points[i % points.size()]));
  }
  if (bg::area(polygon) < 0.0) {
    bg::reverse(polygon);
  }
  return polygon;
}

template <typename T, typename U>
double SquaredDistance(const T& vertex0, const U& vertex1) {
  const auto [x0, y0] = vertex0;
  const auto [x1, y1] = vertex1;
  return (x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1);
}

template <typename T>
double SquaredEdgeLength(const T& vertices, const Edge& edge) {
  const auto [a, b] = edge;
  return SquaredDistance(vertices[a], vertices[b]);
}

class Solver : public SolverBase {
 public:
  SolverOutputs solve(const SolverArguments& args) override {
    hole_ = args.problem->hole_polygon;
    vertices_ = args.problem->vertices;
    edges_ = args.problem->edges;
    epsilon_ = args.problem->epsilon;
    hole_polygon_ = ToBoostPolygon(hole_);

    SVisualEditorPtr editor;
    if (args.visualize) {
      editor = std::make_shared<SVisualEditor>(args.problem, "HopGridAnnealingSolver2", "visualize");
    }

    bool show(true);
    const int N = vertices_.size();
    auto pose = vertices_;
    double cost = std::numeric_limits<double>::infinity();
    double best_feasible_cost = std::numeric_limits<double>::infinity();
    std::vector<Point> best_feasible_pose;

    SPinnedIndex pinned_index(rng_, N, editor);

    const int num_iters = 1000000;
    const double T0 = 1.0e1;
    const double T1 = 1.0e-2;
    double progress = 0.0;

    integer ymin = INT_MAX, ymax = INT_MIN;
    integer xmin = INT_MAX, xmax = INT_MIN;
    for (auto p : hole_) {
      xmin = std::min(xmin, get_x(p));
      ymin = std::min(ymin, get_y(p));
      xmax = std::max(xmax, get_x(p));
      ymax = std::max(ymax, get_y(p));
    }

    std::vector<std::vector<int>> emat(vertices_.size());
    for (const auto &e : edges_) {
      emat[e.first].emplace_back(e.second);
      emat[e.second].emplace_back(e.first);
    }
    neighbors_.resize(vertices_.size());
    for (int i = 0; i < vertices_.size(); ++i) {
      std::set<int> neighbors;
      for (const auto &v : emat[i]) {
        if (v != i) {
          neighbors.emplace(v);
        }
        for (const auto &v2 : emat[v]) {
          if (v2 != i) {
            neighbors.emplace(v2);
          }
          for (const auto &v3 : emat[v2]) {
            if (v3 != i) {
              neighbors.emplace(v3);
            }
          }
        }
      }
      for (auto v : neighbors) {
        neighbors_[i].emplace_back(v);
      }
    }

    // lesser version of tonagi's idea
//    for (auto& p : pose) { p = hole_[0]; }

    auto evaluate_and_descide_rollback = [&]() -> bool {
      auto [feasible, updated_cost, real_cost] = Evaluate(pose, show);

      // tonagi's idea.
      if (false || !best_feasible_pose.empty()) {
        auto res = judge(*args.problem, pose);
        if (!res.fit_in_hole()) {
          feasible = false;
//          updated_cost = DBL_MAX;
        }
      }

#if 0
      auto judge_valid = judge(*args.problem, pose).is_valid();
      if (feasible != judge_valid) {
        LOG(INFO) << feasible << " " << judge_valid;
        if (editor) {
          editor->set_pose(args.problem->create_solution(pose));
          while (true) {
            int c = editor->show(1);
            if (c == 27) break;
          }
        }
      }
#endif

      if (feasible && real_cost < best_feasible_cost) {
        best_feasible_cost = real_cost;
        best_feasible_pose = pose;
        if (editor) editor->set_persistent_custom_stat(fmt::format("best_cost = {}", best_feasible_cost));
      }
      const double T = std::pow(T0, 1.0 - progress) * std::pow(T1, progress);
      if (std::uniform_real_distribution(0.0, 1.0)(rng_) < std::exp(-(updated_cost - cost) / T)) {
        cost = updated_cost;
        return false; // accepted
      } else {
        return true; // rejected
      }
    };
    evaluate_and_descide_rollback();

    auto single_small_change = [&] { // ynasu87 original
      const int v = pinned_index.sample_movable_index();
      const int dx = std::normal_distribution(0.0, 10.0)(rng_);
      const int dy = std::normal_distribution(0.0, 10.0)(rng_);
      auto& [x, y] = pose[v];
      x += dx;
      y += dy;
      if (evaluate_and_descide_rollback()) {
        x -= dx;
        y -= dy;
      }
    };

    auto shift = [&] {
      const int dx = std::uniform_int_distribution(-1, 1)(rng_);
      const int dy = std::uniform_int_distribution(-1, 1)(rng_);
      auto pose_bak = pose;
      for (int i : pinned_index.movable_indices) {
        pose[i].first += dx;
        pose[i].second += dy;
      }
      if (evaluate_and_descide_rollback()) {
        pose = pose_bak;
      }
    };

    auto slight_rotate = [&] {
      const double deg = std::uniform_real_distribution(-180.0, 180.0)(rng_);
      auto pose_bak = pose;

      integer curr_ymin = INT_MAX, curr_ymax = INT_MIN;
      integer curr_xmin = INT_MAX, curr_xmax = INT_MIN;
      for (auto p : hole_) {
        curr_xmin = std::min(curr_xmin, get_x(p));
        curr_ymin = std::min(curr_ymin, get_y(p));
        curr_xmax = std::max(curr_xmax, get_x(p));
        curr_ymax = std::max(curr_ymax, get_y(p));
      }

      const double cx = double(curr_xmin + curr_xmax) / 2;
      const double cy = double(curr_ymin + curr_ymax) / 2;
      const double sin = std::sin(deg * 3.1415 / 180.0);
      const double cos = std::cos(deg * 3.1415 / 180.0);
      for (int i : pinned_index.movable_indices) {
        auto& p = pose[i];
        const double dx = p.first - cx;
        const double dy = p.second - cy;
        p.first  = std::round(dx * cos - dy * sin + cx);
        p.second = std::round(dx * sin + dy * cos + cy);
      }
      if (evaluate_and_descide_rollback()) {
        pose = pose_bak;
      }
    };

    auto flip = [&] { // from FlipAnnealingSolver
      const int v0 = pinned_index.sample_movable_index();
      const int v1 = pinned_index.sample_movable_index();
      if (v0 == v1) return;
      auto pose_bak = pose;
      auto reflect = [](Point a, Point c, Point v) {
        return v - 2 * double(dot(v - c, a)) / double(dot(a, a)) * a;
      };
      auto diff = pose[v1] - pose[v0];
      Point n = {get_y(diff), -get_x(diff)};
      for (int v2 : pinned_index.movable_indices) {
        if (ccw(pose[v0], pose[v1], pose[v2])) {
          pose[v2] = reflect(n, pose[v0], pose[v2]);
        }
      }
      if (evaluate_and_descide_rollback()) {
        pose = pose_bak;
      }
    };

    auto edges_cache = edges_from_vertex(*args.problem);
    auto hop_grid = [&] { // jump to a tolerated (by at least one edge) point.
      const int pivot = pinned_index.sample_movable_index();
      const auto pivot_bak = pose[pivot];
      const auto& edges = edges_cache[pivot];
      std::map<Point, double> good_pos; // position -> number of good grid vote.
      for (auto eid : edges) {
        auto [u, v] = args.problem->edges[eid];
        const int counter_vid = u == pivot ? v : u;
        const auto org_d2 = distance2(args.problem->vertices[pivot], args.problem->vertices[counter_vid]);
        for (int y = ymin; y <= ymax; ++y) {
          for (int x = xmin; x <= xmax; ++x) {
            const auto moved_d2 = distance2({ x, y }, pose[counter_vid]);
            if (tolerate(org_d2, moved_d2, epsilon_)) {
              Point jump {x, y};
              auto it = good_pos.find(jump);
              if (it == good_pos.end()) {
                good_pos.insert(it, {jump, 1.0});
              } else {
                it->second += 1.0;
              }
            }
          }
        }
      }
      // emphasize large votes.
      double total_votes = 0;
      for (auto& [pos, vote] : good_pos) {
        vote = std::pow(vote, 5);
        total_votes += vote;
      }
      const double select_accum_vote = std::uniform_real_distribution<double>(0.0, total_votes)(rng_);
      double accum_vote = 0;
      for (auto& [pos, vote] : good_pos) {
        accum_vote += vote;
        if (select_accum_vote <= accum_vote) {
          pose[pivot] = pos;
          break;
        }
      }
      if (evaluate_and_descide_rollback()) {
        pose[pivot] = pivot_bak;
      }
    };

    using Action = std::function<void()>;
    std::vector<std::pair<double, Action>> action_probs = {
      {0.98, single_small_change},
      {0.005, slight_rotate},
      {0.005, shift},
      {0.005, hop_grid},
      {0.005, flip},
    };
    {
      // normalize probs
      double p = 0.0;
      for (int i = 0; i < action_probs.size(); ++i) { p += action_probs[i].first; }
      for (int i = 0; i < action_probs.size(); ++i) { action_probs[i].first /= p; }
    }

    for (int iter = 0; iter < num_iters; ++iter) {
      progress = 1.0 * iter / num_iters;

      const double p_action = std::uniform_real_distribution(0.0, 1.0)(rng_);
      double p_accum = 0.0;
      for (int i = 0; i < action_probs.size(); ++i) {
        p_accum += action_probs[i].first;
        if (p_action < p_accum) {
          action_probs[i].second();
        }
      }

      if (iter % 100 == 0) {
        show = true;
        std::cerr << cost << ", " << best_feasible_cost << std::endl;
      } else {
        show = false;
      }
      if (editor && iter % 100 == 0) {
        editor->set_oneshot_custom_stat(fmt::format("iter = {}/{}", iter, num_iters));
        editor->set_pose(args.problem->create_solution(pose));
        if (auto show_result = editor->show(1); show_result.edit_result) {
          pose = show_result.edit_result->pose_after_edit->vertices;
          auto [feasiblecost, updated_cost, real_cost] = Evaluate(pose, true);
          cost = updated_cost;
          pinned_index.update_movable_index();
        }
      }
    }

    SolverOutputs outputs;
    if (best_feasible_pose.empty()) {
      outputs.solution = args.problem->create_solution(pose);
    } else {
      outputs.solution = args.problem->create_solution(best_feasible_pose);
    }
    return outputs;
  }

  template <typename P>
  std::tuple<bool, double, double> Evaluate(const std::vector<P>& pose, bool show) const {
    double deformation_cost = 0.0;
    double protrusion_cost = 0.0;
    double dislikes_cost = 0.0;
    double neighbor_cost = 0.0;

    const double tolerance = epsilon_ / 1'000'000.0;
    int vidx(0);
    for (const auto& vertex : pose) {
      protrusion_cost += bg::distance(ToBoostPoint(vertex), hole_polygon_);
      for (const auto& v : neighbors_[vidx]) {
        neighbor_cost += 1.0 / (SquaredDistance(vertex, pose[v]) + 1e-6);
      }
/*
      for (int i = 0; i < vertices_.size(); ++i) {
        if (i != vidx) {
          neighbor_cost += 1.0 / (SquaredDistance(vertex, pose[i]) + 1e-6);
        }
      }
*/
      ++vidx;
    }
    for (const auto& edge : edges_) {
      const auto [a, b] = edge;
      Point2d pa = pose[a];
      Point2d pb = pose[b];
      shrink(pa, pb);
      BoostLinestring linestring{ToBoostPoint(pa), ToBoostPoint(pb)};
      std::vector<BoostLinestring> differences;
      bg::difference(linestring, hole_polygon_, differences);
      for (const auto& segment : differences) {
        protrusion_cost += bg::length(segment);
      }

      const auto d0 = SquaredEdgeLength(vertices_, edge);
      const auto d1 = SquaredEdgeLength(pose, edge);
      deformation_cost += 1.0e1 * std::max(0.0, std::abs(d1 / d0 - 1.0) - tolerance);
    }

    double max_best(0.0);
    for (const auto h : hole_) {
      double best = std::numeric_limits<double>::infinity();
      for (const auto v : pose) {
        best = std::min(best, SquaredDistance(h, v));
      }
      max_best = std::max(max_best, best);
      dislikes_cost += best * 1.0e-2;
    }
    dislikes_cost += max_best;

    deformation_cost *= 1e2;
    protrusion_cost *= 1e3;
    dislikes_cost *= 1e-1;

    neighbor_cost *= 1e1;

    if (show) {
      std::cerr << max_best << ", " << deformation_cost << ", " << protrusion_cost << ", " << dislikes_cost << ", " << neighbor_cost << std::endl;
    }
    const bool feasible = deformation_cost + protrusion_cost == 0.0;
    const double cost = deformation_cost + protrusion_cost + dislikes_cost + neighbor_cost;

    return {feasible, cost, max_best};
  }

 private:
  std::mt19937 rng_;
  std::vector<Point> hole_;
  std::vector<Point> vertices_;
  std::vector<Edge> edges_;
  std::vector<std::vector<int>> neighbors_;
  integer epsilon_;
  BoostPolygon hole_polygon_;
};

}

REGISTER_SOLVER("HopGridAnnealingSolver2", HopGridAnnealingSolver2::Solver);
// vim:ts=2 sw=2 sts=2 et ci


//==================================================================================================
/*!
  @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_ARCH_COMMON_SIMD_FUNCTION_ERFCX_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ERFCX_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/arch/common/detail/generic/erf_kernel.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/erfc.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/logical_andnot.hpp>
#include <boost/simd/function/nbtrue.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/sign.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF ( erfcx_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::double_<A0>, X>
                          )
  {
   A0 operator() (const A0& x) const BOOST_NOEXCEPT
    {
      A0 y =  bs::abs(x);
      A0 lim1 = Ratio<A0, 15, 32>();
      A0 lim2 = A0(4);
      A0 res1(0), res2(0);
      auto test1 = (y <=lim1);
      std::size_t nb = bs::nbtrue(test1);
      if(nb > 0)
      {
        A0 ysq;
        A0 tmp = detail::erf_kernel1<A0>::erf1(x, y, ysq);
        res1 = oneminus(tmp)*exp(ysq);
        if (nb >= A0::static_size)
          return res1;
      }
      auto test2 = bs::is_less(x, lim2);
      auto test3 = bs::logical_andnot(test2, test1);
      std::size_t nb1 = bs::nbtrue(test3);
      if(nb1 > 0)
      {
        res2  = detail::erf_kernel1<A0>::erf2(x, y);
        A0 resb = res2;
        detail::erf_kernel1<A0>::finalize3(resb, x);
        res2 = if_else(is_ltz(x), resb, res2);
        nb += nb1;
        res2 = bs::if_else(test1, res1, res2);
        if (nb >= A0::static_size)
          return res2;
      }
      else
      {
        res2 =  res1;
      }
      auto test4 = logical_not(logical_or(test1, test2));
      A0 res3 = detail::erf_kernel1<A0>::erf3(x, y);
      A0 res3b = res3;
      detail::erf_kernel1<A0>::finalize3(res3b, x);
      res3 =  if_else (is_ltz(x), res3b, res3);
      return if_nan_else(is_nan(x), if_else(test4, res3, res2));
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( erfcx_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::single_<A0>, X>
                          )
  {
    A0 operator() (const A0& a0) const BOOST_NOEXCEPT
    {

      auto test1 = bs::is_less(a0,  Ratio<A0, 2, 3>());
      std::size_t nb = bs::nbtrue(test1);
      A0 r2 = Zero<A0>(), r1 = Zero<A0>();
      if(nb > 0)
      {
        r1 = expx2(a0)*erfc(a0);
      }
      if(nb < A0::static_size)
      {
        A0 z =  a0/inc(a0) - Ratio<A0, 2, 5>();
        r2 = detail::erf_kernel<A0>::erfc2(z);
        r2 = if_else(is_ltz(r2), bs::rsqrt(bs::Pi<A0>())/a0, r2);
#ifndef BOOST_SIMD_NO_INFINITIES
        r2 = if_else(is_nan(a0), a0, if_zero_else(a0 == Inf<A0>(), r2));
#endif
      }
      return if_else(test1, r1, r2);
    }

  };

} } }

#endif