#include <Eigen/Core>
#include "mex.h"

using namespace Eigen;
using namespace std;

// TODO openmp version
// TODO roll counting in with second message passing
// TODO scaling instead of logs?

// NOTE: mxSetProperty and possibly mxGetProperty make copies, even with
// classdef < handle! lame! I believe mxGetField does NOT make a copy, though I
// haven't tested it recently

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{

    /* SETUP */
    if (nrhs != 5) { mexErrMsgTxt("wrong number of arguments\n"); }

    //// pull out inputs

    // data
    if (!mxGetField(prhs[0],0,"data")) { mexErrMsgTxt("data missing 'data' field\n"); }
    double *alldata = mxGetPr(mxGetField(prhs[0],0,"data"));
    int bigT = mxGetN(mxGetField(prhs[0],0,"data")); // total length of the data
    int num_subparts = mxGetM(mxGetField(prhs[0],0,"data")); // number of sub-parts

    if (!mxGetField(prhs[0],0,"resources")) { mexErrMsgTxt("data missing 'resources' field\n"); }
    double *allresources = mxGetPr(mxGetField(prhs[0],0,"resources"));

    if (!mxGetField(prhs[0],0,"starts")) { mexErrMsgTxt("data missing 'starts' field\n"); }
    double *starts = mxGetPr(mxGetField(prhs[0],0,"starts"));
    int num_sequences = max(mxGetM(mxGetField(prhs[0],0,"starts")),
            mxGetM(mxGetField(prhs[0],0,"starts")));

    if (!mxGetField(prhs[0],0,"lengths")) { mexErrMsgTxt("data missing 'lengths' field\n"); }
    double *lengths = mxGetPr(mxGetField(prhs[0],0,"lengths"));

    // parameters struct
    if (!mxGetField(prhs[1],0,"learns")) { mexErrMsgTxt("model missing 'learns' field\n"); }
    double *learns = mxGetPr(mxGetField(prhs[1],0,"learns"));
    int num_resources = max(mxGetM(mxGetField(prhs[1],0,"learns")),
            mxGetN(mxGetField(prhs[1],0,"learns")));

    if (!mxGetField(prhs[1],0,"forgets")) { mexErrMsgTxt("model missing 'forgets' field\n"); }
    double *forgets = mxGetPr(mxGetField(prhs[1],0,"forgets"));

    if (!mxGetField(prhs[1],0,"guesses")) { mexErrMsgTxt("model missing 'guesses' field\n"); }
    double *guess = mxGetPr(mxGetField(prhs[1],0,"guesses"));

    if (!mxGetField(prhs[1],0,"slips")) { mexErrMsgTxt("model missing 'slips' field\n"); }
    double *slip = mxGetPr(mxGetField(prhs[1],0,"slips"));

    if (!mxGetField(prhs[1],0,"prior")) { mexErrMsgTxt("model missing 'prior' field\n"); }
    double prior = mxGetScalar(mxGetField(prhs[1],0,"prior"));

    Array2d initial_distn;
    initial_distn << 1-prior, prior;

    MatrixXd As(2,2*num_resources);
    for (int n=0; n<num_resources; n++) {
        As.col(2*n) << 1-learns[n], learns[n];
        As.col(2*n+1) << forgets[n], 1-forgets[n];
    }
    ArrayXXd Als(2,2*num_resources);
    Als = As.array().log();

    Array2Xd Bn(2,2*num_subparts);
    for (int n=0; n<num_subparts; n++) {
        Bn.col(2*n) << log(1-guess[n]), log(slip[n]); // incorrect
        Bn.col(2*n+1) << log(guess[n]), log(1-slip[n]); // correct
    }

    //// outputs

    // rhs outputs
    Map<ArrayXXd,Aligned> all_trans_softcounts(mxGetPr(prhs[2]),2,2*num_resources);
    all_trans_softcounts.setZero();
    Map<Array2Xd,Aligned> all_emission_softcounts(mxGetPr(prhs[3]),2,2*num_subparts);
    all_emission_softcounts.setZero();
    Map<Array2d,Aligned> all_initial_softcounts(mxGetPr(prhs[4]));
    all_initial_softcounts.setZero();

    // lhs outputs
    Map<Array2Xd,Aligned> alphal_out(NULL,2,bigT);
    Map<Array2Xd,Aligned> betal_out(NULL,2,bigT);
    double *total_loglike = NULL;
    switch (nlhs) {
        case 3:
            plhs[2] = mxCreateDoubleMatrix(2,bigT,mxREAL);
            new (&betal_out) Map<Array2Xd,Aligned>(mxGetPr(plhs[2]),2,bigT);
        case 2:
            plhs[1] = mxCreateDoubleMatrix(2,bigT,mxREAL);
            new (&alphal_out) Map<Array2Xd,Aligned>(mxGetPr(plhs[1]),2,bigT);
        case 1:
            plhs[0] = mxCreateDoubleScalar(0.);
            total_loglike = mxGetPr(plhs[0]);
    }

    /* COMPUTATION */

    for (int sequence_index=0; sequence_index < num_sequences; sequence_index++) {
        // NOTE: -1 because Matlab indexing starts at 1
        int sequence_start = ((int) starts[sequence_index]) - 1;
        int T = (int) lengths[sequence_index];

        Map<ArrayXXd> data(alldata + num_subparts*sequence_start,num_subparts,T);
        double *resources = allresources + sequence_start;

        //// likelihoods
        Array2Xd likelihoods(2,T);
        likelihoods.setZero();
        for (int t=0; t<T; t++) {
            for (int n=0; n<num_subparts; n++) {
                if (data(n,t) != 0) {
                    likelihoods.col(t) += Bn.col(2*n + (data(n,t) == 2));
                }
            }
        }

        //// forward messages
        Array2Xd alphal(2,T);
        alphal.col(0) = initial_distn.log() + likelihoods.col(0);
        for (int t=0; t<T-1; t++) {
            double cmax = alphal.col(t).maxCoeff();
            alphal.col(t+1) = (As.block(0,2*(resources[t]-1),2,2) // NOTE matlab indexing
                    * (alphal.col(t) - cmax).exp().matrix()).array().log()
                    + cmax + likelihoods.col(t+1);
        }

        //// backward messages
        Array2Xd betal(2,T);
        betal.col(T-1).setZero();
        for (int t=T-2; t>=0; t--) {
            Array2d thesum = likelihoods.col(t+1) + betal.col(t+1);
            double cmax = thesum.maxCoeff();
            betal.col(t) = (As.block(0,2*(resources[t]-1),2,2).transpose() // NOTE matlab indexing
                    * (thesum - cmax).exp().matrix()).array().log() + cmax;
        }

        //// statistic counting
        // TODO could roll this into second message sweep
        Array2d temp = alphal.col(0) + betal.col(0);
        double cmax = temp.maxCoeff();
        double loglike = log((temp - cmax).exp().sum()) + cmax;

        ArrayXXd trans_softcounts(2,2*num_resources);
        trans_softcounts.setZero();
        Array2Xd emission_softcounts(2,2*num_subparts);
        emission_softcounts.setZero();

        for (int t=0; t<T-1; t++) {
            Array22d pair; // cols = t, rows=t+1
            pair.rowwise() = alphal.col(t).transpose();
            pair.colwise() += (betal.col(t+1) + likelihoods.col(t+1));
            pair += Als.block(0,2*(resources[t]-1),2,2); // NOTE matlab indexing
            pair -= loglike;
            pair = pair.exp();

            trans_softcounts.block(0,2*(resources[t]-1),2,2) += pair; // NOTE matlab indexing

            Array2d single = pair.colwise().sum();
            for (int n=0; n<num_subparts; n++) {
                if (data(n,t) != 0) {
                    emission_softcounts.col(2*n + (data(n,t) == 2)) += single;
                }
            }
        }
        // NOTE: last iteration needed for emission softcounts
        int t=T-1;
        Array2d single = alphal.col(t);
        single -= loglike;
        single = single.exp();
        for (int n=0; n<num_subparts; n++) {
            if (data(n,t) != 0) { emission_softcounts.col(2*n + (data(n,t) == 2)) += single;
            }
        }

        Array2d initial_softcounts = betal.col(0) + alphal.col(0);
        initial_softcounts -= loglike;
        initial_softcounts = initial_softcounts.exp();

        // NOTE: these will need to change to base types for openmp
        all_trans_softcounts += trans_softcounts;
        all_emission_softcounts += emission_softcounts;
        all_initial_softcounts += initial_softcounts;

        switch (nlhs) {
            case 3:
                betal_out.block(0,sequence_start,2,T) = betal;
            case 2:
                alphal_out.block(0,sequence_start,2,T) = alphal;
            case 1:
                *total_loglike += loglike;
        }

    }
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_TWO_PROD_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_TWO_PROD_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
  @file
  @brief Definition of the two_prod function
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /// @brief Hierarchy tag for two_prod function
    struct two_prod_ : ext::elementwise_<two_prod_>
    {
      typedef ext::elementwise_<two_prod_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_two_prod_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::two_prod_, Site> dispatching_two_prod_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::two_prod_, Site>();
    }
    template<class... Args>
    struct impl_two_prod_;
  }

  /*!
    @brief

    For any two reals @c a0 and @c a1 two_prod computes two reals
    @c r0 and @c r1 so that:

    @code
    r0 = a0* a1
    r1 = r0 -(a0 * a1)
    @endcode

    using perfect arithmetic. Its main usage is to be able to compute
    sum of reals and the residual error using IEEE 754 arithmetic.

    @param a0 First parameter of the sum
    @param a1 Second parameter of the sum

    @return A Fusion Sequence containing @c a0+a1 and the residual.
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::two_prod_, two_prod, 2)

  /*!
    @brief

    For any two reals @c a0 and @c a1 two_prod computes two reals
    @c r0 and @c r1 so that:

    @code
    r0 = a0 * a1
    r1 = r0 -(a0 * a1)
    @endcode

    using perfect arithmetic. Its main usage is to be able to compute
    sum of reals and the residual error using IEEE 754 arithmetic.

    @param a0 First parameter of the sum
    @param a1 Second parameter of the sum
    @param a2 L-Value that will receive @c a0+a1

    @return The sum residual.
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL( tag::two_prod_, two_prod
                                            , (A0 const&)(A1 const&)(A2&)
                                            , 3
                                            )

  /*!
    @brief

    For any two reals @c a0 and @c a1 two_prod computes two reals
    @c r0 and @c r1 so that:

    @code
    r0 = a0 * a1
    r1 = r0 -(a0 * a1)
    @endcode

    using perfect arithmetic. Its main usage is to be able to compute
    sum of reals and the residual error using IEEE 754 arithmetic.

    @param a0 First parameter of the sum
    @param a1 Second parameter of the sum
    @param a2 L-Value that will receive @c a0+a1.
    @param a3 L-Value that will receive @c a0+a1 residual.
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL( tag::two_prod_, two_prod
                                            , (A0 const&)(A1 const&)(A2&)(A3&)
                                            , 4
                                            )
} }

#endif


// Copyright 2016 by Glukhov V. O. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#pragma once
#include "activation_function.hpp"
#include "util.hpp"
#include <armadillo>
#include <memory>
#include <utility>
#include <cstdint>

namespace cnn
{
	namespace nn
	{
		class BaseLayer
		{
		public:
			BaseLayer(arma::uword height, arma::uword width, arma::uword depth,
					  std::size_t amount, std::unique_ptr<BaseActivationFunction> activFunc);
			virtual ~BaseLayer() = default;
			// propagate signal from bottom to top
			virtual void Forward(std::shared_ptr<arma::Cube<double>> input) = 0;
			// propagate error from top to bottom and compute gradient
			virtual std::pair<tensor4d, tensor4d> Backward(
				const std::shared_ptr<arma::Cube<double>> &prevLocalLoss) = 0;
			// propagate error from top to bottom and compute hessian
			virtual std::pair<tensor4d, tensor4d> Backward2nd(
				const std::shared_ptr<arma::Cube<double>> &prevLocalLoss) = 0;
			// get propagated local error to for previous layer
			const std::shared_ptr<arma::Cube<double>>& LocalLoss() const noexcept;
			std::shared_ptr<arma::Cube<double>> Output() const noexcept;
			std::shared_ptr<arma::Cube<double>> ReceptiveField() const noexcept;

			tensor4d& Weights() noexcept
			{
				return weights_;
			}
			tensor4d& BiasWeights() noexcept
			{
				return biasWeights_;
			}

			//const tensor4d& GetWeights() const noexcept;
			//const tensor4d& GetBiasWeights() const noexcept;
			//void SetWeights(const tensor4d& weights);
			//void SetWeights(tensor4d&& weights);
			//void SetBiasWeights(const tensor4d& biasWeights);
			//void SetBiasWeights(tensor4d&& biasWeights);

			bool LoadWeights(std::ifstream& in);
			bool SaveWeights(std::ofstream& out) const;
			// initialize all weights in this layer using Gaussian distribution
			void InitWeights() noexcept;
			bool is_initialized() const noexcept;
		protected:
			tensor4d weights_;
			tensor4d biasWeights_;

			// propagated local error to the next layer
			std::shared_ptr<arma::Cube<double>> localLoss_;
			// y = f(v)
			std::shared_ptr<arma::Cube<double>> output_;
			// v = operator(input, weights)
			std::shared_ptr<arma::Cube<double>> receptiveField_;
			// forwarded signal from previous layer
			std::shared_ptr<arma::Cube<double>> input_;
			// nonlinearity
			std::unique_ptr<BaseActivationFunction> activFunc_;

			//weights parameters
//			std::size_t amount_;
//			arma::uword depth_;
//			arma::uword width_;
//			arma::uword height_;
			// weights status
			bool initialized_;
		};

		inline BaseLayer::BaseLayer(arma::uword height, arma::uword width, arma::uword depth,
									std::size_t amount,
									std::unique_ptr<BaseActivationFunction> activFunc)
			: weights_(height, width, depth, amount)
			/*, biasWeights_(1, 1, depth, amount)*/,
			activFunc_(std::move(activFunc)),
//			amount_(amount), depth_(depth), width_(width), height_(height),
			initialized_(false)
		{}

		inline const std::shared_ptr<arma::Cube<double>>& BaseLayer::LocalLoss() const noexcept
		{
			return localLoss_;
		}

		inline std::shared_ptr<arma::Cube<double>> BaseLayer::Output() const noexcept
		{
			return output_;
		}

		inline std::shared_ptr<arma::Cube<double>> BaseLayer::ReceptiveField() const noexcept
		{
			return receptiveField_;
		}

		/*inline const tensor4d& BaseLayer::GetWeights() const noexcept
		{
			return weights_;
		}

		inline const tensor4d& BaseLayer::GetBiasWeights() const noexcept
		{
			return biasWeights_;
		}

		inline void BaseLayer::SetWeights(const tensor4d& weights)
		{
			weights_ = weights;
		}

		inline void BaseLayer::SetWeights(tensor4d&& weights)
		{
			weights_ = std::move(weights_);
		}

		inline void BaseLayer::SetBiasWeights(const tensor4d& biasWeights)
		{
			biasWeights_ = biasWeights;
		}

		inline void BaseLayer::SetBiasWeights(tensor4d&& biasWeights)
		{
			biasWeights_ = std::move(biasWeights);
		}*/

		inline bool BaseLayer::LoadWeights(std::ifstream& in)
		{
			// for all common or polling layers
			if (weights_.n_size == 0)
				return true;
			if (!in.is_open())
				return false;
			for (std::size_t n = 0; n < weights_.n_size; ++n) {
				if (!(weights_.data[n].load(in, arma::arma_binary)
					&& biasWeights_.data[n].load(in, arma::arma_binary))) {
					in.clear();
					return false;
				}
			}
			initialized_ = true;
			return true;
		}

		inline bool BaseLayer::SaveWeights(std::ofstream& out) const
		{
			if (weights_.n_size == 0)
				return true;
			if (!out.is_open())
				return false;
			for (std::size_t n = 0; n < weights_.n_size; ++n) {
				if (!(weights_.data[n].save(out, arma::arma_binary)
					  && biasWeights_.data[n].save(out, arma::arma_binary))) {
					out.clear();
					return false;
				}
			}
			return true;
		}

		inline void BaseLayer::InitWeights() noexcept
		{
			if (weights_.n_size == 0)
				return;

			for (std::size_t n = 0; n < weights_.n_size; ++n) {
				weights_.data[n] = 0.1 * arma::randn<arma::Cube<double>>(
					weights_.n_rows, weights_.n_cols, weights_.n_slices);
				biasWeights_.data[n] = 0.1 * arma::randn<arma::Cube<double>>(
					biasWeights_.n_rows, biasWeights_.n_cols, biasWeights_.n_slices);
			}
			initialized_ = true;
		}

		inline bool BaseLayer::is_initialized() const noexcept
		{
			return initialized_;
		}
	}
}


#include <iostream>
#include <Eigen/Eigenvalues>
#include <Eigen/Dense>
#include <vector>
#include <math.h>
#include <cmath>
#include <fstream>
#include <string>
#include <vector>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/features/shot_omp.h>
#include <pcl/features/fpfh.h>

#include <Config.hpp>
#include <edgeExtraction.hpp>

int main(int argc, char *argv[])
{
    // *Parse Config
    Config config;
    std::string input_pcd = config.input_pcd;
    std::string output_pcd = config.output_pcd;

    // *Read point cloud
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_edge(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::io::loadPCDFile(input_pcd, *cloud);
    std::cout << "Number of points in the input cloud is:" << cloud->points.size() << std::endl;

    // *Create the filtering object
    // pcl::VoxelGrid<pcl::PointXYZI> sor;
    // sor.setInputCloud(cloud);
    // sor.setLeafSize(0.1f, 0.1f, 0.1f);
    // sor.filter(*cloud);

    edgeExtraction(cloud, cloud_edge);

    pcl::io::savePCDFileASCII(output_pcd, *cloud);
    std::cerr << "Saved " << cloud->size() << " data points to test_pcd.pcd." << std::endl;

    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    viewer.showCloud(cloud_edge);
    while (!viewer.wasStopped())
    {
    }

    return 0;
}


/* Copyright 2018 Ignacio Torroba (ignaciotb@kth.se)
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef CORRESPONDENCECLASS_HPP
#define CORRESPONDENCECLASS_HPP

#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Float32.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>

#include <eigen3/Eigen/Eigen>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/LU>

#include <boost/scoped_ptr.hpp>

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/inverse_chi_squared.hpp>

#include <vector>
#include <fstream>
#include <queue>
#include <math.h>
#include <algorithm>
#include <functional>
#include <list>
#include <iostream>
#include <cctype>

#include "utils/ekf_utils.hpp"

/**
 * @brief The CorrespondenceClass class
 * Auxiliar class containing all the info related to a correspondence
 * made between a landmark j and a measurement i at time t
 */

struct jacobian_components{
    double mu_0;
    double mu_1;
    double mu_2;
    double c_3;
    double c_4;
    double c_5;
    double s_3;
    double s_4;
    double s_5;
    Eigen::Matrix3d R_fls_base_;
};

typedef struct jacobian_components h_comp;

class CorrespondenceClass{

public:

    double psi_;
    double d_m_;
    Eigen::MatrixXd H_t_;
    Eigen::MatrixXd S_inverted_;
    Eigen::Vector3d landmark_pos_;
    Eigen::VectorXd nu_;
    std::pair<int, double> i_j_;

    CorrespondenceClass(){}

    CorrespondenceClass(const int &, const double &){}
    /**
     * @brief computeH
     * @param mu_hat
     * @param lm_odom
     * Computes the jacobian of the measurment model
     */

    CorrespondenceClass(CorrespondenceClass&&) = default; // forces a move constructor despite having a destructor
    CorrespondenceClass& operator=(CorrespondenceClass&&) = default; // force a move assignment anyway

    virtual ~CorrespondenceClass(){}

    virtual std::tuple<Eigen::Vector3d, Eigen::Vector3d> measModel(const tf::Vector3&, const tf::Transform&){
        return std::make_tuple(Eigen::Vector3d(), Eigen::Vector3d());
    }

    virtual Eigen::VectorXd backProjectNewLM(const Eigen::VectorXd&, const tf::Transform&){
        return Eigen::VectorXd();
    }

    virtual void computeH(const h_comp ,
                  const tf::Vector3 , const Eigen::Vector3d ){}
    /**
     * @brief computeS
     * @param sigma
     * @param Q
     * S = H*Q*H^T + Q
     */
    virtual void computeMHLDistance(const Eigen::MatrixXd &, const Eigen::MatrixXd &){}
    /**
     * @brief computeNu
     * @param z_hat_i
     * @param z_i
     * Computes the innovation
     */
    virtual void computeNu(const Eigen::Vector3d &, const Eigen::Vector3d &){}
    /**
     * @brief computeLikelihood
     * Likelihood of the correspondence mj, zi
     * It also outputs the Mahalanobis distance between z_hat_i, z_i for outlier detection
     */
    virtual void computeLikelihood(){}

private:
};

#endif // CORRESPONDENCECLASS_HPP


///////////////////////////////////////////////////////////////////////////////
// extended_p_square_quantile.hpp
//
//  Copyright 2005 Daniel Egloff. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_EXTENDED_SINGLE_QUANTILE_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_EXTENDED_SINGLE_QUANTILE_HPP_DE_01_01_2006

#include <vector>
#include <functional>
#include <boost/throw_exception.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/permutation_iterator.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>
#include <boost/accumulators/statistics/extended_p_square.hpp>
#include <boost/accumulators/statistics/weighted_extended_p_square.hpp>
#include <boost/accumulators/statistics/times2_iterator.hpp>

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4127) // conditional expression is constant
#endif

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // extended_p_square_quantile_impl
    //  single quantile estimation
    /**
        @brief Quantile estimation using the extended \f$P^2\f$ algorithm for weighted and unweighted samples

        Uses the quantile estimates calculated by the extended \f$P^2\f$ algorithm to compute
        intermediate quantile estimates by means of quadratic interpolation.

        @param quantile_probability The probability of the quantile to be estimated.
    */
    template<typename Sample, typename Impl1, typename Impl2> // Impl1: weighted/unweighted // Impl2: linear/quadratic
    struct extended_p_square_quantile_impl
      : accumulator_base
    {
        typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type float_type;
        typedef std::vector<float_type> array_type;
        typedef iterator_range<
            detail::lvalue_index_iterator<
                permutation_iterator<
                    typename array_type::const_iterator
                  , detail::times2_iterator
                >
            >
        > range_type;
        // for boost::result_of
        typedef float_type result_type;

        template<typename Args>
        extended_p_square_quantile_impl(Args const &args)
          : probabilities(
                boost::begin(args[extended_p_square_probabilities])
              , boost::end(args[extended_p_square_probabilities])
            )
        {
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            typedef
                typename mpl::if_<
                    is_same<Impl1, weighted>
                  , tag::weighted_extended_p_square
                  , tag::extended_p_square
                >::type
            extended_p_square_tag;

            extractor<extended_p_square_tag> const some_extended_p_square = {};

            array_type heights(some_extended_p_square(args).size());
            std::copy(some_extended_p_square(args).begin(), some_extended_p_square(args).end(), heights.begin());

            this->probability = args[quantile_probability];

            typename array_type::const_iterator iter_probs = std::lower_bound(this->probabilities.begin(), this->probabilities.end(), this->probability);
            std::size_t dist = std::distance(this->probabilities.begin(), iter_probs);
            typename array_type::const_iterator iter_heights = heights.begin() + dist;

            // If this->probability is not in a valid range return NaN or throw exception
            if (this->probability < *this->probabilities.begin() || this->probability > *(this->probabilities.end() - 1))
            {
                if (std::numeric_limits<result_type>::has_quiet_NaN)
                {
                    return std::numeric_limits<result_type>::quiet_NaN();
                }
                else
                {
                    std::ostringstream msg;
                    msg << "probability = " << this->probability << " is not in valid range (";
                    msg << *this->probabilities.begin() << ", " << *(this->probabilities.end() - 1) << ")";
                    boost::throw_exception(std::runtime_error(msg.str()));
                    return Sample(0);
                }

            }

            if (*iter_probs == this->probability)
            {
                return heights[dist];
            }
            else
            {
                result_type res;

                if (is_same<Impl2, linear>::value)
                {
                    /////////////////////////////////////////////////////////////////////////////////
                    // LINEAR INTERPOLATION
                    //
                    float_type p1 = *iter_probs;
                    float_type p0 = *(iter_probs - 1);
                    float_type h1 = *iter_heights;
                    float_type h0 = *(iter_heights - 1);

                    float_type a = numeric::fdiv(h1 - h0, p1 - p0);
                    float_type b = h1 - p1 * a;

                    res = a * this->probability + b;
                }
                else
                {
                    /////////////////////////////////////////////////////////////////////////////////
                    // QUADRATIC INTERPOLATION
                    //
                    float_type p0, p1, p2;
                    float_type h0, h1, h2;

                    if ( (dist == 1 || *iter_probs - this->probability <= this->probability - *(iter_probs - 1) ) && dist != this->probabilities.size() - 1 )
                    {
                        p0 = *(iter_probs - 1);
                        p1 = *iter_probs;
                        p2 = *(iter_probs + 1);
                        h0 = *(iter_heights - 1);
                        h1 = *iter_heights;
                        h2 = *(iter_heights + 1);
                    }
                    else
                    {
                        p0 = *(iter_probs - 2);
                        p1 = *(iter_probs - 1);
                        p2 = *iter_probs;
                        h0 = *(iter_heights - 2);
                        h1 = *(iter_heights - 1);
                        h2 = *iter_heights;
                    }

                    float_type hp21 = numeric::fdiv(h2 - h1, p2 - p1);
                    float_type hp10 = numeric::fdiv(h1 - h0, p1 - p0);
                    float_type p21  = numeric::fdiv(p2 * p2 - p1 * p1, p2 - p1);
                    float_type p10  = numeric::fdiv(p1 * p1 - p0 * p0, p1 - p0);

                    float_type a = numeric::fdiv(hp21 - hp10, p21 - p10);
                    float_type b = hp21 - a * p21;
                    float_type c = h2 - a * p2 * p2 - b * p2;

                    res = a * this->probability * this-> probability + b * this->probability + c;
                }

                return res;
            }

        }

    public:
        // make this accumulator serializeable
        // TODO: do we need to split to load/save and verify that the parameters did not change?
        template<class Archive>
        void serialize(Archive & ar, const unsigned int file_version)
        {
            ar & probabilities;
            ar & probability;
        }

    private:

        array_type probabilities;
        mutable float_type probability;

    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::extended_p_square_quantile
//
namespace tag
{
    struct extended_p_square_quantile
      : depends_on<extended_p_square>
    {
        typedef accumulators::impl::extended_p_square_quantile_impl<mpl::_1, unweighted, linear> impl;
    };
    struct extended_p_square_quantile_quadratic
      : depends_on<extended_p_square>
    {
        typedef accumulators::impl::extended_p_square_quantile_impl<mpl::_1, unweighted, quadratic> impl;
    };
    struct weighted_extended_p_square_quantile
      : depends_on<weighted_extended_p_square>
    {
        typedef accumulators::impl::extended_p_square_quantile_impl<mpl::_1, weighted, linear> impl;
    };
    struct weighted_extended_p_square_quantile_quadratic
      : depends_on<weighted_extended_p_square>
    {
        typedef accumulators::impl::extended_p_square_quantile_impl<mpl::_1, weighted, quadratic> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::extended_p_square_quantile
// extract::weighted_extended_p_square_quantile
//
namespace extract
{
    extractor<tag::extended_p_square_quantile> const extended_p_square_quantile = {};
    extractor<tag::extended_p_square_quantile_quadratic> const extended_p_square_quantile_quadratic = {};
    extractor<tag::weighted_extended_p_square_quantile> const weighted_extended_p_square_quantile = {};
    extractor<tag::weighted_extended_p_square_quantile_quadratic> const weighted_extended_p_square_quantile_quadratic = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(extended_p_square_quantile)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(extended_p_square_quantile_quadratic)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_extended_p_square_quantile)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_extended_p_square_quantile_quadratic)
}

using extract::extended_p_square_quantile;
using extract::extended_p_square_quantile_quadratic;
using extract::weighted_extended_p_square_quantile;
using extract::weighted_extended_p_square_quantile_quadratic;

// extended_p_square_quantile(linear) -> extended_p_square_quantile
template<>
struct as_feature<tag::extended_p_square_quantile(linear)>
{
    typedef tag::extended_p_square_quantile type;
};

// extended_p_square_quantile(quadratic) -> extended_p_square_quantile_quadratic
template<>
struct as_feature<tag::extended_p_square_quantile(quadratic)>
{
    typedef tag::extended_p_square_quantile_quadratic type;
};

// weighted_extended_p_square_quantile(linear) -> weighted_extended_p_square_quantile
template<>
struct as_feature<tag::weighted_extended_p_square_quantile(linear)>
{
    typedef tag::weighted_extended_p_square_quantile type;
};

// weighted_extended_p_square_quantile(quadratic) -> weighted_extended_p_square_quantile_quadratic
template<>
struct as_feature<tag::weighted_extended_p_square_quantile(quadratic)>
{
    typedef tag::weighted_extended_p_square_quantile_quadratic type;
};

// for the purposes of feature-based dependency resolution,
// extended_p_square_quantile and weighted_extended_p_square_quantile
// provide the same feature as quantile
template<>
struct feature_of<tag::extended_p_square_quantile>
  : feature_of<tag::quantile>
{
};
template<>
struct feature_of<tag::extended_p_square_quantile_quadratic>
  : feature_of<tag::quantile>
{
};
// So that extended_p_square_quantile can be automatically substituted with
// weighted_extended_p_square_quantile when the weight parameter is non-void
template<>
struct as_weighted_feature<tag::extended_p_square_quantile>
{
    typedef tag::weighted_extended_p_square_quantile type;
};

template<>
struct feature_of<tag::weighted_extended_p_square_quantile>
  : feature_of<tag::extended_p_square_quantile>
{
};

// So that extended_p_square_quantile_quadratic can be automatically substituted with
// weighted_extended_p_square_quantile_quadratic when the weight parameter is non-void
template<>
struct as_weighted_feature<tag::extended_p_square_quantile_quadratic>
{
    typedef tag::weighted_extended_p_square_quantile_quadratic type;
};
template<>
struct feature_of<tag::weighted_extended_p_square_quantile_quadratic>
  : feature_of<tag::extended_p_square_quantile_quadratic>
{
};

}} // namespace boost::accumulators

#ifdef _MSC_VER
# pragma warning(pop)
#endif

#endif


#include "../../inc/const.h"
#include <algorithm>
#include <cassert>
#include <chrono>
#include <climits>
#include <cstdio>
#include <fcntl.h>
#include <iostream>
#include <map>
#include <omp.h>
#include <queue>
#include <unistd.h>
#include <Eigen/Dense>

#define NDEBUG
//#define VERIFY

#define NUM_OF_ROWS (READ_BUF_SIZE / KERNEL_SIZE)
#define NUM_OF_COLS (KERNEL_SIZE)

using namespace std;

class Timer {
  public:
    Timer() : m_beg(clock_::now()) {}
    void reset() { m_beg = clock_::now(); }

    double elapsed() const {
      return std::chrono::duration_cast<std::chrono::milliseconds>(clock_::now() -
          m_beg)
        .count();
    }

  private:
    typedef std::chrono::high_resolution_clock clock_;
    typedef std::chrono::duration<double, std::ratio<1>> second_;
    std::chrono::time_point<clock_> m_beg;
};

#define CONV_LOCAL_BATCH_SIZE (64L)
typedef Eigen::Matrix<char, CONV_LOCAL_BATCH_SIZE, KERNEL_SIZE, Eigen::RowMajor> dataMatrix;
typedef Eigen::Map<dataMatrix > mapDataMatrix;

typedef Eigen::Matrix<char, KERNEL_SIZE, NUM_OF_KERNELS> kernelMatrix;
typedef Eigen::Map<kernelMatrix > mapKernelMatrix;

typedef Eigen::Matrix<short, CONV_LOCAL_BATCH_SIZE, NUM_OF_KERNELS, Eigen::RowMajor> resMatrix;
typedef Eigen::Map<resMatrix > mapResMatrix;

char kernels[NUM_OF_KERNELS][KERNEL_SIZE];
mapKernelMatrix kernel_matrix = mapKernelMatrix(kernels[0]);
char data_buf[NUM_OF_ROWS][NUM_OF_COLS];
volatile short expected_result[NUM_OF_ROWS][NUM_OF_KERNELS];

void read_kernels() {
  int fd;
#ifndef NDEBUG
  if ((fd = open("../../data_gen/conv_kernel.txt", O_RDWR)) < 0) {
#else
  if ((fd = open("/mnt/centos/conv_kernel.txt", O_RDWR)) < 0) {
#endif
    std::cout << "file open failed" << std::endl;
    exit(-1);
  };
  char read_buf[READ_BUF_SIZE];
  char *kernel_ptr = *kernels;
  int read_bytes = 0;
  while (read_bytes < KERNEL_SIZE * NUM_OF_KERNELS) {
    read_bytes += read(fd, read_buf, KERNEL_SIZE * NUM_OF_KERNELS - read_bytes);
  }
  for (int i = 0; i < KERNEL_SIZE * NUM_OF_KERNELS; i++) {
    *(kernel_ptr++) = read_buf[i];
  }
  close(fd);
}

inline void conv(char *data, short *result, int size) {
  long long iter_times = (size + (CONV_LOCAL_BATCH_SIZE * KERNEL_SIZE) - 1)
                    / (CONV_LOCAL_BATCH_SIZE * KERNEL_SIZE);
  //for (long long i = 0; i * CONV_LOCAL_BATCH_SIZE * KERNEL_SIZE < size; i++) {
  #pragma omp parallel for
  for (long long i = 0; i < iter_times; i++) {
    mapDataMatrix data_matrix = mapDataMatrix(data + i * CONV_LOCAL_BATCH_SIZE * KERNEL_SIZE);
    mapResMatrix result_matrix = mapResMatrix(result + i * CONV_LOCAL_BATCH_SIZE * NUM_OF_KERNELS);
    result_matrix.noalias() = data_matrix.template cast<short>() * kernel_matrix.template cast<short> ();
  }
}

void read_compute() {

  int fd;
#ifndef NDEBUG
  if ((fd = open("../../data_gen/conv_data.txt", O_RDWR)) < 0) {
#else
  if ((fd = open("/mnt/centos/conv_data.txt", O_RDWR)) < 0) {
#endif
    std::cout << "file open failed" << std::endl;
    exit(-1);
  };
  unsigned long long total_read_bytes = 0;
  while (total_read_bytes != (NUM_OF_TOTAL_ROWS * KERNEL_SIZE)) {
    unsigned long long read_bytes = 0;
    char *data_buf_ptr = *data_buf;
    while (read_bytes != READ_BUF_SIZE) {
      int tmp = read(fd, data_buf_ptr + read_bytes, READ_BUF_SIZE - read_bytes);
      if (!tmp) {
        std::cout << "no enough data to calc,"
          << "result may contain meaningless result"
          << std::endl;
        break;
      }
      read_bytes += tmp;
    }
    total_read_bytes += read_bytes;
    std::cout << "total read bytes: " << total_read_bytes << std::endl;
    conv(data_buf[0], (short *) expected_result[0], read_bytes);

#ifdef VERIFY
    unsigned char *expected_ptr = (unsigned char *) expected_result[0];
    for (int i = 0; i < read_bytes; i++)
      std::cout << std::hex << (int) *(expected_ptr++) << " ";
    std::cout << std::endl;
#endif

#ifndef NDEBUG
    for (int i = 0; i < read_bytes / sizeof(short); i++)
      std::cout << (*expected_result)[i] << std::endl;
#endif
  }
  close(fd);
}

int main(int argc, char **argv) {
  omp_set_num_threads(1);
  Timer timer;
  puts("read_kernels().....");
  read_kernels();

  puts("read_compute().....");
  timer.reset();
  read_compute();
  cout << "elapsed time = " << timer.elapsed() / 1000 << endl;

  return 0;
}


#include <deal.II/base/function.h>
#include <deal.II/base/function_parser.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/table_handler.h>

#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/petsc_parallel_sparse_matrix.h>
#include <deal.II/lac/petsc_parallel_vector.h>
#include <deal.II/lac/petsc_solver.h>
#include <deal.II/lac/petsc_precondition.h>
#include <deal.II/lac/sparsity_tools.h>
#include <deal.II/lac/vector.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <deal.II/distributed/grid_refinement.h>
#include <deal.II/distributed/tria.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/fe/fe_q.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>

#include <mandy/elastic_tensor.h>
#include <mandy/lattice_tensor.h>

#include <mandy/crystal_symmetry_group.h>

#include <fstream>
#include <iostream>

#include <algorithm>    // std::transform
#include <functional>   // std::plus

namespace mandy
{
  /**
   * Solve the system Ax=b, where A is the vector-valued operator of
   * the Laplace-type and b is a linear function.
   */
  template <int dim>
  class ElasticProblem
  {
  public:

    /**
     * Class constructor.
     */
    ElasticProblem (const std::string &prm);

    /**
     * Class destructor.
     */
    ~ElasticProblem ();

    /**
     * Wrapper function, that controls the order of excecution.
     */
    void run ();

    /**
     * Get coefficients from the parameter file specified at run time.
     */
    void get_coefficients ();

  private:

    /**
     * Make intial coarse grid.
     */
    void make_coarse_grid ();

    /**
     * Setup system matrices and vectors.
     */
    void setup_system ();

    /**
     * Assemble system matrices and vectors.
     */
    void assemble_system();

    /**
     * Solve the linear algebra system.
     */
    unsigned int solve ();

    /**
     * Output results, ie., finite element functions and derived
     * quantitites for this cycle.
     */
    void output_results (const unsigned int cycle);

    /**
     * Refine grid based on Kelly's error estimator working on the
     * material id (solution vector).
     */
    void refine_grid ();

    /**
     * MPI communicator.
     */
    MPI_Comm mpi_communicator;

    /**
     * A distributed grid on which all computations are done.
     */
    dealii::parallel::distributed::Triangulation<dim> triangulation;

    /**
     * Scalar DoF handler primarily used for interpolating material
     * identification.
     */
    dealii::DoFHandler<dim> dof_handler;

    /**
     * Scalar valued finite element primarily used for interpolating
     * material iudentification.
     */
    dealii::FESystem<dim> finite_element;

    /**
     * Index set of locally owned DoFs.
     */
    dealii::IndexSet locally_owned_dofs;

    /**
     * Index set of locally relevant DoFs.
     */
    dealii::IndexSet locally_relevant_dofs;

    /**
     * A list of (hanging node) constraints.
     */
    dealii::ConstraintMatrix constraints;

    /**
     * System matrix - a mass matrix.
     */
    dealii::PETScWrappers::MPI::SparseMatrix system_matrix;

    /**
     * Locally relevant solution vector.
     */
    dealii::PETScWrappers::MPI::Vector locally_relevant_solution;

    /**
     * System right hand side function - interpolated function.
     */
    dealii::PETScWrappers::MPI::Vector system_rhs;

    /**
     * Parallel iostream.
     */
    dealii::ConditionalOStream pcout;

    /**
     * Stop clock.
     */
    dealii::TimerOutput timer;

    /**
     * Input parameter file.
     */
    dealii::ParameterHandler parameters;

    /**
     * Tensor of elastic coefficients.
     */
    mandy::Physics::ElasticTensor<mandy::CrystalSymmetryGroup::wurtzite> elastic_tensor;

    /**
     * Vector of elastic coefficients.
     */
    std::vector<double> elastic_coefficients;

    /**
     * Tensor of lattice coefficients.
     */
    mandy::Physics::LatticeTensor<mandy::CrystalSymmetryGroup::wurtzite> lattice_tensor;

    /**
     * Vector of lattice coefficients.
     */
    std::vector<double> lattice_coefficients;

  }; // LinearElasticity

  /**
   * Class constructor.
   */
  template <int dim>
  ElasticProblem<dim>::ElasticProblem (const std::string &prm)
    :
    mpi_communicator (MPI_COMM_WORLD),
    triangulation (mpi_communicator,
                   typename dealii::Triangulation<dim>::MeshSmoothing
                   (dealii::Triangulation<dim>::smoothing_on_refinement |
                    dealii::Triangulation<dim>::smoothing_on_coarsening)),
    dof_handler (triangulation),
    finite_element (dealii::FE_Q<dim> (2), dim),
    // ---
    pcout (std::cout, (dealii::Utilities::MPI::this_mpi_process (mpi_communicator) == 0)),
    timer (mpi_communicator, pcout,
	   dealii::TimerOutput::summary,
	   dealii::TimerOutput::wall_times)
  {
    parameters.enter_subsection ("Material");
    {
      parameters.declare_entry ("Material function", "0",
				dealii::Patterns::Anything (),
				"A functional description of the material.");

      parameters.declare_entry ("Lattice background",
				"0, 0, 0",
				dealii::Patterns::List (dealii::Patterns::Anything (), 1, 3, ","),
				"Size of the lattice of the background");

      parameters.declare_entry ("Lattice inclusion",
				"0, 0, 0",
				dealii::Patterns::List (dealii::Patterns::Anything (), 1, 3, ","),
				"Size of the lattice of an inclusion");

      parameters.declare_entry ("Elastic background",
				"0, 0, 0, 0, 0",
				dealii::Patterns::List (dealii::Patterns::Anything (), 1, 5, ","),
				"Elastic coefficients of the background");

      parameters.declare_entry ("Elastic inclusion",
				"0, 0, 0, 0, 0",
				dealii::Patterns::List (dealii::Patterns::Anything (), 1, 5, ","),
				"Elastic coefficients of an inclusion");
    }
    parameters.leave_subsection ();

    parameters.parse_input (prm);
  }

  /**
   * Class destructor.
   */
  template <int dim>
  ElasticProblem<dim>::~ElasticProblem ()
  {
    // Wipe DoF handlers.
    dof_handler.clear ();
  }

  /**
   * Setup system matrices and vectors.
   */
  template <int dim>
  void ElasticProblem<dim>::setup_system ()
  {
    dealii::TimerOutput::Scope time (timer, "setup system");

    // Determine locally relevant DoFs.
    dof_handler.distribute_dofs (finite_element);
    locally_owned_dofs = dof_handler.locally_owned_dofs ();
    dealii::DoFTools::extract_locally_relevant_dofs (dof_handler, locally_relevant_dofs);

    // Initialise distributed vectors.
    locally_relevant_solution.reinit (locally_owned_dofs, locally_relevant_dofs,
				      mpi_communicator);
    system_rhs.reinit (locally_owned_dofs,
		       mpi_communicator);

    // Setup hanging node constraints.
    constraints.clear ();
    constraints.reinit (locally_relevant_dofs);

    dealii::DoFTools::make_hanging_node_constraints (dof_handler, constraints);
    dealii::DoFTools::make_zero_boundary_constraints (dof_handler, constraints);
    constraints.close ();

    // Finally, create a distributed sparsity pattern and initialise
    // the system matrix from that.
    dealii::DynamicSparsityPattern dsp (locally_relevant_dofs);
    dealii::DoFTools::make_sparsity_pattern (dof_handler, dsp, constraints, false);
    dealii::SparsityTools::distribute_sparsity_pattern (dsp,
							dof_handler.n_locally_owned_dofs_per_processor (),
							mpi_communicator,
							locally_relevant_dofs);

    system_matrix.reinit (locally_owned_dofs, locally_owned_dofs,
                          dsp, mpi_communicator);

  }

  /**
   * Assemble the linear algebra system.
   */
  template <int dim>
  void
  ElasticProblem<dim>::assemble_system ()
  {
    dealii::TimerOutput::Scope time (timer, "assemble system");

    // Define quadrature rule to be used.
    const dealii::QGauss<dim> quadrature_formula (3);

    dealii::FEValues<dim> fe_values (finite_element, quadrature_formula,
				     dealii::update_values            |
				     dealii::update_gradients         |
				     dealii::update_quadrature_points |
				     dealii::update_JxW_values);

    const unsigned int dofs_per_cell = finite_element.dofs_per_cell;
    const unsigned int n_q_points    = quadrature_formula.size ();

    dealii::FullMatrix<double> cell_matrix (dofs_per_cell, dofs_per_cell);
    dealii::Vector<double> cell_vector (dofs_per_cell);
    std::vector<dealii::types::global_dof_index> local_dof_indices (dofs_per_cell);

    // A vector of material values at each quadrature point and the
    // function to be parsed from the input file.
    dealii::FunctionParser<dim> material_function;

    parameters.enter_subsection ("Material");
    {
      material_function.initialize (dealii::FunctionParser<dim>::default_variable_names (),
				    parameters.get ("Material function"),
				    typename dealii::FunctionParser<dim>::ConstMap ());
    }
    parameters.leave_subsection ();

    std::vector<double> material_function_values (n_q_points);

    // Get lattice parameters from file.
    std::vector<double> lattice_coefficients_background;
    std::vector<double> lattice_coefficients_inclusion;

    // Get elastic coefficients from input file.
    std::vector<double> elastic_coefficients_background;
    std::vector<double> elastic_coefficients_inclusion;

    parameters.enter_subsection ("Material");
    {
      lattice_coefficients_background = dealii::Utilities::string_to_double
	(dealii::Utilities::split_string_list (parameters.get ("Lattice background"), ','));

      lattice_coefficients_inclusion = dealii::Utilities::string_to_double
	(dealii::Utilities::split_string_list (parameters.get ("Lattice inclusion"), ','));

      elastic_coefficients_background = dealii::Utilities::string_to_double
	(dealii::Utilities::split_string_list (parameters.get ("Elastic background"), ','));

      elastic_coefficients_inclusion = dealii::Utilities::string_to_double
	(dealii::Utilities::split_string_list (parameters.get ("Elastic inclusion"), ','));
    }
    parameters.leave_subsection ();

    AssertThrow (elastic_coefficients_background.size ()==elastic_coefficients_inclusion.size (),
		 dealii::ExcDimensionMismatch (elastic_coefficients_background.size (),
					       elastic_coefficients_inclusion.size ()));

    typename dealii::DoFHandler<dim>::active_cell_iterator
      cell = dof_handler.begin_active (),
      endc = dof_handler.end ();

    for (; cell!=endc; ++cell)
      if (cell->subdomain_id () == dealii::Utilities::MPI::this_mpi_process (mpi_communicator))
	{
	  cell_matrix = 0;
	  cell_vector = 0;
	  fe_values.reinit (cell);

	  // Extract vector-values from FEValues.
	  const dealii::FEValuesExtractors::Vector u (0);

	  // Obtain the material identification on this cell and
	  // transfer it to a strain description.
	  material_function.value_list (fe_values.get_quadrature_points (),
					material_function_values);

	  for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
	    {

	      elastic_coefficients.clear ();

	      for (unsigned int i=0; i<elastic_coefficients_inclusion.size (); ++i)
		elastic_coefficients.push_back (material_function_values[q_point]*elastic_coefficients_inclusion[i] +
						(1.-material_function_values[q_point])*elastic_coefficients_background[i]);

	      elastic_tensor.set_coefficients (elastic_coefficients);
	      elastic_tensor.distribute_coefficients ();

	      Assert (elastic_tensor.is_symmetric (), dealii::ExcMessage ("Tensor not symmetric"));

	      lattice_coefficients.clear ();

	      for (unsigned int i=0; i<lattice_coefficients_inclusion.size (); ++i)
		lattice_coefficients.push_back (material_function_values[q_point] *
						(lattice_coefficients_inclusion[i]/lattice_coefficients_background[i]) - 1.);
	      lattice_tensor.set_coefficients (lattice_coefficients);
	      lattice_tensor.distribute_coefficients ();

	      for (unsigned int i=0; i<dofs_per_cell; ++i)
		{
		  const dealii::Tensor<2, dim> u_i_grad = fe_values[u].symmetric_gradient (i, q_point);

		  for (unsigned int j=0; j<dofs_per_cell; ++j)
		    {
		      const dealii::Tensor<2, dim> u_j_grad = fe_values[u].symmetric_gradient (j, q_point);

		      // Local stiffness matrix.
		      cell_matrix (i,j) +=
			contract (u_i_grad, elastic_tensor, u_j_grad) *
			fe_values.JxW (q_point);

		    }

		  // Local right hand side vector.
		  cell_vector (i) +=
		    contract (u_i_grad, elastic_tensor, lattice_tensor) *
		    fe_values.JxW (q_point);

		}

	    } // q_point

	  cell->get_dof_indices (local_dof_indices);

	  constraints.distribute_local_to_global (cell_matrix, cell_vector,
						  local_dof_indices,
						  system_matrix, system_rhs);
	} // cell!=endc

    system_matrix.compress (dealii::VectorOperation::add);
    system_rhs.compress (dealii::VectorOperation::add);
  }

  /**
   * Solve the linear algebra system.
   */
  template <int dim>
  unsigned int
  ElasticProblem<dim>::solve ()
  {
    dealii::TimerOutput::Scope time (timer, "solve");

    dealii::PETScWrappers::MPI::Vector completely_distributed_solution (locally_owned_dofs, mpi_communicator);
    dealii::SolverControl solver_control (dof_handler.n_dofs (), 1e-06);
    dealii::PETScWrappers::SolverBicgstab solver (solver_control, mpi_communicator);
    dealii::PETScWrappers::PreconditionBlockJacobi preconditioner (system_matrix);

    solver.solve (system_matrix, completely_distributed_solution, system_rhs,
		  preconditioner);

    // Ensure that all ghost elements are also copied as necessary.
    constraints.distribute (completely_distributed_solution);
    locally_relevant_solution = completely_distributed_solution;

    // Return the number of iterations (last step) of the solve.
    return solver_control.last_step ();
  }

  /**
   * Output results.
   */
  template <int dim>
  void
  ElasticProblem<dim>::output_results (const unsigned int cycle)
  {
    dealii::TimerOutput::Scope time (timer, "output_results");

    dealii::DataOut<dim> data_out;
    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (locally_relevant_solution, "displacement");

    dealii::Vector<float> subdomain (triangulation.n_active_cells ());
    for (unsigned int i=0; i<subdomain.size(); ++i)
      subdomain (i) = triangulation.locally_owned_subdomain ();
    data_out.add_data_vector (subdomain, "subdomain");

    data_out.build_patches ();

    const std::string filename = ("displacement-" +
                                  dealii::Utilities::int_to_string (cycle, 2) +
                                  "." +
                                  dealii::Utilities::int_to_string
                                  (triangulation.locally_owned_subdomain (), 4));

    std::ofstream output ((filename + ".vtu").c_str ());
    data_out.write_vtu (output);

    if (dealii::Utilities::MPI::this_mpi_process(mpi_communicator) == 1)
      {
	std::vector<std::string> filenames;

	for (unsigned int i=0;
	     i<dealii::Utilities::MPI::n_mpi_processes (mpi_communicator);
	     ++i)
	  filenames.push_back ("displacement-" +
			       dealii::Utilities::int_to_string (cycle, 2) +
			       "." +
			       dealii::Utilities::int_to_string (i, 4) +
			       ".vtu");
	std::ofstream master_output (("displacement-" +
				      dealii::Utilities::int_to_string (cycle, 2) +
				      ".pvtu").c_str ());

	data_out.write_pvtu_record (master_output, filenames);
      }
  }

  /**
   * Run the application in the order specified.
   */
  template <int dim>
  void
  ElasticProblem<dim>::run ()
  {
    // To solve the Elastic problem, first we need to solve the
    // problem of a mterial identification. This is done by solving a
    // function against the mass matrix.

    // Create a coarse grid according to the parameters given in the
    // input file.
    dealii::GridGenerator::hyper_cube (triangulation, -2.5, 2.5);
    triangulation.refine_global (3);

    pcout << "   Number of active cells:       "
	  << triangulation.n_global_active_cells ()
	  << std::endl;

    setup_system ();

    pcout << "   Number of degrees of freedom: "
	  << dof_handler.n_dofs ()
	  << std::endl;

    assemble_system ();

    const unsigned int n_iterations = solve ();

    pcout << "   Solved in " << n_iterations
	  << " iterations."
	  << std::endl;

    pcout << "   Linfty-norm:                  "
	  << locally_relevant_solution.linfty_norm ()
	  << std::endl;

    output_results (0);
  }

} // namespace mandy

/**
 * Main function: Initialise problem and run it.
 */
int main (int argc, char *argv[])
{
  // Initialise MPI
  dealii::Utilities::MPI::MPI_InitFinalize mpi_initialization (argc, argv, 1);

  try
    {
      // mandy::MaterialID<3> material_id ("step-4.prm");
      // material_id.run ();

      mandy::ElasticProblem<3> elastic_problem ("elastic.prm");
      elastic_problem.run ();
    }

  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }

  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include <eve/function/ellint_2.hpp>
#include <boost/math/special_functions/ellint_2.hpp>
#include <eve/function/next.hpp>
#include <eve/function/prev.hpp>
#include <eve/function/is_denormal.hpp>
#include <eve/constant/nan.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/minlog.hpp>
#include <eve/platform.hpp>

#include <cmath>

TTS_CASE_TPL("Check eve::ellint_2 return type", EVE_TYPE)
{
  TTS_EXPR_IS(eve::ellint_2(T(0)), T);
}

TTS_CASE_TPL("Check eve::ellint_2 behavior one parameter", EVE_TYPE)
{
  using v_t = eve::element_type_t<T>;

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_IEEE_EQUAL(eve::ellint_2(eve::nan(eve::as<T>())) , eve::nan(eve::as<T>()) );
  }

  TTS_ULP_EQUAL(eve::ellint_2(T(1)) , T(boost::math::ellint_2(v_t(1))), 0.5);
  TTS_ULP_EQUAL(eve::ellint_2(T(-1)), T(boost::math::ellint_2(v_t(-1))), 0.5);
  TTS_ULP_EQUAL( eve::ellint_2(T( 0.)), T(boost::math::ellint_2(v_t(0))), 0.5);
  TTS_ULP_EQUAL( eve::ellint_2(T( 0.5)), T(boost::math::ellint_2(v_t(0.5))), 1.0);
  TTS_ULP_EQUAL( eve::ellint_2(T( 0.9)), T(boost::math::ellint_2(v_t(0.9))), 0.5);

}

TTS_CASE_TPL("Check eve::ellint_2 behavior two parameter", EVE_TYPE)
{
  using v_t = eve::element_type_t<T>;
  using eve::as;
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_IEEE_EQUAL(eve::ellint_2(eve::pio_2(as<T>()), eve::nan(eve::as<T>())) , eve::nan(eve::as<T>()) );
  }

  TTS_ULP_EQUAL( eve::ellint_2(eve::pio_2(as<T>()), T( 1.)),  T(boost::math::ellint_2(v_t(1)  , eve::pio_2(as<v_t>()))), 2.0);
  TTS_ULP_EQUAL( eve::ellint_2(eve::pio_2(as<T>()), T(-1.)),  T(boost::math::ellint_2(v_t(-1) , eve::pio_2(as<v_t>()))), 2.0);
  TTS_ULP_EQUAL( eve::ellint_2(eve::pio_2(as<T>()), T( 0.)),  T(boost::math::ellint_2(v_t(0)  , eve::pio_2(as<v_t>()))), 2.0);
  TTS_ULP_EQUAL( eve::ellint_2(eve::pio_2(as<T>()), T( 0.5)), T(boost::math::ellint_2(v_t(0.5), eve::pio_2(as<v_t>()))), 2.0);
  TTS_ULP_EQUAL( eve::ellint_2(eve::pio_2(as<T>()), T( 0.9)), T(boost::math::ellint_2(v_t(0.9), eve::pio_2(as<v_t>()))), 2.0);
  TTS_ULP_EQUAL( eve::ellint_2(eve::pio_4(as<T>()), T( 0.)),  T(boost::math::ellint_2(v_t(0)  , eve::pio_4(as<v_t>()))), 2.0);
  TTS_ULP_EQUAL( eve::ellint_2(eve::pio_4(as<T>()), T( 0.5)), T(boost::math::ellint_2(v_t(0.5), eve::pio_4(as<v_t>()))), 2.0);
  TTS_ULP_EQUAL( eve::ellint_2(eve::pio_4(as<T>()), T( 0.9)), T(boost::math::ellint_2(v_t(0.9), eve::pio_4(as<v_t>()))), 2.0);
  TTS_ULP_EQUAL( eve::ellint_2(eve::pio_4(as<T>()), T( 1.0)), T(boost::math::ellint_2(v_t(1.0), eve::pio_4(as<v_t>()))), 2.0);
  TTS_ULP_EQUAL( eve::ellint_2(3*eve::pio_4(as<T>()), T( 0.5)), T(boost::math::ellint_2(v_t(0.5), 3*eve::pio_4(as<v_t>()))), 2.0);
  TTS_ULP_EQUAL( eve::ellint_2(3*eve::pio_4(as<T>()), T( 0.9)), T(boost::math::ellint_2(v_t(0.9), 3*eve::pio_4(as<v_t>()))), 2.0);
  TTS_ULP_EQUAL( eve::ellint_2(3*eve::pio_4(as<T>()), T( 0.)),  T(boost::math::ellint_2(v_t(0)  , 3*eve::pio_4(as<v_t>()))), 2.0);
  TTS_ULP_EQUAL( eve::ellint_2(T(100), T( 0.5)), T(boost::math::ellint_2(v_t(0.5), v_t(100))), 2.0);
  TTS_ULP_EQUAL( eve::ellint_2(T(100), T( 0.9)), T(boost::math::ellint_2(v_t(0.9), v_t(100))), 2.0);
  TTS_ULP_EQUAL( eve::ellint_2(T(100), T( 0.)),  T(boost::math::ellint_2(v_t(0)  , v_t(100))), 2.0);

}


#include <iostream>
#include <random>
#include <sstream>
//
#include <perfevent/perfevent.hpp>
//
#include <boost/random/mersenne_twister.hpp>
//
#include <pxart/pxart.hpp>

using namespace std;

struct benchmark {
  explicit benchmark(size_t samples) : n{samples} {
    cout << "# iterations = " << n << "\n"
         << "\n";
  }

  template <typename RNG, typename Function>
  benchmark& run_function(const char* name, RNG&& rng,
                          Function function) noexcept {
    float pi{};
    {
      params.setParam("#                name", name);
      PerfEventBlock e(n, params, header);
      pi = function(forward<RNG>(rng), n);
    }
    cout << "# pi = " << pi << "\n";

    header = false;
    return *this;
  }
  template <typename RNG, typename Function>
  benchmark& run_function(const char* name, RNG&& rng1, RNG&& rng2,
                          Function function) noexcept {
    float pi{};
    {
      params.setParam("#                name", name);
      PerfEventBlock e(n, params, header);
      pi = function(forward<RNG>(rng1), forward<RNG>(rng2), n);
    }
    cout << "# pi = " << pi << "\n";

    header = false;
    return *this;
  }

  template <typename RNG>
  benchmark& run_std(const char* name, RNG&& rng) noexcept {
    return run_function(name, forward<RNG>(rng), [](RNG&& rng, uint32_t n) {
      return pxart::gallery::base::monte_carlo_pi<float>(forward<RNG>(rng), n);
    });
  }
  template <typename RNG>
  benchmark& run_std(const char* name, RNG&& rng1, RNG&& rng2) noexcept {
    return run_function(name, forward<RNG>(rng1), forward<RNG>(rng2),
                        [](RNG&& rng1, RNG&& rng2, uint32_t n) {
                          return pxart::gallery::base::monte_carlo_pi<float>(
                              forward<RNG>(rng1), forward<RNG>(rng2), n);
                        });
  }
  template <typename RNG>
  benchmark& run_uniform(const char* name, RNG&& rng) noexcept {
    return run_function(name, forward<RNG>(rng), [](RNG&& rng, uint32_t n) {
      return pxart::gallery::monte_carlo_pi<float>(forward<RNG>(rng), n);
    });
  }
  template <typename RNG>
  benchmark& run_uniform(const char* name, RNG&& rng1, RNG&& rng2) noexcept {
    return run_function(name, forward<RNG>(rng1), forward<RNG>(rng2),
                        [](RNG&& rng1, RNG&& rng2, uint32_t n) {
                          return pxart::gallery::monte_carlo_pi<float>(
                              forward<RNG>(rng1), forward<RNG>(rng2), n);
                        });
  }

  template <typename RNG>
  benchmark& run_cache(const char* name, RNG&& rng) noexcept {
    return run_function(name, forward<RNG>(rng), [](RNG&& rng, uint32_t n) {
      return pxart::gallery::cache::monte_carlo_pi<float>(forward<RNG>(rng), n);
    });
  }
  template <typename RNG>
  benchmark& run_cache(const char* name, RNG&& rng1, RNG&& rng2) noexcept {
    return run_function(name, forward<RNG>(rng1), forward<RNG>(rng2),
                        [](RNG&& rng1, RNG&& rng2, uint32_t n) {
                          return pxart::gallery::cache::monte_carlo_pi<float>(
                              forward<RNG>(rng1), forward<RNG>(rng2), n);
                        });
  }

#ifdef __AVX2__
  template <typename RNG>
  benchmark& run_simd256(const char* name, RNG&& rng) noexcept {
    return run_function(name, forward<RNG>(rng), [](RNG&& rng, uint32_t n) {
      return pxart::gallery::simd256::vprng::monte_carlo_pi(forward<RNG>(rng),
                                                            n);
    });
  }
  template <typename RNG>
  benchmark& run_simd256(const char* name, RNG&& rng1, RNG&& rng2) noexcept {
    return run_function(name, forward<RNG>(rng1), forward<RNG>(rng2),
                        [](RNG&& rng1, RNG&& rng2, uint32_t n) {
                          return pxart::gallery::simd256::vprng::monte_carlo_pi(
                              forward<RNG>(rng1), forward<RNG>(rng2), n);
                        });
  }
#endif  // __AVX2__

#ifdef __SSE4_1__
  template <typename RNG>
  benchmark& run_simd128(const char* name, RNG&& rng) noexcept {
    return run_function(name, forward<RNG>(rng), [](RNG&& rng, uint32_t n) {
      return pxart::gallery::simd128::vprng::monte_carlo_pi(forward<RNG>(rng),
                                                            n);
    });
  }
  template <typename RNG>
  benchmark& run_simd128(const char* name, RNG&& rng1, RNG&& rng2) noexcept {
    return run_function(name, forward<RNG>(rng1), forward<RNG>(rng2),
                        [](RNG&& rng1, RNG&& rng2, uint32_t n) {
                          return pxart::gallery::simd128::vprng::monte_carlo_pi(
                              forward<RNG>(rng1), forward<RNG>(rng2), n);
                        });
  }
#endif  // __SSE4_1__

  benchmark& separate() noexcept {
    header = true;
    cout << "\n";
    return *this;
  }

  benchmark& heading(const char* text) {
    cout << text << "\n";
    return *this;
  }

  size_t n{};
  BenchmarkParameters params{};
  bool header = true;
};

int main(int argc, char** argv) {
  uint64_t n = 100'000'000;

  if (2 != argc) {
    cout << "usage:\n" << argv[0] << " <number of elements>\n";
  } else {
    stringstream input{argv[1]};
    input >> n;
  }

  random_device rd{};

  benchmark{n}  //
      .heading("# STL MT19937")
      .run_std("naive", std::mt19937{rd()})
      .run_uniform("uniform", std::mt19937{rd()})
      .separate()
      .heading("# Boost MT19937")
      .run_std("naive", boost::random::mt19937{rd()})
      .run_uniform("uniform", boost::random::mt19937{rd()})
      .separate()
      .heading("# pxart MT19937")
      .run_std("naive", pxart::mt19937{rd})
      .run_uniform("uniform", pxart::mt19937{rd})
#ifdef __AVX2__
      .run_cache("AVX cache", pxart::simd256::mt19937{rd})
      .run_simd256("AVX", pxart::simd256::mt19937{rd})
#endif  // __AVX2__
#ifdef __SSE4_1__
      .run_cache("SSE cache", pxart::simd128::mt19937{rd})
      .run_simd128("SSE", pxart::simd128::mt19937{rd})
#endif  // __SSE4_1__
      .separate()
      .heading("# pxart Xoroshiro128+")
      .run_std("naive", pxart::xrsr128p{rd})
      .run_std("2 x naive", pxart::xrsr128p{rd}, pxart::xrsr128p{rd})
      .run_uniform("uniform", pxart::xrsr128p{rd})
      .run_uniform("2 x uniform", pxart::xrsr128p{rd}, pxart::xrsr128p{rd})
#ifdef __AVX2__
      .run_cache("AVX cache", pxart::simd256::xrsr128p{rd})
      .run_cache("2 x AVX cache", pxart::simd256::xrsr128p{rd},
                 pxart::simd256::xrsr128p{rd})
      .run_simd256("AVX", pxart::simd256::xrsr128p{rd})
      .run_simd256("2 x AVX", pxart::simd256::xrsr128p{rd},
                   pxart::simd256::xrsr128p{rd})
#endif  // __AVX2__
#ifdef __SSE4_1__
      .run_cache("SSE cache", pxart::simd128::xrsr128p{rd})
      .run_cache("2 x SSE cache", pxart::simd128::xrsr128p{rd},
                 pxart::simd128::xrsr128p{rd})
      .run_simd128("SSE", pxart::simd128::xrsr128p{rd})
      .run_simd128("2 x SSE", pxart::simd128::xrsr128p{rd},
                   pxart::simd128::xrsr128p{rd})
#endif  // __SSE4_1__
      .separate()
      .heading("# pxart MSWS")
      .run_std("naive", pxart::msws{rd})
      .run_std("2 x naive", pxart::msws{rd}, pxart::msws{rd})
      .run_uniform("uniform", pxart::msws{rd})
      .run_uniform("2 x uniform", pxart::msws{rd}, pxart::msws{rd})
#ifdef __AVX2__
      .run_cache("AVX cache", pxart::simd256::msws{rd})
      .run_cache("2 x AVX cache", pxart::simd256::msws{rd},
                 pxart::simd256::msws{rd})
      .run_simd256("AVX", pxart::simd256::msws{rd})
      .run_simd256("2 x AVX", pxart::simd256::msws{rd},
                   pxart::simd256::msws{rd})
#endif  // __AVX2__
#ifdef __SSE4_1__
      .run_cache("SSE cache", pxart::simd128::msws{rd})
      .run_cache("2 x SSE cache", pxart::simd128::msws{rd},
                 pxart::simd128::msws{rd})
      .run_simd128("SSE", pxart::simd128::msws{rd})
      .run_simd128("2 x SSE", pxart::simd128::msws{rd},
                   pxart::simd128::msws{rd})
#endif  // __SSE4_1__
      //
      ;
}


#include "libsnark/gadgetlib1/gadgets/basic_gadgets.hpp"
#include "libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp"
#include "libsnark/common/default_types/r1cs_ppzksnark_pp.hpp"
#include "libsnark/common/utils.hpp"
#include <boost/optional.hpp>

using namespace libsnark;
using namespace std;

#include "gadget.hpp"
#include "gadget_neg.hpp"

template<typename ppzksnark_ppT>
r1cs_ppzksnark_keypair<ppzksnark_ppT> generate_keypair(const int jw[32])
{
    typedef Fr<ppzksnark_ppT> FieldT;

    //根据预先定义的计算门和约束生成公共参数秘钥对
    //证明生成端若需要采用对应的公共秘钥生成证明数据成功，则必须使两端的数据符合预先定于的计算约束(R1=R2+R3+X)
    //如此，当验证端根据对应的验证秘钥验证证明数据为真时，验证者就能够相信对应的交易中是符合预定义的计算约束的，而不是生成假证明以通过检查
    protoboard<FieldT> pb;
    l_gadget<FieldT> g(pb);
    g.generate_r1cs_constraints();
    const r1cs_constraint_system<FieldT> constraint_system = pb.get_constraint_system();

    cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;

    return r1cs_ppzksnark_generator<ppzksnark_ppT>(constraint_system);
}

template<typename ppzksnark_ppT>
r1cs_ppzksnark_keypair<ppzksnark_ppT> generate_keypair_neg(const int jw[2][32])
{
    typedef Fr<ppzksnark_ppT> FieldT;

    //根据预先定义的计算门和约束生成公共参数秘钥对
    //证明生成端若需要采用对应的公共秘钥生成证明数据成功，则必须使两端的数据符合预先定于的计算约束(R1+X=R2+R3)
    //如此，当验证端根据对应的验证秘钥验证证明数据为真时，验证者就能够相信对应的交易中是符合预定义的计算约束的，而不是生成假证明以通过检查
    protoboard<FieldT> pb;
    l_gadget_neg<FieldT> g(pb);
    g.generate_r1cs_constraints();
    const r1cs_constraint_system<FieldT> constraint_system = pb.get_constraint_system();

    cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;

    return r1cs_ppzksnark_generator<ppzksnark_ppT>(constraint_system);
}

template<typename ppzksnark_ppT>
boost::optional<r1cs_ppzksnark_proof<ppzksnark_ppT>> generate_proof(r1cs_ppzksnark_proving_key<ppzksnark_ppT> proving_key,
                                                                   const bit_vector &h1,
                                                                   const bit_vector &h2,
                                                                   const bit_vector &h3,
                                                                   const bit_vector &r1,
                                                                   const bit_vector &r2,
                                                                   const bit_vector &r3,
																   const bit_vector &x,
																   const int jw[32]
                                                                   )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    l_gadget<FieldT> g(pb);
    g.generate_r1cs_constraints();
    g.generate_r1cs_witness(h1, h2, h3, r1, r2, r3,x);

    if (!pb.is_satisfied()) {
      std::cout << "System not satisfied!" << std::endl;
        return boost::none;
    }

    return r1cs_ppzksnark_prover<ppzksnark_ppT>(proving_key, pb.primary_input(), pb.auxiliary_input());
}

template<typename ppzksnark_ppT>
boost::optional<r1cs_ppzksnark_proof<ppzksnark_ppT>> generate_proof_neg(r1cs_ppzksnark_proving_key<ppzksnark_ppT> proving_key,
                                                                   const bit_vector &h1,
                                                                   const bit_vector &h2,
                                                                   const bit_vector &h3,
                                                                   const bit_vector &r1,
                                                                   const bit_vector &r2,
                                                                   const bit_vector &r3,
																   const bit_vector &x,
																   const int jw[2][32]
                                                                   )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    l_gadget_neg<FieldT> g(pb);
    g.generate_r1cs_constraints();
    g.generate_r1cs_witness(h1, h2, h3, r1, r2, r3,x);

    if (!pb.is_satisfied()) {
      std::cout << "System not satisfied!" << std::endl;
        return boost::none;
    }

    return r1cs_ppzksnark_prover<ppzksnark_ppT>(proving_key, pb.primary_input(), pb.auxiliary_input());
}

template<typename ppzksnark_ppT>
bool verify_proof(r1cs_ppzksnark_verification_key<ppzksnark_ppT> verification_key,
                  r1cs_ppzksnark_proof<ppzksnark_ppT> proof,
                  const bit_vector &h1,
                  const bit_vector &h2,
                  const bit_vector &h3,
				  const bit_vector &x
                 )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    const r1cs_primary_input<FieldT> input = l_input_map<FieldT>(h1, h2, h3,x);

    std::cout << "**** After l_input_map *****" << std::endl;

    return r1cs_ppzksnark_verifier_strong_IC<ppzksnark_ppT>(verification_key, input, proof);

}


/*
 * nav_graph.cpp
 *
 * Graph Library for Autonomous and Dynamic Systems
 *
 * author:  Pierrick Koch <pierrick.koch@laas.fr>
 * created: 2013-06-12
 * license: BSD
 */
#include <string>
#include <cmath>
#include <ctime>
#include <limits>

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

#include "gladys/nav_graph.hpp"

namespace gladys {

void nav_graph::_load() {
    float weight;
    vertex_t vert_w, vert_n, vert_e, vert_s;
    const gdalwrap::raster& weight_map = map.get_weight_band();
    // most of the time this is equal to sqrt(2)/2
    float hypotenuse = 0.5 * std::sqrt( scale_x*scale_x + scale_y*scale_y );

    // XXX wrong, it must come from the under layer
    time_t t = std::time(0);

    double utm_x = map.get_utm_pose_x(),
           utm_y = map.get_utm_pose_y();

    for (size_t px_x = 0; px_x < width;  px_x++)
    for (size_t px_y = 0; px_y < height; px_y++) {
        // weight is a float in seconds per meter
        // or > if obstacle (+inf)
        // or < if unknown
        weight = weight_map[px_x + px_y * width];

        // if unknown, then weight is max (100)
        // in order to allow exploration plan in unknown areas.
        if (weight <= 0)
            weight = 100.0;

        vert_w = get_vertex_or_create(utm_x + scale_x * (px_x - 0.5), utm_y + scale_y * (px_y      ));
        vert_n = get_vertex_or_create(utm_x + scale_x * (px_x      ), utm_y + scale_y * (px_y - 0.5));
        // new vertex
        vert_e = new_vertex          (utm_x + scale_x * (px_x + 0.5), utm_y + scale_y * (px_y      ));
        vert_s = new_vertex          (utm_x + scale_x * (px_x      ), utm_y + scale_y * (px_y + 0.5));

        if ( weight == std::numeric_limits<float>::infinity() ) // OBSTACLE
            continue;

        // create edges and set weight
        // length = .5 * math.sqrt( scale_x**2 + scale_y**2 )
        edge e;
        e.t = t;
        e.weight = hypotenuse * weight;
        boost::add_edge(vert_w, vert_n, e, g);
        boost::add_edge(vert_n, vert_e, e, g);
        boost::add_edge(vert_e, vert_s, e, g);
        boost::add_edge(vert_s, vert_w, e, g);
        // also add straight connexions
        e.weight = std::abs(scale_y) * weight;
        boost::add_edge(vert_n, vert_s, e, g); // length = scale_y
        e.weight = std::abs(scale_x) * weight;
        boost::add_edge(vert_w, vert_e, e, g); // length = scale_x
    }
}

path_t nav_graph::astar_search(const point_xy_t& start, const point_xy_t& goal) const {
    vertex_t goal_v = get_closest_vertex(goal);
    astar_goal_visitor vis(goal_v);
    path_t shortest_path;
    nav_goal_heuristic heuristic(g, goal_v);
    std::vector<vertex_t> predecessors(num_vertices(g));
    std::vector<double> distances(boost::num_vertices(g));
    std::vector<double> ranks(boost::num_vertices(g), -1.0);
    std::vector<boost::default_color_type> colors(boost::num_vertices(g));
    try {
        boost::astar_search(
            g, get_closest_vertex(start), heuristic,
            boost::predecessor_map(predecessors.data()).
                distance_map(distances.data()).
                weight_map(boost::get(&edge::weight, g)).
                rank_map(ranks.data()).
                color_map(colors.data()).
                visitor(vis)
        );
    } catch (found_goal) {
        for(vertex_t v = goal_v;; v = predecessors[v]) {
            shortest_path.push_front(g[v].pt);
            if (predecessors[v] == v)
                break;
        }
    }
    return shortest_path;
}

detailed_path_t nav_graph::detailed_astar_search(const point_xy_t& start, const point_xy_t& goal) const {
    detailed_path_t res;

    vertex_t goal_v = get_closest_vertex(goal);
    astar_goal_visitor vis(goal_v);
    nav_goal_heuristic heuristic(g, goal_v);

    std::vector<vertex_t> predecessors(num_vertices(g));
    std::vector<double> distances(boost::num_vertices(g));
    std::vector<double> ranks(boost::num_vertices(g), -1.0);
    std::vector<boost::default_color_type> colors(boost::num_vertices(g));

    try {
        boost::astar_search(
            g, get_closest_vertex(start), heuristic,
            boost::predecessor_map(predecessors.data()).
                distance_map(distances.data()).
                weight_map(boost::get(&edge::weight, g)).
                rank_map(ranks.data()).
                color_map(colors.data()).
                visitor(vis)
        );
    } catch (found_goal) {
        for(vertex_t v = goal_v;; v = predecessors[v]) {
            res.path.push_front(g[v].pt);
            res.costs.push_front( distances[ v ]);
            if (predecessors[v] == v)
                break;
        }
    }
    return res;
}

path_cost_util_t nav_graph::astar_search(const points_t& start, const points_t& goal) const {
    vertex_t gv;
    vertices_t goal_v;
    for (auto& p : goal)
        goal_v.push_back( get_closest_vertex( p ) );

    astar_goals_visitor vis(goal_v);

    path_t shortest_path;
    nav_goals_heuristic heuristic(g, goal_v);
    std::vector<vertex_t> predecessors(num_vertices(g));
    std::vector<double> distances(boost::num_vertices(g));
    std::vector<double> ranks(boost::num_vertices(g), -1.0);
    std::vector<boost::default_color_type> colors(boost::num_vertices(g));
    try {
        // NOTE: pass by a "virtual node" as a starting point in the OPEN list (see: color map)
        boost::astar_search(
            g, get_closest_vertex(start[0]), heuristic,
            boost::predecessor_map(predecessors.data()).
                distance_map(distances.data()).
                weight_map(boost::get(&edge::weight, g)).
                rank_map(ranks.data()).
                color_map(colors.data()).
                visitor(vis)
        );
    } catch (found_goal& e) {
        gv = e.g ;
        for(vertex_t v = e.g;; v = predecessors[v]) {
            shortest_path.push_front(g[v].pt);
            if (predecessors[v] == v)
                break;
        }
    }
    path_cost_util_t res;
    res.path = shortest_path;
    if (res.path.size() == 0 ) // no path_found
        res.cost = std::numeric_limits<float>::infinity();
    else
        res.cost = distances[ gv ];
    return res;
}

std::vector<double> nav_graph::single_source_all_costs(const point_xy_t& start, const points_t& goals){

    std::vector<vertex_t> predecessors(num_vertices(g));
    std::vector<double> distances(boost::num_vertices(g));

    dijkstra_shortest_paths(g, get_closest_vertex(custom_to_utm(start)),
            boost::predecessor_map(boost::make_iterator_property_map(predecessors.begin(), get(boost::vertex_index, g))).
                    distance_map(boost::make_iterator_property_map(distances.begin(), get(boost::vertex_index, g))).
                    weight_map(boost::get(&edge::weight, g)));

    std::vector<double> retval;
    boost::property_map<graph_t, boost::vertex_index_t>::type index = get(boost::vertex_index, g);
    for(auto pt : goals){
        unsigned int i = index[get_closest_vertex(custom_to_utm(pt))];
        if(predecessors[i] == i){
            retval.push_back(std::numeric_limits<double>::infinity());
        } else {
            retval.push_back(distances[i]);
        }
    }

    return retval;
}

void nav_graph::write_graphviz(std::ostream& out) const {
    std::vector<std::string> vert_label(vertices.size());
    for (auto& kv : vertices)
        vert_label[kv.second] = to_string(kv.first);
    // TODO write edge weight as well
    boost::write_graphviz( out, g,
        boost::make_label_writer(vert_label.data()) );
}

void nav_graph::write_graphviz(const std::string& filepath) const {
    std::ofstream of( filepath );
    write_graphviz( of );
    of.close();
}

} // namespace gladys


#include <cassert>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <random>
#include <stdexcept>
#include <string>
#include <tuple>
#include <utility>
#include <vector>

#include <boost/geometry.hpp>
#include <csv.h>

#include "DataStructures/Geometry/SummedAreaTables/OctagonalSummedAreaTable.h"
#include "DataStructures/Geometry/Area.h"
#include "DataStructures/Geometry/CoordinateTransformation.h"
#include "DataStructures/Geometry/Point.h"
#include "DataStructures/Graph/Attributes/LatLngAttribute.h"
#include "DataStructures/Graph/Attributes/SequentialVertexIdAttribute.h"
#include "DataStructures/Graph/Graph.h"
#include "DataStructures/Utilities/Matrix.h"
#include "DataStructures/Utilities/OriginDestination.h"
#include "Tools/CommandLine/CommandLineParser.h"
#include "Tools/CommandLine/ProgressBar.h"
#include "Tools/Constants.h"
#include "Tools/Math.h"
#include "Tools/Timer.h"

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

void printUsage() {
  std::cout <<
      "Usage: RadiationModel [-s <factor>] -g <file> -r <file> -grid <file> -o <file>\n"
      "Generates travel demand data for the specified road network according to the\n"
      "radiation model, using the German Census 2011 population grid.\n"
      "  -s <factor>       scale the population by <factor> (defaults to 1.0)\n"
      "  -d <meters>       the max. distance between a cell's center and mapped segment\n"
      "  -seed <seed>      start the random number generator with <seed>\n"
      "  -g <file>         the network under study in binary format\n"
      "  -r <file>         restrict origins/destinations to area from OSM POLY file\n"
      "  -grid <file>      the German Census 2011 population grid\n"
      "  -o <file>         place output in <file>\n"
      "  -help             display this help and exit\n";
}

int main(int argc, char* argv[]) {
  try {
    CommandLineParser clp(argc, argv);
    if (clp.isSet("help")) {
      printUsage();
      return EXIT_SUCCESS;
    }

    const auto areaFilename = clp.getValue<std::string>("r");
    const auto gridFilename = clp.getValue<std::string>("grid");
    const auto graphFilename = clp.getValue<std::string>("g");
    const auto outputFilename = clp.getValue<std::string>("o");
    const double scale = clp.getValue<double>("s", 1.0);
    const int maxDist = clp.getValue<int>("d", 5000);

    // Read the graph from file.
    std::ifstream graphFile(graphFilename, std::ios::binary);
    if (!graphFile.good())
      throw std::invalid_argument("file not found -- '" + graphFilename + "'");
    StaticGraph<VertexAttrs<LatLngAttribute, SequentialVertexIdAttribute>> graph(graphFile);
    graphFile.close();

    if (graph.numVertices() > 0 && graph.sequentialVertexId(0) == INVALID_VERTEX)
      FORALL_VERTICES(graph, v)
        graph.sequentialVertexId(v) = v;

    // Read the population grid from file.
    std::vector<std::pair<::Point, int>> gridCells;
    ::Point minCell(INFTY, INFTY);
    ::Point maxCell(-INFTY, -INFTY);
    ::Point cell;
    int numInhabitants;
    io::CSVReader<3, io::trim_chars<>, io::no_quote_escape<';'>> gridFile(gridFilename);
    gridFile.read_header(io::ignore_extra_column, "x_mp_100m", "y_mp_100m", "Einwohner");
    while (gridFile.read_row(cell.x(), cell.y(), numInhabitants))
      if (numInhabitants > 0) {
        gridCells.emplace_back(cell, numInhabitants);
        minCell.min(cell);
        maxCell.max(cell);
      }

    // Allocate and fill the population matrix.
    const auto gridBounds = maxCell - minCell;
    const int numRows = gridBounds.y() / 100 + 1;
    const int numCols = gridBounds.x() / 100 + 1;
    Matrix<int> populationGrid(numRows, numCols, 0);
    for (const auto& cell : gridCells) {
      const auto pos = cell.first - minCell;
      populationGrid(numRows - pos.y() / 100 - 1, pos.x() / 100) = cell.second;
    }

    // Build an R-tree storing the road segments.
    using Point = bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree>>;
    using Segment = bg::model::segment<Point>;
    using RoadSegment = std::tuple<Segment, int, int>;
    std::vector<RoadSegment> roadSegments;
    FORALL_VALID_EDGES(graph, u, e) {
      const int v = graph.edgeHead(e);
      const Point tail(graph.latLng(u).lngInDeg(), graph.latLng(u).latInDeg());
      const Point head(graph.latLng(v).lngInDeg(), graph.latLng(v).latInDeg());
      roadSegments.emplace_back(Segment(tail, head), u, v);
    }
    bgi::rtree<RoadSegment, bgi::quadratic<16>> rTree(roadSegments);

    // Map each cell C inside the area under study to the road segment nearest to the center of C.
    const auto sourceCrs = CoordinateTransformation::WGS_84;
    const auto targetCrs = CoordinateTransformation::ETRS89_LAEA_EUROPE;
    CoordinateTransformation trans(sourceCrs, targetCrs);
    Area area;
    area.importFromOsmPolyFile(areaFilename);
    const auto box = area.boundingBox();
    LatLng nw(box.northEast().y(), box.southWest().x());
    LatLng se(box.southWest().y(), box.northEast().x());
    double easting, northing;
    trans.forward(nw.lngInDeg(), nw.latInDeg(), easting, northing);
    const auto min = ::Point(std::round(easting), std::round(northing)) - minCell;
    trans.forward(se.lngInDeg(), se.latInDeg(), easting, northing);
    const auto max = ::Point(std::round(easting), std::round(northing)) - minCell;
    const ::Point minCoveredCell((min.x() + 50) / 100, numRows - (min.y() + 50) / 100 - 1);
    const ::Point maxCoveredCell((max.x() + 50) / 100, numRows - (max.y() + 50) / 100 - 1);
    Matrix<int> representative(numRows, numCols, INVALID_VERTEX);
    int population = 0;
    int numUnmappedCells = 0;
    for (int x = minCoveredCell.x(); x <= maxCoveredCell.x(); ++x)
      for (int y = minCoveredCell.y(); y <= maxCoveredCell.y(); ++y) {
        const auto c = ::Point(x * 100, (numRows - y - 1) * 100) + minCell;
        double lng, lat;
        trans.reverse(c.x(), c.y(), lng, lat);
        const LatLng center(lat, lng);
        if (area.contains({center.longitude(), center.latitude()})) {
          population += populationGrid(y, x);
          const Point queryPoint(center.lngInDeg(), center.latInDeg());
          RoadSegment nearestRoadSegment;
          rTree.query(bgi::nearest(queryPoint, 1), &nearestRoadSegment);
          Segment segment;
          int u, v;
          std::tie(segment, u, v) = nearestRoadSegment;
          if (bg::distance(queryPoint, segment) * EARTH_RADIUS <= maxDist) {
            const auto distToU = bg::comparable_distance(queryPoint, segment.first);
            const auto distToV = bg::comparable_distance(queryPoint, segment.second);
            representative(y, x) = distToU < distToV && graph.containsEdge(v, u) ? u : v;
          } else {
            ++numUnmappedCells;
          }
        }
      }

    // Open the output file.
    std::ofstream out(outputFilename + ".csv");
    if (!out.good())
      throw std::invalid_argument("file cannot be opened -- '" + outputFilename + ".csv'");
    out << "# Input graph: " << graphFilename << '\n';
    out << "# Methodology: radiation model (" << scale << ")\n";
    out << "origin,destination\n";

    // Generate trips between the grid cells.
    Timer timer;
    std::cout << "Generating trips: ";
    const auto coveredGridBounds = maxCoveredCell - minCoveredCell;
    ProgressBar bar((coveredGridBounds.x() + 1) * (coveredGridBounds.y() + 1));
    OctagonalSummedAreaTable sat(populationGrid);
    std::vector<OriginDestination> result;
    std::default_random_engine rand(clp.getValue<int>("seed", 19900325));
    for (int srcX = minCoveredCell.x(); srcX <= maxCoveredCell.x(); ++srcX)
      for (int srcY = minCoveredCell.y(); srcY <= maxCoveredCell.y(); ++srcY) {
        if (representative(srcY, srcX) != INVALID_VERTEX && populationGrid(srcY, srcX) > 0)
          for (int dstX = minCoveredCell.x(); dstX <= maxCoveredCell.x(); ++dstX)
            for (int dstY = minCoveredCell.y(); dstY <= maxCoveredCell.y(); ++dstY)
              if (representative(dstY, dstX) != INVALID_VERTEX && populationGrid(dstY, dstX) > 0) {
                if (srcX == dstX && srcY == dstY)
                  continue;
                const ::Point src(srcX, srcY);
                const ::Point dst(dstX, dstY);
                const double radius = src.getEuclideanDistanceTo(dst);
                int64_t srcPop = populationGrid(srcY, srcX);
                int64_t dstPop = populationGrid(dstY, dstX);
                int64_t surroundingPop = sat.sumOverOctagon(src, radius);

                // Does the surrounding population include the destination population?
                const int height = std::round(0.92387953251128675613 * radius); // r * cos(pi / 8)
                const int side = std::round(0.38268343236508977173 * radius);   // r * sin(pi / 8)
                if (src.getManhattanDistanceTo(dst) <= height + side &&
                    src.getChebyshevDistanceTo(dst) <= height)
                  surroundingPop -= dstPop;
                assert(surroundingPop > 0);

                // Pick the number of trips between the source and destination cell.
                double p = 1.0 * srcPop * dstPop / (surroundingPop * (dstPop + surroundingPop));
                assert(p > 0); assert(p <= 1);
                int numTrips = std::binomial_distribution<>(std::round(scale * srcPop), p)(rand);

                // Generate the chosen number of trips.
                for (int i = 0; i < numTrips; ++i)
                  result.emplace_back(representative(srcY, srcX), representative(dstY, dstX));
              }
        ++bar;
      }
    const int elapsed = timer.elapsed();
    std::cout << "done (" << elapsed << "ms)." << std::endl;

    std::cout << "Writing OD-pairs to file..." << std::flush;
    for (const auto& record : result) {
      out << graph.sequentialVertexId(record.origin) << ',';
      out << graph.sequentialVertexId(record.destination) << '\n';
    }
    std::cout << " done." << std::endl;
  } catch (std::exception& e) {
    std::cerr << argv[0] << ": " << e.what() << std::endl;
    std::cerr << "Try '" << argv[0] <<" -help' for more information." << std::endl;
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_CONSTANTS_20_PI_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_20_PI_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>
namespace nt2
{
  namespace tag
  {
   /*!
     @brief _20_pi generic tag

     Represents the _20_pi constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( _20_pi, double
                                , 63, 0x427b53d1       //20\pi
                                , 0x404f6a7a2955385ell  //20\pi
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::_20_pi, Site> dispatching__20_pi(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::_20_pi, Site>();
   }
   template<class... Args>
   struct impl__20_pi;
  }
  /*!
    Constant \f$20\pi\f$.

    @par Semantic:

    For type T0:

    @code
    T0 r = _20_pi<T0>();
    @endcode

    is similar to:

    @code
    T0 r = Twenty<T0>()*Pi<T0>();
    @endcode

    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::_20_pi, _20_pi);
}

#endif


/// ---------------------------------------------------------------------------
/// @section LICENSE
///
/// Copyright (c) 2016 Georgia Tech Research Institute (GTRI)
///               All Rights Reserved
///
/// 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.
/// ---------------------------------------------------------------------------
/// @file filename.ext
/// @author Kevin DeMarco <kevin.demarco@gtri.gatech.edu>
/// @author Eric Squires <eric.squires@gtri.gatech.edu>
/// @version 1.0
/// ---------------------------------------------------------------------------
/// @brief A brief description.
///
/// @section DESCRIPTION
/// A long description.
/// ---------------------------------------------------------------------------
#include <gtest/gtest.h>
#include <gmock/gmock.h>
#include <scrimmage/math/State.h>
#include <Eigen/Dense>
#define _USE_MATH_DEFINES
#include <cmath>

namespace sc = scrimmage;
using Eigen::Vector3d;

TEST(test_state, local_frames) {

    sc::State state1, state2;
    state1.pos() = Vector3d(1, 0, 0);
    state1.quat().set(M_PI, 0, 0);

    state2.pos() = Vector3d(1, 0, 1);
    state2.quat().set(0, 0, 0);
    Vector3d position_local_to_state1 = \
        state1.rel_pos_local_frame(state2.pos());

    EXPECT_NEAR(0, position_local_to_state1(0), 1e-10);
    EXPECT_NEAR(0, position_local_to_state1(1), 1e-10);
    EXPECT_NEAR(-1, position_local_to_state1(2), 1e-10);

    Vector3d position_local_to_state2 = \
        state2.rel_pos_local_frame(state1.pos());

    EXPECT_NEAR(0, position_local_to_state2(0), 1e-10);
    EXPECT_NEAR(0, position_local_to_state2(1), 1e-10);
    EXPECT_NEAR(-1, position_local_to_state2(2), 1e-10);
}

TEST(test_state, rel_az) {
    sc::State state;
    state.pos() << 0, 0, 0;
    state.vel() << 0, 0, 0;
    state.quat().set(M_PI / 4, M_PI / 4, M_PI / 4);

    Eigen::Vector3d vec1(1, 0, 50);
    double rel_az1 = state.rel_az(vec1);

    Eigen::Vector3d vec2(0, 1, 50);
    double rel_az2 = state.rel_az(vec2);

    EXPECT_NEAR(-M_PI / 4, rel_az1, 1e-10);
    EXPECT_NEAR(M_PI / 4, rel_az2, 1e-10);
}