/*!
 * *****************************************************************************
 *   \file genDepthFromSBS.cpp
 *   \author moennen
 *   \brief
 *   \date 2018-10-01
 *   *****************************************************************************/

#include "utils/imgFileLst.h"
#include "utils/cv_utils.h"

#include <opencv2/features2d/features2d.hpp>
#include "opencv2/calib3d/calib3d.hpp"

#include <libopticalFlow/oclVarOpticalFlow.h>

#include <glm/glm.hpp>

#include <boost/filesystem.hpp>

#include <iostream>
#include <algorithm>

using namespace std;
using namespace cv;
using namespace glm;
using namespace boost;

namespace
{
//------------------------------------------------------------------------------
//
void resizeToMax( Mat& img, const uvec2 sampleSz )
{
   uvec2 imgSz( img.cols, img.rows );
   // random rescale
   const float ds = std::max( (float)sampleSz.y / imgSz.y, (float)sampleSz.x / imgSz.x );
   if ( ds < 1.0 ) resize( img, img, Size(), ds, ds, CV_INTER_AREA );
}

//------------------------------------------------------------------------------
//
Mat depthToImg( const Mat& depth )
{
   Mat depth0 = depth.clone() * 255.0;
   Mat depth1;
   depth0.convertTo( depth1, CV_8UC1 );
   applyColorMap( depth1, depth0, COLORMAP_JET );
   return depth0;
}

//------------------------------------------------------------------------------
//
Mat flowToWrite( const Mat& flow )
{
   vector<Mat> splitFlow( 3 );
   split( flow, &splitFlow[0] );
   splitFlow[2] = Mat( flow.rows, flow.cols, CV_32FC1, 0.0f );
   Mat out( flow.rows, flow.cols, CV_32FC3 );
   merge( &splitFlow[0], 3, out );
   return out;
}

//------------------------------------------------------------------------------
//
Mat flowToImg( const Mat& flow, const bool leg = false )
{
   Mat flow_split[2];
   Mat t_split[3];
   Mat bgr;
   split( flow, flow_split );
   if ( leg )
   {
      t_split[0] = Mat::zeros( flow_split[0].size(), flow_split[0].type() );
      t_split[1] = flow_split[1];
      t_split[2] = flow_split[0];
      merge( t_split, 3, bgr );

      normalize( bgr, bgr, 0.0, 1.0, NORM_MINMAX, -1 );
   }
   else
   {
      Mat hsv;
      Mat magnitude, angle;
      cartToPolar( flow_split[0], flow_split[1], magnitude, angle, true );
      normalize( magnitude, magnitude, 0, 1, NORM_MINMAX );
      t_split[0] = angle;  // already in degrees - no normalization needed
      t_split[1] = Mat::ones( angle.size(), angle.type() );
      t_split[2] = magnitude;
      merge( t_split, 3, hsv );
      cvtColor( hsv, bgr, COLOR_HSV2BGR );
   }
   return bgr;
}

//------------------------------------------------------------------------------
//
Mat flowToErr( const Mat& flow, const Mat& imgFrom, const Mat& imgTo )
{
   Mat lk( flow.rows, flow.cols, CV_32FC1 );

#pragma omp parallel for
   for ( size_t y = 0; y < flow.rows; y++ )
   {
      float* lk_data = lk.ptr<float>( y );
      const vec2* flow_data = flow.ptr<vec2>( y );
      const vec3* from_data = imgFrom.ptr<vec3>( y );

      for ( size_t x = 0; x < flow.cols; x++ )
      {
         const vec2 f = flow_data[x];
         const vec3 from = from_data[x];
         const vec3 to = cv_utils::imsample32FC3<vec3>( imgTo, vec2( x, y ) + f );
         lk_data[x] = distance( from, to );
      }
   }

   return lk;
}

//------------------------------------------------------------------------------
//
bool rectifyStereoPair( Mat& matRight, Mat& matLeft )
{
   cvtColor( matRight, matRight, COLOR_RGB2BGR );
   cvtColor( matLeft, matLeft, COLOR_RGB2BGR );

   // Points detection
   const int minHessian = 400;
   vector<KeyPoint> arrKPtsRight, arrKPtsLeft;
   Mat matDescRight, matDescLeft;
   Ptr<ORB> detector = ORB::create( 1750, 1.2f, 4, 31, 0, 2, ORB::HARRIS_SCORE, 31, 10 );
   detector->detectAndCompute( matRight, noArray(), arrKPtsRight, matDescRight );
   detector->detectAndCompute( matLeft, noArray(), arrKPtsLeft, matDescLeft );

   Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create( "BruteForce-Hamming" );
   vector<vector<DMatch>> matchess;
   matcher->radiusMatch( matDescRight, matDescLeft, matchess, 50.0f );

   std::cout << arrKPtsRight.size() << " / " << arrKPtsLeft.size() << " --> " << matchess.size()
             << " " << matchess[0].size() << endl;

   vector<DMatch> matches;
   matches.reserve( matchess.size() );
   for ( const auto m : matchess )
   {
      if ( !m.empty() ) matches.push_back( m.front() );
   }

   const size_t best_k = std::min( (size_t)75u, matches.size() );
   if ( matches.size() > best_k )
   {
      sort( matches.begin(), matches.end(), []( const DMatch& a, const DMatch& b ) {
         return a.distance < b.distance;
      } );
   }

   vector<Point2f> arrPtsRight( best_k );
   vector<Point2f> arrPtsLeft( best_k );
   for ( size_t k = 0; k < best_k; ++k )
   {
      arrPtsRight[k] = arrKPtsRight[matches[k].queryIdx].pt;
      arrPtsLeft[k] = arrKPtsLeft[matches[k].trainIdx].pt;
   }

   //-- Draw matches
   matches.resize( best_k );
   Mat img_matches;
   drawMatches( matRight, arrKPtsRight, matLeft, arrKPtsLeft, matches, img_matches );
   imshow( "Matches", img_matches );

   waitKey( 0 );

   // Mat F = findFundamentalMat(arrPtsRight, arrPtsLeft, FM_RANSAC, 2.5, 0.99);
   Mat F = findFundamentalMat( arrPtsRight, arrPtsLeft, CV_FM_8POINT );

   cout << "Fundamental : " << endl;
   cout << F << endl;

   Mat HRight( 4, 4, CV_64F );
   Mat HLeft( 4, 4, CV_64F );
   stereoRectifyUncalibrated( arrPtsRight, arrPtsLeft, F, matRight.size(), HRight, HLeft );

   HLeft = HRight.inv() * HLeft;
   HRight = Mat::eye( HRight.rows, HRight.cols, CV_64F );

   cout << "Homo : " << endl;
   cout << HLeft << endl;

   warpPerspective( matRight, matRight, HRight, matRight.size() );
   warpPerspective( matLeft, matLeft, HLeft, matLeft.size() );

   imshow( "MatRight", matRight );
   imshow( "MatLeft", matLeft );

   cvtColor( matRight, matRight, COLOR_BGR2RGB );
   cvtColor( matLeft, matLeft, COLOR_BGR2RGB );

   return true;
}

//------------------------------------------------------------------------------
//
bool rectifyStereoPairOf( Mat& matRight, Mat& matLeft, OclVarOpticalFlow& ofEst )
{
   // compute the left to right optical flow
   Mat matOfL2R( matLeft.rows, matLeft.cols, CV_32FC2 );
   ofEst.compute(
       reinterpret_cast<const float*>( matRight.ptr() ),
       reinterpret_cast<const float*>( matLeft.ptr() ),
       matLeft.cols,
       matLeft.rows,
       reinterpret_cast<float*>( matOfL2R.ptr() ) );

   // create matches

   const ivec2 grid_res = {matRight.cols / 33, matRight.rows / 33};
   const vec2 delta = {matRight.cols / ( grid_res.x + 1 ), matRight.rows / ( grid_res.y + 1 )};

   vector<DMatch> matches;
   matches.reserve( grid_res.x * grid_res.y );
   vector<KeyPoint> arrKPtsRight;
   arrKPtsRight.reserve( grid_res.x * grid_res.y );
   vector<KeyPoint> arrKPtsLeft;
   arrKPtsLeft.reserve( grid_res.x * grid_res.y );

   for ( int j = 0; j < grid_res.y; ++j )
   {
      const float y = (j+1) * delta.y;
      for ( int i = 0; i < grid_res.x; ++i )
      {
         const size_t idx = j * grid_res.x + i;
         const float x = (i+1) * delta.x;
         const vec2 pos = {x, y};
         const vec2 uv = cv_utils::imsample32F<vec2>( matLeft, pos );

         const vec3 rgb_l = cv_utils::imsample32F<vec3>( matLeft, pos );
         const vec3 rgb_r = cv_utils::imsample32F<vec3>( matRight, pos + uv );

         matches.emplace_back( arrKPtsRight.size(), arrKPtsLeft.size(), distance( rgb_l, rgb_r ) );
         arrKPtsRight.emplace_back( Point2f( pos.x + uv.x, pos.y + uv.y ), 3.0 );
         arrKPtsLeft.emplace_back( Point2f( pos.x, pos.y ), 3.0f );
      }
   }

   cvtColor( matRight, matRight, COLOR_BGR2RGB );
   cvtColor( matLeft, matLeft, COLOR_BGR2RGB );

   //-- Draw matches
   Mat img_matches;
   drawMatches( matRight, arrKPtsRight, matLeft, arrKPtsLeft, matches, img_matches );
   imshow( "Matches", img_matches );

   vector<Point2f> arrPtsRight( arrKPtsRight.size() );
   for ( size_t k = 0; k < arrKPtsRight.size(); ++k )  arrPtsRight[k] = arrKPtsRight[k].pt;
   vector<Point2f> arrPtsLeft( arrKPtsLeft.size() );
   for ( size_t k = 0; k < arrKPtsLeft.size(); ++k )  arrPtsLeft[k] = arrKPtsLeft[k].pt;

   // Mat F = findFundamentalMat(arrPtsRight, arrPtsLeft, FM_RANSAC, 2.5, 0.99);
   Mat F = findFundamentalMat( arrPtsRight, arrPtsLeft, CV_FM_8POINT );

   cout << "Fundamental : " << endl;
   cout << F << endl;

   Mat HRight( 4, 4, CV_64F );
   Mat HLeft( 4, 4, CV_64F );
   stereoRectifyUncalibrated( arrPtsRight, arrPtsLeft, F, matRight.size(), HRight, HLeft );

   HLeft = HRight.inv() * HLeft;
   HRight = Mat::eye( HRight.rows, HRight.cols, CV_64F );

   cout << "Homo : " << endl;
   cout << HLeft << endl;

   //warpPerspective( matRight, matRight, HRight, matRight.size() );
   warpPerspective( matLeft, matLeft, HLeft, matLeft.size() );

   imshow( "MatRight", matRight );
   imshow( "MatLeft", matLeft );

   waitKey( 0 );

   cvtColor( matRight, matRight, COLOR_BGR2RGB );
   cvtColor( matLeft, matLeft, COLOR_BGR2RGB );

   return true;
}

//------------------------------------------------------------------------------
//
bool flowToDisp(
    const Mat& flowR,
    const Mat& flowL,
    const Mat& imgFrom,
    const Mat& imgTo,
    Mat& disp,
    Mat& mask,
    const bool isInverted )
{
   disp = Mat( flowR.rows, flowR.cols, CV_32FC1 );
   mask = Mat( flowR.rows, flowR.cols, CV_8UC1 );

   Mat errR = flowToErr( flowR, imgFrom, imgTo );

#pragma omp parallel for
   for ( size_t y = 0; y < flowR.rows; y++ )
   {
      const vec2* f_row_r_data = flowR.ptr<vec2>( y );
      const float* err_data = errR.ptr<float>( y );
      const vec2* f_row_l_data = flowL.ptr<vec2>( y );

      float* d_row_data = disp.ptr<float>( y );
      unsigned char* u_row_data = mask.ptr<unsigned char>( y );

      for ( size_t x = 0; x < flowR.cols; x++ )
      {
         const vec2 mtR = f_row_r_data[x];
         const float dispR = sign( mtR.x ) * length( mtR );
         const vec2 mtRL = vec2( -1.0 ) * cv_utils::imsample32F<vec2>( flowL, vec2( x, y ) + mtR );
         const float dispRL = sign( mtRL.x ) * length( mtRL );

         const float dispReg = mix( dispR, dispRL, 0.5 );
         const bool undef = ( abs( mtR.y ) > 1.0 ) || ( abs( mtRL.y ) > 1.0 ) ||
                            ( err_data[x] > 0.5 ) || ( distance( mtR, mtRL ) > 1.0 );

         u_row_data[x] = undef ? 0 : 255;
         d_row_data[x] = ( isInverted ? 1.0 : -1.0 ) * dispReg;
      }
   }

   normalize( disp, disp, 0.0, 1.0, NORM_MINMAX, -1, mask );

   const float masked = ( 1.0f - ( cv::mean( mask )[0] / 255.0f ) );

   return true;  // masked < 0.65;
}
}

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

const string keys =
    "{help h usage ? |         | print this message   }"
    "{@imgFileLst    |         | images list   }"
    "{@imgRootDir    |         | images root dir   }"
    "{@imgOutDir     |         | images output dir   }"
    "{outWidth       |640      | output width }"
    "{outHeight      |380      | output height }"
    "{show           |         |    }"
    "{nowrite        |         |    }";

int main( int argc, char* argv[] )
{
   CommandLineParser parser( argc, argv, keys );
   if ( parser.has( "help" ) )
   {
      parser.printMessage();
      return ( 0 );
   }

   const uvec2 minSz =
       uvec2( parser.get<unsigned>( "outWidth" ), parser.get<unsigned>( "outHeight" ) );

   const bool toLinear = false;
   const bool doShow = parser.get<bool>( "show" );
   const bool doWrite = !parser.get<bool>( "nowrite" );

   const filesystem::path outRootPath( parser.get<string>( "@imgOutDir" ) );

   // Create the list of image triplets
   ImgNFileLst imgLst(
       2,
       parser.get<string>( "@imgFileLst" ).c_str(),
       parser.get<string>( "@imgRootDir" ).c_str() );
   if ( imgLst.size() == 0 )
   {
      cerr << "Invalid dataset : " << parser.get<string>( "@imgFileLst" ) << endl;
      return -1;
   }

   // Create the optical flow estimator
   OclVarOpticalFlow::params_t ofParams = OclVarOpticalFlow::getDefaultParams();
   ofParams.nonLinearIter = 9;
   ofParams.robustIter = 5;
   ofParams.solverIter = 5;
   ofParams.lambda = 0.1;
   ofParams.gamma = 75;

   // NB : need to disable OpenCV/OpenCL by setting the envvar :
   // setenv OPENCV_OPENCL_RUNTIME 0
   OclVarOpticalFlow ofEstimator( minSz.x, minSz.y, false, ofParams );

   for ( size_t i = 0; i < imgLst.size(); ++i )
   {
      const string outBasename = filesystem::path( imgLst( i, 0 ) ).stem().string();
      const string videoIdname = outBasename.substr( 0, outBasename.find_first_of( "_" ) );

      Mat matRight = cv_utils::imread32FC3( imgLst.filePath( i, 0 ), toLinear, true );
      Mat matLeft = cv_utils::imread32FC3( imgLst.filePath( i, 1 ), toLinear, true );
      if ( matRight.empty() || matLeft.empty() ) continue;

      resizeToMax( matRight, minSz );
      resizeToMax( matLeft, minSz );

      // compute the right to left optical flow
      ofEstimator.setImgSize( matRight.cols, matRight.rows );

      rectifyStereoPairOf( matRight, matLeft, ofEstimator );

      Mat matOfR2L( matRight.rows, matRight.cols, CV_32FC2 );
      ofEstimator.compute(
          reinterpret_cast<const float*>( matLeft.ptr() ),
          reinterpret_cast<const float*>( matRight.ptr() ),
          matRight.cols,
          matRight.rows,
          reinterpret_cast<float*>( matOfR2L.ptr() ) );

      // compute the left to right optical flow
      Mat matOfL2R( matLeft.rows, matLeft.cols, CV_32FC2 );
      ofEstimator.compute(
          reinterpret_cast<const float*>( matRight.ptr() ),
          reinterpret_cast<const float*>( matLeft.ptr() ),
          matLeft.cols,
          matLeft.rows,
          reinterpret_cast<float*>( matOfL2R.ptr() ) );

      // compute the disparity and disparity mask
      Mat matDepth, matMask;
      if ( !flowToDisp( matOfR2L, matOfL2R, matRight, matLeft, matDepth, matMask, false ) )
         continue;

      cvtColor( matRight, matRight, COLOR_BGR2RGB );
      cvtColor( matLeft, matLeft, COLOR_BGR2RGB );

      const filesystem::path fRight(
          videoIdname + "/" + outBasename + string( "_right_rgb" ) + ".png" );
      const filesystem::path fLeft(
          videoIdname + "/" + outBasename + string( "_left_rgb" ) + ".png" );
      const filesystem::path fFlowR2L(
          videoIdname + "/" + outBasename + string( "_r2l_mflow" ) + ".exr" );
      const filesystem::path fFlowL2R(
          videoIdname + "/" + outBasename + string( "_l2r_mflow" ) + ".exr" );
      const filesystem::path fDepth( videoIdname + "/" + outBasename + string( "_d" ) + ".exr" );
      const filesystem::path fMask( videoIdname + "/" + outBasename + string( "_a" ) + ".png" );

      if ( doWrite )
      {
         imwrite( filesystem::path( outRootPath / fRight ).string().c_str(), matRight * 255.0 );
         imwrite( filesystem::path( outRootPath / fLeft ).string().c_str(), matLeft * 255.0 );
         imwrite(
             filesystem::path( outRootPath / fFlowR2L ).string().c_str(), flowToWrite( matOfR2L ) );
         imwrite(
             filesystem::path( outRootPath / fFlowL2R ).string().c_str(), flowToWrite( matOfL2R ) );
         imwrite( filesystem::path( outRootPath / fDepth ).string().c_str(), matDepth );
         imwrite( filesystem::path( outRootPath / fMask ).string().c_str(), matMask );
      }

      cout << fRight.string() << " " << fLeft.string() << " " << fDepth.string() << " "
           << fMask.string() << " " << fFlowR2L.string() << " " << fFlowL2R.string() << endl;

      // display
      if ( doShow )
      {
         imshow( "Right", matRight );
         imshow( "Left", matLeft );
         imshow( "FlowR2L", flowToImg( matOfR2L, true ) );
         imshow( "FlowL2R", flowToImg( matOfL2R, true ) );
         imshow( "Disp", depthToImg( matDepth ) );
         imshow( "Depth", matDepth );
         imshow( "Mask", matMask );
         waitKey( 0 );
      }
   }

   return ( 0 );
}


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Purpose:  Implementation of the nzmg (New Zealand Map Grid) projection.
//           Very loosely based upon DMA code by Bradford W. Drew
// Author:   Gerald Evenden
// Copyright (c) 1995, Gerald Evenden

// 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 BOOST_GEOMETRY_PROJECTIONS_NZMG_HPP
#define BOOST_GEOMETRY_PROJECTIONS_NZMG_HPP

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_zpoly1.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct nzmg {}; // New Zealand Map Grid

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace nzmg
    {

            static const double epsilon = 1e-10;
            static const int Nbf = 5;
            static const int Ntpsi = 9;
            static const int Ntphi = 8;

            template <typename T>
            inline T sec5_to_rad() { return 0.4848136811095359935899141023; }
            template <typename T>
            inline T rad_to_sec5() { return 2.062648062470963551564733573; }

            template <typename T>
            inline const pj_complex<T> * bf()
            {
                static const pj_complex<T> result[] = {
                    {.7557853228,    0.0},
                    {.249204646,    .003371507},
                    {-.001541739,    .041058560},
                    {-.10162907,    .01727609},
                    {-.26623489,    -.36249218},
                    {-.6870983,    -1.1651967}
                };
                return result;
            }

            template <typename T>
            inline const T * tphi()
            {
                static const T result[] = { 1.5627014243, .5185406398, -.03333098,
                                            -.1052906,   -.0368594,     .007317,
                                             .01220,      .00394,      -.0013 };
                return result;
            }
            template <typename T>
            inline const T * tpsi()
            {
                static const T result[] = { .6399175073, -.1358797613, .063294409, -.02526853, .0117879,
                                           -.0055161,     .0026906,   -.001333,     .00067,   -.00034 };
                return result;
            }

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_nzmg_ellipsoid
                : public base_t_fi<base_nzmg_ellipsoid<T, Parameters>, T, Parameters>
            {
                inline base_nzmg_ellipsoid(const Parameters& par)
                    : base_t_fi<base_nzmg_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T rad_to_sec5 = nzmg::rad_to_sec5<T>();

                    pj_complex<T> p;
                    const T * C;
                    int i;

                    lp_lat = (lp_lat - this->m_par.phi0) * rad_to_sec5;
                    for (p.r = *(C = tpsi<T>() + (i = Ntpsi)); i ; --i)
                        p.r = *--C + lp_lat * p.r;
                    p.r *= lp_lat;
                    p.i = lp_lon;
                    p = pj_zpoly1(p, bf<T>(), Nbf);
                    xy_x = p.i;
                    xy_y = p.r;
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    static const T sec5_to_rad = nzmg::sec5_to_rad<T>();

                    int nn, i;
                    pj_complex<T> p, f, fp, dp;
                    T den;
                    const T* C;

                    p.r = xy_y;
                    p.i = xy_x;
                    for (nn = 20; nn ;--nn) {
                        f = pj_zpolyd1(p, bf<T>(), Nbf, &fp);
                        f.r -= xy_y;
                        f.i -= xy_x;
                        den = fp.r * fp.r + fp.i * fp.i;
                        p.r += dp.r = -(f.r * fp.r + f.i * fp.i) / den;
                        p.i += dp.i = -(f.i * fp.r - f.r * fp.i) / den;
                        if ((fabs(dp.r) + fabs(dp.i)) <= epsilon)
                            break;
                    }
                    if (nn) {
                        lp_lon = p.i;
                        for (lp_lat = *(C = tphi<T>() + (i = Ntphi)); i ; --i)
                            lp_lat = *--C + p.r * lp_lat;
                        lp_lat = this->m_par.phi0 + p.r * lp_lat * sec5_to_rad;
                    } else
                        lp_lon = lp_lat = HUGE_VAL;
                }

                static inline std::string get_name()
                {
                    return "nzmg_ellipsoid";
                }

            };

            // New Zealand Map Grid
            template <typename Parameters>
            inline void setup_nzmg(Parameters& par)
            {
                typedef typename Parameters::type calc_t;
                static const calc_t d2r = geometry::math::d2r<calc_t>();

                /* force to International major axis */
                par.ra = 1. / (par.a = 6378388.0);
                par.lam0 = 173. * d2r;
                par.phi0 = -41. * d2r;
                par.x0 = 2510000.;
                par.y0 = 6023150.;
            }

    }} // namespace detail::nzmg
    #endif // doxygen

    /*!
        \brief New Zealand Map Grid projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Fixed Earth
        \par Example
        \image html ex_nzmg.gif
    */
    template <typename T, typename Parameters>
    struct nzmg_ellipsoid : public detail::nzmg::base_nzmg_ellipsoid<T, Parameters>
    {
        inline nzmg_ellipsoid(const Parameters& par) : detail::nzmg::base_nzmg_ellipsoid<T, Parameters>(par)
        {
            detail::nzmg::setup_nzmg(this->m_par);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::nzmg, nzmg_ellipsoid, nzmg_ellipsoid)

        // Factory entry(s)
        template <typename T, typename Parameters>
        class nzmg_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<nzmg_ellipsoid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        inline void nzmg_init(detail::base_factory<T, Parameters>& factory)
        {
            factory.add_to_factory("nzmg", new nzmg_entry<T, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_NZMG_HPP


#define BOOST_TEST_MODULE TEST_SCORING
#include <boost/test/unit_test.hpp>
#include "libs/scores_evaluation/score_evaluation.h"

BOOST_AUTO_TEST_CASE(test_empty_data_set)
{
    actions::experiment_input empty_data_set;
    const auto output = calculate(confusion_matric_test{}, empty_data_set);
    BOOST_TEST(output.true_pos == 0u);
    BOOST_TEST(output.true_neg == 0u);
    BOOST_TEST(output.false_neg == 0u);
    BOOST_TEST(output.false_pos == 0u);
}

BOOST_AUTO_TEST_CASE(test_single_entry_true_pos)
{
    actions::experiment_input data_set{
        mlmodels::class_data{1.f},
        mlmodels::class_data{1.f}
    };
    const auto output = calculate(confusion_matric_test{}, data_set);
    BOOST_TEST(output.true_pos == 1u);
    BOOST_TEST(output.true_neg == 0u);
    BOOST_TEST(output.false_neg == 0u);
    BOOST_TEST(output.false_pos == 0u);
}

BOOST_AUTO_TEST_CASE(test_single_entry_true_neg)
{
    actions::experiment_input data_set{
        mlmodels::class_data{0.f},
        mlmodels::class_data{0.f}
    };
    const auto output = calculate(confusion_matric_test{}, data_set);
    BOOST_TEST(output.true_pos == 0u);
    BOOST_TEST(output.true_neg == 1u);
    BOOST_TEST(output.false_neg == 0u);
    BOOST_TEST(output.false_pos == 0u);
}

BOOST_AUTO_TEST_CASE(test_single_entry_false_pos)
{
    actions::experiment_input data_set{
        mlmodels::class_data{1.f},
        mlmodels::class_data{0.f}
    };
    const auto output = calculate(confusion_matric_test{}, data_set);
    BOOST_TEST(output.true_pos == 0u);
    BOOST_TEST(output.true_neg == 0u);
    BOOST_TEST(output.false_neg == 0u);
    BOOST_TEST(output.false_pos == 1u);
}

BOOST_AUTO_TEST_CASE(test_single_entry_false_neg)
{
    actions::experiment_input data_set{
        mlmodels::class_data{0.f},
        mlmodels::class_data{1.f}
    };
    const auto output = calculate(confusion_matric_test{}, data_set);
    BOOST_TEST(output.true_pos == 0u);
    BOOST_TEST(output.true_neg == 0u);
    BOOST_TEST(output.false_neg == 1u);
    BOOST_TEST(output.false_pos == 0u);
}

BOOST_AUTO_TEST_CASE(test_single_entry_4tp_2tn_1fp)
{
    actions::experiment_input data_set{
        mlmodels::class_data{1.f, 1.f, 1.f, 0.f, 1.f, 0.f, 1.f},
        mlmodels::class_data{1.f, 0.f, 1.f, 0.f, 1.f, 0.f, 1.f}
    };
    const auto output = calculate(confusion_matric_test{}, data_set);
    BOOST_TEST(output.true_pos == 4u);
    BOOST_TEST(output.true_neg == 2u);
    BOOST_TEST(output.false_neg == 0u);
    BOOST_TEST(output.false_pos == 1u);
}


extern "C" {
	#include "photospline/splinetable.h"
	#include "photospline/bspline.h"
}

#include <I3Test.h>
#include <boost/filesystem.hpp>
#include <sys/time.h>
#include <limits>

namespace fs = boost::filesystem;

struct TableSet {
	fs::path abs, prob;
};

static void
splinetable_destructor(struct splinetable *table) {
	if (!table) return;

	splinetable_free(table);
	delete table;

}

static TableSet
get_splinetables()
{
	ENSURE(getenv("I3_TESTDATA") != NULL,
	    "I3_TESTDATA must be defined in the parent shell.");

	const std::string I3_TESTDATA(getenv("I3_TESTDATA"));

	fs::path abs_table(I3_TESTDATA + "/photospline/ems_z0_a0.pt.abs.fits");
	fs::path prob_table(I3_TESTDATA + "/photospline/ems_z0_a0.pt.prob.fits");

	ENSURE(fs::exists(abs_table), "Amplitude table does not exist.");
	ENSURE(fs::exists(prob_table), "Quantile table does not exists.");

	TableSet tabset;
	tabset.abs = abs_table;
	tabset.prob = prob_table;

	return tabset;
}

static boost::shared_ptr<struct splinetable>
load_splinetable(fs::path &fname)
{
	boost::shared_ptr<struct splinetable> table(new struct splinetable, splinetable_destructor);
	ENSURE(readsplinefitstable(fname.string().c_str(), table.get()) == 0, "Table can be read.");

	return table;
}

static void
compare_tables(struct splinetable *oldtable, struct splinetable *newtable)
{
	ENSURE_EQUAL(oldtable->ndim, newtable->ndim, "Number of dimensions match");
	size_t arraysize = 1;
	for (int i=0; i < oldtable->ndim; i++) {
		ENSURE_EQUAL(oldtable->naxes[i], newtable->naxes[i], "Coefficient array sizes match");
		arraysize *= oldtable->naxes[i];
		ENSURE_EQUAL(oldtable->order[i], newtable->order[i], "Spline orders match");
		ENSURE_EQUAL(oldtable->periods[i], newtable->periods[i], "Spline periods match");
		ENSURE_EQUAL(oldtable->nknots[i], newtable->nknots[i], "Size of knot vectors match");
		for (int j=0; j < oldtable->nknots[i]; j++)
			ENSURE_EQUAL(oldtable->knots[i][j], newtable->knots[i][j], "Knot positions match");
		ENSURE_EQUAL(oldtable->extents[i][0], newtable->extents[i][0], "Lower extents match");
		ENSURE_EQUAL(oldtable->extents[i][1], newtable->extents[i][1], "Upper extents match");
	}

	for (size_t i=0; i < arraysize; i++)
		ENSURE_EQUAL(oldtable->coefficients[i], newtable->coefficients[i], "Coefficients match");

	ENSURE_EQUAL(oldtable->naux, newtable->naux, "Number of auxiliary keywords match");
	for (int i=0; i < oldtable->naux; i++) {
		ENSURE_EQUAL(std::string(oldtable->aux[i][0]), std::string(newtable->aux[i][0]), "Keys match");
		ENSURE_EQUAL(std::string(oldtable->aux[i][1]), std::string(newtable->aux[i][1]), "Values match");
	}
}

TEST_GROUP(BoundaryIssues);

TEST(DiskFile)
{
	TableSet tables = get_splinetables();
	boost::shared_ptr<struct splinetable> oldtable = load_splinetable(tables.abs);

	fs::path tmp("photospline-write-test.fits");
	if (fs::exists(tmp))
		fs::remove(tmp);
	ENSURE_EQUAL(writesplinefitstable(tmp.string().c_str(), oldtable.get()), 0, "Table can be written");

	boost::shared_ptr<struct splinetable> newtable = load_splinetable(tmp);

	compare_tables(oldtable.get(), newtable.get());

	fs::remove(tmp);
}

TEST(MemoryFile)
{
	TableSet tables = get_splinetables();
	boost::shared_ptr<struct splinetable> oldtable = load_splinetable(tables.abs);

	struct splinetable_buffer buf;
	buf.mem_alloc = &malloc;
	buf.mem_realloc = &realloc;
	ENSURE_EQUAL(writesplinefitstable_mem(&buf, oldtable.get()), 0, "Table can be written");

	boost::shared_ptr<struct splinetable> newtable(new struct splinetable, splinetable_destructor);
	ENSURE_EQUAL(readsplinefitstable_mem(&buf, newtable.get()), 0, "Table can be read.");
	free(buf.data);

	compare_tables(oldtable.get(), newtable.get());
}

/*
 * Check that analytic convolution works and preserves the monotonicity
 * of the arrival-time CDF.
 */
TEST(Convolution)
{
	TableSet tables = get_splinetables();
	boost::shared_ptr<struct splinetable> table = load_splinetable(tables.prob);
	const unsigned time_dim = 3;
	double sigma = 10.0;
	int n_knots = 3;
	double knots[3] = {-2*sigma, 0, 2*sigma};
	unsigned i;
	unsigned ndim = table->ndim;
	double tablecoords[ndim], base, nudge, q0_raw, q0_conv, q1_raw, q1_conv;
	const double eps = std::numeric_limits<double>::epsilon();
	const double tmax = table->extents[time_dim][1];
	int centers[ndim];

	for (i = 0; i < ndim; i++) {
		double low, high;
		low = table->extents[i][0];
		high = table->extents[i][1];
		tablecoords[i] = (low+high)/2.0;
	}

	tablecoords[time_dim] = table->knots[time_dim][0]+eps;
	ENSURE_EQUAL(tablesearchcenters(table.get(), tablecoords, centers), 0);
	q0_raw = ndsplineeval(table.get(), tablecoords, centers, 0);
	tablecoords[time_dim] = table->extents[time_dim][1];
	ENSURE_EQUAL(tablesearchcenters(table.get(), tablecoords, centers), 0);
	q1_raw = ndsplineeval(table.get(), tablecoords, centers, 0);

	ENSURE_DISTANCE(q1_raw-q0_raw, 1, 1e-3,
	    "Arrival-time CDF is normalized to within 0.1%%.");

	base = q0_raw;
	for (i = 1; i+table->order[time_dim]+1 < table->nknots[time_dim]; i++) {
		tablecoords[time_dim] = table->knots[time_dim][i];
		ENSURE_EQUAL(tablesearchcenters(table.get(), tablecoords, centers), 0);
		nudge = ndsplineeval(table.get(), tablecoords, centers, 0);
		ENSURE(nudge - base >= 0,
		    "Arrival-time CDF is monotonic.");
		base = nudge;
	}

	int err = splinetable_convolve(table.get(), time_dim, knots, n_knots);
	ENSURE_EQUAL(err, 0, "Convolution succeeds.");

	tablecoords[time_dim] = table->knots[time_dim][0]*(1-eps);
	ENSURE_EQUAL(tablesearchcenters(table.get(), tablecoords, centers), 0);
	q0_conv = ndsplineeval(table.get(), tablecoords, centers, 0);

	base = q0_conv;
	for (i = 1; i < table->nknots[time_dim]; i++) {
		tablecoords[time_dim] = table->knots[time_dim][i];
		if (tablecoords[time_dim] > table->extents[time_dim][1])
			break;
		ENSURE_EQUAL(tablesearchcenters(table.get(), tablecoords, centers), 0);
		nudge = ndsplineeval(table.get(), tablecoords, centers, 0);
		ENSURE(nudge - base >= 0,
		    "Arrival-time CDF remains monotonic.");
		base = nudge;
	}

	tablecoords[time_dim] = table->extents[time_dim][1];
	ENSURE_EQUAL(tablesearchcenters(table.get(), tablecoords, centers), 0);
	q1_conv = ndsplineeval(table.get(), tablecoords, centers, 0);

	ENSURE(table->extents[time_dim][1] < tmax,
	    "t_max is slightly smaller after convolution.");
	ENSURE(q1_conv - q1_raw > -10*std::numeric_limits<float>::epsilon(),
	    "Maximum quantile (at the end of support) is not diminished.");

	ENSURE_DISTANCE(q1_raw-q0_raw, q1_conv-q0_conv, 10*std::numeric_limits<float>::epsilon(),
	    "Arrival-time CDF remains normalized after convolution.");
}

/*
 * The quantiles in the time dimension are continous. This can only be true
 * if the spline evaluation code is capable of handling points near the
 * edges of the knot fields that are supported by fewer than (order+1)
 * splines.
 */
TEST(QuantileContinuity)
{
	const unsigned time_dim = 3;
	TableSet tables = get_splinetables();
	boost::shared_ptr<struct splinetable> table = load_splinetable(tables.prob);

	unsigned i;
	unsigned ndim = table->ndim;
	double tablecoords[ndim], base, nudge;
	const double eps = std::numeric_limits<double>::epsilon();
	int centers[ndim];

	for (i = 0; i < ndim; i++) {
		double low, high;
		low = table->extents[i][0];
		high = table->extents[i][1];
		tablecoords[i] = (low+high)/2.0;
	}

	/* Check the transition into the knot field from the left. */
	tablecoords[time_dim] = table->knots[time_dim][0];
	ENSURE(tablesearchcenters(table.get(), tablecoords, centers) != 0,
	    "tablesearchcenters() fails right at the left edge of the knot field.");

	tablecoords[time_dim] += eps;

	ENSURE_EQUAL(tablesearchcenters(table.get(), tablecoords, centers), 0,
	    "tablesearchcenters() succeeds just inside the left edge of the knot field.");
	ENSURE_EQUAL(centers[time_dim], table->order[time_dim],
	    "centers[time_dim] holds the first fully-supported knot index.");

	base = ndsplineeval(table.get(), tablecoords, centers, 0);

	ENSURE(base >= 0, "Base quantile is positive.");
	ENSURE_DISTANCE(0, base, std::numeric_limits<float>::epsilon(),
	    "Time quantile is continuous "
	    "(slope is less than FLOAT_EPSILON/DBL_EPSILON)");

	/*
	 * Now, step over the intertior knots, checking for continuity
	 * at every knot crossing.
	 */
	for (i = 1; i+1 < table->nknots[time_dim]; i++) {
		tablecoords[time_dim] = table->knots[time_dim][i]*(1-eps);
		ENSURE_EQUAL(tablesearchcenters(table.get(), tablecoords, centers), 0,
		    "tablesearchcenters() succeeds inside the knot field.");

		base = ndsplineeval(table.get(), tablecoords, centers, 0);
		ENSURE(base >= 0);

		tablecoords[time_dim] = table->knots[time_dim][i];
		ENSURE_EQUAL(tablesearchcenters(table.get(), tablecoords, centers), 0,
		    "tablesearchcenters() succeeds inside the knot field.");

		nudge = ndsplineeval(table.get(), tablecoords, centers, 0);
		ENSURE(nudge >= 0);

		ENSURE_DISTANCE(base, nudge, std::numeric_limits<float>::epsilon(),
		    "Time quantile is continuous "
		    "(slope is less than FLOAT_EPSILON/DBL_EPSILON)");

		base = nudge;

		tablecoords[time_dim] = table->knots[time_dim][i]*(1+eps);
		ENSURE_EQUAL(tablesearchcenters(table.get(), tablecoords, centers), 0,
		    "tablesearchcenters() succeeds inside the knot field.");

		nudge = ndsplineeval(table.get(), tablecoords, centers, 0);
		ENSURE(nudge >= 0);

		ENSURE_DISTANCE(base, nudge, std::numeric_limits<float>::epsilon(),
		    "Time quantile is continuous "
		    "(slope is less than FLOAT_EPSILON/DBL_EPSILON)");
	}

	/* Check the transition into the knot field from the right */
	tablecoords[time_dim] = table->knots[time_dim][table->nknots[time_dim]-1]*(1+eps);
	ENSURE(tablecoords[time_dim] > table->knots[time_dim][table->nknots[time_dim]-1]);
	ENSURE(tablesearchcenters(table.get(), tablecoords, centers) != 0,
	    "tablesearchcenters() fails right at the right edge of the knot field.");

	tablecoords[time_dim] = table->knots[time_dim][table->nknots[time_dim]-1];

	ENSURE_EQUAL(tablesearchcenters(table.get(), tablecoords, centers), 0,
	    "tablesearchcenters() succeeds just inside the right edge of the knot field.");
	ENSURE_EQUAL(centers[time_dim],
	    table->nknots[time_dim]-table->order[time_dim]-2,
	    "centers[time_dim] holds the first fully-supported knot index.");

	base = ndsplineeval(table.get(), tablecoords, centers, 0);

	ENSURE(base >= 0, "Base quantile is positive.");
	ENSURE_DISTANCE(0, base, std::numeric_limits<float>::epsilon(),
	    "Time quantile is continuous "
	    "(slope is less than FLOAT_EPSILON/DBL_EPSILON)");
}

TEST(ndssplineeval_vs_ndssplineeval_gradient)
{
	srand(42);

	TableSet tables = get_splinetables();
	boost::shared_ptr<struct splinetable> table = load_splinetable(tables.prob);

	const int ndim = table->ndim;
	ENSURE(ndim < 6);
	for (int i=0; i < 1000; i++) {
		/* Generate coordinates uniformly in the support of the spline */
		double x[ndim];
		int centers[ndim];
		for (int j=0; j < ndim; j++) {
			double p = double(rand())/double(RAND_MAX);
			x[j] = table->extents[j][0] + p*(table->extents[j][1]-table->extents[j][0]);
		}
		ENSURE_EQUAL(tablesearchcenters(table.get(), x, centers), 0);
		double evaluate;
		double evaluate_with_gradient[ndim+1];
		evaluate = ndsplineeval(table.get(), x, centers, 0);
		ndsplineeval_gradient(table.get(), x, centers, evaluate_with_gradient);
		ENSURE_EQUAL(evaluate, evaluate_with_gradient[0],
		    "ndsplineeval() and ndssplineeval_gradient() yield identical evaluates");
		for (int j=0; j < ndim; j++) {
			evaluate = ndsplineeval(table.get(), x, centers, 1 << j);
			ENSURE_EQUAL(evaluate, evaluate_with_gradient[1+j],
			    "ndsplineeval() and ndssplineeval_gradient() yield identical derivatives");
		}
	}
}

/*
 * bsplvb_simple() can be made to return sensical values anywhere
 * in the knot field.
 */

TEST(bsplvb_simple_vs_bspline)
{
	unsigned i;
	const unsigned n_knots = 10;
	const int order = 2;
	double x, *knots;
	int center, offset;
	float localbasis_bsplvb[order+1], localbasis_bspline[order+1];
	struct timeval thetime;
	std::vector<double> knotvec;

	/* Seed our crappy little PSRNG. */
	gettimeofday(&thetime, NULL);
	srand(thetime.tv_sec);
	/* Generate a random knot field. */
	for (i = 0; i < n_knots; i++)
		knotvec.push_back(double(rand())/double(RAND_MAX));
	std::sort(knotvec.begin(), knotvec.end());
	knots = &knotvec.front();

	/* Before the first fully-supported knot. */
	for (i = 0; i < order+1; i++) {
		x = (knots[i]+knots[i+1])/2.0; /* Less than fully-supported */
		center = order; /* First fully-supported spline. */

		ENSURE(int(i) <= center);

		bsplvb_simple(knots, n_knots, x, center /* left */,
		    order+1 /* jhigh */, localbasis_bsplvb /* biatx */);

		for (offset = -order; offset <= 0; offset++) {
			ENSURE(offset+order >= 0);
			localbasis_bspline[offset+order] =
			    bspline(knots, x, center + offset, order);
		}

		for (offset = 0; offset < order+1; offset++) {
			ENSURE_DISTANCE(localbasis_bspline[offset], localbasis_bsplvb[offset],
			std::numeric_limits<double>::epsilon());
		}
	}

	/* Within the support. */
	for (i = order+1; i < n_knots-order-1; i++) {
		x = (knots[i]+knots[i+1])/2.0;
		center = i;

		bsplvb_simple(knots, n_knots, x, center /* left */,
		    order+1 /* jhigh */, localbasis_bsplvb /* biatx */);

		for (offset = -order; offset <= 0; offset++) {
			ENSURE(offset+order >= 0);
			localbasis_bspline[offset+order] =
			    bspline(knots, x, center + offset, order);
		}

		for (offset = 0; offset < order+1; offset++) {
			ENSURE_DISTANCE(localbasis_bspline[offset], localbasis_bsplvb[offset],
			std::numeric_limits<double>::epsilon());
		}
	}

	/* After the last first fully-supported knot. */
	for (i = n_knots-order-1; i < n_knots-2; i++) {
		x = (knots[i]+knots[i+1])/2.0; /* Less than fully-supported */
		center = n_knots-order-2; /* Last fully-supported spline. */

		ENSURE(int(i) >= center);

		bsplvb_simple(knots, n_knots, x, center /* left */,
		    order+1 /* jhigh */, localbasis_bsplvb /* biatx */);

		for (offset = -order; offset <= 0; offset++) {
			ENSURE(offset+order >= 0);
			localbasis_bspline[offset+order] =
			    bspline(knots, x, center + offset, order);
		}

		for (offset = 0; offset < order+1; offset++) {
			ENSURE_DISTANCE(localbasis_bspline[offset], localbasis_bsplvb[offset],
			std::numeric_limits<double>::epsilon());
		}
	}
}

/*
 * bspline_deriv_nonzero() can be made to return sensical values anywhere
 * in the knot field.
 */

TEST(bspline_deriv_nonzero_vs_bspline_deriv)
{
	unsigned i;
	const unsigned n_knots = 10;
	const int order = 2;
	double x, *knots;
	int center, offset;
	float localbasis_bsplvb[order+1], localbasis_bspline[order+1];
	struct timeval thetime;
	std::vector<double> knotvec;
	/* This calculation is less stable. */
	double tol = 10*std::numeric_limits<float>::epsilon();

	/* Seed our crappy little PSRNG. */
	gettimeofday(&thetime, NULL);
	srand(thetime.tv_sec);
	/* Generate a random knot field. */
	for (i = 0; i < n_knots; i++)
		knotvec.push_back(double(rand())/double(RAND_MAX));
	std::sort(knotvec.begin(), knotvec.end());
	knots = &knotvec.front();

	/* Before the first fully-supported knot. */
	for (i = 0; i < order+1; i++) {
		x = (knots[i]+knots[i+1])/2.0; /* Less than fully-supported */
		center = order; /* First fully-supported spline. */

		ENSURE(int(i) <= center);

		bspline_deriv_nonzero(knots, n_knots, x, center /* left */,
		    order /* jhigh */, localbasis_bsplvb /* biatx */);

		for (offset = -order; offset <= 0; offset++) {
			ENSURE(offset+order >= 0);
			localbasis_bspline[offset+order] =
			    bspline_deriv(knots, x, center + offset, order, 1);
		}

		for (offset = 0; offset < order+1; offset++) {
			ENSURE_DISTANCE(localbasis_bspline[offset], localbasis_bsplvb[offset], tol);
		}
	}

	/* Within the support. */
	for (i = order+1; i < n_knots-order-1; i++) {
		x = (knots[i]+knots[i+1])/2.0;
		center = i;

		bspline_deriv_nonzero(knots, n_knots, x, center /* left */,
		    order, localbasis_bsplvb /* biatx */);

		for (offset = -order; offset <= 0; offset++) {
			ENSURE(offset+order >= 0);
			localbasis_bspline[offset+order] =
			    bspline_deriv(knots, x, center + offset, order, 1);
		}

		for (offset = 0; offset < order+1; offset++) {
			ENSURE_DISTANCE(localbasis_bspline[offset], localbasis_bsplvb[offset], tol);
		}
	}

	/* After the last first fully-supported knot. */
	for (i = n_knots-order-1; i < n_knots-2; i++) {
		x = (knots[i]+knots[i+1])/2.0; /* Less than fully-supported */
		center = n_knots-order-2; /* Last fully-supported spline. */

		ENSURE(int(i) >= center);

		bspline_deriv_nonzero(knots, n_knots, x, center /* left */,
		    order /* jhigh */, localbasis_bsplvb /* biatx */);

		for (offset = -order; offset <= 0; offset++) {
			ENSURE(offset+order >= 0);
			localbasis_bspline[offset+order] =
			    bspline_deriv(knots, x, center + offset, order, 1);
		}

		for (offset = 0; offset < order+1; offset++) {
			ENSURE_DISTANCE(localbasis_bspline[offset], localbasis_bsplvb[offset], tol);
		}
	}
}

/*
 * bspline_nonzero() can be made to return sensical values anywhere
 * in the knot field.
 */

TEST(bspline_nonzero_vs_bspline)
{
	unsigned i;
	const unsigned n_knots = 10;
	const int order = 2;
	double x, *knots;
	int center, offset;
	float localbasis_bsplvb[order+1], localbasis_bspline[order+1],
	    localbasis_bsplvb_deriv[order+1], localbasis_bspline_deriv[order+1];
	struct timeval thetime;
	std::vector<double> knotvec;
	/* This calculation is less stable. */
	double tol = 10*std::numeric_limits<float>::epsilon();

	/* Seed our crappy little PSRNG. */
	gettimeofday(&thetime, NULL);
	srand(thetime.tv_sec);
	/* Generate a random knot field. */
	for (i = 0; i < n_knots; i++)
		knotvec.push_back(double(rand())/double(RAND_MAX));
	std::sort(knotvec.begin(), knotvec.end());
	knots = &knotvec.front();

	/* Before the first fully-supported knot. */
	for (i = 0; i < order+1; i++) {
		x = (knots[i]+knots[i+1])/2.0; /* Less than fully-supported */
		center = order; /* First fully-supported spline. */

		ENSURE(int(i) <= center);

		bspline_nonzero(knots, n_knots, x, center /* left */,
		    order /* jhigh */, localbasis_bsplvb, localbasis_bsplvb_deriv);

		for (offset = -order; offset <= 0; offset++) {
			ENSURE(offset+order >= 0);
			localbasis_bspline[offset+order] =
			    bspline(knots, x, center + offset, order);
			localbasis_bspline_deriv[offset+order] =
			    bspline_deriv(knots, x, center + offset, order, 1);
		}

		for (offset = 0; offset < order+1; offset++) {
			ENSURE_DISTANCE(localbasis_bspline[offset], localbasis_bsplvb[offset], tol);
			ENSURE_DISTANCE(localbasis_bspline_deriv[offset], localbasis_bsplvb_deriv[offset], tol);

		}
	}

	/* Within the support. */
	for (i = order+1; i < n_knots-order-1; i++) {
		x = (knots[i]+knots[i+1])/2.0;
		center = i;

		bspline_nonzero(knots, n_knots, x, center /* left */,
		    order /* jhigh */, localbasis_bsplvb, localbasis_bsplvb_deriv);

		for (offset = -order; offset <= 0; offset++) {
			ENSURE(offset+order >= 0);
			localbasis_bspline[offset+order] =
			    bspline(knots, x, center + offset, order);
			localbasis_bspline_deriv[offset+order] =
			    bspline_deriv(knots, x, center + offset, order, 1);
		}

		for (offset = 0; offset < order+1; offset++) {
			ENSURE_DISTANCE(localbasis_bspline[offset], localbasis_bsplvb[offset], tol);
			ENSURE_DISTANCE(localbasis_bspline_deriv[offset], localbasis_bsplvb_deriv[offset], tol);

		}
	}

	/* After the last first fully-supported knot. */
	for (i = n_knots-order-1; i < n_knots-2; i++) {
		x = (knots[i]+knots[i+1])/2.0; /* Less than fully-supported */
		center = n_knots-order-2; /* Last fully-supported spline. */

		ENSURE(int(i) >= center);

		bspline_nonzero(knots, n_knots, x, center /* left */,
		    order /* jhigh */, localbasis_bsplvb, localbasis_bsplvb_deriv);

		for (offset = -order; offset <= 0; offset++) {
			ENSURE(offset+order >= 0);
			localbasis_bspline[offset+order] =
			    bspline(knots, x, center + offset, order);
			localbasis_bspline_deriv[offset+order] =
			    bspline_deriv(knots, x, center + offset, order, 1);
		}

		for (offset = 0; offset < order+1; offset++) {
			ENSURE_DISTANCE(localbasis_bspline[offset], localbasis_bsplvb[offset], tol);
			ENSURE_DISTANCE(localbasis_bspline_deriv[offset], localbasis_bsplvb_deriv[offset], tol);

		}
	}
}

/*
 * bspline_nonzero() gives the same result as bsplvb_simple(), ergo the
 * value bases used in ndsplineeval() and ndsplineeval_gradient() are
 * identical. Roundoff error in the derivative basis calculation
 * scales with the spline order, so we use a large number here.
 */

TEST(single_basis_vs_multi)
{
	unsigned i;
	const unsigned n_knots = 100;
	const int order = 5;
	double x, *knots;
	int center, offset;
	float localbasis_bspline_nonzero[order+1], localbasis_bspline_nonzero_deriv[order+1],
	    localbasis_bsplvb_simple[order+1], localbasis_bspline_deriv_nonzero[order+1];
	// bsplvb() may access up to *order* elements off either end
	// Pad accordingly.
	std::vector<double> knotvec(n_knots + 2*order, 0.);

	srand(0);
	/* Generate a random knot field. */
	for (i = 0; i < n_knots; i++)
		knotvec[i+order] = (double(rand())/double(RAND_MAX));
	std::sort(knotvec.begin()+order, knotvec.end()-order);
	knots = &knotvec[order];

	/* Before the first fully-supported knot. */
	for (i = 0; i < order+1; i++) {
		x = (knots[i]+knots[i+1])/2.0; /* Less than fully-supported */
		center = order; /* First fully-supported spline. */

		ENSURE(int(i) <= center);

		/* As used in ndssplineeval_gradient() */
		bspline_nonzero(knots, n_knots, x, center /* left */,
		    order /* jhigh */, localbasis_bspline_nonzero, localbasis_bspline_nonzero_deriv);
		/* As used in ndsplineeval() */
		bsplvb_simple(knots, n_knots, x, center /* left */,
		    order + 1 /* jhigh */, localbasis_bsplvb_simple);
		bspline_deriv_nonzero(knots, n_knots, x, center,
		    order, localbasis_bspline_deriv_nonzero);

		for (offset = 0; offset < order+1; offset++) {
			ENSURE_EQUAL(localbasis_bspline_nonzero[offset], localbasis_bsplvb_simple[offset]);
			ENSURE_EQUAL(localbasis_bspline_nonzero_deriv[offset], localbasis_bspline_deriv_nonzero[offset]);

		}

	}

	/* Within the support. */
	for (i = order+1; i < n_knots-order-1; i++) {
		x = (knots[i]+knots[i+1])/2.0;
		center = i;

		bspline_nonzero(knots, n_knots, x, center /* left */,
		    order /* jhigh */, localbasis_bspline_nonzero, localbasis_bspline_nonzero_deriv);
		bsplvb_simple(knots, n_knots, x, center /* left */,
		    order + 1 /* jhigh */, localbasis_bsplvb_simple);
		bspline_deriv_nonzero(knots, n_knots, x, center,
		    order, localbasis_bspline_deriv_nonzero);

		for (offset = 0; offset < order+1; offset++) {
			ENSURE_EQUAL(localbasis_bspline_nonzero[offset], localbasis_bsplvb_simple[offset]);
			ENSURE_EQUAL(localbasis_bspline_nonzero_deriv[offset], localbasis_bspline_deriv_nonzero[offset]);
		}
	}

	/* After the last first fully-supported knot. */
	for (i = n_knots-order-1; i < n_knots-2; i++) {
		x = (knots[i]+knots[i+1])/2.0; /* Less than fully-supported */
		center = n_knots-order-2; /* Last fully-supported spline. */

		ENSURE(int(i) >= center);

		bspline_nonzero(knots, n_knots, x, center /* left */,
		    order /* jhigh */, localbasis_bspline_nonzero, localbasis_bspline_nonzero_deriv);
		bsplvb_simple(knots, n_knots, x, center /* left */,
		    order + 1 /* jhigh */, localbasis_bsplvb_simple);
		bspline_deriv_nonzero(knots, n_knots, x, center,
		    order, localbasis_bspline_deriv_nonzero);

		for (offset = 0; offset < order+1; offset++) {
			ENSURE_EQUAL(localbasis_bspline_nonzero[offset], localbasis_bsplvb_simple[offset]);
			ENSURE_EQUAL(localbasis_bspline_nonzero_deriv[offset], localbasis_bspline_deriv_nonzero[offset]);
		}
	}
}


// Copyright 2015, 2017 Peter Dimov.
//
// Distributed under the Boost Software License, Version 1.0.
//
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/mp11/utility.hpp>
#include <boost/mp11/integral.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <type_traits>

int main()
{
    using boost::mp11::mp_eval_if_c;
    using boost::mp11::mp_identity;

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_eval_if_c<true, char[], mp_identity, void, void, void>, char[]>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_eval_if_c<false, char[], mp_identity, void()>, mp_identity<void()>>));

    using boost::mp11::mp_eval_if;
    using boost::mp11::mp_eval_if_q;
    using boost::mp11::mp_quote;

    using qt_identity = mp_quote<mp_identity>;

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_eval_if<std::true_type, char[], mp_identity, void, void, void>, char[]>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_eval_if<std::false_type, char[], mp_identity, void()>, mp_identity<void()>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_eval_if_q<std::true_type, char[], qt_identity, void, void, void>, char[]>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_eval_if_q<std::false_type, char[], qt_identity, void()>, mp_identity<void()>>));

    using boost::mp11::mp_int;

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_eval_if<mp_int<-7>, char[], mp_identity, void, void, void>, char[]>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_eval_if<mp_int<0>, char[], mp_identity, void()>, mp_identity<void()>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_eval_if_q<mp_int<-7>, char[], qt_identity, void, void, void>, char[]>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_eval_if_q<mp_int<0>, char[], qt_identity, void()>, mp_identity<void()>>));

    using boost::mp11::mp_size_t;

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_eval_if<mp_size_t<14>, char[], mp_identity, void, void, void>, char[]>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_eval_if<mp_size_t<0>, char[], mp_identity, void()>, mp_identity<void()>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_eval_if_q<mp_size_t<14>, char[], qt_identity, void, void, void>, char[]>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_eval_if_q<mp_size_t<0>, char[], qt_identity, void()>, mp_identity<void()>>));

    return boost::report_errors();
}


#include "ManifoldUtils.hpp"

#include <wmtk/utils/VectorUtils.h>

// clang-format off
#include <wmtk/utils/DisableWarnings.hpp>
#include <lagrange/mesh_cleanup/resolve_nonmanifoldness.h>
#include <wmtk/utils/EnableWarnings.hpp>
// clang-format on

#include <igl/extract_manifold_patches.h>
#include <igl/is_edge_manifold.h>
#include <igl/remove_unreferenced.h>
#include <spdlog/fmt/bundled/ranges.h>
#include <spdlog/fmt/ostr.h>
#include <spdlog/spdlog.h>
#include <Eigen/Core>

#include <vector>

namespace wmtk::manifold_internal {

void resolve_nonmanifoldness(Vertices& V, Facets& F, std::vector<size_t>& modified_vertices)
{
    using Scalar = Vertices::Scalar;
    using Index = Facets::Scalar;
    using Mesh = lagrange::Mesh<Vertices, Facets>;
    using AttributeArray = Mesh::AttributeArray;

    constexpr Index Invalid = std::numeric_limits<Index>::max();

    // For this, we assume that sizeof(double) == sizeof(uint64_t), which should be the case on all
    // platforms...
    static_assert(
        sizeof(Scalar) == sizeof(Index),
        "Vertex scalar type and facet index type should have the same size");
    auto mesh = lagrange::create_mesh(std::move(V), std::move(F));

    // Create a vertex attribute to track vertex id
    AttributeArray old_vids(mesh->get_num_vertices(), 1);
    for (Index v = 0; v < mesh->get_num_vertices(); ++v) {
        old_vids(v, 0) = static_cast<double>(v);
    }
    mesh->add_vertex_attribute("old_vids");
    mesh->import_vertex_attribute("old_vids", std::move(old_vids));

    const Index old_num_vertices = mesh->get_num_vertices();

    // Resolve non-manifoldness
    mesh = lagrange::resolve_nonmanifoldness(*mesh);
    const Index new_num_vertices = mesh->get_num_vertices();

    // Export vertices + facets + original ids
    mesh->export_vertices(V);
    mesh->export_facets(F);
    mesh->export_vertex_attribute("old_vids", old_vids);

    // Check which vids has been duplicated
    std::vector<Index> old_to_new(old_num_vertices, Invalid);
    std::vector<bool> is_vertex_modified(new_num_vertices, false);
    for (Index new_idx = 0; new_idx < new_num_vertices; ++new_idx) {
        Index old_idx = static_cast<double>(old_vids(new_idx, 0));
        assert(old_idx >= 0 && old_idx < old_num_vertices);
        if (old_to_new[old_idx] != Invalid) {
            is_vertex_modified[old_to_new[old_idx]] = true;
            is_vertex_modified[new_idx] = true;
        } else {
            old_to_new[old_idx] = new_idx;
        }
    }

    // List all duplicated vertices
    modified_vertices.clear();
    for (size_t v = 0; v < is_vertex_modified.size(); ++v) {
        if (is_vertex_modified[v]) {
            modified_vertices.push_back(v);
        }
    }
}

} // namespace wmtk::manifold_internal

bool wmtk::separate_to_manifold(
    const std::vector<Eigen::Vector3d>& vertices,
    const std::vector<std::array<size_t, 3>>& faces,
    std::vector<Eigen::Vector3d>& out_v,
    std::vector<std::array<size_t, 3>>& out_f,
    std::vector<size_t>& mod_v)
{
    using Vertices = wmtk::manifold_internal::Vertices;
    using Facets = wmtk::manifold_internal::Facets;

    Vertices V(vertices.size(), 3);
    for (auto i = 0; i < vertices.size(); i++) V.row(i) = vertices[i];
    Facets F(faces.size(), 3);
    for (auto i = 0; i < faces.size(); i++) F.row(i) << faces[i][0], faces[i][1], faces[i][2];

    wmtk::manifold_internal::resolve_nonmanifoldness(V, F, mod_v);

    out_v.resize(V.rows());
    out_f.resize(F.rows());
    for (auto i = 0; i < V.rows(); i++) out_v[i] = V.row(i);
    for (auto i = 0; i < F.rows(); i++)
        out_f[i] = std::array<size_t, 3>{{(size_t)F(i, 0), (size_t)F(i, 1), (size_t)F(i, 2)}};

    return true;
}


#include <boost/serialization/export.hpp>
#include "LocallyConnected1D.hpp"
#include "LayerModel.hpp"

using namespace std;
using namespace snn;
using namespace internal;

BOOST_CLASS_EXPORT(LocallyConnected1D)

LocallyConnected1D::LocallyConnected1D(LayerModel& model, shared_ptr<NeuralNetworkOptimizer> optimizer)
    : FilterLayer(model, optimizer)
{
    const int rest = this->shapeOfInput[0] % this->sizeOfFilterMatrix == 0 ? 0 : 1;

    this->shapeOfOutput = {
        this->shapeOfInput[0] / this->sizeOfFilterMatrix + rest,
        this->numberOfFilters
    };
    this->sizeOfNeuronInputs = this->sizeOfFilterMatrix * this->shapeOfInput[1];
}

inline
unique_ptr<BaseLayer> LocallyConnected1D::clone(std::shared_ptr<NeuralNetworkOptimizer> optimizer) const
{
    auto layer = make_unique<LocallyConnected1D>(*this);
    for (int n = 0; n < layer->getNumberOfNeurons(); ++n)
    {
        layer->neurons[n].setOptimizer(optimizer);
    }
    return layer;
}

int LocallyConnected1D::isValid() const
{
    for (auto& neuron : neurons)
    {
        if (neuron.getNumberOfInputs() != this->sizeOfFilterMatrix * this->shapeOfInput[1])
            return 203;
    }
    return this->FilterLayer::isValid();
}

inline
vector<float> LocallyConnected1D::createInputsForNeuron(const int neuronIndex, const vector<float>& inputs)
{
    const int beginIndex = neuronIndex * this->sizeOfNeuronInputs;
    const int endIndex = beginIndex + this->sizeOfNeuronInputs;

    if (endIndex <= this->shapeOfInput[0])
        return vector<float>(inputs.begin() + beginIndex, inputs.begin() + endIndex);
    else
    {
        auto v = vector<float>(inputs.begin() + beginIndex, inputs.begin() + this->shapeOfInput[1]);
        v.resize(this->sizeOfNeuronInputs, 0);
        return v;
    }
}

inline
void LocallyConnected1D::insertBackOutputForNeuron(const int neuronIndex, const std::vector<float>& error, std::vector<float>& errors)
{
    const int beginIndex = neuronIndex * this->sizeOfNeuronInputs;
    for (int e = 0; e < (int)error.size(); ++e)
    {
        const int i = beginIndex + e;
        errors[i] += error[e];
    }
}

inline
bool LocallyConnected1D::operator==(const BaseLayer& layer) const
{
    return this->FilterLayer::operator==(layer);
}

inline
bool LocallyConnected1D::operator!=(const BaseLayer& layer) const
{
    return !(*this == layer);
}


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

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_ASSERT(at(int_<0>, list(0, '1', 2.0)) == 0);
    BOOST_HANA_CONSTEXPR_ASSERT(at(int_<1>, list(0, '1', 2.0)) == '1');
    BOOST_HANA_CONSTEXPR_ASSERT(at(int_<2>, list(0, '1', 2.0)) == 2.0);
    //! [main]
}


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>

int main() {
  using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS>;
  Graph g(3); // Create a graph with 3 vertices.
  boost::add_edge(0, 1, g);
  boost::add_edge(1, 2, g);
  boost::dynamic_properties dp;
  //dp.property("style", get(&StyleProperty::style, g));
  dp.property("node_id", get(boost::vertex_index, g));
  boost::write_graphviz_dp(std::cout, g, dp);
  return 0;
}


// loop_mc_tools.hpp
#define REAL 0
#define IMAG 1

//#include <iostream>
//#include <vector>
//#include <fstream>
//#include <math.h>
//#include <time.h>
//#include <boost/random.hpp>
//#include <boost/filesystem.hpp>
//
//#include <sstream>
//
#include <complex>
#include <fftw3.h>
#include <fstream>
#include "lattice.hpp"

#ifndef estimator_hpp_
#define estimator_hpp_

using namespace std;
typedef complex<double> dcmplx;

/******************************************************************************************************/
class Estimator
{
    public:

        //Estimator(Lattice &lat, string dat_f_dir);
        //Estimator(int length,int height,int seed,string dat_f_dir);
        Estimator(string dat_f_dir,const Lattice& lat,const Lattice& br_lat);

        Lattice lat;
        // background lat
        Lattice br_lat;

        // for writing to file that is just the current running average of the bins
        int number_bins_compleated_so_far;

        int length;
        int height;
        int seed;

        ofstream out_file;
        virtual void do_measurement()=0;

        // before write to file
        virtual void divide_hist_bins_by(int dev_by)=0;

        // after we have done the above (or not if N_measure = 1) we write hist_bins to file
        virtual void write()=0;

        // after we write something to file we need to clear the hist bins
        virtual void clear_hist_bins()=0;

        //tells us the directory in which the files will be written
        string dir_to_write;

        int file_precision;

        void close_wf(void);

};

#endif


// Input/Output
#include <iostream>

// Files
#include <fstream>

// Open3D
#include "Open3D/Open3D.h"

// Maths
#include <Eigen/Core>

// Namespaces
using namespace open3d;
using namespace std;

int main()
{
    // Read triangle mesh "test_mesh.ply"
    geometry::TriangleMesh mesh;

    // Use the ReadTrianbleMesh function defined under TriangleMeshIO.h
    //io::ReadTriangleMesh("../../examples/TestData/test_mesh_Small.ply", mesh);
    io::ReadTriangleMesh("../../examples/TestData/test_mesh.ply", mesh);

    // Intro messages
    cout << "\nIntel Solution for Open3D by Selim Ozel: \n";
    cout << "Test mesh has " << mesh.vertices_.size() << " vertices. \n";
    cout << "Test mesh has " << mesh.triangles_.size() << " triangles. \n";

    // Compute adjacency list
    mesh.ComputeAdjacencyList();
    if (mesh.HasAdjacencyList())
    {
        cout << "Adjacency mesh exists and computed. \n";
    }

    // Compute all identically colored connected components
    vector<vector<int>> result = mesh.IdenticallyColoredConnectedComponents();

    // Print results to file
    ofstream outputFileHandle;
    outputFileHandle.open("results.txt");

    for (int i = 0; i<result.size() ; i++)
    {
        for (int j = 0; j<result[i].size(); j++)
        {
            outputFileHandle << result[i][j] << " ";
        }
        outputFileHandle << "\n";
    }
    outputFileHandle.close();

    cout << "Results written to \"results.txt\" \n";

    return 0;
}


/*
 * @file
 * NOIntegratorUKF.hpp
 *
 *  Created on: 03.09.2018
 *      Author: tomlucas
 */

#ifndef ESTIMATORS_LAMBDAPATHUKFEXTENDED_HPP
#define ESTIMATORS_LAMBDAPATHUKFEXTENDED_HPP

#include "UKF.hpp"
#include "Models/LambdaPathWithVelocityModel.hpp"

#include <Eigen_Utils.hpp>

#include "../Plugins/imu_plugin.hpp"
#include "StateBoxes/OrientationBox.hpp"

#include <iostream>

namespace zavi
::estimator {
	/**
	 * makes use of LambdaPathWithVelocityModel and classic UKF
	 * Improved estimation of the position by estimating the velocity via forward velocity prior
	 */
	class LambdaPathUKFExtended: public UKF<zavi::estimator::model::LambdaPathWithVelocityModel> {

	public :
		enum CallbackIndices {CI_AFTER_IMU_CALLBACK,     //!< at end of imu callback
			CI_AT_START_IMU_CALLBACK};					//!< at start of imu callback
	protected:
		zavi::eigen_util::FuncCaller lambda_path;
		/**
		 * Protected constructor only call from createUKFIntegrator
		 * @param imu pointer to main imu
		 */

		//template<typename functor>
		LambdaPathUKFExtended(std::shared_ptr<plugin::IMU_Plugin> imu, eigen_util::FuncCaller & func):UKF<zavi::estimator::model::LambdaPathWithVelocityModel>(imu), lambda_path(func) {

			state_boxes::OrientationSpeedBox::INNER_T<double> orient_std=orient_std.Ones()*0.;

			orient_std(3)=orient_std(4)=orient_std(5)=1.;
			//orient_std(0,0)=orient_std(1,0)=orient_std(2,0)=0.1;
			std::get<state_boxes::OrientationSpeedBox>(box_model.boxes).std=orient_std;
			state_boxes::LambdaPathBoxNoAcceleration::INNER_T<double > std=std.Zero();
			std(0)=10.;
			std(1)=0.1;
			std::get<state_boxes::LambdaPathBoxNoAcceleration>(box_model.boxes).std=std;
			state_boxes::CartesianBox::INNER_T<double > cart_std=cart_std.Zero();
			cart_std(6)=cart_std(7)=cart_std(8)= 100;
			std::get<state_boxes::CartesianBox>(box_model.boxes).std=cart_std;
		}

	public:
		zavi::eigen_util::FuncCaller getFuncCaller() {
			return lambda_path;
		}
		/**
		 * Callback for imu
		 * @param plug the imu plugin which has new data
		 * @param estimator the estimator which gets the new data
		 * @param time the current time stamp
		 */
		static void imuCallback(plugin::SensorPlugin * plug,
				void* estimator, double time) {
			static double last_time=time-0.0001;     // fix for zero start time
			double time_diff=time-last_time;
			LambdaPathUKFExtended * esti = static_cast<LambdaPathUKFExtended *>(estimator);
			esti->saveStatesForSmoothing(time_diff);
			esti->notifyCallbacks(CI_AT_START_IMU_CALLBACK);
			plugin::IMU_Plugin *imu = static_cast<plugin::IMU_Plugin*>(plug);
			/*	esti->state_vector(2,0)=(*imu->getAcceleration())(0,0);
			 esti->covariance(2,2)=(*imu->getAccelerationCov())(0,0);
			 esti->state_vector.block<3,1>(esti->box_model.orientation_speed_start,0)=*(imu->getAngularVelocity());

			 esti->covariance.block<3,3>(esti->box_model.orientation_start+3,esti->box_model.orientation_start+3)=*(imu->getAngularVelocityCov());*/
			//		zavi::plot::liveHeatMap<0>(esti->covariance,"Covariance of UKF");
			//zavi::plot::liveStateDraw<1>(*imu->getAngularVelocity(), "AR");
			//zavi::plot::liveStateDraw<2>(*imu->getAcceleration(), "Acceleration");
			//	zavi::plot::liveStateDraw<3>(esti->getBoxModel().acceleration_measurement(esti->state_vector,NULL), "estimated acceleration");
			//esti->state_vector.block<3,1>(6,0)=(imu->acceleration_inc);
			Eigen::Matrix<double,1,1> target=target.Zero();
			Eigen::Matrix<double,1,1> lambda_noise=lambda_noise.Identity()*0.2;
			esti->measurementStep(target,time_diff,esti->box_model.dot_lambda_measurement<double>,lambda_noise);
			//esti->state_vector(esti->box_model.lambda_velocity_start,0)=esti->box_model.getVelocity(esti->getStateVector()).norm();
			esti->dynamicStep(time_diff);
			//zavi::plot::liveStateDraw<1,double,9>(esti->state_vector.block<9,1>(0,0),"Cart Vector");
			//zavi::plot::liveStateDraw<2,double,12>(esti->state_vector.block<12,1>(11,0),"Rotation Vector");
			Eigen::Matrix3d noise=imu->getAccelerationCov()+Eigen::Matrix3d::Identity()*0.2;
			Eigen::Matrix3d orient_noise=imu->getAngularVelocityCov();
			esti->measurementStep(imu->getAngularVelocity(),time_diff,esti->box_model.orientation_change_measurement<double>,orient_noise);
			esti->measurementStep(imu->getAcceleration(),time_diff,esti->box_model.acceleration_measurement<double>,noise);

			last_time=time;
			esti->notifyCallbacks(CI_AFTER_IMU_CALLBACK);
		}

		/**
		 * Create an IntegratorEstimatorUKF shared pointer
		 * @param imu the imu to read the basic data
		 * @return shared pointer to the estimator
		 */
		//template<typename functor>
		static std::shared_ptr<zavi::estimator::LambdaPathUKFExtended> createLambdaPathUKFExtended(std::shared_ptr<plugin::IMU_Plugin> imu, eigen_util::FuncCaller func) {
			std::shared_ptr<zavi::estimator::LambdaPathUKFExtended > estimator(new LambdaPathUKFExtended(imu,func));
			imu->addDataCallback(imuCallback,estimator);
			return estimator;
		}

		virtual Eigen::Vector3d getEstimatedPosition() {
			return lambda_path(state_vector(9,0));
		}

		virtual Eigen::Quaterniond getEstimatedOrientation() {
			return Eigen::Quaterniond(Eigen::Matrix3d(state_vector.block<9,1>(MODEL_TYPE::orientation_start,0).data()));     //< static orientation
		}

		virtual Eigen::Matrix3d getEstimatedPositionError() {
			double sigma= sqrt(covariance(9,9));
			double l=state_vector(9,0);
			Eigen::Vector3d mu=lambda_path(l);
			Eigen::Matrix3d variance=variance.Zero();
			for(int i=-1; i<=1; i++) {
				Eigen::Vector3d diff=lambda_path(i*sigma+l)-mu;
				variance+=diff*diff.transpose();
			}
			variance/=2.;
			return variance;
		}

		virtual void run(double freq, bool (*running)()) {

		}

	private:
		//Eigen::Matrix<double, NOUKF_STATE_DIM, NOUKF_STATE_DIM> state_transition;     //< state transition matrix
		//Eigen::Matrix<double, NOUKF_STATE_DIM, NOUKF_INPUT_DIM> input_gain;//< input matrix
	};

}
//zavi::estimator

#endif /* ESTIMATORS_LAMBDAPATHUKFEXTENDED_HPP */


// Copyright (c) 2014-2019 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <chainparams.h>
#include <net.h>
#include <validation.h>

#include <test/util/setup_common.h>

#include <boost/signals2/signal.hpp>
#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(validation_tests, TestingSetup)

static void TestBlockSubsidyHalvings(const Consensus::Params& consensusParams)
{
    int maxHalvings = 100;
    CAmount nInitialSubsidy = 50 * COIN;

    for (int nHalvings = 0; nHalvings < maxHalvings; nHalvings++) {
        int nHeight = nHalvings * consensusParams.nSubsidyHalvingInterval;
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
        switch (nHalvings) {
            case 0: BOOST_CHECK_EQUAL(nSubsidy, nInitialSubsidy); break;
            case 1: BOOST_CHECK_EQUAL(nSubsidy, 25 * COIN); break;
            case 2: BOOST_CHECK_EQUAL(nSubsidy, round(12.5 * COIN)); break;
            case 3: BOOST_CHECK_EQUAL(nSubsidy, round(6.25 * COIN)); break;
            case 4: BOOST_CHECK_EQUAL(nSubsidy, round(6.56 * COIN)); break;
            case 5: BOOST_CHECK_EQUAL(nSubsidy, round(7.02 * COIN)); break;
            case 6: BOOST_CHECK_EQUAL(nSubsidy, round(7.51 * COIN)); break;
            case 7: BOOST_CHECK_EQUAL(nSubsidy, round(8.04 * COIN)); break;
            case 8: BOOST_CHECK_EQUAL(nSubsidy, round(8.6 * COIN)); break;
            case 9: BOOST_CHECK_EQUAL(nSubsidy, round(9.2 * COIN)); break;
            case 10: BOOST_CHECK_EQUAL(nSubsidy, round(9.85 * COIN)); break;
            case 11: BOOST_CHECK_EQUAL(nSubsidy, round(10.54 * COIN)); break;
            case 12: BOOST_CHECK_EQUAL(nSubsidy, round(11.28 * COIN)); break;
            case 13: BOOST_CHECK_EQUAL(nSubsidy, round(12.06 * COIN)); break;
            case 14: BOOST_CHECK_EQUAL(nSubsidy, round(12.91 * COIN)); break;
            case 15: BOOST_CHECK_EQUAL(nSubsidy, round(13.81 * COIN)); break;
            case 16: BOOST_CHECK_EQUAL(nSubsidy, round(14.78 * COIN)); break;
            case 17: BOOST_CHECK_EQUAL(nSubsidy, round(15.81 * COIN)); break;
            case 18: BOOST_CHECK_EQUAL(nSubsidy, round(16.92 * COIN)); break;
            case 19: BOOST_CHECK_EQUAL(nSubsidy, round(18.11 * COIN)); break;
            case 20: BOOST_CHECK_EQUAL(nSubsidy, round(19.37 * COIN)); break;
            case 21: BOOST_CHECK_EQUAL(nSubsidy, round(20.73 * COIN)); break;
            case 99: BOOST_CHECK_EQUAL(nSubsidy, round(4060.02 * COIN)); break;
        }
    }
}

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    TestBlockSubsidyHalvings(chainParams->GetConsensus()); // As in main
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    CAmount nSum = 0;
    for (int nHeight = 0; nHeight < 14000000; nHeight += 1000) {
        CAmount nSubsidy = GetBlockSubsidy(nHeight, chainParams->GetConsensus());
        nSum += nSubsidy * 1000;
        BOOST_CHECK(MoneyRange(nSum));
    }
    BOOST_CHECK_EQUAL(nSum, CAmount{136707550000000000});
}

static bool ReturnFalse() { return false; }
static bool ReturnTrue() { return true; }

BOOST_AUTO_TEST_CASE(test_combiner_all)
{
    boost::signals2::signal<bool (), CombinerAll> Test;
    BOOST_CHECK(Test());
    Test.connect(&ReturnFalse);
    BOOST_CHECK(!Test());
    Test.connect(&ReturnTrue);
    BOOST_CHECK(!Test());
    Test.disconnect(&ReturnFalse);
    BOOST_CHECK(Test());
    Test.disconnect(&ReturnTrue);
    BOOST_CHECK(Test());
}
BOOST_AUTO_TEST_SUITE_END()


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

#include <vector>
#include <set>

#include "graph/PBQPGraph.hpp"
#include "graph/Vector.hpp"
#include "graph/PBQPNode.hpp"
#include "graph/PBQPEdge.hpp"
#include "reduction/PBQPReduction.hpp"
#include "reduction/degree/DegreeZeroReducer.hpp"
#include "graph/PBQPSolution.hpp"

#include "util/TestUtils.hpp"

namespace pbqppapa {

BOOST_AUTO_TEST_CASE(emptyGraphTest) {
	//make sure this doesnt explode
	PBQPGraph<int> graph;
	DegreeZeroReducer<int> zeroReducer (&graph);
	std::vector<PBQPGraph<int>*> result = zeroReducer.reduce();
}

BOOST_AUTO_TEST_CASE(simpleNodeReduction) {
	PBQPGraph<int> graph = PBQPGraph<int>();
	for (int i = 0; i < 20; i++) {
		int arr[] { 3, 1 };
		Vector<int> vek (2, arr);
		PBQPNode<int>* node = graph.addNode(vek);
	}
	DegreeZeroReducer<int> zeroReducer(&graph);
	std::vector<PBQPGraph<int>*> result = zeroReducer.reduce();
	BOOST_CHECK_EQUAL(result.size(), 1);
	PBQPGraph<int>* resultGraph = result [0];
	BOOST_CHECK_EQUAL(resultGraph->getNodeCount(), 0);
}

}


/***************************************************************************************************************************
 * pendulum_PID.cpp
 *
 * Author: Yuxi
 *
 * Update Time: 2020.7.10
 *
 * 说明: PID控制倒立摆位置与速度
 *
***************************************************************************************************************************/

#include <ros/ros.h>

#include <iostream>
#include <math.h>

#include <math_utils.h>
#include <Frame_tf_utils.h>

#include <Eigen/Eigen>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3.h>

#include <fstream>
#include <math.h>
#include <string>
#include <time.h>
#include <queue>
#include <vector>
#include <cstdlib>
#include <stdlib.h>
#include <iostream>
#include <stdio.h>
#include <std_msgs/Bool.h>
#include <px4_command/command.h>

using namespace std;
//---------------------------------------vicon定位相关------------------------------------------
Eigen::Quaterniond q_uav_mocap;                           // 无人机当前姿态(四元数)

Eigen::Vector3d Euler_uav_mocap;                              //无人机当前姿态 (vicon)

Eigen::Vector3d pose_pen_mocap;
Eigen::Vector3d pose_uav_mocap;

Eigen::Vector3d twist_uav_mocap;
Eigen::Vector3d twist_pen_mocap;

float error_last = 0;

enum Command
{
    Idle,
    Takeoff,
    Move_ENU,
    Move_Body,
    Hold,
    Land,
    Disarm,
    Failsafe_land,
};
px4_command::command Command_now;
//---------------------------------------飞行参数---------------------------------------------
float x_lim;    // x轴地理围栏限制
float y_lim;    // y轴地理围栏限制
float z_lim;    // z轴地理围栏限制
float sleep_time;
float z_max;
float delta;
float height;
float time_delay;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
float get_dt(ros::Time last);                                                        // 获取时间
float cal_error_deriv(float error_now, float dt);                                    // 求微分
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void euler_uav_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    Eigen::Vector3d pose_uav_mocap_enu(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    pose_uav_mocap = pose_uav_mocap_enu;
    // Read the Quaternion from the Vrpn Package [Frame: Vicon[ENU]]
    Eigen::Quaterniond q_uav_mocap_enu(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z);
    q_uav_mocap = q_uav_mocap_enu;

    // Transform the Quaternion to Euler Angles
    Euler_uav_mocap = quaternion_to_euler(q_uav_mocap);

}

void euler_pen_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    Eigen::Vector3d pose_pen_mocap_enu(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    pose_pen_mocap = pose_pen_mocap_enu;
}

void twist_uav_cb(const geometry_msgs::TwistStamped::ConstPtr& msg)
{
    Eigen::Vector3d twist_uav_mocap_enu(msg->twist.angular.x, msg->twist.angular.y, msg->twist.angular.z);
    twist_uav_mocap = twist_uav_mocap_enu;
}

void twist_pen_cb(const geometry_msgs::TwistStamped::ConstPtr& msg)
{
    Eigen::Vector3d twist_pen_mocap_enu(msg->twist.angular.x, msg->twist.angular.y, msg->twist.angular.z);
    twist_pen_mocap = twist_pen_mocap_enu;
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "pendulum_PID");
    ros::NodeHandle nh("~");

    // 频率 [20hz]
    ros::Rate rate(50.0);

    // 【订阅】订阅倒立摆的位置数据（Vicon）
        // 【订阅】vicon uav Euler角
    ros::Subscriber vicon_uav_sub = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node/UAV_6_3/pose", 10, euler_uav_cb);

    // 【订阅】vicon pendulum Euler角
    ros::Subscriber euler_sub = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node/pendulum/pose", 10, euler_pen_cb);

    // 【订阅】vicon uav Euler角
    ros::Subscriber vicon_uav_twist_sub = nh.subscribe<geometry_msgs::TwistStamped>("/vrpn_client_node/UAV_6_3/twist", 10, twist_uav_cb);

    // 【订阅】vicon pendulum Euler角
    ros::Subscriber euler_pen_twist_sub = nh.subscribe<geometry_msgs::TwistStamped>("/vrpn_client_node/pendulum/twist", 10, twist_pen_cb);

    // 【发布】发送给position_control.cpp的命令
    ros::Publisher move_pub = nh.advertise<px4_command::command>("/px4/command", 10);

    //参数读取
    nh.param<float>("x_lim", x_lim, 2.0);
    nh.param<float>("y_lim", y_lim, 2.0);
    nh.param<float>("z_lim", z_lim, 0.5);
    nh.param<float>("z_max", z_max, 2.5);
    nh.param<float>("sleep_time", sleep_time, 5.0);
    nh.param<float>("delta", delta, 0.0);
    nh.param<float>("height", height, 0.1);
    nh.param<float>("time_delay", time_delay, 0.02);

    int check_flag;
    // 这一步是为了程序运行前检查一下参数是否正确
    // 输入1,继续，其他，退出程序
    cout << "x_lim" << x_lim << "[m]" << endl;
    cout << "y_lim" << y_lim << "[m]" << endl;
    cout << "z_lim" << z_lim << "[m]" << endl;
    cout << "z_max" << z_max << "[m]" << endl;
    cout << "delta: " << delta << "m" << endl;
    cout << "height: " << height << "m" << endl;
    cout << "time_delay: " << time_delay << endl;

    cout << "Please check the parameter and setting，1 for go on， else for quit: " << endl;
    cin >> check_flag;

    if(check_flag != 1)
        return -1;

    int i = 0;
    int comid = 0;

    // write into files
    ofstream ofile;
    ofile.open("/home/amov/fly_logs/theta_P_D.txt");
    while(i < sleep_time){

        Command_now.command = Move_ENU;       //Move模式
        Command_now.sub_mode = 0;             //子模式：位置控制模式
        Command_now.pos_sp[0] = 0;
        Command_now.pos_sp[1] = 0;
        Command_now.pos_sp[2] = 1.5;
        Command_now.yaw_sp = 0;
        Command_now.comid = comid;
        comid++;

        move_pub.publish(Command_now);

        rate.sleep();

        i++;
    }

    ros::Time begin_time = ros::Time::now();
    float last_time = get_dt(begin_time);
    float dt = 0;

    while (ros::ok())
    {
        // 回调一次
        ros::spinOnce();

        float cur_time = get_dt(begin_time);
        dt = cur_time  - last_time;

	ofile << cur_time  << "	   ";

        if(dt < 0.005)
            dt = 0.005;

        if(dt > 0.03)
            dt = 0.03;

        last_time = cur_time;

        float uav_theta = Euler_uav_mocap[1];

	float error_pos = pose_pen_mocap[0] - pose_uav_mocap[0] - delta - height * sin(uav_theta);
	//float error_pos = pose_pen_mocap[0] - pose_uav_mocap[0] - delta;
        float pen_theta = asin(error_pos / 0.24);

        //float theta_0 = pen_theta - uav_theta;
	float theta_0 = pen_theta;

        float theta_dot = cal_error_deriv(theta_0, dt);

	if(theta_dot > 8)
	    theta_dot = 8;
	else if(theta_dot < -8)
	    theta_dot = -8;
	else
	    theta_dot = theta_dot;

	//float theta = theta_0;
        //float theta = pen_theta + time_delay * theta_dot - uav_theta;
	float theta = pen_theta + time_delay * theta_dot;

	ofile << theta_dot << "	" << theta * (180/3.14) << endl;

        float twist_theta = twist_pen_mocap[0] - twist_uav_mocap[0];

	int mode;
	if((theta * 180/3.14) > -10 && (theta * 180/3.14) < 10)
		mode = 6;
	else
		mode = 5;

        Command_now.command = Move_ENU;
        Command_now.sub_mode = 5;
        Command_now.pos_sp[0] = 0 - theta;
        Command_now.pos_sp[1] = pose_uav_mocap[1];
        Command_now.pos_sp[2] = pose_uav_mocap[2];
        Command_now.vel_sp[0] = 0 - theta_dot;
        Command_now.yaw_sp = 0;
        Command_now.comid = comid;
        comid++;

        if(pose_uav_mocap[0] < x_lim && pose_uav_mocap[0] > -x_lim
        && pose_uav_mocap[1] < y_lim && pose_uav_mocap[1] > -y_lim
        && pose_uav_mocap[2] > z_lim && pose_uav_mocap[2] < z_max){
            move_pub.publish(Command_now);
        }
        else
        {
            Command_now.command = Land;
            move_pub.publish(Command_now);
        }

        rate.sleep();
    }

    ofile.close();
    return 0;
}

//获取当前时间 单位：秒
float get_dt(ros::Time last)
{
    ros::Time time_now = ros::Time::now();
    float currTimeSec = time_now.sec-last.sec;
    float currTimenSec = time_now.nsec / 1e9 - last.nsec / 1e9;
    return (currTimeSec + currTimenSec);
}

float cal_error_deriv(float error_now, float dt)
{
    float error_dot_now;
    error_dot_now = (error_now - error_last) / dt;

    error_last = error_now;

    return error_dot_now;
}


#include <benchmark/benchmark.h>
#include <random>
#include <unordered_map>
#include <absl/random/random.h>
#include <boost/random.hpp>
#include "pcg_random.hpp"
#include "pcg_uint128.hpp"

template<typename G>
static void BenchSeed(benchmark::State &state) {
    std::random_device rd;
    for (auto _ : state) {

        benchmark::DoNotOptimize(rd());
    }
}

BENCHMARK_TEMPLATE(BenchSeed, std::random_device);

template<typename G>
static void BenchEngine(benchmark::State &state) {
    for (auto _ : state) {
        G gen{};
        benchmark::DoNotOptimize(gen());
    }
}

BENCHMARK_TEMPLATE(BenchEngine, std::ranlux24_base);
BENCHMARK_TEMPLATE(BenchEngine, std::ranlux48_base);
BENCHMARK_TEMPLATE(BenchEngine, std::ranlux24);
BENCHMARK_TEMPLATE(BenchEngine, std::ranlux48);
BENCHMARK_TEMPLATE(BenchEngine, std::knuth_b);
BENCHMARK_TEMPLATE(BenchEngine, std::minstd_rand0);
BENCHMARK_TEMPLATE(BenchEngine, std::minstd_rand);
BENCHMARK_TEMPLATE(BenchEngine, std::mt19937);
BENCHMARK_TEMPLATE(BenchEngine, std::mt19937_64);

BENCHMARK_TEMPLATE(BenchEngine, boost::minstd_rand0);
BENCHMARK_TEMPLATE(BenchEngine, boost::minstd_rand);
BENCHMARK_TEMPLATE(BenchEngine, boost::rand48);
BENCHMARK_TEMPLATE(BenchEngine, boost::mt19937);
BENCHMARK_TEMPLATE(BenchEngine, boost::mt19937_64);
BENCHMARK_TEMPLATE(BenchEngine, boost::mt11213b);

template<typename P>
static void BenchPCG(benchmark::State &state) {

    for (auto _ : state) {
        P p;
        //    p.seed(pcg_extras::seed_seq_from<std::random_device>());
        benchmark::DoNotOptimize(p());
    }
}

BENCHMARK_TEMPLATE(BenchPCG, pcg32);
BENCHMARK_TEMPLATE(BenchPCG, pcg32_oneseq);
BENCHMARK_TEMPLATE(BenchPCG, pcg32_unique);
BENCHMARK_TEMPLATE(BenchPCG, pcg32_fast);

BENCHMARK_TEMPLATE(BenchPCG, pcg64);
BENCHMARK_TEMPLATE(BenchPCG, pcg64_oneseq);
BENCHMARK_TEMPLATE(BenchPCG, pcg64_unique);
BENCHMARK_TEMPLATE(BenchPCG, pcg64_fast);

BENCHMARK_TEMPLATE(BenchPCG, pcg8_once_insecure);
BENCHMARK_TEMPLATE(BenchPCG, pcg16_once_insecure);
BENCHMARK_TEMPLATE(BenchPCG, pcg32_once_insecure);
BENCHMARK_TEMPLATE(BenchPCG, pcg64_once_insecure);

BENCHMARK_TEMPLATE(BenchPCG, pcg8_oneseq_once_insecure);
BENCHMARK_TEMPLATE(BenchPCG, pcg16_oneseq_once_insecure);
BENCHMARK_TEMPLATE(BenchPCG, pcg32_oneseq_once_insecure);
BENCHMARK_TEMPLATE(BenchPCG, pcg64_oneseq_once_insecure);

BENCHMARK_TEMPLATE(BenchPCG, pcg32_k2);
BENCHMARK_TEMPLATE(BenchPCG, pcg32_k2_fast);

BENCHMARK_TEMPLATE(BenchPCG, pcg32_k64);
BENCHMARK_TEMPLATE(BenchPCG, pcg32_k64_oneseq);
BENCHMARK_TEMPLATE(BenchPCG, pcg32_k64_fast);

BENCHMARK_TEMPLATE(BenchPCG, pcg32_c64);
BENCHMARK_TEMPLATE(BenchPCG, pcg32_c64_oneseq);
BENCHMARK_TEMPLATE(BenchPCG, pcg32_c64_fast);

BENCHMARK_TEMPLATE(BenchPCG, pcg64_k32);
BENCHMARK_TEMPLATE(BenchPCG, pcg64_k32_oneseq);
BENCHMARK_TEMPLATE(BenchPCG, pcg64_k32_fast);

BENCHMARK_TEMPLATE(BenchPCG, pcg64_c32);
BENCHMARK_TEMPLATE(BenchPCG, pcg64_c32_oneseq);
BENCHMARK_TEMPLATE(BenchPCG, pcg64_c32_fast);

BENCHMARK_TEMPLATE(BenchPCG, pcg32_k1024);
BENCHMARK_TEMPLATE(BenchPCG, pcg32_k1024_fast);

BENCHMARK_TEMPLATE(BenchPCG, pcg32_c1024);
BENCHMARK_TEMPLATE(BenchPCG, pcg32_c1024_fast);

BENCHMARK_TEMPLATE(BenchPCG, pcg64_k1024);
BENCHMARK_TEMPLATE(BenchPCG, pcg64_k1024_fast);

BENCHMARK_TEMPLATE(BenchPCG, pcg64_c1024);
BENCHMARK_TEMPLATE(BenchPCG, pcg64_c1024_fast);

BENCHMARK_TEMPLATE(BenchPCG, pcg32_k16384);
BENCHMARK_TEMPLATE(BenchPCG, pcg32_k16384_fast);

template<typename P>
static void BenchABEngine(benchmark::State &state) {
    P pcg;
    for (auto _ : state) {
        benchmark::DoNotOptimize(pcg());
    }
}

BENCHMARK_TEMPLATE(BenchABEngine, absl::random_internal::pcg32_2018_engine);
BENCHMARK_TEMPLATE(BenchABEngine, absl::random_internal::pcg64_2018_engine);

BENCHMARK_TEMPLATE(BenchABEngine, absl::random_internal::randen_engine<uint32_t>);
BENCHMARK_TEMPLATE(BenchABEngine, absl::random_internal::randen_engine<uint64_t>);
BENCHMARK_TEMPLATE(BenchABEngine, absl::random_internal::RandenPool<uint64_t>);

BENCHMARK_TEMPLATE(BenchABEngine, absl::random_internal::NonsecureURBGBase<absl::random_internal::pcg32_2018_engine>);
BENCHMARK_TEMPLATE(BenchABEngine, absl::random_internal::NonsecureURBGBase<absl::random_internal::pcg64_2018_engine>);

BENCHMARK_TEMPLATE(BenchABEngine, absl::random_internal::NonsecureURBGBase<absl::random_internal::randen_engine<uint32_t>>);
BENCHMARK_TEMPLATE(BenchABEngine, absl::random_internal::NonsecureURBGBase<absl::random_internal::randen_engine<uint64_t>>);

int main(int argc, char **argv) {
    benchmark::Initialize(&argc, argv);
    benchmark::RunSpecifiedBenchmarks();
    return 0;
}


//          Copyright Gavin Band 2008 - 2012.
// 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 SNPTEST_CASE_CONTROL_REGRESSION_DESIGN_HPP
#define SNPTEST_CASE_CONTROL_REGRESSION_DESIGN_HPP

#include <vector>
#include <iostream>
#include <memory>
#include <boost/noncopyable.hpp>
#include <boost/function.hpp>
#include "Eigen/Core"
#include "metro/SampleRange.hpp"

namespace metro {
	namespace regression {
		struct Design: public boost::noncopyable
		{
			typedef std::auto_ptr< Design > UniquePtr ;
		public:
			typedef Eigen::VectorXd Point ;
			typedef Eigen::VectorXd Vector ;
			typedef Eigen::RowVectorXd RowVector ;
			typedef Eigen::MatrixXd Matrix ;
			//typedef Eigen::Matrix< char, Eigen::Dynamic, Eigen::Dynamic > NonmissingnessMatrix ;
			typedef Matrix NonmissingnessMatrix ;
			typedef Eigen::Block< Matrix > MatrixBlock ;
			typedef Eigen::Block< Matrix const > ConstMatrixBlock ;
			typedef std::vector< metro::SampleRange > SampleRanges ;
			typedef boost::function< std::string const& ( int parameter_i ) > GetPredictorNames ;
			enum Transform { eIdentity = 0, eMeanCentre = 1 } ;

		public:

			static UniquePtr create(
				Matrix const& outcome, SampleRanges const& nonmissing_outcome, std::vector< std::string > const& outcome_names,
				Matrix const& covariates, SampleRanges const& nonmissing_covariates, std::vector< std::string > const& covariate_names,
				std::vector< std::string > const& predictor_names,
				Transform transform = eIdentity,
				std::vector< int > const& interacting_covariates = std::vector< int >()
			) ;

			Design(
				Matrix const& outcome, SampleRanges const& nonmissing_outcome, std::vector< std::string > const& outcome_names,
				Matrix const& covariates, SampleRanges const& nonmissing_covariates, std::vector< std::string > const& covariate_names,
				std::vector< std::string > const& predictor_names,
				Transform transform = eIdentity,
				std::vector< int > const& interacting_covariates = std::vector< int >()
			) ;

			static UniquePtr create(
				Matrix const& outcome, SampleRanges const& nonmissing_outcome, std::vector< std::string > const& outcome_names,
				std::vector< std::string > const& predictor_names,
				Transform transform = eIdentity
			) ;

			Design(
				Matrix const& outcome, SampleRanges const& nonmissing_outcome, std::vector< std::string > const& outcome_names,
				std::vector< std::string > const& predictor_names,
				Transform transform = eIdentity
			) ;

			// Design( Design const& other ) ;

			// add a single continuous covariate
			// missing values are encoded by NaN
			void add_single_covariate(
				std::string const& name,
				boost::function< double( std::size_t ) > const& data
			) ;
			// add a single discrete covariate (which expands into multiple columns of zeroes and ones)
			// levels are specified by non-negative integers, with missing values
			// encoded by -1.
			// A seperate function must be supplied to give the names of levels.
			void add_discrete_covariate(
				std::string const& name,
				boost::function< int( std::size_t ) > const& data,
				boost::function< std::string( int ) > const& levelNames,
				int numberOfLevels
			) ;

			int const number_of_predictors() const { return m_predictor_names.size() ; }
			int const number_of_interaction_terms() const { return number_of_predictors() * m_design_matrix_interaction_columns.size() ; }

			std::string const& get_predictor_name( std::size_t i ) const ;
			std::vector< std::string > const& design_matrix_column_names() const ;
			std::string const& get_outcome_name( std::size_t i ) const ;

			Design& set_outcome(
				Matrix const& outcome,
				SampleRanges const& nonmissingness,
				std::vector< std::string > const& names
			) ;

			// Set predictors with uncertainty
			// We assume there are K possible combinations of all the predictor values,
			// and that each sample takes the kth set of predictor values with probability given
			// by the specified matrix.  Here
			//
			// - levels is a K x d matrix  (K levels for d predictors)
			// - probabilities is N x K    (N samples, probabilities for K levels)
			//
			// N = number of samples
			// K = number of different possible levels of the predictors
			// and d = number of predictors.
			Design& set_predictors(
				Matrix const& levels,
				Matrix const& probabilities,
				SampleRanges const& nonmissingness
			) ;

			// Set predictors without uncertainty
			// We assume that the nth row of the given matrix
			// specifies the predictor values for the nth sample.
			Design& set_predictors(
				Matrix const& predictors,
				std::vector< metro::SampleRange > const& nonmissingness
			) ;

			Matrix const& matrix() const { return m_design_matrix ; }
			ConstMatrixBlock matrix( metro::SampleRange const& range ) const {
				return m_design_matrix.block(
					range.begin(), 0,
					range.size(), m_design_matrix.cols()
				) ;
			}
			Design& set_predictor_level( int level ) ;
			Design& set_predictor_level( int level, metro::SampleRange const& ) ;
			Design& set_predictor_level( int level, std::vector< metro::SampleRange > const& ) ;

			Matrix const& get_predictor_level_probabilities() const { return m_predictor_level_probabilities ; }
			int const get_number_of_predictor_levels() const { return m_predictor_level_probabilities.cols() ; }
			// Matrix const& get_predictor_levels() const { return m_predictor_levels ; }

			Matrix const& outcome() const { return m_outcome ; }

			SampleRanges const& nonmissing_samples() const { return m_nonmissing_samples ; }

			std::string get_summary() const ;
		private:
			// Outcome variables
			Matrix m_outcome ;
			std::vector< std::string > m_outcome_names ;
			SampleRanges m_nonmissing_outcome ;

			// Predictor variables
			Matrix m_predictor_level_probabilities ;
			Matrix m_predictor_levels ;
			std::vector< std::string > const m_predictor_names ;
			SampleRanges m_nonmissing_predictors ;

			// Covariate variables
			std::vector< Matrix > m_covariates ;
			std::vector< std::string > m_covariate_names ;
			SampleRanges m_nonmissing_covariates ;

			// Interactors
			std::vector< int > const m_predictor_covariate_interactions ;
			// Design matrix column transform - e.g. mean centre.
			Transform const m_transform ;

			// The current design matrix that holds current predictor values and covariates.
			Matrix m_design_matrix ;

			// SampleRange that tracks samples that have no nonmissing data
			// in outcome, covariates, or predictors
			// Invariant: this the intersection of nonmissing samples implied
			// by m_nonmissing_outcome, m_nonmissing_predictors, and m_nonmissing_covariates.
			SampleRanges m_nonmissing_samples ;

			std::vector< int > m_design_matrix_interaction_columns ;
			std::vector< std::string > m_design_matrix_column_names ;

		private:
			SampleRanges compute_nonmissing_samples(
				SampleRanges const& nonmissing_outcome,
				SampleRanges const& nonmissing_predictors,
				SampleRanges const& nonmissing_covariates
			) const ;
			void recalculate() ;
			void calculate_design_matrix(
				int const,
				std::vector< Matrix > const& covariates,
				std::vector< std::string > const& covariate_names,
				std::vector< std::string > const& predictor_names,
				std::vector< int > const& interaction_cols,
				Matrix* result,
				std::vector< int >* design_matrix_interaction_cols,
				std::vector< std::string > * design_matrix_column_names
			) const ;

			void mean_centre_predictor_levels(
				Matrix const& probs,
				Matrix* predictor_levels,
				SampleRanges const& included_samples
			) const ;
		} ;
	}
}

#endif


#ifndef WARP_POINT_3D_YAW
#define WARP_POINT_3D_YAW

#pragma once

#include <pcl/registration/warp_point_rigid.h>
#include <boost/shared_ptr.hpp>

namespace pcl {
namespace registration {
  /** \brief @b WarpPoint3DYaw enables 4D (1D rotation around yaw + 3D translation)
   * transformations for points.
   *
   * \note The class is templated on the source and target point types as well as on the
   * output scalar of the transformation matrix (i.e., float or double). Default: float.
   * \author Radu B. Rusu
   * \ingroup registration
   */
  template<typename PointSourceT, typename PointTargetT, typename Scalar = float>
  class WarpPoint3DYaw : public WarpPointRigid<PointSourceT, PointTargetT, Scalar>
  {
  public:
    using Matrix4 = typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::Matrix4;
    using VectorX = typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::VectorX;

    using Ptr = boost::shared_ptr<WarpPoint3DYaw<PointSourceT, PointTargetT, Scalar>>;
    using ConstPtr =
      boost::shared_ptr<const WarpPoint3DYaw<PointSourceT, PointTargetT, Scalar>>;

    /** \brief Constructor. */
    WarpPoint3DYaw() : WarpPointRigid<PointSourceT, PointTargetT, Scalar>(4) {}

    /** \brief Empty destructor */
    ~WarpPoint3DYaw() {}

    /** \brief Set warp parameters.
     * \param[in] p warp parameters (tx ty rz)
     */
    void setParam(const VectorX &p) override
    {
      assert(p.rows() == this->getDimension());
      Matrix4 &trans = this->transform_matrix_;

      trans = Matrix4::Identity();

      // Copy the rotation and translation components
      trans.block(0, 3, 4, 1) = Eigen::Matrix<Scalar, 4, 1>(p[0], p[1], p[2], 1.0);

      // Add only yaw rotation
      Eigen::AngleAxis<Scalar> yawAngle(p[3], Eigen::Matrix<Scalar, 3, 1>(0, 0, 1));
      Eigen::Matrix3f m;
      m = yawAngle;
      trans.topLeftCorner(3, 3) = m;
    }
  };
}// namespace registration
}// namespace pcl

#endif /* WARP_POINT_3D_YAW */