#include <igl/embree/EmbreeIntersector.h>
#include <igl/Hit.h>
#include <igl/matlab/mexErrMsgTxt.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>
#undef assert
#define assert( isOK ) ( (isOK) ? (void)0 : (void) mexErrMsgTxt(C_STR(__FILE__<<":"<<__LINE__<<": failed assertion `"<<#isOK<<"'"<<std::endl) ) )
#include <igl/matlab/MexStream.h>
#include <Eigen/Core>
#include <limits>
#include <mex.h>

using namespace std;
using namespace Eigen;
using namespace igl;
using namespace igl::matlab;
using namespace igl::embree;
void mexFunction(int nlhs, mxArray *plhs[],
    int nrhs, const mxArray *prhs[])
{
  // This is useful for debugging whether Matlab is caching the mex binary
  //mexPrintf("%s %s\n",__TIME__,__DATE__);
  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);
  // Embree uses singles
  MatrixXf V,source,dir;
  MatrixXi F;
  mexErrMsgTxt(nrhs==4,"nrhs should == 4");

  parse_rhs_double(prhs+0,source);
  parse_rhs_double(prhs+1,dir);
  parse_rhs_double(prhs+2,V);
  parse_rhs_index( prhs+3,F);
  mexErrMsgTxt(source.cols()==3,"source must be #source by 3");
  mexErrMsgTxt(dir.cols()==3,"dir must be #dir by 3");
  mexErrMsgTxt(source.rows() == dir.rows(), "#source must equal #dir");
  mexErrMsgTxt(V.cols()==3,"V must be #V by 3");
  mexErrMsgTxt(F.cols()==3,"F must be #F by 3");

  const int n = source.rows();
  Eigen::Matrix<int, Eigen::Dynamic, 1> id(n);
  Eigen::Matrix<float, Eigen::Dynamic, 1> t(n);
  Eigen::Matrix<float, Eigen::Dynamic, 3> lambda(n,3);

  EmbreeIntersector ei;
  ei.init(V,F,true);
  for(int si = 0;si<n;si++)
  {
    Eigen::Vector3f s = source.row(si);
    Eigen::Vector3f d = dir.row(si);
    igl::Hit hit;
    const float tnear = 1e-4f;
    if(ei.intersectRay(s,d,hit,tnear))
    {
      id(si) = hit.id;
      t(si) = hit.t;
      lambda(si,0) = 1.0-hit.u-hit.v;
      lambda(si,1) = hit.u;
      lambda(si,2) = hit.v;
    }else
    {
      id(si) = -1;
      t(si) = std::numeric_limits<float>::infinity();
      lambda.row(si).setZero();
    }
  }

  switch(nlhs)
  {
    default:
    {
      mexErrMsgTxt(false,"Too many output parameters.");
    }
    case 3:
    {
      prepare_lhs_double(lambda,plhs+2);
      // Fall through
    }
    case 2:
    {
      prepare_lhs_double(t,plhs+1);
      // Fall through
    }
    case 1:
    {
      prepare_lhs_index(id,plhs+0);
      // Fall through
    }
    case 0: break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
}


#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include "gpu_func.h"
#include <sys/time.h>
#include "proxgpu_types.h"

// Compute the gradient at B
numeric get_value_only(cox_cache &dev_cache,
                  cox_data &dev_data,
                  cox_param &dev_param,
                  numeric *B,
                  int *ncase,
                  int *ncase_cumu,
                  int K,
                  int p,
                  cublasHandle_t handle,
                  cudaStream_t *streams,
                  numeric *cox_val_host)
{
    numeric result = 0.0;
    for (int k = 0; k < K; ++k)
    {
        int n = ncase[k];
        int offset = ncase_cumu[k];
        compute_product(dev_data.X+offset*p, B+p*k, dev_cache.eta+offset, n, p, streams[k], handle,CUBLAS_OP_N);
        apply_exp(dev_cache.eta+offset, dev_cache.exp_eta+offset, n, streams[k]);
        // Save rev_cumsum result to dev_cache.outer_accumu to avoid more cache variables
        rev_cumsum(dev_cache.exp_eta+offset, dev_cache.outer_accumu+offset ,n, streams[k]);
        adjust_ties(dev_cache.outer_accumu+offset, dev_data.rankmin+offset, dev_cache.exp_accumu+offset, n, streams[k]);

        get_coxvalue(dev_cache.exp_accumu+offset, dev_cache.eta+offset, dev_data.censor+offset, dev_cache.cox_val+k, n, streams[k]);
        cudaMemcpyAsync(cox_val_host+k, dev_cache.cox_val+k, sizeof(numeric)*1, cudaMemcpyDeviceToHost, streams[k]);
    }

    cudaDeviceSynchronize();
    for(int k = 0; k < K; ++k)
    {
        result += cox_val_host[k];
    }
    return result;
}

// Compute the gradient at B and save the result to dev_grad
numeric get_gradient(cox_cache &dev_cache,
                  cox_data &dev_data,
                  cox_param &dev_param,
                  numeric *dev_grad,
                  numeric *B,
                  int *ncase,
                  int *ncase_cumu,
                  int K,
                  int p,
                  cublasHandle_t handle,
                  cudaStream_t *streams,
                  bool get_val = false,
                  numeric *cox_val_host=0)
{
    for(int k = 0; k <K; ++k)
    {
        int n = ncase[k];
        int offset = ncase_cumu[k];
        compute_product(dev_data.X+offset*p, B+p*k, dev_cache.eta+offset, n, p, streams[k], handle,CUBLAS_OP_N);
        apply_exp(dev_cache.eta+offset, dev_cache.exp_eta+offset, n, streams[k]);
        // Save rev_cumsum result to dev_cache.outer_accumu to avoid more cache variables
        rev_cumsum(dev_cache.exp_eta+offset, dev_cache.outer_accumu+offset ,n, streams[k]);
        adjust_ties(dev_cache.outer_accumu+offset, dev_data.rankmin+offset, dev_cache.exp_accumu+offset, n, streams[k]);
        // Above is  _update_exp()
        // Below is _update_outer()
        // Save the result of division to residual to avoid more cache variables
        cwise_div(dev_data.censor+offset, dev_cache.exp_accumu+offset, dev_cache.residual+offset,  n, streams[k]);
        cumsum(dev_cache.residual+offset, n, streams[k]);
        adjust_ties(dev_cache.residual+offset, dev_data.rankmax+offset, dev_cache.outer_accumu+offset, n, streams[k]);
        mult_add(dev_cache.residual+offset,
                 dev_cache.exp_eta+offset,
                 dev_cache.outer_accumu+offset,
                 dev_data.censor+offset,
                 n, streams[k]);
        // residual ready
        compute_product(dev_data.X+offset*p,
                        dev_cache.residual+offset,
                        dev_grad + k*p, n, p, streams[k], handle, CUBLAS_OP_T);
        // Gradient ready
        // get_val will modify eta, but it's fine
        if(get_val)
        {
            get_coxvalue(dev_cache.exp_accumu+offset, dev_cache.eta+offset, dev_data.censor+offset, dev_cache.cox_val+k, n, streams[k]);
            cudaMemcpyAsync(cox_val_host+k, dev_cache.cox_val+k, sizeof(numeric)*1, cudaMemcpyDeviceToHost, streams[k]);
        }
    }
    numeric result = 0.0;
    if (get_val)
    {
        cudaDeviceSynchronize();
        for(int k = 0; k < K; ++k)
        {
            result += cox_val_host[k];
        }

    }
    return result;
}

// [[Rcpp::export]]
Rcpp::List solve_path(const Rcpp::List & X_list,
                        const Rcpp::List & censoring_list,
                        MatrixXd B,
                        const Rcpp::List & rankmin_list,
                        const Rcpp::List & rankmax_list,
                        double step_size,
                        VectorXd lambda_1_all,
                        VectorXd lambda_2_all,
                        Eigen::RowVectorXd penalty_factor, // Penalty factor for each group of variables
                        int niter, // Maximum number of iterations
                        double linesearch_beta,
                        double eps, // convergence criteria
                        double tol = 1e-10// line search tolerance
                        )
{
    // B is a long and skinny matrix now! rows are features and cols are responses
    const int p = B.rows();
    const int K = B.cols();
    // Create CUDA streams and handle
    cudaStream_t *streams = (cudaStream_t *) malloc(K * sizeof(cudaStream_t));
    for (int k = 0; k<K; ++k)
    {
        cudaStreamCreate(&streams[k]);
    }
    cublasHandle_t handle;
    cublasCreate(&handle);
    cudaStream_t copy_stream;
    cudaStream_t nest_stream;
    cudaStreamCreate(&copy_stream);
    cudaStreamCreate(&nest_stream);

    cox_data dev_data;
    cox_cache dev_cache;
    cox_param dev_param;
    numeric *cox_val_host=(numeric *)malloc(sizeof(numeric)*K);
    MatrixXd host_B(p,K);

    int *ncase_cumu = (int *)malloc(sizeof(int)*(K+1));
    ncase_cumu[0] = 0;
    int *ncase = (int*)malloc(sizeof(int)*K);
    std::vector<MapMatd> X_all;
    std::vector<MapVecd> censor; // Modify this in R
    std::vector<MapVeci> rankmin;
    std::vector<MapVeci> rankmax;
    for (int k = 0; k<K; ++k)
    {
        X_all.emplace_back(Rcpp::as<MapMatd>(X_list[k]));
        censor.emplace_back(Rcpp::as<MapVecd>(censoring_list[k]));
        rankmin.emplace_back(Rcpp::as<MapVeci>(rankmin_list[k]));
        rankmax.emplace_back(Rcpp::as<MapVeci>(rankmax_list[k]));
        ncase[k] = X_all[k].rows();
        ncase_cumu[k+1] = ncase_cumu[k] + ncase[k];
    }

    allocate_device_memory(dev_data, dev_cache, dev_param, ncase_cumu[K], K, p);

    // initialize parameters on the device
    cudaMemcpy(dev_param.B, &B(0,0), sizeof(numeric)*K*p, cudaMemcpyHostToDevice);
    cudaMemcpy(dev_param.v, &B(0,0), sizeof(numeric)*K*p, cudaMemcpyHostToDevice);
    cudaMemcpy(dev_param.penalty_factor, &penalty_factor(0), sizeof(numeric)*p, cudaMemcpyHostToDevice);

    // Copy the data
    for(int k = 0; k <K; ++k)
    {
        int n = ncase[k];
        cudaMemcpyAsync(dev_data.X+ncase_cumu[k]*p, &(X_all[k](0,0)), sizeof(numeric) * p *n, cudaMemcpyHostToDevice, streams[k]);
        cudaMemcpyAsync(dev_data.censor+ncase_cumu[k], &(censor[k][0]), sizeof(numeric) *n, cudaMemcpyHostToDevice, streams[k]);
        cudaMemcpyAsync(dev_data.rankmin+ncase_cumu[k], &(rankmin[k][0]), sizeof(int)*n, cudaMemcpyHostToDevice, streams[k]);
        cudaMemcpyAsync(dev_data.rankmax+ncase_cumu[k], &(rankmax[k][0]), sizeof(int)*n, cudaMemcpyHostToDevice, streams[k]);
    }

    numeric cox_val;
    numeric cox_val_next;
    numeric rhs_ls; // right-hand side of line search condition
    numeric diff; // Max norm of the difference of two consecutive iterates
    numeric lambda_1;
    numeric lambda_2;
    const int num_lambda = lambda_1_all.size();
    numeric step_size_intial = step_size;
    Rcpp::List result(num_lambda);
    bool stop; // Stop line searching
    numeric weight_old, weight_new;
    // Initialization done, starting solving the path
    struct timeval start, end;
    for (int lam_ind = 0; lam_ind < num_lambda; ++lam_ind){
        gettimeofday(&start, NULL);

        lambda_1 = lambda_1_all[lam_ind];
        lambda_2 = lambda_2_all[lam_ind];
        weight_old = 1.0;
        step_size = step_size_intial;
        // Inner iteration
        for (int i = 0; i < niter; ++i)
        {

            // Set prev_B = B
            cublas_copy(dev_param, K*p, copy_stream, handle);
            // Wait for Nesterov weight update
            cudaStreamSynchronize(nest_stream);
            // Update the gradient at v, compute cox_val at v
            cox_val = get_gradient(dev_cache,
                                    dev_data,
                                    dev_param,
                                    dev_param.grad,
                                    dev_param.v,
                                    ncase,
                                    ncase_cumu,
                                    K,
                                    p,
                                    handle,
                                    streams,
                                    true,
                                    cox_val_host);

            // Enter line search
            while(true)
            {
                // Update  B
                update_parameters(dev_param,
                                    K,
                                    p,
                                    step_size,
                                    lambda_1,
                                    lambda_2);

                // Get cox_val at updated B
                cox_val_next = get_value_only(dev_cache,
                                                dev_data,
                                                dev_param,
                                                dev_param.B,
                                                ncase,
                                                ncase_cumu,
                                                K,
                                                p,
                                                handle,
                                                streams,
                                                cox_val_host);

                stop = false;
                // This block are the line search conditions
                if(abs((cox_val_next - cox_val)/fmax(1.0, abs(cox_val_next))) > tol){
                    rhs_ls = cox_val + ls_stop_v1(dev_param, step_size,K,p);
                    stop = (cox_val_next <= rhs_ls);
                } else
                {
                    get_gradient(dev_cache,
                                dev_data,
                                dev_param,
                                dev_param.grad_ls,
                                dev_param.B,
                                ncase,
                                ncase_cumu,
                                K,
                                p,
                                handle,
                                streams);
                    rhs_ls = ls_stop_v2(dev_param, step_size,K,p);
                    stop = (rhs_ls >= 0);

                }

                if (stop)
                {
                    break;
                }
                step_size /= linesearch_beta;
            }

            diff = max_diff(dev_param, K, p);
            if (diff < eps)
            {
                std::cout << "convergence based on parameter change reached in " << i <<" iterations\n";
                std::cout << "current step size is " << step_size << std::endl;
                gettimeofday(&end, NULL);
                double delta  = ((end.tv_sec  - start.tv_sec) * 1000000u + end.tv_usec - start.tv_usec) / 1.e6;
                std::cout <<  "elapsed time is " << delta << " seconds" << std::endl;
                Rcpp::checkUserInterrupt();
                break;
            }

             // Nesterov weight
            weight_new = 0.5*(1+sqrt(1+4*weight_old*weight_old));
            nesterov_update(dev_param,K,p, weight_old, weight_new, nest_stream, handle);
            weight_old = weight_new;

            if (i != 0 && i % 100 == 0)
            {
                std::cout << "reached " << i << " iterations\n";
                gettimeofday(&end, NULL);
                double delta  = ((end.tv_sec  - start.tv_sec) * 1000000u + end.tv_usec - start.tv_usec) / 1.e6;
                std::cout <<  "elapsed time is " << delta  << " seconds" << std::endl;
                Rcpp::checkUserInterrupt();
            }

        }
        cudaMemcpy(&host_B(0,0), dev_param.B, sizeof(numeric)*K*p, cudaMemcpyDeviceToHost);
        result[lam_ind] = host_B;
        std::cout << "Solution for the " <<  lam_ind+1 << "th lambda pair is obtained\n";
    }

    free_device_memory(dev_data, dev_cache, dev_param);
    cublasDestroy(handle);
    for (int si = 0; si< K;++si){
        cudaStreamDestroy(streams[si]);
    }
    cudaStreamDestroy(copy_stream);
    cudaStreamDestroy(nest_stream);
    free(streams);
    free(ncase_cumu);
    free(ncase);
    return result;
}


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

#include <boost/simd/meta/hierarchy/simd.hpp>

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>
#include <cmath>
#include <boost/simd/function/simd/any.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/fms.hpp>
#include <boost/simd/function/ifrexp.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/ilog2.hpp>
#include <boost/simd/function/is_lez.hpp>
#include <boost/simd/function/is_ngez.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/plain.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/tofloat.hpp>

#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/invlog_2.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>

#include <boost/simd/detail/constant/invlog_2hi.hpp>
#include <boost/simd/detail/constant/invlog_2lo.hpp>

#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

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

  BOOST_DISPATCH_OVERLOAD_IF ( log2_
                          , (typename A0,typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::single_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 const& a0) const BOOST_NOEXCEPT
    {
      return musl_(log2)(a0);
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( log2_
                          , (typename A0,typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::double_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 const& a0) const BOOST_NOEXCEPT
    {
      return musl_(log2)(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log2_
                          , (typename A0,typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::arithmetic_<A0>, X >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 const& a0) const BOOST_NOEXCEPT
    {
      return bitwise_cast<A0>(bs::ilog2(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log2_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::musl_tag
                          , bs::pack_< bd::single_<A0>, X>
                          )
  {
    /* origin: FreeBSD /usr/src/lib/msun/src/e_log2f.c */
    /*
     * ====================================================
     * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
     *
     * Developed at SunPro, a Sun Microsystems, Inc. business.
     * Permission to use, copy, modify, and distribute this
     * software is freely granted, provided that this notice
     * is preserved.
     * ====================================================
     */
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      A0 x =  a0;
      iA0 k(0);
      auto isnez = is_nez(a0);
#ifndef BOOST_SIMD_NO_DENORMALS
      auto test = is_less(a0, Smallestposval<A0>())&&isnez;
      if (any(test))
      {
        k = if_minus(test, k, iA0(25));
        x = if_else(test, x*A0(33554432ul), x);
      }
#endif
      uiA0 ix = bitwise_cast<uiA0>(x);
      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      ix += 0x3f800000 - 0x3f3504f3;
      k += bitwise_cast<iA0>(ix>>23) - 0x7f;
      ix = (ix&0x007fffff) + 0x3f3504f3;
      x =  bitwise_cast<A0>(ix);
      A0 f = dec(x);
      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3eccce13, 0x3e789e26>(w);
      A0 t2= z*horn<A0, 0x3f2aaaaa, 0x3e91e9ee>(w);
      A0 R = t2 + t1;
      A0 hfsq = Half<A0>()*sqr(f);

      A0 dk = tofloat(k);
      A0 r =   fma(fms(s, hfsq+R, hfsq)+f, Invlog_2<A0>(), dk);
      // The original algorithm does some extra calculation in place of the return line
      // to get extra precision but this is uneeded for float as the exhaustive test shows
      // a 0.5 ulp maximal error on the full range.
      // Moreover all log2(exp2(i)) i =  1..31 are flint
      // I leave the code here in case an exotic proc will not play the game.
      //       A0  hi = f - hfsq;
      //       hi =  bitwise_and(hi, uiA0(0xfffff000ul));
      //       A0  lo = fma(s, hfsq+R, f - hi - hfsq);
      //       A0 r = (lo+hi)*detail::Invlog_2lo<A0>() + lo*detail::Invlog_2hi<A0>() + hi*detail::Invlog_2hi<A0>() + k;

#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(a0), zz);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log2_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::musl_tag
                             , bs::pack_< bd::double_<A0>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
    /* origin: FreeBSD /usr/src/lib/msun/src/e_log2f.c */
    /*
     * ====================================================
     * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
     *
     * Developed at SunPro, a Sun Microsystems, Inc. business.
     * Permission to use, copy, modify, and distribute this
     * software is freely granted, provided that this notice
     * is preserved.
     * ====================================================
     */
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      A0 x =  a0;
      uiA0 hx = bitwise_cast<uiA0>(x) >> 32;
      iA0 k(0);
      auto isnez = is_nez(a0);

#ifndef BOOST_SIMD_NO_DENORMALS
      auto test = is_less(a0, Smallestposval<A0>())&&isnez;
      if (any(test))
      {
        k = if_minus(test, k, iA0(54));
        x = if_else(test, x*A0(18014398509481984ull), x);
      }
#endif
      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      hx += 0x3ff00000 - 0x3fe6a09e;
      k += bitwise_cast<iA0>(hx>>20) - 0x3ff;
      hx = (hx&0x000fffff) + 0x3fe6a09e;
      x = bitwise_cast<A0>(hx<<32 | (bitwise_and(0xffffffffull, bitwise_cast<uiA0>(x))));

      A0 f = dec(x);
      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3fd999999997fa04ll, 0x3fcc71c51d8e78afll, 0x3fc39a09d078c69fll > (w);
      A0 t2= z*horn<A0, 0x3fe5555555555593ll, 0x3fd2492494229359ll
                      , 0x3fc7466496cb03dell, 0x3fc2f112df3e5244ll> (w);
      A0 R = t2 + t1;
      A0 hfsq = Half<A0>()*sqr(f);
//        return -(hfsq-(s*(hfsq+R))-f)*Invlog_2<A0>()+dk;  // fast ?

      /*
       * f-hfsq must (for args near 1) be evaluated in extra precision
       * to avoid a large cancellation when x is near sqrt(2) or 1/sqrt(2).
       * This is fairly efficient since f-hfsq only depends on f, so can
       * be evaluated in parallel with R.  Not combining hfsq with R also
       * keeps R small (though not as small as a true `lo' term would be),
       * so that extra precision is not needed for terms involving R.
       *
       * Compiler bugs involving extra precision used to break Dekker's
       * theorem for spitting f-hfsq as hi+lo, unless double_t was used
       * or the multi-precision calculations were avoided when double_t
       * has extra precision.  These problems are now automatically
       * avoided as a side effect of the optimization of combining the
       * Dekker splitting step with the clear-low-bits step.
       *
       * y must (for args near sqrt(2) and 1/sqrt(2)) be added in extra
       * precision to avoid a very large cancellation when x is very near
       * these values.  Unlike the above cancellations, this problem is
       * specific to base 2.  It is strange that adding +-1 is so much
       * harder than adding +-ln2 or +-log10_2.
       *
       * This uses Dekker's theorem to normalize y+val_hi, so the
       * compiler bugs are back in some configurations, sigh.  And I
       * don't want to used double_t to avoid them, since that gives a
       * pessimization and the support for avoiding the pessimization
       * is not yet available.
       *
       * The multi-precision calculations for the multiplications are
       * routine.
       */

      /* hi+lo = f - hfsq + s*(hfsq+R) ~ log(1+f) */
      A0  hi = f - hfsq;
      hi =  bitwise_and(hi, (Allbits<uiA0>() << 32));
      A0 lo = fma(s, hfsq+R, f - hi - hfsq);

      A0 val_hi = hi*Invlog_2hi<A0>();
      A0 val_lo = fma(lo+hi, Invlog_2lo<A0>(), lo*Invlog_2hi<A0>());

      A0 dk = tofloat(k);
      A0 w1 = dk + val_hi;
      val_lo += (dk - w1) + val_hi;
      val_hi = w1;
      A0 r =  val_lo + val_hi;
#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(a0), zz);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log2_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::plain_tag
                             , bs::pack_< bd::single_<A0>, X>
                             )
  {
    /* origin: FreeBSD /usr/src/lib/msun/src/e_log2f.c */
    /*
     * ====================================================
     * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
     *
     * Developed at SunPro, a Sun Microsystems, Inc. business.
     * Permission to use, copy, modify, and distribute this
     * software is freely granted, provided that this notice
     * is preserved.
     * ====================================================
     */
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using iA0 = bd::as_integer_t<A0,   signed>;
      A0 x =  a0;
      iA0 k(0);
      auto isnez = is_nez(a0);
#ifndef BOOST_SIMD_NO_DENORMALS
      auto test = is_less(a0, Smallestposval<A0>())&&isnez;
      if (any(test))
      {
        k = if_minus(test, k, iA0(25));
        x = if_else(test, x*A0(33554432ul), x);
      }
#endif
      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      iA0 kk;
      std::tie(x, kk) = ifrexp(x);
      A0 x_lt_sqrthf = genmask(Sqrt_2o_2<A0>() > x);
      k += kk + bitwise_cast<iA0>(x_lt_sqrthf);
      A0 f = dec(x+bitwise_and(x, x_lt_sqrthf));
      A0 dk = tofloat(k);

      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3eccce13, 0x3e789e26>(w);
      A0 t2= z*horn<A0, 0x3f2aaaaa, 0x3e91e9ee>(w);
      A0 R = t2 + t1;
      A0 hfsq = Half<A0>()*sqr(f);

      A0 r =   fma(fms(s, hfsq+R, hfsq)+f, Invlog_2<A0>(), dk);
      // The original algorithm does some extra calculation in place of the return line
      // to get extra precision but this is uneeded for float as the exhaustive test shows
      // a 0.5 ulp maximal error on the full range.
      // Moreover all log2(exp2(i)) i =  1..31 are flint
      // I leave the code here in case an exotic proc will not play the game.
      //       A0  hi = f - hfsq;
      //       hi =  bitwise_and(hi, uiA0(0xfffff000ul));
      //       A0  lo = fma(s, hfsq+R, f - hi - hfsq);
      //       A0 r = (lo+hi)*Invlog_2lo<A0>() + lo*Invlog_2hi<A0>() + hi*Invlog_2hi<A0>() + k;

#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(a0), zz);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log2_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::plain_tag
                             , bs::pack_< bd::double_<A0>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      /* origin: FreeBSD /usr/src/lib/msun/src/e_log2f.c */
      /*
       * ====================================================
       * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
       *
       * Developed at SunPro, a Sun Microsystems, Inc. business.
       * Permission to use, copy, modify, and distribute this
       * software is freely granted, provided that this notice
       * is preserved.
       * ====================================================
       */
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      A0 x =  a0;
      uiA0 hx = bitwise_cast<uiA0>(x) >> 32;
      iA0 k(0);
      auto isnez = is_nez(a0);

#ifndef BOOST_SIMD_NO_DENORMALS
      auto test = is_less(a0, Smallestposval<A0>())&&isnez;
      if (any(test))
      {
        k = if_minus(test, k, iA0(54));
        x = if_else(test, x*A0(18014398509481984ull), x);
      }
#endif
      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      iA0 kk;
      std::tie(x, kk) = ifrexp(x);
      A0 x_lt_sqrthf = genmask(Sqrt_2o_2<A0>() > x);
      k += kk + bitwise_cast<iA0>(x_lt_sqrthf);
      A0 f = dec(x+bitwise_and(x, x_lt_sqrthf));
      A0 dk = tofloat(k);

      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3fd999999997fa04ll, 0x3fcc71c51d8e78afll, 0x3fc39a09d078c69fll > (w);
      A0 t2= z*horn<A0, 0x3fe5555555555593ll, 0x3fd2492494229359ll
        , 0x3fc7466496cb03dell, 0x3fc2f112df3e5244ll> (w);
      A0 R = t2 + t1;
      A0 hfsq = Half<A0>()*sqr(f);
//        return -(hfsq-(s*(hfsq+R))-f)*Invlog_2<A0>()+dk;  // fast ?

      /*
       * f-hfsq must (for args near 1) be evaluated in extra precision
       * to avoid a large cancellation when x is near sqrt(2) or 1/sqrt(2).
       * This is fairly efficient since f-hfsq only depends on f, so can
       * be evaluated in parallel with R.  Not combining hfsq with R also
       * keeps R small (though not as small as a true `lo' term would be),
       * so that extra precision is not needed for terms involving R.
       *
       * Compiler bugs involving extra precision used to break Dekker's
       * theorem for spitting f-hfsq as hi+lo, unless double_t was used
       * or the multi-precision calculations were avoided when double_t
       * has extra precision.  These problems are now automatically
       * avoided as a side effect of the optimization of combining the
       * Dekker splitting step with the clear-low-bits step.
       *
       * y must (for args near sqrt(2) and 1/sqrt(2)) be added in extra
       * precision to avoid a very large cancellation when x is very near
       * these values.  Unlike the above cancellations, this problem is
       * specific to base 2.  It is strange that adding +-1 is so much
       * harder than adding +-ln2 or +-log10_2.
       *
       * This uses Dekker's theorem to normalize y+val_hi, so the
       * compiler bugs are back in some configurations, sigh.  And I
       * don't want to used double_t to avoid them, since that gives a
       * pessimization and the support for avoiding the pessimization
       * is not yet available.
       *
       * The multi-precision calculations for the multiplications are
       * routine.
       */

      /* hi+lo = f - hfsq + s*(hfsq+R) ~ log(1+f) */
      A0  hi = f - hfsq;
      hi =  bitwise_and(hi, (Allbits<uiA0>() << 32));
      A0 lo = fma(s, hfsq+R, f - hi - hfsq);

      A0 val_hi = hi*Invlog_2hi<A0>();
      A0 val_lo = fma(lo+hi, Invlog_2lo<A0>(), lo*Invlog_2hi<A0>());

      A0 w1 = dk + val_hi;
      val_lo += (dk - w1) + val_hi;
      val_hi = w1;
      A0 r =  val_lo + val_hi;
#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(a0), zz);
    }
  };

} } }

#endif


// (C) Copyright Jeremy Siek 2001. Permission to copy, use, modify,
// sell and distribute this software is granted provided this
// copyright notice appears in all copies. This software is provided
// "as is" without express or implied warranty, and with no claim as
// to its suitability for any purpose.

// Sample output:
//  1
//  1
//  0
//  0
//  1

#include <iostream>
#include <boost/dynamic_bitset.hpp>
int main() {
  boost::dynamic_bitset<> x(5); // all 0's by default
  x[0] = 1;
  x[1] = 1;
  x[4] = 1;
  for (boost::dynamic_bitset<>::size_type i = 0; i < x.size(); ++i)
    std::cout << x[i];
  std::cout << "\n";
  std::cout << x << "\n";
  return EXIT_SUCCESS;
}


/*
 * @Description:
 * @Author: Ren Qian
 * @Date: 2019-07-17 18:27:40
 */
#ifndef LIDAR_LOCALIZATION_SENSOR_DATA_IMU_DATA_HPP_
#define LIDAR_LOCALIZATION_SENSOR_DATA_IMU_DATA_HPP_

#include <deque>
#include <cmath>
#include <Eigen/Dense>

namespace lidar_localization {
class IMUData {
  public:
    class Orientation {
      public:
        double x = 0.0;
        double y = 0.0;
        double z = 0.0;
        double w = 0.0;

      public:
        void Normlize() {
          double norm = sqrt(pow(x, 2.0) + pow(y, 2.0) + pow(z, 2.0) + pow(w, 2.0));
          x /= norm;
          y /= norm;
          z /= norm;
          w /= norm;
        }
    };

    struct LinearAcceleration {
      double x = 0.0;
      double y = 0.0;
      double z = 0.0;
    };

    struct AngularVelocity {
      double x = 0.0;
      double y = 0.0;
      double z = 0.0;
    };

    struct AccelBias {
      double x = 0.0;
      double y = 0.0;
      double z = 0.0;
    };

    struct GyroBias {
      double x = 0.0;
      double y = 0.0;
      double z = 0.0;
    };

    double time = 0.0;

    Orientation orientation;

    LinearAcceleration linear_acceleration;
    AngularVelocity angular_velocity;

    AccelBias accel_bias;
    GyroBias gyro_bias;

  public:
    // 把四元数转换成旋转矩阵送出去
    Eigen::Matrix3f GetOrientationMatrix() const;
    static bool SyncData(std::deque<IMUData>& UnsyncedData, std::deque<IMUData>& SyncedData, double sync_time);
};
}
#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cstdlib>
#include <boost/test/minimal.hpp>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/timer.hpp>

using namespace std;

void test(int n)
{
    mtl::compressed2D<double> A(n*n, n*n);
    boost::timer start;
    {
	mtl::mat::inserter<mtl::compressed2D<double> > ins(A);
	for (int i= 0; i < n; i++) {
	    int r= i * n, re= r+n;
	    if (i > 0)
		for (int j= r; j < re; j++)
		    ins[j][j-n] << -1.0;
	    ins[r][r] << 4.0;
	    ins[r][r+1] << -1;
	    for (int j= r+1; j < re-1; j++) {
		ins[j][j-1] << -1.0;
		ins[j][j] << 4.0;
		ins[j][j+1] << -1.0;
	    }
	    ins[re-1][re-2] << -1.0;
	    ins[re-1][re-1] << 4.0;
	    if (i < n-1)
		for (int j= r; j < re; j++)
		    ins[j][j+n] << -1.0;
	}
    }
    //cout << "A is \n" << A;
    cout << "Insertion took " << start.elapsed() << '\n';

}

// create d by d matrix with nnz non-zeros
void test2(int d, int nnz)
{
    mtl::compressed2D<double> A(d, d);
    boost::timer start;
    {
	mtl::mat::inserter<mtl::compressed2D<double> > ins(A, int(1.2 * nnz / double(d)));
	std::cout << "Slot size is " << int(1.2 * nnz / double(d)) << '\n';
	for (int i= 0; i < nnz; i++) {
	    int r= rand(), c= rand();
	    assert(r % d >= 0); assert(c % d >= 0);
	    ins[r % d][c % d] << 2.3;
	}
    }
    cout << "Insertion took " << start.elapsed() << '\n';
}

int test_main(int argc, char* argv[])
{
    // test(atoi(argv[1]));
    test2(atoi(argv[1]), atoi(argv[2]));

    return 0;
}


/*
  [auto_generated]
  boost/numeric/odeint/external/vexcl/vexcl_norm_inf.hpp

  [begin_description]
  vector_space_norm_inf specialization for vexcl
  [end_description]

  Copyright 2009-2013 Karsten Ahnert
  Copyright 2009-2013 Mario Mulansky

  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)
*/

#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_NORM_INF_HPP_DEFINED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_NORM_INF_HPP_DEFINED

#include <map>
#include <algorithm>

#include <vexcl/vector.hpp>
#include <vexcl/multivector.hpp>
#include <vexcl/reductor.hpp>

#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>

namespace boost {
namespace numeric {
namespace odeint {

// specialization for vexcl vector
template <typename T>
struct vector_space_norm_inf< vex::vector<T> > {
    typedef T result_type;

    T operator()( const vex::vector<T> &x ) const {
        const auto &max = vex::get_reductor<T, vex::MAX>(x.queue_list());

        return max( fabs(x) );
    }
};

// specialization for vexcl multivector
template <typename T, size_t N>
struct vector_space_norm_inf< vex::multivector<T, N> > {
    typedef T result_type;

    T operator()( const vex::multivector<T, N> &x ) const {
        const auto &max = vex::get_reductor<T, vex::MAX>(x.queue_list());

        // Reducing a multivector results in std::array<T, N>:
        auto m = max( fabs(x) );

        // We will need to reduce it even further:
        return *std::max_element(m.begin(), m.end());
    }
};

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_NORM_INF_HPP_DEFINED


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

//[point_centroid_example

#include <iostream>

#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/accumulate.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/types/fundamental.hpp>

namespace compute = boost::compute;

// the point centroid example calculates and displays the
// centroid of a set of 3D points stored as float4's
int main()
{
    using compute::float4_;

    // get default device and setup context
    compute::device device = compute::system::default_device();
    compute::context context(device);
    compute::command_queue queue(context, device);

    // point coordinates
    float points[] = { 1.0f, 2.0f, 3.0f, 0.0f,
                       -2.0f, -3.0f, 4.0f, 0.0f,
                       1.0f, -2.0f, 2.5f, 0.0f,
                       -7.0f, -3.0f, -2.0f, 0.0f,
                       3.0f, 4.0f, -5.0f, 0.0f };

    // create vector for five points
    compute::vector<float4_> vector(5, context);

    // copy point data to the device
    compute::copy(
        reinterpret_cast<float4_ *>(points),
        reinterpret_cast<float4_ *>(points) + 5,
        vector.begin(),
        queue
    );

    // calculate sum
    float4_ sum = compute::accumulate(
        vector.begin(), vector.end(), float4_(0, 0, 0, 0), queue
    );

    // calculate centroid
    float4_ centroid;
    for(size_t i = 0; i < 3; i++){
        centroid[i] = sum[i] / 5.0f;
    }

    // print centroid
    std::cout << "centroid: " << centroid << std::endl;

    return 0;
}

//]


#include <memory>
#include <iostream>
#include <boost/program_options.hpp>

#include <sdm/exception.hpp>

#include <sdm/core/state/state.hpp>
#include <sdm/core/state/base_state.hpp>

#include <sdm/core/action/action.hpp>
#include <sdm/core/action/base_action.hpp>
#include <sdm/core/action/det_decision_rule.hpp>

#include <sdm/core/space/discrete_space.hpp>
#include <sdm/core/space/multi_discrete_space.hpp>
#include <sdm/core/space/function_space.hpp>

using namespace sdm;
using namespace std;
namespace po = boost::program_options;

int main(int argc, char **argv)
{
    number horizon, truncation, num_agents;
    std::vector<number> list_num_histories, list_num_action;

    po::options_description options("Options");
    options.add_options()
    ("num_agents,n", po::value<number>(&num_agents)->default_value(2), "the number of agents")
    ("action,a", po::value<std::vector<number>>(&list_num_action)->multitoken(), "list of number of observation")
    ("list_num_histories,h", po::value<std::vector<number>>(&list_num_histories)->multitoken(), "list of number of individual histories");

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, options), vm);
    po::notify(vm);

    std::cout << "Num Agents : " << num_agents << std::endl;
    std::cout << "Histories : " << list_num_histories << std::endl;
    std::cout << "Actions : " << list_num_action << std::endl;

    std::vector<std::shared_ptr<Space>> history_spaces, action_spaces;
    for (number agent_id = 0; agent_id < num_agents; agent_id++)
    {
        std::vector<std::shared_ptr<Item>> history_list, action_list;
        for (number history = 0; history < list_num_histories[agent_id]; history++)
        {
            history_list.push_back(std::make_shared<DiscreteState>(history));
        }
        for (number action = 0; action < list_num_action[agent_id]; action++)
        {
            action_list.push_back(std::make_shared<DiscreteAction>(action));
        }
        history_spaces.push_back(std::make_shared<DiscreteSpace>(history_list));
        action_spaces.push_back(std::make_shared<DiscreteSpace>(action_list));
    }
    auto history_space = std::make_shared<MultiDiscreteSpace>(history_spaces, false);
    auto action_space = std::make_shared<MultiDiscreteSpace>(action_spaces, false);

    std::vector<std::shared_ptr<Space>> vector_indiv_space;
    for (int agent_id = 0; agent_id < num_agents; agent_id++)
    {
        // Get history space of agent i
        auto history_space_i = std::static_pointer_cast<Joint<std::shared_ptr<Space>>>(history_space)->get(agent_id);
        // Get action space of agent i
        auto action_space_i = std::static_pointer_cast<Joint<std::shared_ptr<Space>>>(action_space)->get(agent_id);
        // Add individual decision rule space of agent i
        vector_indiv_space.push_back(std::make_shared<FunctionSpace<DeterministicDecisionRule>>(history_space_i, action_space_i, false));
    }

    // Now we can return a discrete space of all joint decision rules
    auto multi_dspace = std::make_shared<MultiDiscreteSpace>(vector_indiv_space, false);

    std::cout << "--- START DECISION RULE GENERATION ---" << std::endl;

    int i = 0;
    for (const auto &joint_dr : *multi_dspace)
    {
        std::cout << "\033[1m\033[36m  <decision id=\"" << i << "\" addr=\"" << joint_dr->str() << "\" /> \033[0m" << std::endl;
        i++;
    }

    std::cout << "--- END DECISION RULE GENERATION ---" << std::endl;

} // END main


/*
 * @Description: Kalman filter based localization on GNSS-INS-Sim
 * @Author: Ge Yao
 * @Date: 2020-11-12 15:14:07
 */
#ifndef LIDAR_LOCALIZATION_FILTERING_GNSS_INS_SIM_FILTERING_HPP_
#define LIDAR_LOCALIZATION_FILTERING_GNSS_INS_SIM_FILTERING_HPP_

#include <string>
#include <deque>
#include <unordered_map>

#include <Eigen/Dense>

#include <yaml-cpp/yaml.h>

#include "lidar_localization/EKFStd.h"

#include "lidar_localization/sensor_data/imu_data.hpp"
#include "lidar_localization/sensor_data/pos_vel_mag_data.hpp"
#include "lidar_localization/sensor_data/pose_data.hpp"

#include "lidar_localization/models/kalman_filter/kalman_filter.hpp"

namespace lidar_localization {

class GNSSINSSimFiltering {
  public:
    GNSSINSSimFiltering();

    bool Init(
      const Eigen::Matrix4f& init_pose,
      const Eigen::Vector3f &init_vel,
      const IMUData &init_imu_data
    );

    bool Update(
      const IMUData &imu_data
    );
    bool Correct(
      const IMUData &imu_data,
      const PosVelMagData &pos_vel_mag_data
    );

    // getters:
    bool HasInited() const { return has_inited_; }

    double GetTime(void) { return kalman_filter_ptr_->GetTime(); }
    Eigen::Matrix4f GetPose(void) { return current_pose_; }
    Eigen::Vector3f GetVel(void) { return current_vel_; }
    void GetOdometry(Eigen::Matrix4f &pose, Eigen::Vector3f &vel);
    void GetStandardDeviation(EKFStd &kf_std_msg);
    void SaveObservabilityAnalysis(void);

  private:
    bool InitWithConfig(void);
    bool InitFusion(const YAML::Node& config_node);

    // init pose setter:
    bool SetInitGNSS(const Eigen::Matrix4f& init_pose);
    bool SetInitPose(const Eigen::Matrix4f& init_pose);

  private:
    bool has_inited_ = false;

    // Kalman filter:
    struct {
      std::string FUSION_METHOD;

      std::unordered_map<std::string, KalmanFilter::MeasurementType> FUSION_STRATEGY_ID;
      KalmanFilter::MeasurementType FUSION_STRATEGY;
    } CONFIG;
    std::shared_ptr<KalmanFilter> kalman_filter_ptr_;
    KalmanFilter::Measurement current_measurement_;

    Eigen::Matrix4f current_gnss_pose_ = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f init_pose_ = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f current_pose_ = Eigen::Matrix4f::Identity();
    Eigen::Vector3f current_vel_ = Eigen::Vector3f::Zero();
    KalmanFilter::Cov current_cov_;
};

} // namespace lidar_localization

#endif // LIDAR_LOCALIZATION_FILTERING_GNSS_INS_SIM_FILTERING_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_FUNCTION_EXP_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_EXP_HPP_INCLUDED

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

  /*!
    @ingroup group-exponential
    This function object returns the exponential of its argument: \f$e^{x}\f$

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

    @par Decorators

      - std_ for floating entries calls @c std::exp

    @see exp2, exp10, pow, expm1, log

    @par Example:

      @snippet exp.cpp exp

    @par Possible output:

      @snippet exp.txt exp

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

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

#endif


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include "MiniAppManager.h"
#include <vector>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <string>

#include <itkImage.h>
#include <itkImageFileReader.h>

#include <itkExceptionObject.h>
#include <itkImageFileWriter.h>

#include <itkMetaDataObject.h>

#include "itkVectorContainer.h"
#include "vnl/vnl_vector_fixed.h"
#include "itkVectorImage.h"

#include "mitkBaseDataIOFactory.h"
#include "mitkDiffusionImage.h"
#include "mitkBaseData.h"
#include "mitkDiffusionCoreObjectFactory.h"
#include "mitkCoreObjectFactory.h"
#include "mitkCoreExtObjectFactory.h"
#include "mitkImageWriter.h"
#include "itkAnalyticalDiffusionQballReconstructionImageFilter.h"
#include <boost/lexical_cast.hpp>
#include <mitkNrrdQBallImageWriter.h>

using namespace mitk;
/**
 * Convert files from one ending to the other
 */
int QballReconstruction(int argc, char* argv[])
{

    if ( argc!=6
         )
    {
        std::cout << std::endl;
        std::cout << "Perform QBall reconstruction on an dwi file" << std::endl;
        std::cout << std::endl;
        std::cout << "usage: " << argv[0] << " " << argv[1] << " <in-file> <lambda> <baseline-threshold> <out-file> " << std::endl;
        std::cout << std::endl;
        return EXIT_FAILURE;
    }

    try
    {
        RegisterDiffusionCoreObjectFactory();

        MITK_INFO << "Loading image ...";
        const std::string s1="", s2="";
        std::vector<BaseData::Pointer> infile = BaseDataIO::LoadBaseDataFromFile( argv[2], s1, s2, false );
        DiffusionImage<short>::Pointer dwi = dynamic_cast<DiffusionImage<short>*>(infile.at(0).GetPointer());

        typedef itk::AnalyticalDiffusionQballReconstructionImageFilter<short,short,float,4,QBALL_ODFSIZE> FilterType;

        dwi->AverageRedundantGradients(0.001);

        FilterType::Pointer filter = FilterType::New();
        filter->SetGradientImage( dwi->GetDirections(), dwi->GetVectorImage() );
        filter->SetBValue(dwi->GetB_Value());
        filter->SetThreshold( boost::lexical_cast<int>(argv[4]) );
        filter->SetLambda(boost::lexical_cast<float>(argv[3]));
        filter->SetNormalizationMethod(FilterType::QBAR_SOLID_ANGLE);
        filter->Update();

        mitk::QBallImage::Pointer image = mitk::QBallImage::New();
        image->InitializeByItk( filter->GetOutput() );
        image->SetVolume( filter->GetOutput()->GetBufferPointer() );

        std::string outfilename = argv[5];
        MITK_INFO << "writing image " << outfilename;
        mitk::NrrdQBallImageWriter::Pointer writer = mitk::NrrdQBallImageWriter::New();
        writer->SetInput(image.GetPointer());
        writer->SetFileName(outfilename.c_str());
        writer->Update();
    }
    catch ( itk::ExceptionObject &err)
    {
        MITK_INFO << "Exception: " << err;
    }
    catch ( std::exception err)
    {
        MITK_INFO << "Exception: " << err.what();
    }
    catch ( ... )
    {
        MITK_INFO << "Exception!";
    }
    return EXIT_SUCCESS;
}
RegisterDiffusionCoreMiniApp(QballReconstruction);


#define EIGEN_DEFAULT_DENSE_INDEX_TYPE int32_t

#include "../repos/stl_reader/stl_reader.h"
#include <array>
#include <deque>
#include <chrono>
#include <random>
#include <fstream>
#include <iostream>
#include <Eigen/Dense>

using Triangle = std::array<Eigen::Vector3f, 3u>;
using CudaTriangle = Eigen::Vector3f*;
using CudaConstTriangle = Eigen::Vector3f const*;
using Triangles = std::deque<Triangle>;

constexpr float   cgEpsilon        = 0.00001f;       // TODO consider if uniform epsilon suits all needs.
constexpr uint32_t cgSignumZero     = 0u;
constexpr uint32_t cgSignumPlus     = 1u;
constexpr uint32_t cgSignumMinus    = 2u;
constexpr uint32_t cgSignumShift0   = 0u;
constexpr uint32_t cgSignumShift1   = 2u;
constexpr uint32_t cgSignumShift2   = 4u;
constexpr uint32_t cgSignumAllZero  = (cgSignumZero  << cgSignumShift0) | (cgSignumZero  << cgSignumShift1) | (cgSignumZero  << cgSignumShift2);
constexpr uint32_t cgSignumAllPlus  = (cgSignumPlus  << cgSignumShift0) | (cgSignumPlus  << cgSignumShift1) | (cgSignumPlus  << cgSignumShift2);
constexpr uint32_t cgSignumAllMinus = (cgSignumMinus << cgSignumShift0) | (cgSignumMinus << cgSignumShift1) | (cgSignumMinus << cgSignumShift2);

constexpr uint32_t cgSignumSelect0a = (cgSignumPlus  << cgSignumShift0) | (cgSignumMinus << cgSignumShift1) | (cgSignumMinus << cgSignumShift2);
constexpr uint32_t cgSignumSelect0b = (cgSignumMinus << cgSignumShift0) | (cgSignumPlus  << cgSignumShift1) | (cgSignumPlus  << cgSignumShift2);
constexpr uint32_t cgSignumSelect0c = (cgSignumZero  << cgSignumShift0) | (cgSignumPlus  << cgSignumShift1) | (cgSignumPlus  << cgSignumShift2);
constexpr uint32_t cgSignumSelect0d = (cgSignumZero  << cgSignumShift0) | (cgSignumMinus << cgSignumShift1) | (cgSignumMinus << cgSignumShift2);
constexpr uint32_t cgSignumSelect0e = (cgSignumPlus  << cgSignumShift0) | (cgSignumZero  << cgSignumShift1) | (cgSignumZero  << cgSignumShift2);
constexpr uint32_t cgSignumSelect0f = (cgSignumMinus << cgSignumShift0) | (cgSignumZero  << cgSignumShift1) | (cgSignumZero  << cgSignumShift2);
constexpr uint32_t cgSignumSelect0g = (cgSignumZero  << cgSignumShift0) | (cgSignumPlus  << cgSignumShift1) | (cgSignumMinus << cgSignumShift2);
constexpr uint32_t cgSignumSelect0h = (cgSignumZero  << cgSignumShift0) | (cgSignumMinus << cgSignumShift1) | (cgSignumPlus  << cgSignumShift2);

constexpr uint32_t cgSignumSelect1a = (cgSignumPlus  << cgSignumShift1) | (cgSignumMinus << cgSignumShift0) | (cgSignumMinus << cgSignumShift2);
constexpr uint32_t cgSignumSelect1b = (cgSignumMinus << cgSignumShift1) | (cgSignumPlus  << cgSignumShift0) | (cgSignumPlus  << cgSignumShift2);
constexpr uint32_t cgSignumSelect1c = (cgSignumZero  << cgSignumShift1) | (cgSignumPlus  << cgSignumShift0) | (cgSignumPlus  << cgSignumShift2);
constexpr uint32_t cgSignumSelect1d = (cgSignumZero  << cgSignumShift1) | (cgSignumMinus << cgSignumShift0) | (cgSignumMinus << cgSignumShift2);
constexpr uint32_t cgSignumSelect1e = (cgSignumPlus  << cgSignumShift1) | (cgSignumZero  << cgSignumShift0) | (cgSignumZero  << cgSignumShift2);
constexpr uint32_t cgSignumSelect1f = (cgSignumMinus << cgSignumShift1) | (cgSignumZero  << cgSignumShift0) | (cgSignumZero  << cgSignumShift2);
constexpr uint32_t cgSignumSelect1g = (cgSignumZero  << cgSignumShift1) | (cgSignumPlus  << cgSignumShift0) | (cgSignumMinus << cgSignumShift2);
constexpr uint32_t cgSignumSelect1h = (cgSignumZero  << cgSignumShift1) | (cgSignumMinus << cgSignumShift0) | (cgSignumPlus  << cgSignumShift2);

// Otherwise select 2, no need for checking and thus no constants.

constexpr uint32_t cgSignumCircumferenceA = (cgSignumZero << cgSignumShift0) | (cgSignumPlus << cgSignumShift1) | (cgSignumPlus << cgSignumShift2);
constexpr uint32_t cgSignumCircumferenceB = (cgSignumZero << cgSignumShift0) | (cgSignumZero << cgSignumShift1) | (cgSignumPlus << cgSignumShift2);
constexpr uint32_t cgSignumCircumferenceC = (cgSignumPlus << cgSignumShift0) | (cgSignumZero << cgSignumShift1) | (cgSignumPlus << cgSignumShift2);
constexpr uint32_t cgSignumCircumferenceD = (cgSignumPlus << cgSignumShift0) | (cgSignumZero << cgSignumShift1) | (cgSignumZero << cgSignumShift2);
constexpr uint32_t cgSignumCircumferenceE = (cgSignumPlus << cgSignumShift0) | (cgSignumPlus << cgSignumShift1) | (cgSignumZero << cgSignumShift2);
constexpr uint32_t cgSignumCircumferenceF = (cgSignumZero << cgSignumShift0) | (cgSignumPlus << cgSignumShift1) | (cgSignumZero << cgSignumShift2);

constexpr uint32_t calculateSignum(float const distances[3]) noexcept {
  uint32_t result = 0u;
  for(int32_t i = 0; i < 3; ++i) {
    int32_t tmp = cgSignumZero;
    if(distances[i] > cgEpsilon) {
      tmp = cgSignumPlus;
    }
    else if(distances[i] < -cgEpsilon) {
      tmp = cgSignumMinus;
    }
    else { // nothing to do
    }
    result |= tmp << (cgSignumShift1 * i);
  }
  return result;
}

void calculateNormals(Eigen::Vector2f const aShape[3], Eigen::Vector2f aNormals[3]) noexcept {
  Eigen::Vector2f side = aShape[1] - aShape[0];
  aNormals[0](0) = -side(1);
  aNormals[0](1) = side(0);
  float correction = 1.0f;
  if(aNormals[0].dot(aShape[2]) < 0.0f) {
    correction = -1.0f;                     // They shall point towards the interior.
    aNormals[0] *= correction;
  }
  else { // nothing to do
  }
  side = aShape[2] - aShape[1];
  aNormals[1](0) = -side(1) * correction;
  aNormals[1](1) = side(0) * correction;
  side = aShape[0] - aShape[2];
  aNormals[2](0) = -side(1) * correction;
  aNormals[2](1) = side(0) * correction;
}

bool doesTouchOther(uint32_t const aSignums) noexcept {
  bool result = (aSignums == cgSignumAllPlus
  || aSignums == cgSignumCircumferenceA
  || aSignums == cgSignumCircumferenceB
  || aSignums == cgSignumCircumferenceC
  || aSignums == cgSignumCircumferenceD
  || aSignums == cgSignumCircumferenceE
  || aSignums == cgSignumCircumferenceF);
if(result) std::cout << "coplanar circumference or interior\n";
  return result;
}

bool checkCornerOnPerimeterAndInterior(Eigen::Vector2f aShape1[3], Eigen::Vector2f aShape2[3]) noexcept {
  Eigen::Vector2f normals1[3]; // i : i->(i+1)%3
  Eigen::Vector2f normals2[3];
  calculateNormals(aShape1, normals1); // Normal vectors point to the center.
  calculateNormals(aShape2, normals2);
  bool result = false;
  for(int32_t indexCorner = 0; indexCorner < 3; ++indexCorner) {
    float distancesCornerFromEachSideOfShape1[3];
    float distancesCornerFromEachSideOfShape2[3];
    for(int32_t indexSide = 0; indexSide < 3; ++indexSide) {
      distancesCornerFromEachSideOfShape2[indexSide] = normals2[indexSide].dot(aShape1[indexCorner] - aShape2[indexSide]);
      distancesCornerFromEachSideOfShape1[indexSide] = normals1[indexSide].dot(aShape2[indexCorner] - aShape1[indexSide]);
    }
    result = result || doesTouchOther(calculateSignum(distancesCornerFromEachSideOfShape2));
    result = result || doesTouchOther(calculateSignum(distancesCornerFromEachSideOfShape1));   // True if one corner is on the sides, corners or interior of the other triangle.
    // Don't break out since it won't use on CUDA.
  }
  return result;
}

bool checkTrueIntersecitonOfSides(Eigen::Vector2f aShape1[3], Eigen::Vector2f aShape2[3]) noexcept {
  bool result = false;
  for(int32_t indexSide1 = 0; indexSide1 < 3; ++indexSide1) {
    for(int32_t indexSide2 = 0; indexSide2 < 3; ++indexSide2) {
      Eigen::Vector2f &side1a = aShape1[indexSide1];
      Eigen::Vector2f &side1b = aShape1[(indexSide1 + 1) % 3];
      Eigen::Vector2f &side2a = aShape2[indexSide2];
      Eigen::Vector2f &side2b = aShape2[(indexSide2 + 1) % 3];
      // Manually solve linear EQ to make sure we have as few branches as possible.
      uint32_t nonzeroAindex = (fabs(side1a(0) - side1b(0)) > cgEpsilon ? 0 : 1);
      float a = side1a(nonzeroAindex) - side1b(nonzeroAindex);
      float b = side2b(nonzeroAindex) - side2a(nonzeroAindex);
      float c = side1a(1 - nonzeroAindex) - side1b(1 - nonzeroAindex);
      float d = side2b(1 - nonzeroAindex) - side2a(1 - nonzeroAindex);
      float determinant = a * d - b * c;
      if(fabs(determinant) > cgEpsilon) {
        float k = side1a(nonzeroAindex) - side2a(nonzeroAindex);
        float l = side1a(1 - nonzeroAindex) - side2a(1 - nonzeroAindex);
        float v = (l * a - c * k) / determinant;
        float u = (k - b * v) / a;
if(u >= 0.0f && u <= 1.0f && v >= 0.0f && v <= 1.0f) std::cout << u << ' ' << v << " coplanar sides intersect\n";
        result = result || (u >= 0.0f && u <= 1.0f && v >= 0.0f && v <= 1.0f);  // The intersection is inside of both sides.
      }
      else { // nothing to do, because the lines are parallel but can't touch each other.
      }
    }
  }
  return result;
}

bool hasCommonPoint(CudaConstTriangle const aShape1, CudaConstTriangle const aShape2, Eigen::Vector3f const &aShape1normal, Eigen::Vector3f const &aShape2normal) noexcept { // coplanar
  Eigen::Vector3f normal;
  if(aShape1normal.dot(aShape2normal) > 0.0f) {
    normal = aShape1normal + aShape2normal;
  }
  else {
    normal = aShape1normal - aShape2normal;
  }
  int32_t indexX = 1;
  int32_t indexY = 2;
  float abs1 = fabs(normal(1)); // Looking for the biggest projection. TODO fabsf for CUDA
  if(abs1 > fabs(normal(0))) {
    indexX = 0;
  }
  else { // nothing to do
  }
  if(fabs(normal(2)) > abs1) {
    indexX = 0;
    indexY = 1;
  }
  else { // nothing to do
  }
  Eigen::Vector2f shape1[3];
  Eigen::Vector2f shape2[3];
  for(int32_t i = 0; i < 3; ++i) {
    shape1[i](0) = aShape1[i](indexX);  // Project 3D triangle to axis-parallel plane.
    shape1[i](1) = aShape1[i](indexY);
    shape2[i](0) = aShape2[i](indexX);
    shape2[i](1) = aShape2[i](indexY);
  }
  bool result = checkCornerOnPerimeterAndInterior(shape1, shape2);
  if(!result) { // Common point may only occur now when sides truly intersect each other.
    result = checkTrueIntersecitonOfSides(shape1, shape2);
  }
  else { // nothing to do
  }
  return result;
}

void calculateIntersectionParameter(
  CudaConstTriangle const aShape
, Eigen::Vector3f const &aIntersectionVector
, float const aDistanceCornerNfromOtherPlane[3]
, uint32_t const aSignumShapeFromOtherPlane
, float &aIntersectionParameterA
, float &aIntersectionParameterB) noexcept {
  int32_t indexCommon, indexA, indexB;
  if(aSignumShapeFromOtherPlane == cgSignumSelect0a
  || aSignumShapeFromOtherPlane == cgSignumSelect0b
  || aSignumShapeFromOtherPlane == cgSignumSelect0c
  || aSignumShapeFromOtherPlane == cgSignumSelect0d
  || aSignumShapeFromOtherPlane == cgSignumSelect0e
  || aSignumShapeFromOtherPlane == cgSignumSelect0f
  || aSignumShapeFromOtherPlane == cgSignumSelect0g
  || aSignumShapeFromOtherPlane == cgSignumSelect0h) {
    indexCommon = 0; indexA = 1; indexB = 2;
  }
  else if(aSignumShapeFromOtherPlane == cgSignumSelect1a
  || aSignumShapeFromOtherPlane == cgSignumSelect1b
  || aSignumShapeFromOtherPlane == cgSignumSelect1c
  || aSignumShapeFromOtherPlane == cgSignumSelect1d
  || aSignumShapeFromOtherPlane == cgSignumSelect1e
  || aSignumShapeFromOtherPlane == cgSignumSelect1f
  || aSignumShapeFromOtherPlane == cgSignumSelect1g
  || aSignumShapeFromOtherPlane == cgSignumSelect1h) {
    indexCommon = 1; indexA = 0; indexB = 2;
  }
  else {
    indexCommon = 2; indexA = 1; indexB = 0;
  }
  float vertexProjections[3];
  for(int32_t i = 0; i < 3; ++i) {
    vertexProjections[i] = aIntersectionVector.dot(aShape[i]);
  }
  aIntersectionParameterA =
   vertexProjections[indexA]
 + (vertexProjections[indexCommon] - vertexProjections[indexA])
 * aDistanceCornerNfromOtherPlane[indexA]
 / (aDistanceCornerNfromOtherPlane[indexA] - aDistanceCornerNfromOtherPlane[indexCommon]);
  aIntersectionParameterB =
   vertexProjections[indexB]
 + (vertexProjections[indexCommon] - vertexProjections[indexB])
 * aDistanceCornerNfromOtherPlane[indexB]
 / (aDistanceCornerNfromOtherPlane[indexB] - aDistanceCornerNfromOtherPlane[indexCommon]);
}

bool hasCommonPoint(CudaConstTriangle const aShape1, CudaConstTriangle const aShape2) noexcept { // Entry point for common point check.
  bool result = false;
  Eigen::Vector3f shape1normal = (aShape1[1] - aShape1[0]).cross(aShape1[2] - aShape1[0]);
  Eigen::Vector3f shape2normal = (aShape2[1] - aShape2[0]).cross(aShape2[2] - aShape2[0]);
  shape1normal.normalize();
  shape2normal.normalize();
  float distanceCornerNofShape1FromPlane2[3];
  float distanceCornerNofShape2FromPlane1[3];
  for(int32_t i = 0; i < 3; ++i) {
    distanceCornerNofShape1FromPlane2[i] = shape2normal.dot(aShape1[i] - aShape2[0]);
    distanceCornerNofShape2FromPlane1[i] = shape1normal.dot(aShape2[i] - aShape1[0]);
  }
  uint32_t signumShape1FromPlane2 = calculateSignum(distanceCornerNofShape1FromPlane2); // These contain info about relation of each point and the other plane.
  uint32_t signumShape2FromPlane1 = calculateSignum(distanceCornerNofShape2FromPlane1);
  if(signumShape1FromPlane2 == cgSignumAllPlus || signumShape1FromPlane2 == cgSignumAllMinus || signumShape2FromPlane1 == cgSignumAllPlus || signumShape2FromPlane1 == cgSignumAllMinus) {
    // Nothing to do: one triangle is completely on the one side of the other's plane
  }
  else {
    Eigen::Vector3f intersectionVector = shape1normal.cross(shape2normal);
    if(intersectionVector.norm() > cgEpsilon && signumShape1FromPlane2 != cgSignumAllZero && signumShape2FromPlane1 != cgSignumAllZero) { // Real intersection, planes are not identical, and both triangles touch the common line.
      intersectionVector.normalize();
      float intersectionParameterAshape1;
      float intersectionParameterBshape1;
      float intersectionParameterAshape2;
      float intersectionParameterBshape2;
      calculateIntersectionParameter(aShape1, intersectionVector, distanceCornerNofShape1FromPlane2, signumShape1FromPlane2, intersectionParameterAshape1, intersectionParameterBshape1); // The two parameters will contain the locations of the touching point.
      calculateIntersectionParameter(aShape2, intersectionVector, distanceCornerNofShape2FromPlane1, signumShape2FromPlane1, intersectionParameterAshape2, intersectionParameterBshape2);
      if(intersectionParameterAshape1 > intersectionParameterBshape1) {
        std::swap(intersectionParameterAshape1, intersectionParameterBshape1);
      }
      else { // nothing to do
      }
      if(intersectionParameterAshape2 > intersectionParameterBshape2) {
        std::swap(intersectionParameterAshape2, intersectionParameterBshape2);
      }
      else { // nothing to do
      }
      if(intersectionParameterAshape1 - cgEpsilon <= intersectionParameterAshape2 && intersectionParameterAshape2 <= intersectionParameterBshape1 + cgEpsilon // Epsilons make possible to check for triangles with corner-corner or corner-edge touch.
      || intersectionParameterAshape1 - cgEpsilon <= intersectionParameterBshape2 && intersectionParameterBshape2 <= intersectionParameterBshape1 + cgEpsilon
      || intersectionParameterAshape2 - cgEpsilon <= intersectionParameterAshape1 && intersectionParameterAshape1 <= intersectionParameterBshape2 + cgEpsilon
      || intersectionParameterAshape2 - cgEpsilon <= intersectionParameterBshape1 && intersectionParameterBshape1 <= intersectionParameterBshape2 + cgEpsilon) {
std::cout << "on intersecting line\n";
        result = true;
      }
      else { // nothing to do
      }
    }
    else {
      result = hasCommonPoint(aShape1, aShape2, shape1normal, shape2normal); // Coplanar triangles
    }
  }
  return result;
}

Eigen::Matrix3f randomTransform() {
  std::default_random_engine generator;
  generator.seed((std::chrono::high_resolution_clock::now() - std::chrono::high_resolution_clock::time_point::min()).count());
  std::uniform_real_distribution<float> distribution(0.0f, 1.0f);
  Eigen::Matrix3f result;
  for(int32_t i = 0; i < 9; ++i) {
    result(i / 3, i % 3) = distribution(generator);
  }
  return result;
}

Triangles readTriangles(char const * const aFilename, Eigen::Matrix3f const &aTransform) {
  Triangles result;
  stl_reader::StlMesh<float, int32_t> mesh(aFilename);
  for(int32_t indexTriangle = 0; indexTriangle < mesh.num_tris(); ++indexTriangle) {
      Triangle triangle;
      for(int32_t indexCorner = 0; indexCorner < 3; ++indexCorner) {
          float const * const coords = mesh.tri_corner_coords(indexTriangle, indexCorner);
          Eigen::Vector3f in;
          for(int32_t i = 0; i < 3; ++i) {
            in(i) = coords[i];
          }
          triangle[indexCorner] = aTransform * in;
      }
      result.push_back(triangle);
  }
  return result;
}

void writeTriangles(Triangles const &aTriangles, char const * const aFilename) {
  std::ofstream out(aFilename);
  out << "solid Exported from Blender-2.82 (sub 7)\n";
  for(auto const & triangle : aTriangles) {
//    Eigen::Vector3f normal = (triangle[1] - triangle[0]).cross(aShape2[2] - aShape2[0]);
//  shape1normal.normalize();
    out << "facet normal 0.000000 0.000000 0.000000\nouter loop\n";
    for(auto const & vertex : triangle) {
      out << "vertex " << vertex(0) << ' ' << vertex(1) << ' ' << vertex(2) << '\n';
    }
    out << "endloop\nendfacet\n";
  }
  out << "endsolid Exported from Blender-2.82 (sub 7)\n";
}

void check(Triangles const &aTriangles) {
  for(int32_t i = 0; i < aTriangles.size(); ++i) {
    for(int32_t j = i + 1; j < aTriangles.size(); ++j) {
      if(hasCommonPoint(aTriangles[i].data(), aTriangles[j].data())) {
        std::cout << "Has common point: " << i << ' ' << j << "\n\n";
      }
      else { // nothing to do
      }
    }
  }
}

int main(int argc, char **argv) {
  int ret = 0;
  if(argc < 2) {
    std::cerr << "Usage: " << argv[0] << " <filenameIn> [filenameOut]\n";
    ret = 1;
  }
  else {
    try {
      auto transform = randomTransform();
      auto triangles = readTriangles(argv[1], transform);
      if(argc >= 3) {
        writeTriangles(triangles, argv[2]);
      }
      else { // nothing to do
      }
      check(triangles);
    }
    catch(std::exception &e) {
      ret = 2;
    }
  }
  return ret;
}


#include <cctbx/miller/f_calc_map.h>

#include <boost/python/class.hpp>

namespace cctbx { namespace miller { namespace boost_python {

  template <typename FloatType>
  struct f_calc_map_wrapper
  {
    typedef f_calc_map<FloatType> wt;

    static void wrap(char const *name) {
      using namespace boost::python;
      class_<wt>(name, no_init)
        .def(init<af::const_ref<miller::index<> > const&,
                  af::const_ref<typename wt::complex_type> const &,
                  bool>
             ((arg("indices"),
               arg("f_calc"),
               arg("anomalous_flag"))))
        .add_property("anomalous_flag", &wt::anomalous_flag)
        .def("import", &wt::import)
        .def("__getitem__", &wt::operator[])
        ;
    }
  };

  void wrap_f_calc_map() {
    f_calc_map_wrapper<double>::wrap("f_calc_map");
  }

}}}


#include "include/util/plotter.h"

#include <Eigen/Core>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>

namespace util {

void Plotter::ShowEpipolarMatch(const cv::Mat &ref, const cv::Mat &curr,
                                const Eigen::Vector2d &px_ref,
                                const Eigen::Vector2d &px_curr) {
  cv::Mat ref_show, curr_show;
  cv::cvtColor(ref, ref_show, CV_GRAY2BGR);
  cv::cvtColor(curr, curr_show, CV_GRAY2BGR);

  cv::circle(ref_show, cv::Point2f(px_ref(0, 0), px_ref(1, 0)), 5,
             cv::Scalar(0, 0, 250), 2);
  cv::circle(curr_show, cv::Point2f(px_curr(0, 0), px_curr(1, 0)), 5,
             cv::Scalar(0, 0, 250), 2);

  cv::imshow("ref", ref_show);
  cv::imshow("curr", curr_show);
  cv::waitKey(1);
}

void Plotter::ShowEpipolarLine(const cv::Mat &ref, const cv::Mat &curr,
                               const Eigen::Vector2d &px_ref,
                               const Eigen::Vector2d &px_min_curr,
                               const Eigen::Vector2d &px_max_curr) {
  cv::Mat ref_show, curr_show;
  cv::cvtColor(ref, ref_show, CV_GRAY2BGR);
  cv::cvtColor(curr, curr_show, CV_GRAY2BGR);

  cv::circle(ref_show, cv::Point2f(px_ref(0, 0), px_ref(1, 0)), 5,
             cv::Scalar(0, 255, 0), 2);
  cv::circle(curr_show, cv::Point2f(px_min_curr(0, 0), px_min_curr(1, 0)), 5,
             cv::Scalar(0, 255, 0), 2);
  cv::circle(curr_show, cv::Point2f(px_max_curr(0, 0), px_max_curr(1, 0)), 5,
             cv::Scalar(0, 255, 0), 2);
  cv::line(curr_show, cv::Point2f(px_min_curr(0, 0), px_min_curr(1, 0)),
           cv::Point2f(px_max_curr(0, 0), px_max_curr(1, 0)),
           cv::Scalar(0, 255, 0), 1);

  cv::imshow("ref", ref_show);
  cv::imshow("curr", curr_show);
  cv::waitKey(1);
}

void Plotter::PlotDepth(const cv::Mat &depth_truth,
                        const cv::Mat &depth_estimate) {
  cv::imshow("depth_truth", depth_truth * 0.4);
  cv::imshow("depth_estimate", depth_estimate * 0.4);
  cv::imshow("depth_error", depth_truth - depth_estimate);
  cv::waitKey(1);
}

}  // util


#include "stdafx.h"

#include "problem.hpp"
#include "utility.hpp"

#include <fstream>
#include <regex>
#include <utility>
#include <unordered_map>
#include <unordered_set>
#include <numeric>
#include <boost/algorithm/string.hpp>

struct advent_2017_15 : problem
{
	advent_2017_15() noexcept : problem(2017, 15) {
	}

protected:
	struct generator
	{
		std::string name;
		uint64_t seed;
		uint64_t factor;
		uint64_t current;

		generator(const std::string& name_, uint64_t seed_, uint64_t factor_) : name(name_), seed(seed_), factor(factor_), current(seed_) {
		}

		uint16_t generate(uint64_t divisor) noexcept {
			for(;;) {
				const uint64_t product = current * factor;
				current = product % 2'147'483'647ui64;
				if((current % divisor) == 0ui64) {
					return current & 0xffff;
				}
			}
		}

		void reset() noexcept {
			current = seed;
		}
	};

	std::unique_ptr<generator> a;
	std::unique_ptr<generator> b;

	void prepare_input(std::ifstream& fin) override {
		std::regex pattern(R"(Generator ([[:upper:]]) starts with ([[:digit:]]*))");
		std::string line;
		std::getline(fin, line);
		std::smatch m;
		std::regex_search(line, m, pattern);
		a = std::make_unique<generator>(m[1], std::stoul(m[2]), 16'807);
		std::getline(fin, line);
		std::regex_search(line, m, pattern);
		b = std::make_unique<generator>(m[1], std::stoul(m[2]), 48'271);
	}

	std::string part_1() override {
		std::size_t pairs = 0;
		for(std::size_t i = 0; i < 40'000'000; ++i) {
			pairs += a->generate(1) == b->generate(1);
		}
		return std::to_string(pairs);
	}

	void tidy_up() noexcept override {
		a->reset();
		b->reset();
	}

	std::string part_2() override {
		std::size_t pairs = 0;
		for(std::size_t i = 0; i < 5'000'000; ++i) {
			pairs += a->generate(4) == b->generate(8);
		}
		return std::to_string(pairs);
	}
};

REGISTER_SOLVER(2017, 15);


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__FEEDBACK__OCP_HPP_
#define SMOOTH__FEEDBACK__OCP_HPP_

/**
 * @file
 * @brief Optimal control problem definition.
 */

#include <Eigen/Core>
#include <smooth/diff.hpp>
#include <smooth/lie_group.hpp>

#include <iostream>

#include "traits.hpp"

namespace smooth::feedback {

// \cond
// Forward declaration
template<LieGroup _X, Manifold _U, int _Nq, int _Ncr, int _Nce>
struct OCPSolution;
// \endcond

/**
 * @brief Optimal control problem definition
 * @tparam _X state space
 * @tparam _U input space
 *
 * Problem is defined on the interval \f$ t \in [0, t_f] \f$.
 * \f[
 * \begin{cases}
 *  \min              & \theta(t_f, x_0, x_f, q)                                         \\
 *  \text{s.t.}       & x(0) = x_0                                                       \\
 *                    & x(t_f) = x_f                                                     \\
 *                    & \dot x(t) = f(t, x(t), u(t))                                     \\
 *                    & q = \int_{0}^{t_f} g(t, x(t), u(t)) \mathrm{d}t                  \\
 *                    & c_{rl} \leq c_r(t, x(t), u(t)) \leq c_{ru} \quad t \in [0, t_f]  \\
 *                    & c_{el} \leq c_e(t_f, x_0, x_f, q) \leq c_{eu}
 * \end{cases}
 * \f]
 *
 * The optimal control problem depends on arbitrary functions \f$ \theta, f, g, c_r, c_e \f$.
 * The type of those functions are template pararamters in this structure.
 *
 * @note To enable automatic differentiation \f$ \theta, f, g, c_r, c_e \f$ must be templated over
 * the scalar type.
 */
template<LieGroup _X, Manifold _U, typename Theta, typename F, typename G, typename CR, typename CE>
struct OCP
{
  /// @brief State space
  using X = _X;
  /// @brief Input space
  using U = _U;

  /// @brief State space dimension
  static constexpr int Nx = Dof<X>;
  /// @brief Input space dimension
  static constexpr int Nu = Dof<U>;
  /// @brief Number of integrals
  static constexpr int Nq = std::invoke_result_t<G, double, X, U>::SizeAtCompileTime;
  /// @brief Number of running constraints
  static constexpr int Ncr = std::invoke_result_t<CR, double, X, U>::SizeAtCompileTime;
  /// @brief Number of end constraints
  static constexpr int Nce =
    std::invoke_result_t<CE, double, X, X, Eigen::Matrix<double, Nq, 1>>::SizeAtCompileTime;

  /// @brief Solution type corresponding to this problem
  using Solution = OCPSolution<X, U, Nq, Ncr, Nce>;

  static_assert(Nx > 0, "Static size required");
  static_assert(Nu > 0, "Static size required");
  static_assert(Nq > 0, "Static size required");
  static_assert(Ncr > 0, "Static size required");
  static_assert(Nce > 0, "Static size required");

  /// @brief Objective function \f$ \theta : R \times X \times X \times R^{n_q} \rightarrow R \f$
  Theta theta;

  /// @brief System dynamics \f$ f : R \times X \times U \rightarrow Tangent<X> \f$
  F f;
  /// @brief Integrals \f$ g : R \times X \times U \rightarrow R^{n_q} \f$
  G g;

  /// @brief Running constraint \f$ c_r : R \times X \times U \rightarrow R^{n_{cr}} \f$
  CR cr;
  /// @brief Running constraint lower bound \f$ c_{rl} \in R^{n_{cr}} \f$
  Eigen::Vector<double, Ncr> crl = Eigen::Vector<double, Ncr>::Zero();
  /// @brief Running constraint upper bound \f$ c_{ru} \in R^{n_{cr}} \f$
  Eigen::Vector<double, Ncr> cru = Eigen::Vector<double, Ncr>::Zero();

  /// @brief End constraint \f$ c_e : R \times X \times X \times R^{n_q} \rightarrow R^{n_{ce}} \f$
  CE ce;
  /// @brief End constraint lower bound \f$ c_{el} \in R^{n_{ce}} \f$
  Eigen::Vector<double, Nce> cel = Eigen::Vector<double, Nce>::Zero();
  /// @brief End constraint upper bound \f$ c_{eu} \in R^{n_{ce}} \f$
  Eigen::Vector<double, Nce> ceu = Eigen::Vector<double, Nce>::Zero();
};

/// @brief Concept that is true for OCP specializations
template<typename T>
concept OCPType = traits::is_specialization_of_v<std::decay_t<T>, OCP>;

/// @brief Concept that is true for FlatOCP specializations
template<typename T>
concept FlatOCPType = OCPType<T> &&(smooth::traits::RnType<typename std::decay_t<T>::X> &&
                                      smooth::traits::RnType<typename std::decay_t<T>::U>);

/**
 * @brief Solution to OCP.
 */
template<LieGroup _X, Manifold _U, int _Nq, int _Ncr, int _Nce>
struct OCPSolution
{
  /// @brief State space
  using X = _X;
  /// @brief Input space
  using U = _U;

  /// @brief Number of integrals
  static constexpr int Nq = _Nq;
  /// @brief Number of running constraints
  static constexpr int Ncr = _Ncr;
  /// @brief Number of end constraints
  static constexpr int Nce = _Nce;

  ///@{
  /// @brief Initial and final time
  double t0, tf;
  //}@

  /// @brief Integral values
  Eigen::Vector<double, Nq> Q{};

  ///@{
  /// @brief Callable functions for state and input
  std::function<U(double)> u;
  std::function<X(double)> x;
  //}@

  /// @brief Multipliers for integral constraints
  Eigen::Vector<double, Nq> lambda_q{};

  /// @brief Multipliers for endpoint constraints
  Eigen::Vector<double, Nce> lambda_ce{};

  /// @brief Multipliers for dynamics equality constraint
  std::function<Eigen::Vector<double, Dof<X>>(double)> lambda_dyn{};

  /// @brief Multipliers for active running constraints
  std::function<Eigen::Vector<double, Ncr>(double)> lambda_cr{};
};

/**
 * @brief Test analytic derivatives for an OCP problem.
 *
 * @tparam DT differentiation method to compare against.
 *
 * @param ocp problem to test derivatives for
 * @param num_trials number of random points to test
 *
 * @todo Make it possible to test a subset of derivatives
 */
template<diff::Type DT = diff::Type::Numerical>
bool test_ocp_derivatives(OCPType auto & ocp, uint32_t num_trials = 1, double eps = 1e-4)
{
  using OCP = std::decay_t<decltype(ocp)>;

  using X = typename OCP::X;
  using U = typename OCP::U;
  using Q = Eigen::Vector<double, OCP::Nq>;

  if (!diff::detail::diffable_order1<decltype(ocp.theta), std::tuple<double, X, X, Q>>) {
    std::cout << "no jacobian for theta\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.theta), std::tuple<double, X, X, Q>>) {
    std::cout << "no hessian for theta\n";
  }
  if (!diff::detail::diffable_order1<decltype(ocp.f), std::tuple<double, X, U>>) {
    std::cout << "no jacobian for f\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.f), std::tuple<double, X, U>>) {
    std::cout << "no hessian for f\n";
  }
  if (!diff::detail::diffable_order1<decltype(ocp.g), std::tuple<double, X, U>>) {
    std::cout << "no jacobian for g\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.g), std::tuple<double, X, U>>) {
    std::cout << "no hessian for g\n";
  }
  if (!diff::detail::diffable_order1<decltype(ocp.cr), std::tuple<double, X, U>>) {
    std::cout << "no jacobian for cr\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.cr), std::tuple<double, X, U>>) {
    std::cout << "no hessian for cr\n";
  }
  if (!diff::detail::diffable_order1<decltype(ocp.ce), std::tuple<double, X, X, Q>>) {
    std::cout << "no jacobian for ce\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.ce), std::tuple<double, X, X, Q>>) {
    std::cout << "no hessian for ce\n";
  }

  const auto cmp = [&eps](const auto & m1, const auto & m2) {
    return (
      // clang-format off
      (m1.cols() == m2.cols())
      && (m1.rows() == m2.rows())
      && (
          m1.isApprox(m2, 1e-4) ||
          Eigen::MatrixXd(m1 - m2).cwiseAbs().maxCoeff() < eps
      )
      // clang-format on
    );
  };

  bool success = true;

  for (auto trial = 0u; trial < num_trials; ++trial) {
    // endpt parameters
    const double tf                        = 1 + static_cast<double>(std::rand()) / RAND_MAX;
    const X x0                             = Random<X>();
    const X xf                             = Random<X>();
    const Eigen::Vector<double, OCP::Nq> q = Eigen::Vector<double, OCP::Nq>::Random();
    const double t                         = 1 + static_cast<double>(std::rand()) / RAND_MAX;
    const X x                              = Random<X>();
    const U u                              = Random<U>();

    // theta
    if constexpr (diff::detail::diffable_order1<decltype(ocp.theta), std::tuple<double, X, X, Q>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.theta, wrt(tf, x0, xf, q));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.theta, wrt(tf, x0, xf, q));

      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of theta: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.theta), std::tuple<double, X, X, Q>>) {
      const auto [f_def, df_def, d2f_def] =
        diff::dr<2, diff::Type::Analytic>(ocp.theta, wrt(tf, x0, xf, q));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.theta, wrt(tf, x0, xf, q));

      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of theta: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }

    // end constraints
    if constexpr (diff::detail::diffable_order1<decltype(ocp.ce), std::tuple<double, X, X, Q>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.ce, wrt(tf, x0, xf, q));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.ce, wrt(tf, x0, xf, q));

      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of ce: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.ce), std::tuple<double, X, X, Q>>) {
      const auto [f_def, df_def, d2f_def] =
        diff::dr<2, diff::Type::Analytic>(ocp.ce, wrt(tf, x0, xf, q));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.ce, wrt(tf, x0, xf, q));

      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of ce: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }

    // dynamics
    if constexpr (diff::detail::diffable_order1<decltype(ocp.f), std::tuple<double, X, U>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.f, wrt(t, x, u));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.f, wrt(t, x, u));
      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of f: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.f), std::tuple<double, X, U>>) {
      const auto [f_def, df_def, d2f_def] = diff::dr<2, diff::Type::Analytic>(ocp.f, wrt(t, x, u));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.f, wrt(t, x, u));
      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of f: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }

    // integrand
    if constexpr (diff::detail::diffable_order1<decltype(ocp.g), std::tuple<double, X, U>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.g, wrt(t, x, u));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.g, wrt(t, x, u));
      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of g: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.g), std::tuple<double, X, U>>) {
      const auto [f_def, df_def, d2f_def] = diff::dr<2, diff::Type::Analytic>(ocp.g, wrt(t, x, u));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.g, wrt(t, x, u));
      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of g: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }

    // running constraints
    if constexpr (diff::detail::diffable_order1<decltype(ocp.cr), std::tuple<double, X, U>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.cr, wrt(t, x, u));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.cr, wrt(t, x, u));
      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of cr: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.cr), std::tuple<double, X, U>>) {
      const auto [f_def, df_def, d2f_def] = diff::dr<2, diff::Type::Analytic>(ocp.cr, wrt(t, x, u));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.cr, wrt(t, x, u));
      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of cr: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }
  }

  return success;
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__OCP_HPP_


// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/**
 * \file run_alignment.hpp
 * \brief
 * \details
 *
 * \author Autonomous Space Robotics Lab (ASRL)
 */
#pragma once
#if 0
#include <boost/unordered_map.hpp>
#include <lgmath/se3/TransformationWithCovariance.hpp>
#include <steam.hpp>

#include <vtr_common/logging.hpp>
#include <vtr_pose_graph/index/rc_graph/rc_graph.hpp>
#include <vtr_pose_graph/relaxation/graph_optimization_problem.hpp>
#endif
namespace vtr {
namespace pose_graph {
#if 0
template <class G>
class RunAlignment {
 public:
  typedef typename G::Ptr GraphPtr;

  typedef typename G::VertexPtr VertexPtr;
  typedef typename G::EdgePtr EdgePtr;
  typedef typename G::RunPtr RunPtr;
  typedef typename G::VertexIdType VertexIdType;
  typedef typename G::EdgeIdType EdgeIdType;
  typedef typename G::RunIdType RunIdType;
  typedef typename G::EdgeType::TransformType TransformType;

  typedef std::map<VertexId, steam::se3::TransformStateVar::Ptr> StateMapType;
  typedef std::map<RunIdType, StateMapType> RunStateMap;

  typedef __shared_ptr<steam::OptimizationProblem> ProblemPtr;
  typedef steam::ParallelizedCostTermCollection::Ptr CostTermPtr;
  typedef std::map<RunIdType, CostTermPtr> CostTermMap;

  typedef steam::LossFunctionBase::Ptr LossFuncPtr;
  typedef steam::BaseNoiseModel<6>::Ptr NoiseModelPtr;
  typedef steam::StaticNoiseModel<6> NoiseModel;
  typedef NoiseModelGenerator<TransformType, 6> ModelGen;

  typedef Eigen::Matrix<double, 6, 6> Matrix6d;

  PTR_TYPEDEFS(RunAlignment)
  DEFAULT_COPY_MOVE(RunAlignment)

  /////////////////////////////////////////////////////////////////////////////
  /// @brief Constructor; automatically initializes vertices to tree expansion
  /////////////////////////////////////////////////////////////////////////////
  RunAlignment(const GraphPtr& graph);

  /////////////////////////////////////////////////////////////////////////////
  /// @brief Lock a state variable
  /////////////////////////////////////////////////////////////////////////////
  void setLock(const VertexIdType& v, bool locked = true);

  /////////////////////////////////////////////////////////////////////////////
  /// @brief Lock a vector of state variables
  /////////////////////////////////////////////////////////////////////////////
  void setLock(const std::vector<VertexIdType>& v, bool locked = true);

  /////////////////////////////////////////////////////////////////////////////
  /// @brief Build the run-to-run cost terms
  /////////////////////////////////////////////////////////////////////////////
  void buildCostTerms(
      const Matrix6d& cov = Matrix6d::Identity(6, 6),
      const LossFuncPtr& lossFunc = LossFuncPtr(new steam::L2LossFunc()));

  /////////////////////////////////////////////////////////////////////////////
  /// @brief Solve the optimization problem using a given solver
  /////////////////////////////////////////////////////////////////////////////
  template <class Solver>
  void optimize(
      const typename Solver::Params& params = typename Solver::Params());

  /////////////////////////////////////////////////////////////////////////////
  /// @brief Get a transform by vertex ID
  /////////////////////////////////////////////////////////////////////////////
  const lgmath::se3::Transformation& at(const VertexIdType& v) const;

  /////////////////////////////////////////////////////////////////////////////
  /// @brief Get a transform by vertex ID
  /////////////////////////////////////////////////////////////////////////////
  inline typename StateMapType::const_iterator begin(
      const RunIdType& run) const {
    return stateMap_.at(run).begin();
  }

  /////////////////////////////////////////////////////////////////////////////
  /// @brief Get a transform by vertex ID
  /////////////////////////////////////////////////////////////////////////////
  inline typename StateMapType::const_iterator end(const RunIdType& run) const {
    return stateMap_.at(run).end();
  }

  /////////////////////////////////////////////////////////////////////////////
  /// @brief Apply the optimization to the graph
  /////////////////////////////////////////////////////////////////////////////
  void apply() const;

  /////////////////////////////////////////////////////////////////////////////
  /// @brief Get the transform between two vertex IDs
  /////////////////////////////////////////////////////////////////////////////
  lgmath::se3::Transformation T_ab(const VertexIdType& v_a,
                                   const VertexIdType& v_b) const;

  /////////////////////////////////////////////////////////////////////////////
  /// @brief Apply the optimization to the graph
  /////////////////////////////////////////////////////////////////////////////
  void applyEdge() const;

 protected:
  /////////////////////////////////////////////////////////////////////////////
  /// @brief Internal function to expand a single run
  /////////////////////////////////////////////////////////////////////////////
  void _buildRun(const RunPtr& run);

  GraphPtr graph_;

  RunStateMap stateMap_;

  CostTermMap costTerms_;
};
#endif
}  // namespace pose_graph
}  // namespace vtr

#include <vtr_pose_graph/relaxation/run_alignment.inl>

namespace vtr {
namespace pose_graph {
#if 0
#ifndef RUN_ALIGNMENT_NO_EXTERN
extern template class RunAlignment<BasicGraph>;
extern template class RunAlignment<RCGraph>;
#endif
#endif
}  // namespace pose_graph
}  // namespace vtr


// Simple development tests

#include "craam/ImMDP.hpp"
#include "craam/Samples.hpp"
#include "craam/Simulation.hpp"
#include "craam/algorithms/values.hpp"

#include "rm/range.hpp"

#include <cassert>
#include <cmath>
#include <iostream>
#include <iterator>
#include <random>

#include <boost/functional/hash.hpp>
#include <iostream>
#include <iterator>

using namespace std;
using namespace craam;
using namespace craam::impl;
using namespace util::lang;

template <class T> void print_vector(vector<T> vec) {
  for (auto &&p : vec) {
    cout << p << " ";
  }
}

/**
A simple simulator class. The state represents a position in a chain
and actions move it up and down. The reward is equal to the position.

Representation
~~~~~~~~~~~~~~
- State: position (int)
- Action: change (int)
*/
class Counter {
private:
  default_random_engine gen;
  bernoulli_distribution d;
  const vector<int> actions_list;
  const int initstate;

public:
  using State = int;
  using Action = int;

  /**
  Define the success of each action
  \param success The probability that the action is actually applied
  */
  Counter(double success, int initstate,
          random_device::result_type seed = random_device{}())
      : gen(seed), d(success), actions_list({1, -1}), initstate(initstate) {}

  int init_state() const { return initstate; }

  pair<double, int> transition(int pos, int action) {
    int nextpos = d(gen) ? pos + action : pos;
    return make_pair(double(pos), nextpos);
  }

  bool end_condition(const int state) { return false; }

  int action(State state, long index) const {
    return actions_list[size_t(index)];
  }

  const vector<int> &get_valid_actions(int state) const { return actions_list; }

  size_t action_count(State) const { return actions_list.size(); }
};

/** A counter that terminates at either end as defined by the end state */
class CounterTerminal : public Counter {
public:
  int endstate;

  CounterTerminal(double success, int initstate, int endstate,
                  random_device::result_type seed = random_device{}())
      : Counter(success, initstate, seed), endstate(endstate) {}

  bool end_condition(const int state) { return (abs(state) >= endstate); }
};

// Hash function for the Counter / CounterTerminal EState above
namespace std {
template <> struct hash<pair<int, int>> {
  size_t operator()(pair<int, int> const &s) const {
    boost::hash<pair<int, int>> h;
    return h(s);
  }
};
} // namespace std

using namespace craam::msen;

int main(void) {

  const int terminal_state = 8;
  const prec_t discount = 0.9;

  CounterTerminal sim(0.9, 0, terminal_state, 1);
  RandomPolicy<CounterTerminal> random_pol(sim);

  auto samples = make_samples<CounterTerminal>();
  simulate(sim, samples, random_pol, 100, 100);
  simulate(sim, samples, [](int) { return 1; }, 10, 20);
  simulate(sim, samples, [](int) { return -1; }, 10, 20);

  SampleDiscretizerSI<typename CounterTerminal::State,
                      typename CounterTerminal::Action>
      sd;
  // initialize action values
  sd.add_action(-1);
  sd.add_action(+1);
  // initialize state values
  for (auto i : util::lang::range(-terminal_state, terminal_state))
    sd.add_state(i);

  sd.add_samples(samples);

  SampledMDP smdp;
  smdp.add_samples(*sd.get_discrete());
  auto mdp = smdp.get_mdp();
  auto &&initial = smdp.get_initial();

  auto &&sol = algorithms::solve_mpi(*mdp, discount);

  cout << "Optimal policy: ";
  print_vector(sol.policy);
  cout << "Return " << sol.total_return(initial) << endl;

  // define observations
  indvec observations(mdp->state_count(), -1);
  size_t last_obs(0), inobs(0);
  cout << "Observations: " << mdp->state_count() << " states  ";
  for (auto i : util::lang::range(size_t(0), mdp->state_count())) {
    // check if this is a terminal state
    if ((*mdp)[i].empty() || inobs >= 2) {
      if (inobs > 0 && (*mdp)[i].size() == 0) {
        last_obs++;
      }
      observations[i] = last_obs++;
      inobs = 0;
    } else {
      observations[i] = last_obs;
      inobs++;
    }
    cout << observations[i] << " ";
  }
  cout << endl;

  MDPI_R mdpi(mdp, observations, initial);
  auto &&randompolicy = mdpi.random_policy(25);

  auto isol = mdpi.solve_reweighted(0, discount, randompolicy);

  isol = mdpi.solve_reweighted(10, discount, randompolicy);

  auto sol_impl =
      solve_mpi(*mdp, discount, numvec(0), mdpi.obspol2statepol(isol));

  cout << "Implementable pol: ";
  print_vector(isol);
  cout << "  Return: " << sol_impl.total_return(initial) << endl;

  cout << "Generating implementable policies (randomly) ..." << endl;

  auto max_return = 0.0;
  indvec max_pol(mdpi.obs_count(), -1);

  for (auto i : util::lang::range(0, 200)) {
    (void)(i);
    auto rand_pol = mdpi.random_policy();

    auto ret =
        solve_mpi(*mdp, discount, numvec(0), mdpi.obspol2statepol(rand_pol))
            .total_return(initial);

    if (ret > max_return) {
      max_pol = rand_pol;
      max_return = ret;
    }
  }

  cout << "Maximal return " << max_return << endl;
  cout << "Best policy: ";
  print_vector(max_pol);
  cout << endl;

  return 0;
}