/* =========================================================================
   Copyright (c) 2010-2012, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

//#define VIENNACL_DEBUG_ALL
//#define VIENNACL_DEBUG_BUILD
//#define VIENNACL_DEBUG_BUILD
//
// *** System
//
#include <iostream>

//
// *** Boost
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/foreach.hpp>

//
// *** ViennaCL
//
//#define VIENNACL_DEBUG_BUILD
#define VIENNACL_WITH_UBLAS
//#define VIENNACL_DEBUG_ALL

#include "viennacl/matrix.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/linalg/inner_prod.hpp"
#include "viennacl/linalg/reduce.hpp"
#include "viennacl/linalg/vector_operations.hpp"
//#include "viennacl/linalg/norm_1.hpp"
//#include "viennacl/linalg/norm_2.hpp"
//#include "viennacl/linalg/norm_inf.hpp"
#include "viennacl/scheduler/io.hpp"

#define CHECK_RESULT(cpu,gpu, op) \
    if ( float delta = fabs ( diff ( cpu, gpu) ) > epsilon ) {\
        std::cout << "# Error at operation: " #op << std::endl;\
        std::cout << "  diff: " << delta << std::endl;\
        retval = EXIT_FAILURE;\
    }\

using namespace boost::numeric;
using namespace viennacl;

template<typename ScalarType, typename VCLMatrixType>
ScalarType diff(ublas::matrix<ScalarType> & mat1, VCLMatrixType & mat2)
{
    ublas::matrix<ScalarType> mat2_cpu(mat2.size1(), mat2.size2());
    viennacl::backend::finish();
    viennacl::copy(mat2, mat2_cpu);
    double ret = 0;
    double act = 0;
    for (unsigned int i = 0; i < mat2_cpu.size1(); ++i)
    {
      for (unsigned int j = 0; j < mat2_cpu.size2(); ++j)
      {
         act = fabs(mat2_cpu(i,j) - mat1(i,j)) / std::max( fabs(mat2_cpu(i, j)), fabs(mat1(i,j)) );
         if (act > ret)
           ret = act;
      }
    }
    //std::cout << ret << std::endl;
    return ret;
}

template<typename ScalarType, unsigned int Alignment>
ScalarType diff ( ublas::vector<ScalarType> & v1, viennacl::vector<ScalarType,Alignment> & v2 ) {
    ublas::vector<ScalarType> v2_cpu ( v2.size() );
    viennacl::copy( v2.begin(), v2.end(), v2_cpu.begin() );
    for ( unsigned int i=0; i<v1.size(); ++i ) {
        if ( std::max ( fabs ( v2_cpu[i] ), fabs ( v1[i] ) ) > 0 )
            v2_cpu[i] = fabs ( v2_cpu[i] - v1[i] ) / std::max ( fabs ( v2_cpu[i] ), fabs ( v1[i] ) );
        else
            v2_cpu[i] = 0.0;
    }
    return norm_inf ( v2_cpu );
}

template<typename ScalarType>
ScalarType diff(ScalarType s, viennacl::scalar<ScalarType> & gs){
  ScalarType other = gs;
  return (s - other) / std::max(s, other);
}

template< typename NumericT, typename Epsilon >
int test_vector ( Epsilon const& epsilon) {
    using namespace viennacl::device_specific;
    int retval = EXIT_SUCCESS;

    unsigned int size = 1024*32;
    ublas::vector<NumericT> cw(size);
    ublas::vector<NumericT> cx(size);
    ublas::vector<NumericT> cy(size);
    ublas::vector<NumericT> cz(size);

//    NumericT s;

    for (unsigned int i=0; i<cw.size(); ++i){
      cw[i]=std::rand()/(NumericT)RAND_MAX;
    }

    std::cout << "Running tests for vector of size " << cw.size() << std::endl;
    viennacl::vector<NumericT> w (size);
    viennacl::vector<NumericT> x (size);
    viennacl::vector<NumericT> y (size);
    viennacl::vector<NumericT> z (size);
    NumericT s = 0;
    viennacl::scalar<NumericT> gs(0);

    cx = 2.0f*cw;
    cy = 3.0f*cw;
    cz = 4.0f*cw;
    viennacl::copy (cw, w);
    viennacl::copy (cx, x);
    viennacl::copy (cy, y);
    viennacl::copy (cz, z);

    NumericT alpha = 3.14;
    NumericT beta = 1;

    // --------------------------------------------------------------------------

//    {
//        std::cout << "w = scalar_vector(alpha) ..." << std::endl;
//        for (unsigned int i = 0; i < size; ++i)
//          cw[i] = alpha;
//        viennacl::scheduler::statement statement(w, viennacl::op_assign(), viennacl::scalar_vector<NumericT>(size,alpha));
//        device_specific::execute(database::get<NumericT>(database::axpy), statement);
//        viennacl::backend::finish();
//        CHECK_RESULT(cw, w, w = scalar_vector(alpha));
//    }

//    {
//        std::cout << "w = x ..." << std::endl;
//        cw = cx;
//        viennacl::scheduler::statement statement(w, viennacl::op_assign(), x);
//        device_specific::execute(database::get<NumericT>(database::axpy), statement);
//        viennacl::backend::finish();
//        CHECK_RESULT(cw, w, w = x);
//    }

//    {
//        std::cout << "w = -x ..." << std::endl;
//        viennacl::scalar<NumericT> s0(1);
//        cw =  -cx;
//        viennacl::scheduler::statement statement(w, viennacl::op_assign(), -s0*x);
//        device_specific::execute(database::get<NumericT>(database::axpy), statement);
//        viennacl::backend::finish();
//        CHECK_RESULT(cw, w, w = -x);
//    }

//    {
//        std::cout << "w = alpha*x + beta*y ..." << std::endl;
//        cw = alpha*cx + beta*cy;
//        viennacl::scheduler::statement statement(w, viennacl::op_assign(), alpha*x + beta*y);
//        device_specific::execute(database::get<NumericT>(database::axpy), statement);
//        viennacl::backend::finish();
//        CHECK_RESULT(cw, w, w = alpha*x + beta*y);
//    }

//    {
//        std::cout << "x = alpha*w + beta*y ..." << std::endl;
//        cx = alpha*cw + beta*cy;
//        viennacl::scheduler::statement statement(x, viennacl::op_assign(), alpha*w + beta*y);
//        device_specific::execute(database::get<NumericT>(database::axpy), statement);
//        viennacl::backend::finish();
//        CHECK_RESULT(cw, w, w = alpha*x + beta*y);
//    }

//    {
//        std::cout << "[Independent] x = alpha*w + beta*y, y = alpha*x + beta*w ..." << std::endl;
//        for (std::size_t i = 0; i < size; ++i)
//        {
//          NumericT tmpcx = cx[i];
//          NumericT tmpcy = cy[i];

//          cx[i] = alpha*cw[i] + beta*tmpcy;
//          cy[i] = alpha*tmpcx + beta*cw[i];
//        }
//        viennacl::scheduler::statement s1(x, viennacl::op_assign(), alpha*w + beta*y);
//        viennacl::scheduler::statement s2(y, viennacl::op_assign(), alpha*x + beta*w);
//        device_specific::execute(database::get<NumericT>(database::axpy), statements_container(s1, s2, statements_container::INDEPENDENT));
//        viennacl::backend::finish();
//        CHECK_RESULT(cx, x, (x = alpha*w + beta*y, y = alpha*x + beta*w));
//        CHECK_RESULT(cy, y, (x = alpha*w + beta*y, y = alpha*x + beta*w));
//    }

//    {
//        std::cout << "[Sequential] x = alpha*w + beta*y, y = alpha*x + beta*w ..." << std::endl;
//        cx = alpha*cw + beta*cy;
//        cy = alpha*cx + beta*cw;
//        viennacl::scheduler::statement s1(x, viennacl::op_assign(), alpha*w + beta*y);
//        viennacl::scheduler::statement s2(y, viennacl::op_assign(), alpha*x + beta*w);
//        device_specific::execute(database::get<NumericT>(database::axpy), statements_container(s1, s2, statements_container::SEQUENTIAL));
//        viennacl::backend::finish();
//        CHECK_RESULT(cx, x, (x = alpha*w + beta*y, y = alpha*x + beta*w));
//        CHECK_RESULT(cy, y, (x = alpha*w + beta*y, y = alpha*x + beta*w));
//    }

//    {
//        std::cout << "w = exp(y) ..." << std::endl;
//        for (std::size_t i = 0; i < size; ++i)
//          cw[i] = std::exp(y[i]);
//        viennacl::scheduler::statement statement(w, viennacl::op_assign(), viennacl::linalg::element_exp(y));
//        device_specific::execute(database::get<NumericT>(database::axpy), statement);
//        viennacl::backend::finish();
//        CHECK_RESULT(cw, w, w = alpha*x + beta*y);
//    }

//    {
//        std::cout << "w = element_prod(x,y) ..." << std::endl;
//        for (std::size_t i = 0; i < size; ++i)
//          cw[i] = x[i]*y[i];
//        viennacl::scheduler::statement statement(w, viennacl::op_assign(), viennacl::linalg::element_prod(x,y));
//        device_specific::execute(database::get<NumericT>(database::axpy), statement);
//        viennacl::backend::finish();
//        CHECK_RESULT(cw, w, w = element_prod(x,y));
//    }

//    {
//        std::cout << "w = x == x" << std::endl;
//        for (unsigned int i=0; i < size; ++i){
//            cw(i) = (cx(i) == cx(i));
//        }
//        viennacl::scheduler::statement statement(w, viennacl::op_assign(), viennacl::linalg::element_eq(x,x));
//        generator::execute(statement, statement.array()[0]);
//        viennacl::backend::finish();
//        CHECK_RESULT(cw, w, w = (x == x))
//    }

//    {
//        std::cout << "w = x != x" << std::endl;
//        for (unsigned int i=0; i < size; ++i){
//            cw(i) = cx(i) != cx(i);
//        }
//        viennacl::scheduler::statement statement(w, viennacl::op_assign(), viennacl::linalg::element_neq(x,x));
//        generator::execute(statement, statement.array()[0]);
//        viennacl::backend::finish();
//        CHECK_RESULT(cw, w, w = x != x)
//    }

//    {
//        std::cout << "w = x > y" << std::endl;
//        for (unsigned int i=0; i < size; ++i){
//            cw(i) = cx(i) > cy(i);
//        }
//        viennacl::scheduler::statement statement(w, viennacl::op_assign(), viennacl::linalg::element_greater(x,y));
//        generator::execute(statement, statement.array()[0]);
//        viennacl::backend::finish();
//        CHECK_RESULT(cw, w, w = x > y)
//    }

//    {
//        std::cout << "w = x >= y" << std::endl;
//        for (unsigned int i=0; i < size; ++i){
//            cw(i) = cx(i) >= cy(i);
//        }
//        viennacl::scheduler::statement statement(w, viennacl::op_assign(), viennacl::linalg::element_geq(x,y));
//        generator::execute(statement, statement.array()[0]);
//        viennacl::backend::finish();
//        CHECK_RESULT(cw, w, w = x > y)
//    }

//    {
//        std::cout << "w = x < y" << std::endl;
//        for (unsigned int i=0; i < size; ++i){
//            cw(i) = cx(i) < cy(i);
//        }
//        viennacl::scheduler::statement statement(w, viennacl::op_assign(), viennacl::linalg::element_less(x,y));
//        generator::execute(statement, statement.array()[0]);
//        viennacl::backend::finish();
//        CHECK_RESULT(cw, w, w = x > y)
//    }

//    {
//        std::cout << "w = x <= y" << std::endl;
//        for (unsigned int i=0; i < size; ++i){
//            cw(i) = cx(i) <= cy(i);
//        }
//        viennacl::scheduler::statement statement(w, viennacl::op_assign(), viennacl::linalg::element_leq(x,y));
//        generator::execute(statement, statement.array()[0]);
//        viennacl::backend::finish();
//        CHECK_RESULT(cw, w, w = x > y)
//    }

//    {
//        std::cout << "w = x.^y" << std::endl;
//        for (unsigned int i=0; i < size; ++i){
//            cw(i) = std::pow(cx(i),cy(i));
//        }
//        viennacl::scheduler::statement statement(w, viennacl::op_assign(), viennacl::linalg::element_pow(x,y));
//        generator::execute(statement, statement.array()[0]);
//        viennacl::backend::finish();
//        CHECK_RESULT(cw, w, w = x.^y)
//    }

//    {
//        std::cout << "s = inner_prod(x,y)..." << std::endl;
//        s = 0;
//        for (unsigned int i=0; i<size; ++i)  s+=cx[i]*cy[i];
//        viennacl::scheduler::statement statement(gs, viennacl::op_assign(), viennacl::linalg::inner_prod(x,y));
//        device_specific::execute(database::get<NumericT>(database::reduction), statement);
//        viennacl::backend::finish();
//        CHECK_RESULT(s, gs, s = inner_prod(x,y));
//    }

//    {
//        std::cout << "s = reduce<add>(x)..." << std::endl;
//        s = 0;
//        for (unsigned int i=0; i<size; ++i)  s+=cx[i];
//        viennacl::scheduler::statement statement(gs, viennacl::op_assign(), viennacl::linalg::reduce<viennacl::op_add>(x));
//        device_specific::execute(profiles::get(REDUCTION_TYPE, NUMERIC_TYPE), statement);
//        viennacl::backend::finish();
//        CHECK_RESULT(s, gs, s = reduce<add>(x));
//    }

//    {
//        std::cout << "s = reduce<fmax>(x)..." << std::endl;
//        s = cx[0];
//        for (unsigned int i=1; i<size; ++i)  s=std::max(s,cx[i]);
//        viennacl::scheduler::statement statement(gs, viennacl::op_assign(), viennacl::linalg::reduce<iennacl::op_fmax>(x));
//        device_specific::execute(database::get<NumericT>(database::reduction), statement);
//        viennacl::backend::finish();
//        CHECK_RESULT(s, gs, s = reduce<mult>(x));
//    }

    {
        std::cout << "s = reduce<argmax>(x)..." << std::endl;
        NumericT s = 0;
        NumericT val = cx[0];
        for (unsigned int i=1; i<size; ++i)
        {
            if (cx[i] > val)
            {
                s = i;
                val = cx[i];
            }
        }
        viennacl::scheduler::statement statement(gs, viennacl::op_assign(), viennacl::linalg::reduce<viennacl::op_argmax>(x));
        device_specific::execute(database::get<NumericT>(database::reduction), statement);
        viennacl::backend::finish();
        CHECK_RESULT(s, gs, s = reduce<argmax>(x));
    }

    return retval;
}

template< typename NumericT, class Layout, typename Epsilon >
int test_matrix ( Epsilon const& epsilon) {
    int retval = EXIT_SUCCESS;

    unsigned int size1 = 1024;
    unsigned int size2 = 1024;

    unsigned int pattern_size1 = 256;
    unsigned int pattern_size2 = 128;

//    unsigned int n_rep1 = size1/pattern_size1;
//    unsigned int n_rep2 = size2/pattern_size2;

    ublas::matrix<NumericT> cA(size1,size2);
    ublas::matrix<NumericT> cB(size1,size2);
    ublas::matrix<NumericT> cC(size1,size2);

    ublas::matrix<NumericT> cPattern(pattern_size1,pattern_size2);

    ublas::vector<NumericT> cx(size1);
    ublas::vector<NumericT> cy(size2);

    for (unsigned int i=0; i<size1; ++i)
        for (unsigned int j=0; j<size2; ++j)
            cA(i,j)=(NumericT)std::rand()/RAND_MAX;

    for (unsigned int i = 0; i < pattern_size1; ++i)
        for (unsigned int j = 0; j < pattern_size2; ++j)
            cPattern(i,j) = (NumericT)std::rand()/RAND_MAX;

    for (unsigned int i=0; i<size2; ++i){
        cx(i) = (NumericT)std::rand()/RAND_MAX;
        cy(i) = (NumericT)std::rand()/RAND_MAX;
    }

//    std::cout << "Running tests for matrix of size " << cA.size1() << "," << cA.size2() << std::endl;

    viennacl::matrix<NumericT,Layout> A (size1, size2);
    viennacl::matrix<NumericT,Layout> B (size1, size2);
    viennacl::matrix<NumericT,Layout> C (size1, size2);

    viennacl::matrix<NumericT, Layout> pattern(pattern_size1, pattern_size2);

    viennacl::vector<NumericT> x(size1);
    viennacl::vector<NumericT> y(size2);

    cB = cA;
    cC = cA;
    viennacl::copy(cA,A);
    viennacl::copy(cB,B);
    viennacl::copy(cC,C);

    viennacl::copy(cx,x);
    viennacl::copy(cy,y);
    viennacl::copy(cPattern,pattern);

//    {
//      std::cout << "C = A + B ..." << std::endl;
//      cC     = ( cA + cB );
//      viennacl::scheduler::statement statement(C, viennacl::op_assign(), A + B);
//      device_specific::execute<device_specific::matrix_axpy_template>(device_specific::database::get<NumericT>(device_specific::database::matrix_axpy), statement);
//      viennacl::backend::finish();
//      CHECK_RESULT(cC, C, C=A+B)
//    }

    {
      std::cout << "C = diag(x) ..." << std::endl;
      for (unsigned int i = 0; i < size1; ++i)
        for (unsigned int j = 0; j < size2; ++j)
          cC(i,j) = (i==j)?cx[i]:0;
      viennacl::scheduler::statement statement(C, viennacl::op_assign(), viennacl::diag(x));
      device_specific::execute<device_specific::matrix_axpy_template>(device_specific::database::get<NumericT>(device_specific::database::matrix_axpy), statement);
      viennacl::backend::finish();
      CHECK_RESULT(cC, C, C=diag(x))
    }

    {
      std::cout << "x = diag(C) ..." << std::endl;
      for (unsigned int i = 0; i < std::min(size1, size2); ++i)
        cx[i] = cC(i,i);
      viennacl::scheduler::statement statement(x, viennacl::op_assign(), viennacl::diag(C));
      device_specific::execute<device_specific::vector_axpy_template>(device_specific::database::get<NumericT>(device_specific::database::vector_axpy), statement);
      viennacl::backend::finish();
      CHECK_RESULT(cx, x, x=diag(C))
    }

    {
      std::cout << "y = row(C, 7) ..." << std::endl;
      for (unsigned int j = 0; j < size2; ++j)
        cy[j] = cC(7,j);
      viennacl::scheduler::statement statement(y, viennacl::op_assign(), viennacl::row(C, 7));
      device_specific::execute<device_specific::vector_axpy_template>(device_specific::database::get<NumericT>(device_specific::database::vector_axpy), statement);
      viennacl::backend::finish();
      CHECK_RESULT(cy, y, y=row(C, 7))
    }

//    {
//      std::cout << "x = diag(C) ..." << std::endl;
//      for (unsigned int i = 0; i < std::min(size1, size2); ++i)
//        cx[i] = cC(i,i);
//      viennacl::scheduler::statement statement(x, viennacl::op_assign(), viennacl::diag(C));
//      device_specific::execute<device_specific::vector_axpy_template>(device_specific::database::get<NumericT>(device_specific::database::vector_axpy), statement);
//      viennacl::backend::finish();
//      CHECK_RESULT(cx, x, x=diag(C))
//    }

//    {
//        std::cout << "C = diag(x) ..." << std::endl;
//        for (unsigned int i = 0; i < size1; ++i){
//          for (unsigned int j = 0; j < size2; ++j){
//            cC(i,j) = (i==j)?cx[i]:0;
//          }
//        }
//        generator::custom_operation op;
//        op.add(mat(C) = generator::diag(vec(x)));
//        op.execute();
//        viennacl::backend::finish();
//        CHECK_RESULT(cC, C, C = diag(x))
//    }

//    {
//        std::cout << "x = diag(C) ..." << std::endl;
//        for (unsigned int i = 0; i < size1; ++i){
//            cx(i) = cA(i,i);
//        }
//        generator::custom_operation op;
//        op.add(vec(x) = generator::diag(mat(A)));
//        op.execute();
//        viennacl::backend::finish();
//        CHECK_RESULT(cx,x, x = diag(A));
//    }

//    {
//        std::cout << "C = repmat(P, M, N) ..." << std::endl;
//        for (unsigned int i = 0; i < size1; ++i)
//            for (unsigned int j = 0; j < size2; ++j)
//                cC(i,j) = cPattern(i%pattern_size1, j%pattern_size2);
//        generator::custom_operation op;
//        op.add(mat(C) = generator::repmat(mat(pattern),n_rep1,n_rep2));
//        op.execute();
//        viennacl::backend::finish();
//        CHECK_RESULT(cC, C, C = repmat(P, M, N))
//    }

//    {
//        std::cout << "C = repmat(x, 1, N) ..." << std::endl;
//        for (unsigned int i = 0; i < size1; ++i)
//            for (unsigned int j = 0; j < size2; ++j)
//                cC(i,j) = cx(i);
//        generator::custom_operation op;
//        op.add(mat(C) = generator::repmat(vec(x),1, C.size2()));
//        op.execute();
//        viennacl::backend::finish();
//        CHECK_RESULT(cC, C, C = repmat(x, 1, N))
//    }

//    {
//        std::cout << "C = trans(repmat(x, 1, N)) ..." << std::endl;
//        for (unsigned int i = 0; i < size1; ++i)
//            for (unsigned int j = 0; j < size2; ++j)
//                cC(i,j) = cx(j);
//        generator::custom_operation op;
//        op.add(mat(C) = generator::trans(generator::repmat(vec(x),1,C.size2())));
//        op.execute();
//        viennacl::backend::finish();
//        CHECK_RESULT(cC, C, C = repmat(x, 1, N))
//    }

//    {
//        std::cout << "C = -A ..." << std::endl;
//        for (unsigned int i = 0; i < size1; ++i)
//            for (unsigned int j = 0; j < size2; ++j)
//                cC(i,j) = -cA(i,j);
//        generator::custom_operation op;
//        op.add(mat(C) = -mat(A));
//        op.execute();
//        viennacl::backend::finish();

//        CHECK_RESULT(cC, C, C = -A)
//    }

//    {
//        std::cout << "C = 1/(1+EXP(-A)) ..." << std::endl;
//        for (unsigned int i = 0; i < size1; ++i)
//            for (unsigned int j = 0; j < size2; ++j)
//                cC(i,j) = 1.0f/(1.0f+std::exp(-cA(i,j)));
//        generator::custom_operation op;
//        op.add(mat(C) = 1.0f/(1.0f+generator::exp(-mat(A))));
//        op.execute();
//        viennacl::backend::finish();
//        CHECK_RESULT(cC, C, C = 1/(1+EXP(-A)))
//    }

    return retval;
}

int main(int argc, char* argv[]){
    std::vector<std::string> args(argv,argv+argc);
    unsigned int requested_device;
    if (argc!=2){
        requested_device=0;
    }
    else{
        requested_device = atoi(args[1].c_str());
    }
    int retval = EXIT_SUCCESS;

    typedef std::vector< viennacl::ocl::platform > platforms_type;
    typedef std::vector<viennacl::ocl::device> devices_type;

    platforms_type platforms = viennacl::ocl::get_platforms();
    size_t num_platforms = platforms.size();

    unsigned int current_device = 0;

    for (unsigned int k=0; k < num_platforms; ++k)
    {
        viennacl::ocl::platform pf(k);
        viennacl::ocl::set_context_device_type(k,CL_DEVICE_TYPE_ALL);
        viennacl::ocl::set_context_platform_index(k,k);
        viennacl::ocl::switch_context(k);
        devices_type dev = viennacl::ocl::current_context().devices();
        for (devices_type::iterator it = dev.begin(); it != dev.end(); ++it){

            if (current_device++ == requested_device ){
                viennacl::ocl::switch_device(*it);
                std::cout << std::endl;
                std::cout << "----------------------------------------------" << std::endl;
                std::cout << "               Device Info" << std::endl;
                std::cout << "----------------------------------------------" << std::endl;
                std::cout << viennacl::ocl::current_device().info() << std::endl;

//                std::cout << std::endl;
//                std::cout << "----------------------------------------------" << std::endl;
//                std::cout << "----------------------------------------------" << std::endl;
//                std::cout << "## Test :: Vector" << std::endl;
//                std::cout << "----------------------------------------------" << std::endl;

//                {
//                    double epsilon = 1.0E-4;

//                    std::cout << "# Testing setup:" << std::endl;
//                    std::cout << "  numeric: float" << std::endl;
//                    retval = test_vector<float> (epsilon);

//                    std::cout << std::endl;

//                    std::cout << "# Testing setup:" << std::endl;
//                    std::cout << "  numeric: double" << std::endl;
//                    retval = test_vector<double> (epsilon);

//                    if ( retval == EXIT_SUCCESS )
//                        std::cout << "# Test passed" << std::endl;
//                    else
//                        return retval;
//              }

              std::cout << std::endl;
              std::cout << "----------------------------------------------" << std::endl;
              std::cout << "----------------------------------------------" << std::endl;
              std::cout << "## Test :: Matrix" << std::endl;
              std::cout << "----------------------------------------------" << std::endl;

              {
                  double epsilon = 1.0E-4;
                  std::cout << "# Testing setup:" << std::endl;

                  std::cout << "  numeric: float" << std::endl;
                  std::cout << "  --------------" << std::endl;
                  std::cout << "  Row-Major"      << std::endl;
                  std::cout << "  --------------" << std::endl;
                  retval = test_matrix<float, viennacl::row_major> (epsilon);

                  std::cout << "  --------------" << std::endl;
                  std::cout << "  Column-Major"      << std::endl;
                  std::cout << "  --------------" << std::endl;
                  retval &= test_matrix<float, viennacl::column_major> (epsilon);

                  std::cout << "  numeric: double" << std::endl;
                  std::cout << "  --------------" << std::endl;
                  std::cout << "  Row-Major"      << std::endl;
                  std::cout << "  --------------" << std::endl;
                  retval = test_matrix<double, viennacl::row_major> (epsilon);

                  std::cout << "  --------------" << std::endl;
                  std::cout << "  Column-Major"      << std::endl;
                  std::cout << "  --------------" << std::endl;
                  retval &= test_matrix<double, viennacl::column_major> (epsilon);

                  if ( retval == EXIT_SUCCESS )
                      std::cout << "# Test passed" << std::endl;
                  else
                      return retval;
              }

            }
        }
    }
}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Header file for the SO3 Lie Group types.
/// \details These types provide a standardized definition for various SO3 quantities.
///
/// \author Kirk MacTavish
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef LGM_SO3_TYPES_HPP
#define LGM_SO3_TYPES_HPP

#include <Eigen/Core>

/////////////////////////////////////////////////////////////////////////////////////////////
/// Lie Group Math - Special Orthogonal Group
/////////////////////////////////////////////////////////////////////////////////////////////
namespace lgmath {
namespace so3 {

/// An axis angle rotation.
/// AxisAngle is a 3x1 axis-angle vector, the magnitude of the angle of rotation
/// can be recovered by finding the norm of the vector, and the axis of rotation is the unit-
/// length vector that arises from normalization. Note that the angle around the axis,
/// aaxis_ba, is a right-hand-rule (counter-clockwise positive) angle from 'a' to 'b'.
typedef Eigen::Vector3d AxisAngle;

/// A rotation matrix.
/// The convention is that C_ba rotates points from frame a to frame b.
typedef Eigen::Matrix3d RotationMatrix;

} // so3
} // lgmath

#endif // LGM_SO3_TYPES_HPP


//==================================================================================================
/*!
  @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_CONSTANT_RATIO_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_RATIO_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Ratio Ratio (function template)

  Generates a constant defined by compile-time rational number representation.

  @headerref{<boost/simd/constant/ratio.hpp>}

  @par Description

  @code
  template<typename T,std::uintmax_t Num, std::uintmax_t Denum> T Ratio();
  @endcode

  Generates a value of type @c T that evaluates to `Num/Denum`.

  @par Template Parameters

  | Name           | Description              |
  |---------------:|:-------------------------|
  | **T**          | the constant type        |
  | **Num**        | the constant numerator   |
  | **Denum**      | the constant denumerator |

  @par Return Value
  A value of type @c T that evaluates to `T(Num)/T(Denum)`.

  @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/ratio.hpp>
#include <boost/simd/constant/simd/ratio.hpp>

#endif


#include "fastmath.hpp"

#include <boost/math/special_functions/log1p.hpp>

#include "jacoblog.hpp"
#include "icsilogw.hpp"
#include "PowFast.hpp"

/** Switch to disable fast math routines for debugging. */
static const bool DisableFastMath = true;

/** Allocate precomputed tables. */
static ICSILog flog(14);
static PowFast fpow(14);
static JacobianLogTable fjacoblog(1 << 11);

/** Fast, approxiate jacobian logarithm: log(1+exp(x)) */
double fast_jacoblog(double x) {

    // Avoid numerical issues arising from extreme values
    if (x >= 60.0) return x;
    else if (x < -60.0) return 0.0;
    else {
        if (DisableFastMath)
            return boost::math::log1p(std::exp(x));
        else
            return fjacoblog.jacobianLog(x);
    }
}

/** Fast, approximate natural logarithm. */
double fast_log(double x) {

    if (DisableFastMath) return std::log(x);

    return flog.log(static_cast<float>(x));
}

/** Fast, approximate exponentiation. */
double fast_exp(double x) {

    if (DisableFastMath) return std::exp(x);

    return fpow.e(static_cast<float>(x));
}


#ifndef EKF_HPP
#define EKF_HPP

#include <Eigen/Dense>
#include <utility>

namespace EKF {
/** Class responsible for the prediction part of a kalman filter.
 *
 * Its two templates arguments are the dimension of the state and the
 * dimension of the control input.
 */
template <int N, int M>
class Predictor {
public:
    typedef Eigen::Matrix<float, N, 1> State;
    typedef Eigen::Matrix<float, N, N> Covariance;
    typedef Eigen::Matrix<float, M, 1> Input;
    typedef Eigen::Matrix<float, N, N> Jacobian;

    Predictor(Covariance process_noise)
        : R(process_noise)
    {
    }

    Predictor()
        : R(Covariance::Zero())
    {
    }

    /** State update function. */
    virtual State g(State state, Input input)
    {
        (void)input;
        return state;
    }

    /** Returns the Jacobian of the state update function. */
    virtual Jacobian G(State state, Input input)
    {
        (void)input;
        (void)state;
        return Jacobian::Identity();
    }

    std::pair<State, Covariance> predict(State mu, Covariance sigma, Input u)
    {
        mu = g(mu, u);
        auto Gn = G(mu, u);
        sigma = Gn * sigma * Gn.transpose() + R;

        return std::pair<State, Covariance>(mu, sigma);
    }

private:
    Covariance R; // < Process covariance
};

/** Class responsible for the correction class of a Kalman filter.
 *
 * The two template arguments are the dimension of the state and the dimension
 * of the measurement.
 */
template <int N, int M>
class Corrector {
public:
    typedef Eigen::Matrix<float, M, 1> Measurement;
    typedef Eigen::Matrix<float, N, 1> State;
    typedef Eigen::Matrix<float, M, N> Jacobian;
    typedef Eigen::Matrix<float, N, N> Covariance;

    virtual Measurement h(State) = 0;
    virtual Jacobian H(State) = 0;

    Corrector(Measurement variance)
        : Q(variance)
    {
    }

    std::pair<State, Covariance> correct(State mu, Covariance sigma, Measurement z)
    {
        auto H = this->H(mu);
        auto K = sigma * H.transpose() * (H * sigma * H.transpose() + Q).inverse();
        mu = mu + K * (z - h(mu));
        sigma = (Covariance::Identity() - K * H) * sigma;

        return std::pair<State, Covariance>(mu, sigma);
    }

private:
    Measurement Q;
};

}; // namespace EKF

#endif


// Boost.Geometry Index
// Unit Test

// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_index_test_common.hpp>

#include <boost/geometry/index/detail/algorithms/segment_intersection.hpp>

//#include <boost/geometry/io/wkt/read.hpp>

template <typename Box, typename Point, typename RelativeDistance>
void test_segment_intersection(Box const& box, Point const& p0, Point const& p1,
                               bool expected_result,
                               RelativeDistance expected_rel_dist)
{
    RelativeDistance rel_dist;
    bool value = bgi::detail::segment_intersection(box, p0, p1, rel_dist);
    BOOST_CHECK(value == expected_result);
    if ( value && expected_result )
        BOOST_CHECK_CLOSE(rel_dist, expected_rel_dist, 0.0001);
}

template <typename Box, typename Point, typename RelativeDistance>
void test_geometry(std::string const& wkt_g, std::string const& wkt_p0, std::string const& wkt_p1,
                   bool expected_result,
                   RelativeDistance expected_rel_dist)
{
    Box box;
    bg::read_wkt(wkt_g, box);
    Point p0, p1;
    bg::read_wkt(wkt_p0, p0);
    bg::read_wkt(wkt_p1, p1);
    test_segment_intersection(box, p0, p1, expected_result, expected_rel_dist);
}

#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>

void test_large_integers()
{
    typedef bg::model::point<int, 2, bg::cs::cartesian> int_point_type;
    typedef bg::model::point<double, 2, bg::cs::cartesian> double_point_type;

    bg::model::box<int_point_type> int_box;
    bg::model::box<double_point_type> double_box;
    int_point_type int_p0, int_p1;
    double_point_type double_p0, double_p1;

    std::string const str_box = "POLYGON((1536119 192000, 1872000 528000))";
    std::string const str_p0 = "POINT(1535000 191000)";
    std::string const str_p1 = "POINT(1873000 529000)";
    bg::read_wkt(str_box, int_box);
    bg::read_wkt(str_box, double_box);
    bg::read_wkt(str_p0, int_p0);
    bg::read_wkt(str_p1, int_p1);
    bg::read_wkt(str_p0, double_p0);
    bg::read_wkt(str_p1, double_p1);

    float int_value;
    bool int_result = bgi::detail::segment_intersection(int_box, int_p0, int_p1, int_value);
    double double_value;
    bool double_result = bgi::detail::segment_intersection(double_box, double_p0, double_p1, double_value);
    BOOST_CHECK(int_result == double_result);
    if ( int_result && double_result )
        BOOST_CHECK_CLOSE(int_value, double_value, 0.0001);
}

int test_main(int, char* [])
{
    typedef bg::model::point<int, 2, bg::cs::cartesian> P2ic;
    typedef bg::model::point<float, 2, bg::cs::cartesian> P2fc;
    typedef bg::model::point<double, 2, bg::cs::cartesian> P2dc;

    typedef bg::model::point<int, 3, bg::cs::cartesian> P3ic;
    typedef bg::model::point<float, 3, bg::cs::cartesian> P3fc;
    typedef bg::model::point<double, 3, bg::cs::cartesian> P3dc;

    test_geometry<bg::model::box<P2ic>, P2ic>("POLYGON((0 1,2 4))", "POINT(0 0)", "POINT(2 5)", true, 1.0f/5);
    test_geometry<bg::model::box<P2fc>, P2fc>("POLYGON((0 1,2 4))", "POINT(0 0)", "POINT(2 5)", true, 1.0f/5);
    test_geometry<bg::model::box<P2dc>, P2dc>("POLYGON((0 1,2 4))", "POINT(0 0)", "POINT(2 5)", true, 1.0/5);
    test_geometry<bg::model::box<P3ic>, P3ic>("POLYGON((0 1 2,2 4 6))", "POINT(0 0 0)", "POINT(2 5 7)", true, 2.0f/7);
    test_geometry<bg::model::box<P3fc>, P3fc>("POLYGON((0 1 2,2 4 6))", "POINT(0 0 0)", "POINT(2 5 7)", true, 2.0f/7);
    test_geometry<bg::model::box<P3dc>, P3dc>("POLYGON((0 1 2,2 4 6))", "POINT(0 0 0)", "POINT(2 5 7)", true, 2.0/7);

    test_geometry<bg::model::box<P2ic>, P2ic>("POLYGON((0 1,2 4))", "POINT(3 4)", "POINT(0 0)", true, 1.0f/3);
    test_geometry<bg::model::box<P2fc>, P2fc>("POLYGON((0 1,2 4))", "POINT(3 4)", "POINT(0 2)", true, 1.0f/3);
    test_geometry<bg::model::box<P2dc>, P2dc>("POLYGON((0 1,2 4))", "POINT(3 4)", "POINT(0 2)", true, 1.0/3);
    test_geometry<bg::model::box<P3ic>, P3ic>("POLYGON((0 1 2,2 4 6))", "POINT(3 5 6)", "POINT(0 3 3)", true, 1.0f/2);
    test_geometry<bg::model::box<P3fc>, P3fc>("POLYGON((0 1 2,2 4 6))", "POINT(3 5 6)", "POINT(0 3 3)", true, 1.0f/2);
    test_geometry<bg::model::box<P3dc>, P3dc>("POLYGON((0 1 2,2 4 6))", "POINT(3 5 6)", "POINT(0 3 3)", true, 1.0/2);

    test_geometry<bg::model::box<P2ic>, P2ic>("POLYGON((0 1,2 4))", "POINT(1 0)", "POINT(1 5)", true, 1.0f/5);
    test_geometry<bg::model::box<P2fc>, P2fc>("POLYGON((0 1,2 4))", "POINT(1 5)", "POINT(1 0)", true, 1.0f/5);
    test_geometry<bg::model::box<P2dc>, P2dc>("POLYGON((0 1,2 4))", "POINT(1 0)", "POINT(1 5)", true, 1.0/5);
    test_geometry<bg::model::box<P3ic>, P3ic>("POLYGON((0 1 2,2 4 6))", "POINT(1 3 0)", "POINT(1 3 7)", true, 2.0f/7);
    test_geometry<bg::model::box<P3fc>, P3fc>("POLYGON((0 1 2,2 4 6))", "POINT(1 3 7)", "POINT(1 3 0)", true, 1.0f/7);
    test_geometry<bg::model::box<P3dc>, P3dc>("POLYGON((0 1 2,2 4 6))", "POINT(1 3 0)", "POINT(1 3 7)", true, 2.0/7);

    test_geometry<bg::model::box<P2ic>, P2ic>("POLYGON((0 1,2 4))", "POINT(0 0)", "POINT(0 5)", true, 0.2f);
    test_geometry<bg::model::box<P2fc>, P2fc>("POLYGON((0 1,2 4))", "POINT(0 5)", "POINT(0 0)", true, 0.2f);
    test_geometry<bg::model::box<P2dc>, P2dc>("POLYGON((0 1,2 4))", "POINT(0 0)", "POINT(0 5)", true, 0.2);

    test_geometry<bg::model::box<P2ic>, P2ic>("POLYGON((0 1,2 4))", "POINT(3 0)", "POINT(3 5)", false, 0.0f);
    test_geometry<bg::model::box<P2fc>, P2fc>("POLYGON((0 1,2 4))", "POINT(3 5)", "POINT(3 0)", false, 0.0f);
    test_geometry<bg::model::box<P2dc>, P2dc>("POLYGON((0 1,2 4))", "POINT(3 0)", "POINT(3 5)", false, 0.0);

    test_geometry<bg::model::box<P2fc>, P2fc>("POLYGON((0 1,2 4))", "POINT(1 0)", "POINT(1 1)", true, 1.0f);
    test_geometry<bg::model::box<P2fc>, P2fc>("POLYGON((0 1,2 4))", "POINT(1 4)", "POINT(1 5)", true, 0.0f);

    test_geometry<bg::model::box<P2fc>, P2fc>("POLYGON((0 1,2 4))", "POINT(0.5 2)", "POINT(1.5 3)", true, 0.0f);

#ifdef HAVE_TTMATH
    typedef bg::model::point<ttmath_big, 2, bg::cs::cartesian> P2ttmc;
    typedef bg::model::point<ttmath_big, 3, bg::cs::cartesian> P3ttmc;

    test_geometry<bg::model::box<P2ttmc>, P2ttmc>("POLYGON((0 1,2 4))", "POINT(0 0)", "POINT(2 5)", true, 1.0f/5);
    test_geometry<bg::model::box<P3ttmc>, P3ttmc>("POLYGON((0 1 2,2 4 6))", "POINT(0 0 0)", "POINT(2 5 7)", true, 2.0f/7);
#endif

    test_large_integers();

    return 0;
}


#include <Eigen/Core>
#include <Eigen/SVD>
#include <iostream>

#define VIENNACL_WITH_OPENCL 1
#define VIENNACL_WITH_EIGEN 1

#include "viennacl/linalg/svd.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/vector.hpp"

using namespace Eigen;
using std::cout;

int main() {
  // initializers
  MatrixXf C;
  viennacl::matrix<float> vcl_C = viennacl::zero_matrix<float>(1e3, 50);
  viennacl::matrix<float> vcl_U = viennacl::zero_matrix<float>(1e3, 1e3);
  viennacl::matrix<float> vcl_V = viennacl::zero_matrix<float>(50, 50);

  // random data
  C.setRandom(1e5, 50);

  // copy eigen to viennacl
  viennacl::copy(C, vcl_C);

  // eigen SVD
  // JacobiSVD<MatrixXf> svd(C, ComputeThinU | ComputeThinV);
  //
  // std::cout << "diag" << std::endl;
  // std::cout << svd.singularValues() << std::endl;
  //
  // std::cout << "U" << std::endl;
  // std::cout << svd.matrixU() << std::endl;
  //
  // std::cout << "V" << std::endl;
  // std::cout << svd.matrixV() << std::endl;

  // viennacl svd

  viennacl::linalg::svd(vcl_C, vcl_U, vcl_V);

  viennacl::vector_base<float> D(vcl_C.handle(),
                                 std::min(vcl_C.size1(), vcl_C.size2()), 0,
                                 vcl_C.internal_size2() + 1);

  // std::cout << "diag" << std::endl;
  // std::cout << D << std::endl;
  //
  // std::cout << "U" << std::endl;
  // std::cout << vcl_U << std::endl;
  //
  // std::cout << "V" << std::endl;
  // std::cout << vcl_V << std::endl;

  std::cout << "V" << std::endl;
  return 0;
}


//
// Created by foxfire on 1/20/18.
//

#ifndef GFX_OPENGL_HPP
#define GFX_OPENGL_HPP

#include "gfx.hpp"

#include <vector>
#include <cstdint>

#define _USE_MATH_DEFINES
#include <GL/glew.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace fox
{
	class counter;
}
struct OBJ_MODEL;

class gfx_opengl : public gfx
{
public:
	gfx_opengl();

	~gfx_opengl() override;

	void init(int w, int h) override;

	void render() override;

	void resize(int w, int h) override;

	void deinit() override;

private:
	int win_w, win_h;
	// an empty vertex array object to bind to
	uint32_t default_vao;

	Eigen::Vector3f eye, target, up;
	Eigen::Affine3f V;
	Eigen::Projective3f P;
	// model matrix (specific to the model instance)
	std::vector<Eigen::Projective3f> MVP;
	std::vector<Eigen::Affine3f> M, MV;
	// TODO: should this be Affine3f ?
	std::vector<Eigen::Matrix3f> normal_matrix;
	// more shader uniforms
	Eigen::Vector4f light_pos, color;
	Eigen::Vector3f La, Ls, Ld;
	Eigen::Vector3f Ka, Ks, Kd;
	float shininess;

	GLuint shader_id, shader_vert_id, shader_frag_id;
	GLuint vertex_vbo, normal_vbo;

	fox::counter *phy_counter;
	fox::counter *fps_counter;
	fox::counter *perf_counter;

	const static uint8_t perf_array_size = 8;
	double phys_times[perf_array_size];
	double gfx_matrix_times[perf_array_size];
	double render_times[perf_array_size];
	double phys_time;
	double gfx_matrix_time;
	double render_time;
	uint8_t perf_index;
	double total_time;

	OBJ_MODEL *mesh;

	void print_info();
	void load_shaders();

	void update_matricies();
};

#endif //GFX_OPENGL_HPP


/*
 * algebra.hpp
 *
 *  Created on: 30 Jul 2017
 *      Author: julianporter
 */

#ifndef ALGEBRA_HPP_
#define ALGEBRA_HPP_

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <cmath>

#define PI 3.14159265359
#define radians(x) (x*PI/180.0)
#define degrees(x) (x*180.0/PI)

namespace mapping {

using Vector = boost::numeric::ublas::vector<double>;
using Zero   = boost::numeric::ublas::zero_vector<double>;
using Matrix = boost::numeric::ublas::matrix<double>;
using ID     = boost::numeric::ublas::identity_matrix<double>;
using ZMatrix= boost::numeric::ublas::zero_matrix<double>;

bool operator==(const Vector &l,const Vector &r);
bool operator!=(const Vector &l,const Vector &r);

bool operator==(const Matrix &l,const Matrix &r);
bool operator!=(const Matrix &l,const Matrix &r);

Matrix transpose(const Matrix &m);

}

#endif /* ALGEBRA_HPP_ */


//==================================================================================================
/**
  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)
**/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/gammaln.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/halfeps.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/five.hpp>
#include <boost/simd/constant/mtwo.hpp>

namespace bs = boost::simd;

template <typename T, std::size_t N, typename Env>
void limit_test(Env& runtime)
{
  using p_t = bs::pack<T, N>;
  using bs::gammaln;

#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(gammaln(bs::Inf<p_t>()), bs::Inf<p_t>(), 0);
  STF_ULP_EQUAL(gammaln(bs::Minf<p_t>()), bs::Nan<p_t>(), 0);
  STF_ULP_EQUAL(gammaln(bs::Nan<p_t>()), bs::Nan<p_t>(), 0);
  STF_ULP_EQUAL(gammaln(bs::Zero<p_t>()), bs::Inf<p_t>(), 0);
#endif
  STF_ULP_EQUAL(gammaln(bs::Mone<p_t>()), bs::Inf<p_t>(), 0);
  STF_ULP_EQUAL(gammaln(bs::Mtwo<p_t>()), bs::Inf<p_t>(), 0);
  STF_ULP_EQUAL(gammaln(bs::One<p_t>()), bs::Zero<p_t>(), 0);
  STF_ULP_EQUAL(gammaln(bs::Two<p_t>()), bs::Zero<p_t>(), 0);
  STF_ULP_EQUAL(gammaln(bs::Mzero<p_t>()),         bs::Inf<p_t>(), 0);

  STF_ULP_EQUAL(gammaln(bs::Halfeps<p_t>()),       p_t( bs::std_(gammaln)(bs::Halfeps<p_t>())), 1);
  STF_ULP_EQUAL(gammaln(bs::Eps<p_t>()),           p_t(bs::std_(gammaln)(bs::Eps<p_t>())), 0.5);
  STF_ULP_EQUAL(gammaln(bs::Half<p_t>()),          p_t(bs::std_(gammaln)(bs::Half<p_t>())), 0.5);
  STF_ULP_EQUAL(gammaln(p_t(1.5)),                 p_t(bs::std_(gammaln)(T(1.5))), 1.5);
  STF_ULP_EQUAL(gammaln(p_t(2.5)),                 p_t(bs::std_(gammaln)(T(2.5))), 0.5);
  STF_ULP_EQUAL(gammaln(p_t(13)) ,                 p_t(bs::std_(gammaln)(T(13))), 0.5);
  STF_ULP_EQUAL(gammaln(p_t(13.5)) ,               p_t(bs::std_(gammaln)(T(13.5))), 0.5);
  STF_ULP_EQUAL(gammaln(p_t(-0.1)),                p_t(bs::std_(gammaln)(T(-0.1))),         1);
  STF_ULP_EQUAL(gammaln(-bs::Half<p_t>()),         p_t(bs::std_(gammaln)(-bs::Half<T>())),  0.5);
  STF_ULP_EQUAL(gammaln(-bs::Halfeps<p_t>()),      p_t(bs::std_(gammaln)(-bs::Halfeps<T>())), 0.5);
  STF_ULP_EQUAL(gammaln(p_t(-27.5)),               p_t(bs::std_(gammaln)(T(-27.5))),            3);
}

STF_CASE_TPL("Check gammaln limit cases" , STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;
  limit_test<T, N>(runtime);
  limit_test<T, N/2>(runtime);
  limit_test<T, N*2>(runtime);
}

template <typename T, std::size_t N, typename Env>
void test(Env& runtime)
{
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : T(-i);
    b[i] = bs::gammaln(a1[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb (&b[0], &b[0]+N);

  STF_ULP_EQUAL(bs::gammaln(aa1), bb, 0.5);
}

STF_CASE_TPL("Check gammaln on pack" , STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;
   test<T, N>(runtime);
   test<T, N/2>(runtime);
   test<T, N*2>(runtime);
}


#ifndef __HODLR_Tree__
#define __HODLR_Tree__

#include <Eigen/Dense>
#include "HODLR_Matrix.hpp"
#include "HODLR_Node.hpp"

class HODLR_Tree
{
private:
    int N;
    int n_levels;
    double tolerance;
    std::vector<int> nodes_in_level;
    HODLR_Matrix* A;
    bool is_sym, is_pd;

    // Vector of levels(which contain nodes) thereby giving the tree:
    std::vector<std::vector<HODLR_Node*>> tree;
    void createTree();
    void createRoot();
    void createChildren(int level_number, int node_number);

    // Methods needed for the nonSPD HODLR solver:
    void factorizeLeafNonSPD(int node_number);
    void factorizeNonLeafNonSPD(int level_number, int node_number);
    void factorizeNonSPD();
    Mat solveLeafNonSPD(int node_number, Mat b);
    Mat solveNonLeafNonSPD(int level_number, int node_number, Mat b);
    Mat solveNonSPD(Mat b);
    dtype logDeterminantNonSPD();

    // Methods needed for the SPD HODLR solver:
    void factorizeLeafSPD(int node_number);
    void factorizeNonLeafSPD(int level_number, int node_number);
    void factorizeSPD();
    void qr(int level_number, int node_number);
    void qrForLevel(int level_number);
    Mat solveLeafSymmetricFactor(int node_number, Mat b);
    Mat solveNonLeafSymmetricFactor(int level_number, int node_number, Mat b);
    //Mat solveSymmetricFactor(Mat b);
    Mat solveLeafSymmetricFactorTranspose(int node_number, Mat b);
    Mat solveNonLeafSymmetricFactorTranspose(int level_number, int node_number, Mat b);
    //Mat solveSymmetricFactorTranspose(Mat b);
    Mat solveSPD(Mat b);
    Mat SymmetricFactorNonLeafProduct(int level_number, int node_number, Mat b);
    Mat SymmetricFactorTransposeNonLeafProduct(int level_number, int node_number, Mat b);
    dtype logDeterminantSPD();

public:
    HODLR_Tree(int n_levels, double tolerance, HODLR_Matrix* A);
    ~HODLR_Tree();

    //  Methods for HODLR solver
    void assembleTree(bool is_sym = false, bool is_pd = false);
    // Gives the box details of the prescribed box and level number:
    void printNodeDetails(int level_number, int node_number);
    // Lists details of all boxes in the tree
    void printTreeDetails();
    void plotTree(std::string image_name);
    Mat matmatProduct(Mat x);
    void factorize();
    Mat solve(Mat b);
    ////////////////////////////////////////////////
    Mat solveSymmetricFactorTranspose(Mat b);
    Mat solveSymmetricFactor(Mat b);
    ////////////////////////////////////////////////
    Mat symmetricFactorProduct(Mat x);
    Mat symmetricFactorTransposeProduct(Mat x);
    Mat getSymmetricFactor();
    dtype logDeterminant();
};

#endif /*__HODLR_Tree__*/


#include <expression/dualtime/FixedPointBDFDualTimeIntegrator.h>
#include <expression/dualtime/VariableImplicitBDFDualTimeIntegrator.h>
#include <spatialops/structured/Grid.h>
#include <spatialops/structured/FVStaggered.h>

#include <test/TestHelper.h>
#include "ExpDecay.h"
#include <expression/Functions.h>
#include <expression/ExpressionTree.h>
#include <expression/ExprPatch.h>

#include <boost/program_options.hpp>
#include <boost/lexical_cast.hpp>

#include <iostream>
#include <fstream>

using std::cout;
using std::endl;

namespace po = boost::program_options;
namespace so = SpatialOps;

typedef so::SVolField FieldT;

double bdf_error_from_dt( double dt, const unsigned designOrder )
{
  const double tend = 1.0;
  const double k    = 1.0;
  const double phi0 = 1.0;
  const double ds   = 0.1*dt;

  try{

    Expr::ExprPatch patch(1);
    SpatialOps::OperatorDatabase sodb;
    Expr::FieldManagerList& fml = patch.field_manager_list();

    const Expr::Tag dtTag    ( "timestep",      Expr::STATE_NONE );
    const Expr::Tag dsTag    ( "dual-timestep", Expr::STATE_NONE );

    Expr::DualTime::BDFDualTimeIntegrator* integrator = new Expr::DualTime::FixedPointBDFDualTimeIntegrator<FieldT>( 0, "BasicDualTime", dtTag, dsTag, designOrder );

    typedef Expr::ConstantExpr<so::SingleValueField>::Builder ConstantSingleValueT;
    integrator->set_physical_time_step_expression( new ConstantSingleValueT( dtTag, dt ) );
    integrator->set_dual_time_step_expression    ( new ConstantSingleValueT( dsTag, ds ) );

    Expr::ExpressionFactory& factory = integrator->factory();

    std::string phiName = "phi";
    const Expr::Tag phiTag   ( phiName,          Expr::STATE_NONE );
    const Expr::Tag phiRHSTag( phiName + "_rhs", Expr::STATE_NONE );
    factory.register_expression( new ExpDecayRHS<FieldT>::Builder( k, phiRHSTag, phiTag ) );

    integrator->add_variable<FieldT>( phiTag.name(), phiRHSTag );

    integrator->prepare_for_integration( fml, sodb, patch.field_info() );

    Expr::ExpressionFactory icFactory;

    const Expr::Tag phiNTag( phiName, Expr::STATE_N );
    const Expr::ExpressionID id = icFactory.register_expression( new Expr::ConstantExpr<FieldT>::Builder( phiNTag, phi0 ) );
    Expr::ExpressionTree icTree( id, icFactory, patch.id() );

    icTree.register_fields( fml );
    icTree.bind_fields( fml );
    icTree.execute_tree();

    double t = 0;

    const unsigned maxIterPerStep = 100;

    unsigned totalIter = 0;
    unsigned stepCount = 0;
    unsigned dualIter  = 0;
    bool     converged = false;

    while( t < ( tend-1e-12 ) ){
      // this (tend - 1e-12) 'trick' is to prevent roundoff error from triggering an extra time step

      dualIter = 0;
      integrator->begin_time_step();

      do{

        integrator->advance_dualtime( converged );

        dualIter++;
        totalIter++;

      } while( !converged && dualIter <= maxIterPerStep );

      integrator->end_time_step();
      stepCount++;
      t += dt;
    }

    FieldT& prediction = fml.field_ref<FieldT>( Expr::Tag( phiName, Expr::STATE_N ) );
    prediction.add_device(CPU_INDEX);
    SpatialOps::SpatFldPtr<FieldT> exact = SpatialOps::SpatialFieldStore::get<FieldT>( prediction );
    *exact <<= phi0*std::exp(-k*tend);
    return SpatialOps::nebo_norm( *exact - prediction );
  }
  catch( std::exception& err ){
    std::cout << err.what() << std::endl;
    return -1;
  }

}

bool bdf_run_test( const double designOrder,
                   const bool check_vs_matlab = false,
                   const double matlabObservedOrder = 0.0 )
{
  typedef std::vector<double> dvec;

  const int ndt = 3;

  dvec dt(ndt);
  dvec e(ndt);

  dt[0] = 0.1;
  dt[1] = 0.05;
  dt[2] = 0.01;

  double meanlog10dt = 0.0;
  double meanlog10error = 0.0;

  for( int i=0; i<ndt; ++i ){
    e[i] = bdf_error_from_dt( dt[i], (unsigned) designOrder );
    meanlog10dt    += std::log10(dt[i])/((double) ndt);
    meanlog10error += std::log10( e[i])/((double) ndt);
  }

  double numerator = 0.0;
  double denominator = 0.0;

  for( int i=0; i<ndt; ++i ){
    const double dt_deviation = std::log10(dt[i]) - meanlog10dt;
    numerator   += dt_deviation*(std::log10(e[i]) - meanlog10error);
    denominator += dt_deviation*dt_deviation;
  }

  double observedOrder = numerator/denominator;

  if( check_vs_matlab ){
    return ( observedOrder > ( matlabObservedOrder - 0.05 ) ) && ( observedOrder < ( matlabObservedOrder + 0.1 ) );
  }
  else{
    return ( observedOrder > ( designOrder - 0.05 ) ) && ( observedOrder < ( designOrder + 0.1 ) );
  }
}

int main( int iarg, char* carg[] )
{
  TestHelper status;
  status( bdf_run_test( 1 ), "BDF-1 accuracy" );
  status( bdf_run_test( 2 ), "BDF-2 accuracy" );
  status( bdf_run_test( 3, true, 1.965900224509586 ), "BDF-3 vs Matlab code" );
  status( bdf_run_test( 4, true, 1.976953783712932 ), "BDF-4 vs Matlab code" );

  if( status.ok() ){
    std::cout << "PASS\n";
    return 0;
  }

  std::cout << "FAIL\n";
  return -1;
}


#ifndef WAVE_WEIGHTING_FUNCTIONS_HPP
#define WAVE_WEIGHTING_FUNCTIONS_HPP

#include <Eigen/Core>

namespace wave {

Eigen::Matrix3d calculatePointToLineWeight(const double *const pt,
                                           const double *const ptA,
                                           const double *const ptB,
                                           const double &variance);

double calculatePointToPlaneWeight(const double *const pt,
                                   const double *const ptA,
                                   const double *const ptB,
                                   const double *const ptC,
                                   const double &variance);

}

#endif //WAVE_WEIGHTING_FUNCTIONS_HPP


//
// Copyright 2019-2020 Mateusz Loskot <mateusz at loskot dot net>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#include <boost/gil.hpp>
#include <boost/gil/extension/numeric/affine.hpp>
#include <boost/gil/extension/numeric/resample.hpp>
#include <boost/gil/extension/numeric/sampler.hpp>

#include <boost/core/lightweight_test.hpp>

#include <cmath>

namespace gil = boost::gil;

// Tolerance predicate for floating point comparison to use with BOOST_TEST_WITH
template <typename T>
struct with_tolerance
{
    with_tolerance(T tolerance) : tolerance(tolerance) {}
    bool operator()(T lhs, T rhs)
    {
        return (std::abs(lhs - rhs) <= tolerance);
    }

private:
    T tolerance;
};

namespace {
constexpr double HALF_PI = 1.57079632679489661923;
}

void test_matrix3x2_default_constructor()
{
    gil::matrix3x2<int> m1;
    BOOST_TEST_EQ(m1.a, 1);
    BOOST_TEST_EQ(m1.b, 0);
    BOOST_TEST_EQ(m1.c, 0);
    BOOST_TEST_EQ(m1.d, 1);
    BOOST_TEST_EQ(m1.e, 0);
    BOOST_TEST_EQ(m1.f, 0);
}

void test_matrix3x2_parameterized_constructor()
{
    gil::matrix3x2<int> m1(1, 2, 3, 4, 5, 6);
    BOOST_TEST_EQ(m1.a, 1);
    BOOST_TEST_EQ(m1.b, 2);
    BOOST_TEST_EQ(m1.c, 3);
    BOOST_TEST_EQ(m1.d, 4);
    BOOST_TEST_EQ(m1.e, 5);
    BOOST_TEST_EQ(m1.f, 6);
}

void test_matrix3x2_copy_constructor()
{
    gil::matrix3x2<int> m1(1, 2, 3, 4, 5, 6);
    gil::matrix3x2<int> m2(m1);
    BOOST_TEST_EQ(m2.a, 1);
    BOOST_TEST_EQ(m2.b, 2);
    BOOST_TEST_EQ(m2.c, 3);
    BOOST_TEST_EQ(m2.d, 4);
    BOOST_TEST_EQ(m2.e, 5);
    BOOST_TEST_EQ(m2.f, 6);
}

void test_matrix3x2_assignment_operator()
{
    gil::matrix3x2<int> m1(1, 2, 3, 4, 5, 6);
    gil::matrix3x2<int> m2;
    m2 = m1;
    BOOST_TEST_EQ(m2.a, 1);
    BOOST_TEST_EQ(m2.b, 2);
    BOOST_TEST_EQ(m2.c, 3);
    BOOST_TEST_EQ(m2.d, 4);
    BOOST_TEST_EQ(m2.e, 5);
    BOOST_TEST_EQ(m2.f, 6);
}

void test_matrix3x2_multiplication_assignment()
{
    gil::matrix3x2<int> m1;
    gil::matrix3x2<int> m2;
    m2 *= m1;
    BOOST_TEST_EQ(m2.a, 1);
    BOOST_TEST_EQ(m2.b, 0);
    BOOST_TEST_EQ(m2.c, 0);
    BOOST_TEST_EQ(m2.d, 1);
    BOOST_TEST_EQ(m2.e, 0);
    BOOST_TEST_EQ(m2.f, 0);

    gil::matrix3x2<int> m3(0, 0, 0, 0, 0, 0);
    m2 *= m3;
    BOOST_TEST_EQ(m2.a, 0);
    BOOST_TEST_EQ(m2.b, 0);
    BOOST_TEST_EQ(m2.c, 0);
    BOOST_TEST_EQ(m2.d, 0);
    BOOST_TEST_EQ(m2.e, 0);
    BOOST_TEST_EQ(m2.f, 0);
}

void test_matrix3x2_matrix3x2_multiplication()
{
    gil::matrix3x2<int> m1;
    gil::matrix3x2<int> m2(0, 0, 0, 0, 0, 0);
    gil::matrix3x2<int> m3;
    m3 = m1 * m2;
    BOOST_TEST_EQ(m3.a, 0);
    BOOST_TEST_EQ(m3.b, 0);
    BOOST_TEST_EQ(m3.c, 0);
    BOOST_TEST_EQ(m3.d, 0);
    BOOST_TEST_EQ(m3.e, 0);
    BOOST_TEST_EQ(m3.f, 0);
}

void test_matrix3x2_vector_multiplication()
{
    gil::matrix3x2<int> m1;
    gil::point<int> v1{2, 4};

    gil::point<int> v2 = v1 * m1;
    BOOST_TEST_EQ(v2.x, 2);
    BOOST_TEST_EQ(v2.y, 4);

    gil::point<int> v3 = gil::transform(m1, v1);
    BOOST_TEST_EQ(v3.x, 2);
    BOOST_TEST_EQ(v3.y, 4);
}

void test_matrix3x2_get_rotate()
{
    auto m1 = gil::matrix3x2<double>::get_rotate(HALF_PI);
    BOOST_TEST_WITH(m1.a, std::cos(HALF_PI), with_tolerance<double>(0.03));
    BOOST_TEST_EQ(m1.b, 1);
    BOOST_TEST_EQ(m1.c, -1);
    BOOST_TEST_WITH(m1.d, std::cos(HALF_PI), with_tolerance<double>(0.03));
    BOOST_TEST_EQ(m1.e, 0);
    BOOST_TEST_EQ(m1.f, 0);
}

void test_matrix3x2_get_scale()
{
    gil::matrix3x2<int> m1;
    m1 = gil::matrix3x2<int>::get_scale(2);
    BOOST_TEST_EQ(m1.a, 2);
    BOOST_TEST_EQ(m1.b, 0);
    BOOST_TEST_EQ(m1.c, 0);
    BOOST_TEST_EQ(m1.d, 2);
    BOOST_TEST_EQ(m1.e, 0);
    BOOST_TEST_EQ(m1.f, 0);
    m1 = gil::matrix3x2<int>::get_scale(2, 4);
    BOOST_TEST_EQ(m1.a, 2);
    BOOST_TEST_EQ(m1.d, 4);
    m1 = gil::matrix3x2<int>::get_scale(gil::point<int>{4, 8});
    BOOST_TEST_EQ(m1.a, 4);
    BOOST_TEST_EQ(m1.d, 8);
}

void test_matrix3x2_get_translate()
{
    gil::matrix3x2<int> m1;
    m1 = gil::matrix3x2<int>::get_translate(2, 4);
    BOOST_TEST_EQ(m1.a, 1);
    BOOST_TEST_EQ(m1.b, 0);
    BOOST_TEST_EQ(m1.c, 0);
    BOOST_TEST_EQ(m1.d, 1);
    BOOST_TEST_EQ(m1.e, 2);
    BOOST_TEST_EQ(m1.f, 4);
    m1 = gil::matrix3x2<int>::get_translate(gil::point<int>{4, 8});
    BOOST_TEST_EQ(m1.e, 4);
    BOOST_TEST_EQ(m1.f, 8);
}

void test_matrix3x2_transform()
{
    gil::matrix3x2<int> m1;
    gil::point<int> v1{2, 4};
    gil::point<int> v2 = gil::transform(m1, v1);
    BOOST_TEST_EQ(v2.x, 2);
    BOOST_TEST_EQ(v2.y, 4);
}

void test_matrix3x2_inverse()
{
    using matrix_t = gil::matrix3x2<double>;
    using point_t = gil::point<double>;

    matrix_t mo = matrix_t::get_translate(0, 16);
    matrix_t mb = matrix_t::get_rotate(HALF_PI);
    auto m = mo * mb;

    point_t p(10, 10);
    point_t q = gil::transform(inverse(m), p);
    point_t p2 = gil::transform(m, q);

    BOOST_TEST_WITH(p.x, p2.x, with_tolerance<double>(1e-9));
    BOOST_TEST_WITH(p.y, p2.y, with_tolerance<double>(1e-9));
}

int main()
{
    test_matrix3x2_default_constructor();
    test_matrix3x2_parameterized_constructor();
    test_matrix3x2_copy_constructor();
    test_matrix3x2_assignment_operator();
    test_matrix3x2_multiplication_assignment();
    test_matrix3x2_matrix3x2_multiplication();
    test_matrix3x2_vector_multiplication();
    test_matrix3x2_get_rotate();
    test_matrix3x2_get_scale();
    test_matrix3x2_get_translate();
    test_matrix3x2_transform();
    test_matrix3x2_inverse();

    return ::boost::report_errors();
}


#ifndef TEST_UTILS_HPP
#define TEST_UTILS_HPP

#if SKYLARK_HAVE_BOOST
#include <boost/test/minimal.hpp>
#endif

#include "../../skylark.hpp"

#include <El.hpp>

namespace test { namespace util {

template < typename InputMatrixType,
           typename OutputMatrixType = InputMatrixType >
struct hash_transform_test_t : public skylark::sketch::hash_transform_t<
    InputMatrixType, OutputMatrixType,
    boost::random::uniform_int_distribution,
    skylark::utility::rademacher_distribution_t > {

    typedef skylark::sketch::hash_transform_t<
        InputMatrixType, OutputMatrixType,
        boost::random::uniform_int_distribution,
        skylark::utility::rademacher_distribution_t >
            hash_t;

    hash_transform_test_t(int N, int S, skylark::base::context_t& context)
        : skylark::sketch::hash_transform_t<InputMatrixType, OutputMatrixType,
          boost::random::uniform_int_distribution,
          skylark::utility::rademacher_distribution_t>(N, S, context)
    {}

    std::vector<size_t> getRowIdx() { return hash_t::row_idx; }
    std::vector<double> getRowValues() { return hash_t::row_value; }
};

template<typename MatrixType>
MatrixType operator-(MatrixType& A, MatrixType& B) {
    MatrixType C;
    El::Copy(A, C);
    El::Axpy(-1.0, B, C);
    return C;
}

template<typename MatrixType>
bool equal(MatrixType& A, MatrixType& B,  double threshold=1.e-4) {
    MatrixType C = A - B;
    double diff_norm = El::Norm(C);
    if (diff_norm < threshold) {
        return true;
    }
    return false;
}

template<typename InputMatrixType,
         typename LeftSingularVectorsMatrixType,
         typename SingularValuesMatrixType,
         typename RightSingularVectorsMatrixType>
bool equal_svd_product(InputMatrixType& A,
    LeftSingularVectorsMatrixType& U,
    SingularValuesMatrixType& S,
    RightSingularVectorsMatrixType& V,
    double threshold=1e-4) {

    El::DistMatrix<double> S_CIRC_CIRC = S;
    std::vector<double> values(S_CIRC_CIRC.Buffer(),
        S_CIRC_CIRC.Buffer() + S_CIRC_CIRC.Height());
    El::Diagonal(S_CIRC_CIRC, values);
    El::DistMatrix<double> S_MC_MR = S_CIRC_CIRC;

    El::DistMatrix<double> A_MC_MR = A;
    El::DistMatrix<double> U_MC_MR = U;
    El::DistMatrix<double> V_MC_MR = V;
    El::DistMatrix<double> US_MC_MR;
    El::DistMatrix<double> USVt_MC_MR;

    US_MC_MR.Resize(U.Height(), S_CIRC_CIRC.Width());
    El::Zero(US_MC_MR);
    USVt_MC_MR.Resize(U.Height(), V.Height());

    El::Zero(USVt_MC_MR);
    El::Gemm(El::NORMAL, El::NORMAL,    1.0, U_MC_MR,
        S_MC_MR, 0.0, US_MC_MR);
    El::Gemm(El::NORMAL, El::TRANSPOSE, 1.0, US_MC_MR,
        V_MC_MR, 0.0, USVt_MC_MR);

    return equal(A_MC_MR, USVt_MC_MR, threshold);
}

#if SKYLARK_HAVE_BOOST

template <typename dense_matrix_t>
void check_equal(const dense_matrix_t& A, const dense_matrix_t& B) {
    double threshold = 1e-7;
    for (int col = 0; col < A.LocalWidth(); col++) {
        for (int row = 0; row < A.LocalHeight(); row++) {
            double diff = fabs(A.GetLocal(row, col) - B.GetLocal(row, col));
            if (diff > threshold) {
                std::cerr << "(" << row << ", " << col << ") diff = "
                          << A.GetLocal(row, col) << " - "
                          << B.GetLocal(row, col) << " = " << diff
                          << std::endl;
                BOOST_FAIL("Matrices differ");
            }
        }
    }
}

void check(El::DistMatrix<double>& A,
    double threshold=1e-4) {
    El::DistMatrix<double> U, V;
    El::DistMatrix<double, El::VR, El::STAR> S_VR_STAR;
    U = A;
    El::SVD(U, U, S_VR_STAR, V);
    bool passed = equal_svd_product(A, U, S_VR_STAR, V, threshold);
    if (!passed) {
        BOOST_FAIL("Failure in [MC, MR] case");
    }

}

template<El::Distribution ColDist>
void check(El::DistMatrix<double, ColDist, El::STAR>& A,
    double threshold=1e-4) {
    El::DistMatrix<double, ColDist, El::STAR> A_CD_STAR, U_CD_STAR;
    El::DistMatrix<double, El::STAR, El::STAR> S_STAR_STAR, V_STAR_STAR;
    A_CD_STAR = A;
    U_CD_STAR = A_CD_STAR;
    El::SVD(U_CD_STAR, U_CD_STAR, S_STAR_STAR, V_STAR_STAR);
    bool passed = equal_svd_product(A_CD_STAR,
        U_CD_STAR, S_STAR_STAR, V_STAR_STAR, threshold);
    if (!passed) {
        BOOST_FAIL("Failure in [VC/VR, *] case");
    }
}

template<El::Distribution RowDist>
void check(El::DistMatrix<double, El::STAR, RowDist>& A,
    double threshold=1e-4) {
    El::DistMatrix<double, RowDist, El::STAR> V_RD_STAR;
    El::DistMatrix<double, El::STAR, El::STAR> S_STAR_STAR, U_STAR_STAR;
    U_STAR_STAR = A;
    El::SVD(U_STAR_STAR, U_STAR_STAR, S_STAR_STAR, V_RD_STAR);
    bool passed = equal_svd_product(A,
        U_STAR_STAR, S_STAR_STAR, V_RD_STAR, threshold);
    if (!passed) {
        BOOST_FAIL("Failure in [*, VC/VR] case");
    }

}

#endif // SKYLARK_HAVE_BOOST

} }

#endif // TEST_UTILS_HPP


// #include <boost/math/constants/constants.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
// #include <boost/multiprecision/float128.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/filesystem/fstream.hpp>
#include <iostream>
#include <iterator>
#include <algorithm>

int main()
{
  // typedef boost::multiprecision::cpp_dec_float_50;
  using namespace boost::filesystem;
  // using namespace boost::lambda;
  using namespace boost::multiprecision;
  using boost::multiprecision::cpp_dec_float_50;
  unsigned int n_roots = 10000U;
  std::vector<cpp_dec_float_50> roots;
  boost::math::cyl_bessel_j_zero(-0.25, 1, n_roots, std::back_inserter(roots));

  path p{"bessel_zeros_short.txt"};
  ofstream ofs{p};
  ofs.precision(std::numeric_limits<cpp_dec_float_50>::digits10);
  std::copy(roots.begin(),
            roots.end(),
            std::ostream_iterator<cpp_dec_float_50>(ofs, "\n"));
}


#include "catch.hpp"

#include <chrono>
#include <fstream>

#include <boost/filesystem.hpp>
#include <boost/scope_exit.hpp>

#include "tntn/File.h"

#include "tntn/geometrix.h"
#include "tntn/Points2Mesh.h"

#include "tntn/MeshIO.h"

#include "delaunator_cpp/Delaunator.h"

// data
#include "vertex_points.h"
#include "triangle_indices.h"

namespace tntn {
namespace unittests {

TEST_CASE("delaunator test", "[tntn]")
{

    // super simple - one triangle
    // points in anti-clockwise order

    std::vector<double> ps_simple{0, 0, 1, 1, 1, 0};

    delaunator_cpp::Delaunator dn;
    if(!dn.triangulate(ps_simple))
    {
        CHECK(false);
    }

    CHECK(dn.triangles.size() == 3);

    for(int i = 0; i < dn.triangles.size(); i += 3)
    {
        for(int j = 0; j < 3; j++)
        {
            int pi = dn.triangles[j];
            CHECK(pi == j);
        }
    }

    std::vector<double> vertex_list_big;
    init_vertex_list(vertex_list_big);

    //delaunator_cpp::Delaunator dn_big;
    if(!dn.triangulate(vertex_list_big))
    {
        CHECK(false);
    }

    std::vector<int> tri_big;
    init_triangle_list(tri_big);

    CHECK(dn.triangles.size() == tri_big.size());
    for(int i = 0; i < dn.triangles.size(); i++)
    {
        CHECK(dn.triangles[i] == tri_big[i]);
    }
}

TEST_CASE("simple delaunay", "[tntn]")
{
    std::vector<Vertex> vlist;

    Vertex v;

    // A (i = 0)
    v.x = 0;
    v.y = 0;
    v.z = 0;

    vlist.push_back(v);

    // B  (i = 1)
    v.x = 100;
    v.y = 0;
    v.z = 0;

    vlist.push_back(v);

    // C  (i = 2)
    v.x = 100;
    v.y = 100;
    v.z = 0;

    vlist.push_back(v);

    // D  (i = 3)
    v.x = 0;
    v.y = 100;
    v.z = 0;

    vlist.push_back(v);

    std::vector<Face> faces;
    generate_delaunay_faces(vlist, faces);

    CHECK(faces.size() == 2);

    int h = 10;
    int w = 20;

    vlist.clear();
    for(int r = 0; r < h; r++)
    {
        for(int c = 0; c < w; c++)
        {
            Vertex vvv;
            vvv.x = c;
            vvv.y = r;

            float dx = c - w / 2;
            float dy = r - h / 2;
            if(dx * dx + dy * dy < 30 * 30)
            {
                vvv.z = 20;
            }
            else
                vvv.z = 0;

            vlist.push_back(vvv);
        }
    }

    faces.clear();

    std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
    generate_delaunay_faces(vlist, faces);
    std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();

    CHECK(faces.size() == (w - 1) * (h - 1) * 2);

    //std::cout <<"delaunay on " << vlist.size()/1000.0 << "k vertices in " << std::chrono::duration_cast<std::chrono::milliseconds>(end - begin).count() / 1000.0 << "seconds" << std::endl;
}

} //namespace unittests
} //namespace tntn


#include "Camera.h"

#include <Eigen/Core>
#include <iostream>

  Camera::Camera(float width, float height, float fovY)
:orientation( Eigen::Quaternionf::Identity()),
  position(0,0,0),
  projectionIsUpdate(false),
  viewIsUpdate(false)
{
   this->fovY = fovY;
   this->width = width;
   this->height = height;
   zFar = 50000.f;
   zNear = 0.1f;
}

Eigen::Vector3f Camera::getPosition()
{
  return position;
}

Eigen::Affine3f Camera::getView()
{
  updateViewMatrix();
  return view;
}

Eigen::Matrix4f Camera::getProjection()
{
  updateProjectionMatrix();
  return projection;
}

void Camera::setPosition(Eigen::Vector3f pos)
{
  viewIsUpdate = false;
  position = pos;
}

void Camera::move(float distance, Eigen::Vector3f axis)
{
  viewIsUpdate = false;
  axis.normalize();
  position += orientation.toRotationMatrix() * axis * distance;
}

void Camera::moveFoward(float distance)
{
  move(distance,Eigen::Vector3f(0,0,1));
}

void Camera::moveVertical(float distance)
{

  move(distance,Eigen::Vector3f(0,1,0));
}

void Camera::moveHorizontal(float distance)
{
  move(distance,Eigen::Vector3f(1,0,0));
}

void Camera::rotate(float theta, Eigen::Vector3f axis)
{
  viewIsUpdate = false;
  Eigen::AngleAxisf angleAxis(theta, axis);
  Eigen::Quaternionf rot(angleAxis);
  rot.normalize();
  orientation = orientation * rot;
  orientation.normalize();
}

void Camera::roll(float theta)
{
  rotate(theta,Eigen::Vector3f(0,0,1));
}

void Camera::pitch(float theta)
{
  rotate(theta,Eigen::Vector3f(1,0,0));
}

void Camera::yaw(float theta)
{
  rotate(theta,Eigen::Vector3f(0,1,0));
}

void Camera::updateViewMatrix()
{
  if(!viewIsUpdate)
  {
    view.linear() = orientation.conjugate().toRotationMatrix();
    view.translation() = - (view.linear() * position);
    viewIsUpdate = true;
  }
}

void Camera::updateProjectionMatrix()
{
  if(!projectionIsUpdate)
  {
    projection.setIdentity();

    float yScale = 1./tan(fovY*0.5);
    float xScale = yScale * height / width;
    projection(0,0) = xScale;
    projection(1,1) = yScale;
    projection(2,2) = -(zFar+zNear)/(zFar-zNear);
    projection(3,2) = -1;
    projection(2,3) = -2*zNear*zFar/(zFar-zNear);
    projection(3,3) = 0;

    projectionIsUpdate = true;
  }
}


#include <algorithm>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <iostream>
#include <string>

enum {
    DIR_N = 1,
    DIR_S,
    DIR_NW,
    DIR_NE,
    DIR_SW,
    DIR_SE
};

static int toDir(const std::string& dir) {
    if (dir == "n") {
        return DIR_N;
    } else if (dir == "s") {
        return DIR_S;
    } else if (dir == "nw") {
        return DIR_NW;
    } else if (dir == "ne") {
        return DIR_NE;
    } else if (dir == "sw") {
        return DIR_SW;
    } else if (dir == "se") {
        return DIR_SE;
    } else {
        std::cerr << "WRONG MOVE!\n";
        exit(1);
    }
};

struct Position
{
    int x = 0;
    int y = 0;
    int z = 0;
    int dist = 0;

    void move(const int dir)
    {
        if (dir == DIR_N) {
            y += 1;
            z -= 1;
        } else if (dir == DIR_S) {
            y -= 1;
            z += 1;
        } else if (dir == DIR_NW) {
            x -= 1;
            y += 1;
        } else if (dir == DIR_NE) {
            x += 1;
            z -= 1;
        } else if (dir == DIR_SW) {
            x -= 1;
            z += 1;
        } else if (dir == DIR_SE) {
            x += 1;
            y -= 1;
        }
        dist = (std::abs(x)+std::abs(y)+std::abs(z)) / 2;
    }
};

int main()
{
    std::string line;
    std::getline(std::cin, line);

    std::vector<float> path;
    Position child;
    {
        std::vector<std::string> words;
        boost::algorithm::split(
            words, line, boost::is_any_of(", "), boost::algorithm::token_compress_on);
        for (const auto& dir : words) {
            child.move(toDir(dir));
            path.push_back(child.dist);
        }
    }

    std::cout << "child dist: " << *path.rbegin() << "\n";
    std::sort(path.begin(), path.end(), std::greater<int>());
    std::cout << "max dist: " << path[0] << "\n";

    return 0;
}


#include <boost/gil.hpp>
#include <boost/gil/extension/io/png.hpp>
#include <boost/gil/image_processing/histogram_equalization.hpp>

using namespace boost::gil;

int main()
{
    gray8_image_t img;

    read_image("test_adaptive.png", img, png_tag{});
    gray8_image_t img_out(img.dimensions());

    // Consider changing image to independent color space, e.g. cmyk
    boost::gil::histogram_equalization(view(img),view(img_out));

    write_view("histogram_gray_equalized.png", view(img_out), png_tag{});

    return 0;
}


#ifndef CPG_HPP
#define CPG_HPP

#include <set>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>

namespace raisim
{

    template<typename T>
    class CPG
    {
    public:
        CPG()
        {
            // 0: WALK; 1: TROT; 2: PACE; 3: GALLOP; -1: STAND
            OFFSET[0] = {0.0, M_PI, M_PI * 0.5, M_PI * 1.5};
            OFFSET[1] = {0.0, M_PI, M_PI, 0.0};
            OFFSET[2] = {0.0, M_PI, 0.0, M_PI};
            OFFSET[3] = {0.0, 0.0, M_PI, M_PI};

            OFFSET_THREE = {0.0, 2.0 / 3.0 * M_PI, 4.0 / 3.0 * M_PI, 0.0};

            q_ << 0.0, 1.0, 1.0, 0.0, 0.0, -1.0, -1.0, 0.0;

            reset_gait_four_leg(gait_);
        }

        inline void update_r_mat_four_leg()
        {
            r_mat_.setZero();

            for (int i = 0; i < 4; ++i)
            {
                for (int j = 0; j < 4; ++j)
                {
                    T theta = offset_[j] - offset_[i];
                    r_mat_(2 * j, 2 * i) = std::cos(theta);
                    r_mat_(2 * j + 1, 2 * i + 1) = std::cos(theta);
                    r_mat_(2 * j, 2 * i + 1) = -std::sin(theta);
                    r_mat_(2 * j + 1, 2 * i) = std::sin(theta);
                }
            }
        }

        inline void update_r_mat_four_leg_transition(int init_gait, int target_gait)
        {
            r_mat_.setZero();
            update_phase();

            for (int i = 0; i < 4; ++i)
            {
                for (int j = 0; j < 4; ++j)
                {
                    T target_theta = OFFSET[target_gait][j] - OFFSET[target_gait][i];
                    T init_theta = OFFSET[init_gait][j] - OFFSET[init_gait][i];
                    int control_variable = get_control_variable(init_gait, target_gait);

                    // T theta = offset_[j] - offset_[i];
                    T theta = target_theta + (target_theta - init_theta) / (OFFSET[target_gait][control_variable] - OFFSET[init_gait][control_variable]) * (phase_[control_variable] - OFFSET[target_gait][control_variable]) * 0.5;

                    r_mat_(2 * j, 2 * i) = std::cos(theta);
                    r_mat_(2 * j + 1, 2 * i + 1) = std::cos(theta);
                    r_mat_(2 * j, 2 * i + 1) = -std::sin(theta);
                    r_mat_(2 * j + 1, 2 * i) = std::sin(theta);

                    // if (isnan(theta))
                    // {
                    //     std::cout << "theta is nan!!!" << std::endl
                    //                 << "i: " << i << " j: " << j << std::endl
                    //                 << "target_theta: " << target_theta << " init_theta: " << init_theta << std::endl
                    //                 << "control variable: " << control_variable << std::endl
                    //                 << "delta control" << OFFSET[target_gait][control_variable] - OFFSET[init_gait][control_variable] << std::endl
                    //                 << "target gait: " << target_gait << " init_gait: " << init_gait << std::endl;
                    // }
                }
            }
        }

        inline void update_r_mat_three_leg()
        {
            r_mat_.setZero();
            int ii = 0;
            int jj = 0;

            for (int i = 0; i < 4; ++i)
            {
                if (i == hold_leg_)
                {
                    continue;
                }

                ii = i > hold_leg_ ? i - 1 : i;

                for (int j = 0; j < 4; ++j)
                {
                    if (j == hold_leg_)
                    {
                        continue;
                    }

                    jj = j > hold_leg_ ? j - 1 : j;

                    T theta = offset_[jj] - offset_[ii];
                    r_mat_(2 * j, 2 * i) = std::cos(theta);
                    r_mat_(2 * j + 1, 2 * i + 1) = std::cos(theta);
                    r_mat_(2 * j, 2 * i + 1) = -std::sin(theta);
                    r_mat_(2 * j + 1, 2 * i) = std::sin(theta);
                }
            }
        }

        inline int get_control_variable(int init_gait, int target_gait)
        {
            int p = 0;
            for (int i = 0; i < 4; ++i)
            {
                if (fabs(OFFSET[target_gait][i] - OFFSET[init_gait][i]) > 0.1)
                {
                    p = i;
                    break;
                }
            }
            return p;
        }

        inline void update_phase()
        {
            for (int i = 0; i < 4; ++i)
            {
                phase_[i] = std::atan2(q_[2 * i + 1], q_[2 * i]);
            }
            for (int i = 1; i < 4; ++i)
            {
                phase_[i] -= phase_[0];
                if (phase_[i] < 0)
                {
                    phase_[i] += M_PI * 2.0;
                }
            }
            phase_[0] = 0.0;
        }

        Eigen::Matrix<T, Eigen::Dynamic, 1> get_raw_phase()
        {
            phase_raw_.setZero();
            for (int i = 0; i < 4; ++i)
            {
                phase_raw_[i] = std::atan2(q_[2 * i + 1], q_[2 * i]);
            }
            return phase_raw_;
        }

        Eigen::Matrix<T, Eigen::Dynamic, 1> get_omega()
        {
            omega_.setZero();
            for (int i = 0; i < 4; ++i)
            {
                omega_[i] = -M_PI / t_ * (1.0 / beta_ / (1.0 + std::exp(-b_ * q_(2 * i + 1))) + 1.0 / (1.0 - beta_) / (1.0 + std::exp(b_ * q_(2 * i + 1))));
                // if (omega_[i] < -3.15*4)
                // {
                //     std::cout << "omega: " << omega_[i] << "; i: " << i << "; t_: " << t_ << "; beta_: " << beta_ << "; q: " << q_(2*i+1) << "; b: " << b_ << "gait: " << gait_ << std::endl;
                // }

                if (gait_ < 0)
                {
                    omega_[i] = 0.0;
                }
            }
            return omega_;
        }

        inline void reset()
        {
            q_ << 0.0, 1.0, 1.0, 0.0, 0.0, -1.0, -1.0, 0.0;
            hold_leg_ = -1;
            reset_gait_four_leg(-1);
        }

        inline void reset_gait_four_leg(int gait)
        {
            // use this function only for reset
            if (gait >= 0)
            {
                gait_ = gait;
                previous_gait_ = gait;
                target_gait_ = gait;

                beta_ = BETA[gait_];
                delta_ = DELTA[gait_];
                t_ = TIME[gait_];
                offset_ = OFFSET[gait_];

                phase_ = offset_;

                for (int i = 0; i < 4; ++i)
                {
                    q_[2 * i] = std::cos(phase_[i]);
                    q_[2 * i + 1] = std::sin(phase_[i]);
                }

                update_r_mat_four_leg();
            }
            else
            {
                gait_ = gait;
                previous_gait_ = gait;
                target_gait_ = gait;

                beta_ = BETA[gait_];
                delta_ = DELTA[gait_];
                t_ = TIME[gait_];
                offset_ = OFFSET[gait_];

                // q_ << -1.0, 0.0, -0.5, 0.866, 0.5, 0.866, 1.0, 0.0;
                // q_dot_.setZero();
                // phase_raw_ << -M_PI, -M_PI / 3.0 * 2.0, M_PI / 3.0, 0.0;
                // phase_ = {0.0, M_PI / 3.0, M_PI / 3.0 * 2.0, M_PI};
                q_ << 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0;
                q_dot_.setZero();
                phase_raw_ << M_PI / 2.0, M_PI / 2.0, M_PI / 2.0, M_PI / 2.0;
                phase_ = {0.0, 0.0, 0.0, 0.0};
            }
        }

        inline void reset_gait_three_leg(int gait)
        {
            gait_ = gait;
            previous_gait_ = gait;
            target_gait_ = gait;

            beta_ = BETA_THREE;
            delta_ = DELTA_THREE;
            t_ = TIME_THREE;
            offset_ = OFFSET_THREE;

            if (gait >= 0)
            {
                phase_ = offset_;

                int ii = 0;

                for (int i = 0; i < 4; ++i)
                {
                    if (i == hold_leg_)
                    {
                        q_[2 * i] = 0.0;
                        q_[2 * i + 1] = -1.0;
                        continue;
                    }
                    ii = i > hold_leg_ ? i - 1 : i;
                    q_[2 * i] = std::cos(phase_[ii]);
                    q_[2 * i + 1] = std::sin(phase_[ii]);
                }

                update_r_mat_three_leg();
            }
            else
            {
                q_ << 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0;
                q_[2 * hold_leg_ + 1] = -1.0;
                q_dot_.setZero();
                phase_raw_ << M_PI / 2.0, M_PI / 2.0, M_PI / 2.0, M_PI / 2.0;
                phase_raw_[hold_leg_] = - M_PI / 2.0;
                phase_ = {0.0, 0.0, 0.0, 0.0};
                phase_[hold_leg_] = -0.5;
            }
        }

        inline void change_gait(int gait)
        {
            if (hold_leg_ < 0)
            {
                if (previous_gait_ < 0 || gait_ < 0 || gait < 0)
                {
                    reset_gait_four_leg(gait);
                }
                else
                {
                    previous_gait_ = target_gait_;
                    target_gait_ = gait;
                    gait_ = target_gait_;

                    beta_ = BETA[gait_];
                    delta_ = DELTA[gait_];
                    t_ = TIME[gait_];
                    offset_ = OFFSET[gait_];

                    update_r_mat_four_leg();
                }
            }
            else
            {
                gait = gait > 0 ? 0 : gait;
                if (previous_gait_ < 0 || gait_ < 0 || gait < 0)
                {
                    reset_gait_three_leg(gait);
                }
                else
                {
                    previous_gait_ = target_gait_;
                    target_gait_ = gait;
                    gait_ = target_gait_;

                    beta_ = BETA_THREE;
                    delta_ = DELTA_THREE;
                    t_ = TIME_THREE;
                    offset_ = OFFSET_THREE;

                    update_r_mat_three_leg();
                }
            }
        }

        inline void three_leg_mode(int hold_leg)
        {
            hold_leg_ = hold_leg;

            // if (hold_leg_ >= 0)
            // {
            //     update_r_mat_three_leg(hold_leg_);
            // }
            // else
            // {
            //     update_r_mat_four_leg();
            // }
        }

        inline void step_()
        {
            for (int i = 0; i < 4; ++i)
            {
                r_square_[i] = q_[2 * i] * q_[2 * i] + q_[2 * i + 1] * q_[2 * i + 1];
            }

            f_mat_.setZero();
            for (int i = 0; i < 4; ++i)
            {
                f_mat_(2 * i, 2 * i) = alpha_ * (mu_ - r_square_[i]);
                f_mat_(2 * i + 1, 2 * i + 1) = gamma_ * (mu_ - r_square_[i]);
                T omega = M_PI / t_ * (1.0 / beta_ / (1.0 + std::exp(-b_ * q_(2 * i + 1))) + 1.0 / (1.0 - beta_) / (1.0 + std::exp(b_ * q_(2 * i + 1))));
                f_mat_(2 * i, 2 * i + 1) = omega;
                f_mat_(2 * i + 1, 2 * i) = -omega;
            }

            q_dot_ = f_mat_ * q_ + r_mat_ * q_ * delta_;

            if (hold_leg_ >= 0)
            {
                // for (int i = 0; i < 4; ++i)
                // {
                //     f_mat_(2 * i, 2 * i) = alpha_ * (mu_ - r_square_[i]);
                //     f_mat_(2 * i + 1, 2 * i + 1) = gamma_ * (mu_ - r_square_[i]);
                //     T omega = M_PI / TIME_THREE * (1.0 / BETA_THREE / (1.0 + std::exp(-b_ * q_(2 * i + 1))) + 1.0 / (1.0 - BETA_THREE) / (1.0 + std::exp(b_ * q_(2 * i + 1))));
                //     f_mat_(2 * i, 2 * i + 1) = omega;
                //     f_mat_(2 * i + 1, 2 * i) = -omega;
                // }
                // q_dot_ = f_mat_ * q_ + r_mat_ * q_ * delta_;

                auto vec_dis = Eigen::Matrix<T, 2, 1>(0.0 - q_[2 * hold_leg_], -1.0 - q_[2 * hold_leg_ + 1]);
                T dis = vec_dis.norm();
                dis = dis > 0.4 ? 1.0 : dis * dis / 0.16;

                q_dot_(2 * hold_leg_) *= dis;
                q_dot_(2 * hold_leg_ + 1) *= dis;
                q_dot_(2 * hold_leg_) += vec_dis(0);
                q_dot_(2 * hold_leg_ + 1) += vec_dis(1);
            }

            q_ = q_ + q_dot_ * dt_;
        }

        inline void step()
        {
            if (hold_leg_ < 0)
            {
                if (gait_ != previous_gait_)
                {
                    update_r_mat_four_leg_transition(previous_gait_, target_gait_);

                    int control_variable = get_control_variable(previous_gait_, target_gait_);
                    // if (fabs(phase_[control_variable] - OFFSET[target_gait_][control_variable]) < 0.1 * phase_[control_variable])
                    if (fabs(phase_[control_variable] - OFFSET[target_gait_][control_variable]) < 0.1 * M_PI)
                    {
                        previous_gait_ = gait_;
                        update_r_mat_four_leg();
                    }
                }
            }

            if (gait_ < 0)
            {
                ;
            }
            else
            {
                step_();
            }
        }

        bool get_transition_status()
        {
            if (gait_ != previous_gait_)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        Eigen::Matrix<T, Eigen::Dynamic, 1> get_status()
        {
            return q_;
        }

        Eigen::Matrix<T, Eigen::Dynamic, 1> get_velocity()
        {
            return q_dot_;
        }

        T get_stance_time()
        {
            if (hold_leg_ < 0)
            {
                return t_ * beta_;
            }
            else
            {
                return TIME_THREE * BETA_THREE;
            }
        }

        int get_gait_index()
        {
            return gait_;
        }

        int get_hold_leg()
        {
            return hold_leg_;
        }

    // private:
        T mu_ = 1.0;
        T alpha_ = 50.0;
        T gamma_ = 50.0;
        T b_ = 50.0;
        T dt_ = 0.01;

        std::array<T, 4> BETA = {0.75, 0.5, 0.5, 0.4};
        std::array<T, 4> DELTA = {1.0, 1.0, 1.0, 1.0};
        std::array<T, 4> TIME = {0.6, 0.5, 0.5, 0.3};
        std::array<std::array<T, 4>, 4> OFFSET;

        T BETA_THREE = 2.0 / 3.0;
        T DELTA_THREE = 1.0;
        T TIME_THREE = 0.45;
        std::array<T, 4> OFFSET_THREE;

        int gait_ = -1;
        int previous_gait_ = -1;
        int target_gait_ = -1;

        T beta_, delta_, t_;
        std::array<T, 4> offset_;
        std::array<T, 4> r_square_;
        std::array<T, 4> phase_; // 0 to 2 * pi

        Eigen::Matrix<T, 8, 1> q_;
        Eigen::Matrix<T, 8, 1> q_dot_;
        Eigen::Matrix<T, 8, 8> r_mat_;
        Eigen::Matrix<T, 8, 8> f_mat_;
        Eigen::Matrix<T, 4, 1> phase_raw_; // -pi to +pi
        Eigen::Matrix<T, 4, 1> omega_;

        int hold_leg_ = -1;
    };

} // namespace raisim

#endif


#pragma once

#include <Eigen/StdVector>
#include <sophus/se2.hpp>
#include <sophus/so2.hpp>

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)

using Scalar = double;
using so2    = Sophus::SO2<Scalar>;
using se2    = Sophus::SE2<Scalar>;

template <int rows, int cols>
using Mat = Eigen::Matrix<double, rows, cols>;

template <int cols>
using Vec = Eigen::Matrix<double, 1, cols>;