///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::data::mngr::default_covariates_model.hpp      //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_SURVIVAL_DATA_DATA_MNGR_DEFAULT_COVARIATES_MODEL_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_DATA_MNGR_DEFAULT_COVARIATES_MODEL_HPP_ER_2009
#include <algorithm>
#include <iterator>
#include <boost/statistics/model/wrap/unary/model.hpp>
#include <boost/statistics/survival/data/data/mngr/default_covariates.hpp>

namespace boost{
namespace statistics{
namespace survival{
namespace data{

    // Combines x_values and model into one class.
    //
    // In generating batches of data, one for each of a set of model parameters,
    // using default_batch, these are two fixed components of the generation
    // process, hence the rationale for combining them.
    template<typename X,typename M>
    class default_covariates_model_mngr
        : public default_covariates_mngr<X>{
        typedef default_covariates_mngr<X> super_;
        public:
        typedef model::model_wrapper<M> model_wrapper_type;
        typedef std::vector<X>   x_values_type;

        // [ Construction ]
        default_covariates_model_mngr();
        template<typename It> default_covariates_model_mngr(
            It b_x,It e_x,      // x values
            model_wrapper_type
        );

        // [ Access ]
        model_wrapper_type model_wrapper()const;

        protected:
        // [ Archive ]
        friend class boost::serialization::access;

        template<class Archive>
        void serialize(Archive & ar, const unsigned int version);

        // [ Variables ]
        M m;
    };

    // [ Construction ]
    template<typename X,typename M>
    default_covariates_model_mngr<X,M>::default_covariates_model_mngr(){}

    template<typename X,typename M>
    template<typename It>
    default_covariates_model_mngr<X,M>::default_covariates_model_mngr(
        It b_x,It e_x,
        model_wrapper_type w
    ):super_(b_x,e_x),m(w.model()){}

    // [ Access ]
    template<typename X,typename M>
    typename default_covariates_model_mngr<X,M>::model_wrapper_type
    default_covariates_model_mngr<X,M>::model_wrapper()const
    {
        return model_wrapper_type(this->m);
    }

    // [ Archive ]
    template<typename X,typename M>
    template<class Archive>
    void default_covariates_model_mngr<X,M>::serialize(
        Archive & ar,
        const unsigned int version
    )
    {
        ar & this->x_values_;
        ar & this->m;
    }

}// data
}// survival
}// statistics
}// boost

#endif


//==============================================================================
//         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
//==============================================================================
#include <nt2/combinatorial/include/functions/factorial.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <vector>
#include <nt2/include/functions/gammaln.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/round.hpp>
#include <nt2/boost_math/include/functions/factorial.hpp>
#include <nt2/include/functions/toint.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/min.hpp>

#include <nt2/sdk/unit/args.hpp>
#include <nt2/sdk/unit/tests/cover.hpp>
#include <nt2/sdk/unit/module.hpp>

NT2_TEST_CASE_TPL(factorial,  NT2_REAL_TYPES)
{
  using nt2::unit::args;
  const std::size_t NR = args("samples", NT2_NB_RANDOM_TEST);
  const double ulpd = args("ulpd", 0);

  const T min = args("min", T(0));
  const T max = args("max", T(10));
  std::cout << "Argument samples a0 chosen in range: [" << min << ",  " << max << "]" << std::endl;
  std::cout << "Argument samples a1 chosen in range: [ a0,  a0+" << max << "]" << std::endl;
  NT2_CREATE_BUF(a0,T, NR, min, max);
  NT2_CREATE_BUF(a1,T, NR, min, max);

  std::vector<T> ref(NR);
  for(std::size_t i=0; i!=NR; ++i)
  {
    a0[i] = nt2::round(a0[i]);
    ref[i] = nt2::boost_math::factorial<double>(nt2::abs(nt2::toint(a0[i])));
  }
  NT2_COVER_ULP_EQUAL(nt2::tag::factorial_, ((T, a0)), ref, ulpd);
}

NT2_TEST_CASE_TPL(factorial_int,  NT2_INTEGRAL_TYPES)
{
  using nt2::unit::args;
  const std::size_t NR = args("samples", NT2_NB_RANDOM_TEST);
  const double ulpd = args("ulpd", 0);

  const T min = args("min", T(0));
  const T max = args("max", T(10));
  std::cout << "Argument samples a0 chosen in range: [" << int(min) << ",  " << int(max) << "]" << std::endl;
  std::cout << "Argument samples a1 chosen in range: [ a0,  a0+" << max << "]" << std::endl;
  NT2_CREATE_BUF(a0,T, NR, min, max);
  NT2_CREATE_BUF(a1,T, NR, min, max);

  std::vector<T> ref(NR);
  for(std::size_t i=0; i!=NR; ++i)
  {
    a0[i]  = nt2::round(a0[i]);
    ref[i] = nt2::min((uint64_t)nt2::min(double(nt2::Valmax<T>()),
                                         nt2::boost_math::factorial<double>(nt2::abs(a0[i]))),
                      (uint64_t)nt2::Valmax<T>());
  }
  NT2_COVER_ULP_EQUAL(nt2::tag::factorial_, ((T, a0)), ref, ulpd);

}


/*

Software License Agreement (BSD License)

Copyright (c) 2016--, Liana Bertoni (liana.bertoni@gmail.com)
  All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* 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.

* Neither the name of the copyright holder(s) 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.
Contact GitHub API Training Shop Blog About
*/

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

#include <boost/scoped_ptr.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include <kdl_parser/kdl_parser.hpp>
#include <urdf/model.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <string>

#include <math.h>
#include <stdio.h>
#include <ctime>
#include <time.h>

#include <rviz_visual_tools/rviz_visual_tools.h>

using namespace std;
using namespace Eigen;
using namespace KDL;

//////////////////////////////////	BOX

Eigen::VectorXd box(3);
////////////////////////////////////////////////

/////////////////////////////////  CONTACT POINT

Eigen::VectorXd cp(7);
////////////////////////////////////////////////

int main (int argc, char **argv)
{

	ros::init(argc, argv, "show_hand_point");	// ROS node
	ros::NodeHandle nh;

	string relative_path_file_in;
	string file_name_in;

	string relative_path_file_hand;
	string file_name_hand;

	nh.param<std::string>("file_name_in", relative_path_file_in, "/box_test/test" );
	nh.param<std::string>("file_name_hand_point", relative_path_file_hand, "/hand_point/hand_points");

	////////////////////////////////////// load the data_base /////////////////////////////////////
	std::string path = ros::package::getPath("grasp_learning");

	file_name_in = path + relative_path_file_in;
	ifstream file_in(file_name_in);

	std::cout << "file: " << file_name_in.c_str() << " is " << (file_in.is_open() == true ? "already" : "not") << " open" << std::endl;
	if(!file_in.is_open())
	return 0;

	file_name_hand = path + relative_path_file_hand;
	ifstream file_hand(file_name_hand);

	std::cout << "file: " << file_name_hand.c_str() << " is " << (file_hand.is_open() == true ? "already" : "not") << " open" << std::endl;
	if(!file_hand.is_open())
	return 0;
	/////////////////////////////////////////////////////////////////////////////////////////////////

    //////////////////////////////////////////////////////////////////////////// TAKE HAND POINT
    std::vector<Eigen::VectorXd> hp; //hand point , each point is a pose ( 7 var , position and orientation with quaternion)

    for(std::string line; getline( file_hand, line, '\n' ); )
	{
		std::vector<double> values_inline;
    	std::istringstream iss_line(line);
    	for(std::string value; getline(iss_line, value, ' ' ); )
    		values_inline.push_back(stod(value));

    	Eigen::VectorXd t(7);

    	double x = values_inline[0];
    	double y = values_inline[1];
    	double z = values_inline[2];

    	double qx = values_inline[3];
    	double qy = values_inline[4];
    	double qz = values_inline[5];
    	double qw = values_inline[6];

    	t(0) = x;
    	t(1) = y;
    	t(2) = z;
    	t(3) = qx;
    	t(4) = qy;
    	t(5) = qz;
    	t(6) = qw;

    	hp.push_back(t);
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////

  	////////////////////////////////////////////////////////////////////////////// TAKE GRASP POINT
    std::vector<std::vector<KDL::Frame>> f_point;
    std::vector<KDL::Frame> fcp; // take a grasp point from a dataset

    for(std::string line; getline( file_in, line, '\n' ); )
	{
		std::vector<double> values_inline;
    	std::istringstream iss_line(line);
    	for(std::string value; getline(iss_line, value, ' ' ); )
    		values_inline.push_back(stod(value));

    	box(0) = values_inline[0];
    	box(1) = values_inline[1];
    	box(2) = values_inline[2];

    	double px = values_inline[3]; //px
		double py = values_inline[4]; //py
		double pz = values_inline[5]; //pz

		double qx = values_inline[6]; //qx
		double qy = values_inline[7]; //qy
		double qz = values_inline[8]; //qz
		double qw = values_inline[9]; //qw

		KDL::Vector t_cp(px,py,pz);
		KDL::Rotation R_cp = Rotation::Quaternion(qx,qy,qz,qw);
		//KDL::Rotation R_cp = Rotation::Quaternion(0,0,0,1);

		KDL::Frame f_cp(R_cp,t_cp);

		fcp.push_back(f_cp);

		std::vector<KDL::Frame> f_hand;

		for(int i = 0; i < hp.size(); i++)
		{
			KDL::Vector t_hp(hp[i](0), hp[i](1), hp[i](2));
			KDL::Rotation R_hp = Rotation::Quaternion(hp[i](3),hp[i](4),hp[i](5),hp[i](6));
			KDL::Frame f_hp(R_hp, t_hp);

			KDL::Frame f = f_cp*f_hp;

			KDL::Rotation R = Rotation::Quaternion(0,0,0,1);

			KDL::Frame f_hp_cp(R,f.p);

			f_hand.push_back(f_hp_cp);

			// f_hand.push_back(f);
		}

		f_point.push_back(f_hand);
    }

    cout << "fcp : " << fcp.size() << endl;
    cout << "f_point : " << f_point.size() << endl;
    cout << "f_hand_point : " << f_point[0].size() << endl;

    /////////////////////////////    SHOW RVIZ    ///////////////////////////////

    ////////////////////////////////////////////////////////////////////////////
	static tf::TransformBroadcaster tf_broadcaster;

	//////////////////////////////////		trasf world
	double px = 0;
	double py = 0;
	double pz = box(2)/2;

	double qx = 0;
	double qy = 0;
	double qz = 0;
	double qw = 1;

	tf::Quaternion rotazione(qx,qy,qz,qw);
    tf::Vector3 traslazione(px,py,pz);
    tf::Transform trasformazione(rotazione, traslazione);
    std::string stringaFrameIdPadre_box = "/world";
	std::string stringaFrameIdFiglio_box = "base_frame";

	tf::StampedTransform ObjToSurfaceBase(trasformazione, ros::Time::now(), stringaFrameIdPadre_box, stringaFrameIdFiglio_box);
	tf_broadcaster.sendTransform(ObjToSurfaceBase);
	/////////////////////////////////////////////////////////////////////////////

	///////////////////////////////////////////////////////////////      show BOX
	// For visualizing things in rviz FOR SHOW BOX
	rviz_visual_tools::RvizVisualToolsPtr visual_tools_box;
	visual_tools_box.reset(new rviz_visual_tools::RvizVisualTools("base_frame","/rviz_visual_markers_box"));

	geometry_msgs::Pose pose_;

	pose_.position.x = 0;
	pose_.position.y = 0;
	pose_.position.z = 0;

	pose_.orientation.x = 0;
	pose_.orientation.y = 0;
	pose_.orientation.z = 0;
	pose_.orientation.w = 1;

	double x_depth = box(0);
	double y_width = box(1);
	double z_height = box(2);

	Eigen::Quaterniond q_box(pose_.orientation.w, pose_.orientation.x, pose_.orientation.y, pose_.orientation.z);
    Eigen::Translation3d t_box(pose_.position.x, pose_.position.y, pose_.position.z);
    Eigen::Affine3d pose_box = Eigen::Affine3d::Identity() * t_box * q_box;
	////////////////////////////////////////////////////////////////////////////////////////////////

	ros::Rate loop_rate(0.5);

	while(nh.ok())
	{
		tf::StampedTransform ObjToSurfaceBase(trasformazione, ros::Time::now(), stringaFrameIdPadre_box, stringaFrameIdFiglio_box); // stamped always frame world
		tf_broadcaster.sendTransform(ObjToSurfaceBase);

		visual_tools_box->publishWireframeCuboid(pose_box,  x_depth,  y_width, z_height); // stamped always box
		//  //x_depth; // y_width // z_height

		/////////////////////////////////////////////////// contact frame
		for(int j = 0 ; j < f_point.size() ; j++)
		{
			for(int i = 0; i < f_point[33].size(); i++)
			{
				std::string stringaFrameIdPadre = "base_frame";
				std::string stringaFrameIdFiglio = "contact_frame_" + std::to_string(i);

				// //////////////////// // point where put my hand

				std::string stringaFrameIdPadre_ = "base_frame";
				std::string stringaFrameIdFiglio_ = "contact_frame_cp" ;

				double px_ = fcp[33].p.x();
				double py_ = fcp[33].p.y();
				double pz_ = fcp[33].p.z();

				double qx_ = 0;
				double qy_ = 0;
				double qz_ = 0;
				double qw_ = 1;

				fcp[33].M.GetQuaternion(qx_,qy_,qz_,qw_);

				tf::Quaternion rotazione_(qx_,qy_,qz_,qw_);
    			tf::Vector3 traslazione_(px_,py_,pz_);
    			tf::Transform trasformazione_(rotazione_, traslazione_);

				tf::StampedTransform ObjToSurfaceT(trasformazione_, ros::Time::now(), stringaFrameIdPadre_, stringaFrameIdFiglio_);
				tf_broadcaster.sendTransform(ObjToSurfaceT);
				// //////////////////// // ///////////////////////////// // ////////////////////////////////////////////////

				// //////////////////// // point where put my hand

				double px = f_point[33][i].p.x();
				double py = f_point[33][i].p.y();
				double pz = f_point[33][i].p.z();

				double qx = 0;
				double qy = 0;
				double qz = 0;
				double qw = 1;

				f_point[33][i].M.GetQuaternion(qx,qy,qz,qw);

				cout << "Position: [ px : " << px << " , py : " << py << " , pz : " << pz << " ]" << endl;
				cout << "Orientation : [ qx : " << qx << " , qy : " << qy << " , qz : " << qz << " , qw : " << qw << " ]" << endl;

				tf::Quaternion rotazione(qx,qy,qz,qw);
    			tf::Vector3 traslazione(px,py,pz);
    			tf::Transform trasformazione(rotazione, traslazione);

				tf::StampedTransform ObjToSurface(trasformazione, ros::Time::now(), stringaFrameIdPadre, stringaFrameIdFiglio);
				tf_broadcaster.sendTransform(ObjToSurface);
			}
		}

		visual_tools_box->triggerBatchPublish();
		ros::spinOnce();
		loop_rate.sleep();
	}

	ros::spinOnce();
	return 0;
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef INTEGRATORHELPERFUNCTIONS_HPP
#define INTEGRATORHELPERFUNCTIONS_HPP

#include <cmath>
#include <vector>

#include "Config.hpp"

#include <Eigen/Core>

#include <boost/mpl/at.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/map.hpp>

#include "cavity/Element.hpp"
#include "utils/QuadratureRules.hpp"

namespace integrator {
/*! \typedef Diagonal
 *  \brief functor handle to the calculation of the diagonal elements
 */
typedef pcm::function<double(const Element &)> Diagonal;

/*! \typedef KernelS
 *  \brief functor handle to the kernelS method in IGreensFunction
 */
typedef pcm::function<double(const Eigen::Vector3d &, const Eigen::Vector3d &)> KernelS;

/*! \typedef KernelD
 *  \brief functor handle to the kernelD method in IGreensFunction
 */
typedef pcm::function<double(const Eigen::Vector3d &, const Eigen::Vector3d &, const Eigen::Vector3d &)> KernelD;

/*! Approximate collocation diagonal for S
 *  \param[in] factor scaling factor for diagonal elements
 *  \param[in] eps    permittivity
 *  \param[in] el     finite element
 */
inline double SI(double factor, double eps, const Element & el)
{
    return (factor * std::sqrt(4 * M_PI / el.area()) * 1.0 / eps);
}

/*! Approximate collocation diagonal for D
 *  \param[in] factor scaling factor for diagonal elements
 *  \param[in] el     finite element
 */
inline double DI(double factor, const Element & el)
{
    return (-factor * std::sqrt(M_PI / el.area()) * 1.0 / el.sphere().radius);
}

/*! Returns matrix representation of the single layer operator by collocation
 *  \param[in] elements list of finite elements
 *  \param[in] diagS    functor for the evaluation of the diagonal of S
 *  \param[in] kernS    function for the evaluation of the off-diagonal of S
 */
inline Eigen::MatrixXd singleLayer(const std::vector<Element> & elements,
                                   const Diagonal & diagS, const KernelS & kernS)
{
    PCMSolverIndex mat_size = elements.size();
    Eigen::MatrixXd S = Eigen::MatrixXd::Zero(mat_size, mat_size);
    for (PCMSolverIndex i = 0; i < mat_size; ++i) {
        // Fill diagonal
        S(i, i) = diagS(elements[i]);
        Eigen::Vector3d source = elements[i].center();
        for (PCMSolverIndex j = 0; j < mat_size; ++j) {
            // Fill off-diagonal
            Eigen::Vector3d probe = elements[j].center();
            if (i != j) S(i, j) = kernS(source, probe);
        }
    }
    return S;
}

/*! Returns matrix representation of the double layer operator by collocation
 *  \param[in] elements list of finite elements
 *  \param[in] diagD    functor for the evaluation of the diagonal of D
 *  \param[in] kernD    function for the evaluation of the off-diagonal of D
 */
inline Eigen::MatrixXd doubleLayer(const std::vector<Element> & elements,
                                   const Diagonal & diagD, const KernelD & kernD)
{
    PCMSolverIndex mat_size = elements.size();
    Eigen::MatrixXd D = Eigen::MatrixXd::Zero(mat_size, mat_size);
    for (PCMSolverIndex i = 0; i < mat_size; ++i) {
        // Fill diagonal
        D(i, i) = diagD(elements[i]);
        Eigen::Vector3d source = elements[i].center();
        for (PCMSolverIndex j = 0; j < mat_size; ++j) {
            // Fill off-diagonal
            Eigen::Vector3d probe = elements[j].center();
            Eigen::Vector3d probeNormal = elements[j].normal();
            probeNormal.normalize();
            if (i != j) D(i, j) = kernD(probeNormal, source, probe);
        }
    }
    return D;
}

/*! \brief Integrates a single layer type operator on a single spherical polygon
 *  \date 2014
 *  \tparam PhiPoints Gaussian rule to be used in the angular phi integration
 *  \tparam ThetaPoints Gaussian rule to be used in the angular theta integration
 *
 *  This is needed for the numerical evaluation of the diagonal elements when using
 *  centroid collocation.
 */
template <int PhiPoints, int ThetaPoints>
double integrateS(const KernelS & F, const Element & e)
{
    double result = 0.0;

    // Get the quadrature rules for azimuthal and polar integrations
    namespace mpl = boost::mpl;
    typedef typename mpl::at<rules_map, mpl::int_<PhiPoints> >::type PhiPolicy;
    typedef typename mpl::at<rules_map, mpl::int_<ThetaPoints> >::type ThetaPolicy;
    QuadratureRule<PhiPolicy> phiRule;
    QuadratureRule<ThetaPolicy> thetaRule;
    int upper_phi = PhiPoints / 2; // Upper limit for loop on phi points
    int upper_theta = ThetaPoints / 2; // Upper limit for loop on theta points

    // Extract relevant data from Element
    int nVertices = e.nVertices();
    Eigen::Vector3d normal = e.normal();
    Sphere sph = e.sphere();
    Eigen::Matrix3Xd vertices = e.vertices();
    Eigen::Matrix3Xd arcs = e.arcs();

    // Calculation of the tangent and the bitangent (binormal) vectors
    // Tangent, Bitangent and Normal form a local reference frame:
    // T <-> x; B <-> y; N <-> z
    Eigen::Vector3d tangent, bitangent;
    tangent_and_bitangent(normal, tangent, bitangent);

    std::vector<double> theta(nVertices), phi(nVertices), phinumb(nVertices+1);
    std::vector<int> numb(nVertices+1);
    // Clean-up heap crap
    std::fill_n(theta.begin(),   nVertices,   0.0);
    std::fill_n(phi.begin(),     nVertices,   0.0);
    std::fill_n(numb.begin(),    nVertices+1, 0);
    std::fill_n(phinumb.begin(), nVertices+1, 0.0);
    // Populate arrays and redefine tangent and bitangent
    e.spherical_polygon(tangent, bitangent, theta, phi, phinumb, numb);

    // Actual integration occurs here
    for (int i = 0; i < nVertices; ++i) { // Loop on edges
        double phiLower = phinumb[i];
        double phiUpper = phinumb[i+1];
        double phiA= (phiUpper - phiLower) / 2.0;
        double phiB = (phiUpper + phiLower) / 2.0;
        double thetaLower = theta[numb[i]];
        double thetaUpper = theta[numb[i+1]];
        double thetaMax = 0.0;
        Eigen::Vector3d oc = (arcs.col(i) - sph.center) / sph.radius;
        double oc_norm = oc.norm();
        double oc_norm2 = std::pow(oc_norm, 2);
        for (int j = 0; j < upper_phi; ++j) { // Loop on Gaussian points: phi integration
            for (int k = 0; k <= 1; ++k) {
                double ph = (2*k - 1) * phiA * phiRule.abscissa(j) + phiB;
                double cos_ph = std::cos(ph);
                double sin_ph = std::sin(ph);
                // We need to calculate the upper bound for the integration on theta, which depends on phi
                if (oc_norm2 < 1.0e-07) { // This means that the edge is centered on the same sphere the tessera belongs to
                    double cotg_thmax = (std::sin(ph-phiLower) / std::tan(thetaUpper) + std::sin(phiUpper-ph) / std::tan(
                                thetaLower)) / std::sin(phiUpper - phiLower);
                    thetaMax = std::atan(1.0 / cotg_thmax);
                } else {
                    Eigen::Vector3d scratch;
                    scratch << tangent.dot(oc), bitangent.dot(oc), normal.dot(oc);
                    double aa = std::pow(tangent.dot(oc)*cos_ph + bitangent.dot(oc)*sin_ph,
                            2) + std::pow(normal.dot(oc), 2);
                    double bb = -normal.dot(oc) * oc_norm2;
                    double cc = std::pow(oc_norm2,
                            2) - std::pow(tangent.dot(oc)*cos_ph + bitangent.dot(oc)*sin_ph, 2);
                    double ds = std::pow(bb, 2) - aa*cc;
                    if (ds < 0.0) ds = 0.0;
                    double cs = (-bb + std::sqrt(ds)) / aa;
                    if (cs > 1.0) cs = 1.0;
                    if (cs < -1.0) cs = 1.0;
                    thetaMax = std::acos(cs);
                }
                double scratch = 0.0;
                if (!(thetaMax < 1.0e-08)) {
                    double thetaA = thetaMax / 2.0;
                    for (int l = 0; l < upper_theta; ++l) { // Loop on Gaussian points: theta integration
                        for (int m = 0; m <= 1; ++m) {
                            double th = (2*m - 1) * thetaA * thetaRule.abscissa(l) + thetaA;
                            double cos_th = std::cos(th);
                            double sin_th = std::sin(th);
                            Eigen::Vector3d point;
                            point(0) = tangent(0) * sin_th * cos_ph
                                + bitangent(0) * sin_th * sin_ph
                                + normal(0) * (cos_th - 1.0);
                            point(1) = tangent(1) * sin_th * cos_ph
                                + bitangent(1) * sin_th * sin_ph
                                + normal(1) * (cos_th - 1.0);
                            point(2) = tangent(2) * sin_th * cos_ph
                                + bitangent(2) * sin_th * sin_ph
                                + normal(2) * (cos_th - 1.0);
                            double value = F(point,
                                    Eigen::Vector3d::Zero()); // Evaluate integrand at Gaussian point
                            scratch += std::pow(sph.radius, 2) * value * sin_th * thetaA * thetaRule.weight(l);
                        }
                    }
                    result += scratch * phiA * phiRule.weight(j);
                }
            }
        }
    }
    return result;
}

/*! \brief Integrates a double layer type operator on a single spherical polygon
 *  \date 2014
 *  \tparam PhiPoints Gaussian rule to be used in the angular phi integration
 *  \tparam ThetaPoints Gaussian rule to be used in the angular theta integration
 *
 *  This is needed for the numerical evaluation of the diagonal elements when using
 *  centroid collocation.
 */
template <int PhiPoints, int ThetaPoints>
double integrateD(const KernelD & F, const Element & e)
{
    double result = 0.0;

    // Get the quadrature rules for azimuthal and polar integrations
    namespace mpl = boost::mpl;
    typedef typename mpl::at<rules_map, mpl::int_<PhiPoints> >::type PhiPolicy;
    typedef typename mpl::at<rules_map, mpl::int_<ThetaPoints> >::type ThetaPolicy;
    QuadratureRule<PhiPolicy> phiRule;
    QuadratureRule<ThetaPolicy> thetaRule;
    int upper_phi = PhiPoints / 2; // Upper limit for loop on phi points
    int upper_theta = ThetaPoints / 2; // Upper limit for loop on theta points

    // Extract relevant data from Element
    int nVertices = e.nVertices();
    Eigen::Vector3d normal = e.normal();
    Sphere sph = e.sphere();
    Eigen::Matrix3Xd vertices = e.vertices();
    Eigen::Matrix3Xd arcs = e.arcs();

    // Calculation of the tangent and the bitangent (binormal) vectors
    // Tangent, Bitangent and Normal form a local reference frame:
    // T <-> x; B <-> y; N <-> z
    Eigen::Vector3d tangent, bitangent;
    tangent_and_bitangent(normal, tangent, bitangent);

    std::vector<double> theta(nVertices), phi(nVertices), phinumb(nVertices+1);
    std::vector<int> numb(nVertices+1);
    // Clean-up heap crap
    std::fill_n(theta.begin(),   nVertices,   0.0);
    std::fill_n(phi.begin(),     nVertices,   0.0);
    std::fill_n(numb.begin(),    nVertices+1, 0);
    std::fill_n(phinumb.begin(), nVertices+1, 0.0);
    // Populate arrays and redefine tangent and bitangent
    e.spherical_polygon(tangent, bitangent, theta, phi, phinumb, numb);

    // Actual integration occurs here
    for (int i = 0; i < nVertices; ++i) { // Loop on edges
        double phiLower = phinumb[i]; // Lower vertex of edge
        double phiUpper = phinumb[i+1]; // Upper vertex of edge
        double phiA = (phiUpper - phiLower) / 2.0;
        double phiB = (phiUpper + phiLower) / 2.0;
        double thetaLower = theta[numb[i]];
        double thetaUpper = theta[numb[i+1]];
        double thetaMax = 0.0;
        Eigen::Vector3d oc = (arcs.col(i) - sph.center) / sph.radius;
        double oc_norm = oc.norm();
        double oc_norm2 = std::pow(oc_norm, 2);
        for (int j = 0; j < upper_phi; ++j) { // Loop on Gaussian points: phi integration
            for (int k = 0; k <= 1; ++k) {
                double ph = (2*k - 1) * phiA * phiRule.abscissa(j) + phiB;
                double cos_phi = std::cos(ph);
                double sin_phi = std::sin(ph);
                if (oc_norm2 < 1.0e-07) { // This should check if oc_norm2 is zero
                    double cotg_thmax = (std::sin(ph-phiLower) / std::tan(thetaUpper) + std::sin(phiUpper-ph) / std::tan(
                                thetaLower)) / std::sin(phiUpper - phiLower);
                    thetaMax = std::atan(1.0 / cotg_thmax);
                } else {
                    Eigen::Vector3d scratch;
                    scratch << tangent.dot(oc), bitangent.dot(oc), normal.dot(oc);
                    double aa = std::pow(tangent.dot(oc)*cos_phi + bitangent.dot(oc)*sin_phi,
                            2) + std::pow(normal.dot(oc), 2);
                    double bb = -normal.dot(oc) * oc_norm2;
                    double cc = std::pow(oc_norm2,
                            2) - std::pow(tangent.dot(oc)*cos_phi + bitangent.dot(oc)*sin_phi, 2);
                    double ds = std::pow(bb, 2) - aa*cc;
                    if (ds < 0.0) ds = 0.0;
                    double cs = (-bb + std::sqrt(ds)) / aa;
                    if (cs > 1.0) cs = 1.0;
                    if (cs < -1.0) cs = 1.0;
                    thetaMax = std::acos(cs);
                }
                double thetaA = thetaMax / 2.0;
                double scratch = 0.0;
                if (!(thetaMax < 1.0e-08)) {
                    for (int l = 0; l < upper_theta; ++l) { // Loop on Gaussian points: theta integration
                        for (int m = 0; m <= 1; ++m) {
                            double th = (2*m - 1) * thetaA  * thetaRule.abscissa(l) + thetaA;
                            double cos_theta = std::cos(th);
                            double sin_theta = std::sin(th);
                            Eigen::Vector3d point;
                            point(0) = tangent(0) * sin_theta * cos_phi
                                + bitangent(0) * sin_theta * sin_phi
                                + normal(0) * (cos_theta - 1.0);
                            point(1) = tangent(1) * sin_theta * cos_phi
                                + bitangent(1) * sin_theta * sin_phi
                                + normal(1) * (cos_theta - 1.0);
                            point(2) = tangent(2) * sin_theta * cos_phi
                                + bitangent(2) * sin_theta * sin_phi
                                + normal(2) * (cos_theta - 1.0);
                            double value = F(e.normal(),
                                    Eigen::Vector3d::Zero(),
                                    point); // Evaluate integrand at Gaussian point
                            scratch += std::pow(sph.radius, 2) * value * sin_theta * thetaA * thetaRule.weight(l);
                        }
                    }
                    result += scratch * phiA * phiRule.weight(j);
                }
            }
        }
    }
    return result;
}
} // namespace integrator

#endif // INTEGRATORHELPERFUNCTIONS_HPP


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Astrodynamics/GroundStations/pointingAnglesCalculator.h"
#include "Tudat/Astrodynamics/GroundStations/groundStationState.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/sphericalBodyShapeModel.h"
#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/body.h"

using namespace tudat::simulation_setup;
using namespace tudat::basic_astrodynamics;
using namespace tudat::ground_stations;
using namespace tudat;

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_pointing_angles_calculator )

BOOST_AUTO_TEST_CASE( test_PointingAnglesCalculator )
{
    // Define Earth shape model (sphere)
    std::shared_ptr< SphericalBodyShapeModel > bodyShape = std::make_shared< SphericalBodyShapeModel >( 6.371E6 );

    // Define test ground station point.
    double groundStationDistance = 6371.0E3;
    Eigen::Vector3d groundStationPosition( groundStationDistance, 0.0, 0.0 );

    double degreesToRadians = unit_conversions::convertDegreesToRadians( 1.0 );

    // Test analytically checked azimuth and elevation
    {
        // Create ground station properties
        std::shared_ptr< GroundStationState > stationState = std::make_shared< GroundStationState >(
                    groundStationPosition, coordinate_conversions::cartesian_position, bodyShape );
        std::shared_ptr< PointingAnglesCalculator > pointAnglesCalculator = std::make_shared< PointingAnglesCalculator >(
                    [ & ]( const double ){ return Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ); },
                    std::bind( &GroundStationState::getRotationFromBodyFixedToTopocentricFrame, stationState, std::placeholders::_1 ) );

        // Define state of viewed point
        double testLatitude = 30.0 * degreesToRadians;
        double testLongitude = 0.0;
        double testRadius = 8.0E7;
        Eigen::Vector3d testSphericalPoint( testRadius, mathematical_constants::PI / 2.0 - testLatitude, testLongitude );
        Eigen::Vector3d testCartesianPoint = coordinate_conversions::convertSphericalToCartesian( testSphericalPoint );

        // Compute azimuth/elevation angles from PointingAnglesCalculator
        double testAzimuth = pointAnglesCalculator->calculationAzimuthAngle( testCartesianPoint, 0.0 );
        double testElevation = pointAnglesCalculator->calculateElevationAngle( testCartesianPoint, 0.0 );

        double expectedAzimuth = 90.0 * degreesToRadians;
        double expectedElevation = 60.0 * degreesToRadians;

        BOOST_CHECK_CLOSE_FRACTION( expectedAzimuth, testAzimuth, 3.0 * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( expectedElevation, testElevation, 3.0 * std::numeric_limits< double >::epsilon( ) );
    }

    // Compare results with data obtained from: http://www.movable-type.co.uk/scripts/latlong.html
    {
        {
            // Create ground station properties
            std::shared_ptr< GroundStationState > stationState = std::make_shared< GroundStationState >(
                        groundStationPosition, coordinate_conversions::cartesian_position, bodyShape );
            std::shared_ptr< PointingAnglesCalculator > pointAnglesCalculator = std::make_shared< PointingAnglesCalculator >(
                        [ & ]( const double ){ return Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ); },
                        std::bind( &GroundStationState::getRotationFromBodyFixedToTopocentricFrame, stationState, std::placeholders::_1 ) );

            // Define state of viewed point
            double testLatitude = 21.0 * degreesToRadians;
            double testLongitude = 84.0 * degreesToRadians;
            double testRadius = 8.0E7;
            Eigen::Vector3d testSphericalPoint;
            testSphericalPoint << testRadius, mathematical_constants::PI / 2.0 - testLatitude, testLongitude;
            Eigen::Vector3d testCartesianPoint = coordinate_conversions::convertSphericalToCartesian( testSphericalPoint );

            // Compute azimuth/elevation angles from PointingAnglesCalculator
            double testAzimuth = pointAnglesCalculator->calculationAzimuthAngle( testCartesianPoint, 0.0 );
            double testElevation = pointAnglesCalculator->calculateElevationAngle( testCartesianPoint, 0.0 );

            // Set azimuth/elevation angles retrieved from website.
            double expectedElevation = mathematical_constants::PI / 2.0 - 9385.0 / 6371.0;
            double expectedAzimuth = mathematical_constants::PI / 2.0 - ( 68.0 + 53.0 / 60.0 + 40.0 / 3600.0 ) * degreesToRadians;

            BOOST_CHECK_CLOSE_FRACTION( expectedAzimuth, testAzimuth, 1.0E-5 );
            BOOST_CHECK_CLOSE_FRACTION( expectedElevation, testElevation, 1.0E-3 );
        }

        {
            // Create ground station properties
            std::shared_ptr< GroundStationState > stationState = std::make_shared< GroundStationState >(
                        groundStationPosition, coordinate_conversions::cartesian_position, bodyShape );
            std::shared_ptr< PointingAnglesCalculator > pointAnglesCalculator = std::make_shared< PointingAnglesCalculator >(
                        [ & ]( const double ){ return Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ); },
                        std::bind( &GroundStationState::getRotationFromBodyFixedToTopocentricFrame, stationState, std::placeholders::_1 ) );

            // Define state of viewed point
            double testLatitude = -38.0 * degreesToRadians;
            double testLongitude = 234.0 * degreesToRadians;
            double testRadius = 8.0E7;
            Eigen::Vector3d testSphericalPoint;
            testSphericalPoint << testRadius, mathematical_constants::PI / 2.0 - testLatitude, testLongitude;
            Eigen::Vector3d testCartesianPoint = coordinate_conversions::convertSphericalToCartesian( testSphericalPoint );

            // Compute azimuth/elevation angles from PointingAnglesCalculator
            double testAzimuth = pointAnglesCalculator->calculationAzimuthAngle( testCartesianPoint, 0.0 );
            double testElevation = pointAnglesCalculator->calculateElevationAngle( testCartesianPoint, 0.0 );

            // Set azimuth/elevation angles retrieved from website.
            double expectedElevation = mathematical_constants::PI / 2.0 - 13080.0 / 6371.0;
            double expectedAzimuth = mathematical_constants::PI / 2.0 - ( 225.0 + 59.0 / 60.0 + 56.0 / 3600.0 ) * degreesToRadians;

            BOOST_CHECK_CLOSE_FRACTION( expectedAzimuth, testAzimuth, 1.0E-5 );
            BOOST_CHECK_CLOSE_FRACTION( expectedElevation, testElevation, 3.0E-2 );

            std::pair< double, double > pointingAngles = pointAnglesCalculator->calculatePointingAngles( testCartesianPoint, 0.0 );

            BOOST_CHECK_CLOSE_FRACTION( pointingAngles.second, testAzimuth, 1.0E-5 );
            BOOST_CHECK_CLOSE_FRACTION( pointingAngles.first, testElevation, 3.0E-2 );
        }
    }

    // Check if inertial->topocentric rotation is handled consistently
    {
        // Define inertial->body-fixed frame rotation
        double poleRightAscension = 56.0 * degreesToRadians;
        double poleDeclination = 45.0 * degreesToRadians;
        Eigen::Quaterniond inertialToBodyFixedFrame =
                reference_frames::getInertialToPlanetocentricFrameTransformationQuaternion(
                    poleDeclination, poleRightAscension, 0.0 );

        // Define ground station position
        groundStationPosition = Eigen::Vector3d( 1234.0E3, -4539E3, 4298E3 );
        std::shared_ptr< GroundStationState > stationState = std::make_shared< GroundStationState >(
                    groundStationPosition, coordinate_conversions::cartesian_position, bodyShape );
        std::shared_ptr< PointingAnglesCalculator > pointAnglesCalculator = std::make_shared< PointingAnglesCalculator >(
                    [ & ]( const double ){ return inertialToBodyFixedFrame; },
                    std::bind( &GroundStationState::getRotationFromBodyFixedToTopocentricFrame, stationState, std::placeholders::_1 ) );

        // Define state of viewed point
        double testLatitude = -38.0 * degreesToRadians;
        double testLongitude = 234.0 * degreesToRadians;
        double testRadius = 8.0E7;
        Eigen::Vector3d testSphericalPoint = Eigen::Vector3d(
                    testRadius, mathematical_constants::PI / 2.0 - testLatitude, testLongitude );
        Eigen::Vector3d testCartesianPoint = coordinate_conversions::convertSphericalToCartesian( testSphericalPoint );

        // Retrieve topocentric position of viewed point from GroundStationState and PointingAnglesCalculator and compare.
        Eigen::Vector3d testPointInLocalFrame = pointAnglesCalculator->convertVectorFromInertialToTopocentricFrame(
                    testCartesianPoint, 0.0 );
        Eigen::Vector3d expectedTestPointInLocalFrame =
                stationState->getRotationFromBodyFixedToTopocentricFrame( 0.0 ) * inertialToBodyFixedFrame *
                testCartesianPoint ;
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    testPointInLocalFrame, expectedTestPointInLocalFrame, ( 10.0 * std::numeric_limits< double >::epsilon( ) ) );
    }

}

BOOST_AUTO_TEST_SUITE_END( )

}

}


//
// SPDX-License-Identifier: MIT
// Copyright (c) 2016-2020 Michael Purcaro, Henry Pratt, Jill Moore, Zhiping Weng
//

#define ARMA_64BIT_WORD
#include <armadillo>

#define likely(x) __builtin_expect ((x), 1)
#define unlikely(x) __builtin_expect ((x), 0)

#include <atomic>
#include <iomanip>
#include <vector>
#include <string>
#include <fstream>
#include <iostream>
#include <algorithm>
#include <mutex>

#include <json/json.h>
#include <json/reader.h>
#include <boost/optional.hpp>
#include <boost/filesystem.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/utility/setup.hpp>

#include <zi/concurrency/concurrency.hpp>
#include <zi/parallel/algorithm.hpp>
#include <zi/parallel/numeric.hpp>

#include "cpp/utility.hpp"
#include "cpp/string_utils.hpp"
#include "cpp/files.hpp"
#include "cpp/gzip_reader.hpp"
#include "cpp/tictoc.hpp"

#include <zi/zargs/zargs.hpp>
ZiARG_int32(nbars, 0, "num bars");

namespace bib {

  namespace a = arma;

  template <typename S, typename C>
  S& join(S& s, const C& vec, const std::string delim){
    // http://stackoverflow.com/a/2519016
    for(auto it = vec.begin(); it != vec.end(); ++it ){
      if( it != vec.begin() ){
          s << delim;
      }
      s << *it;
    }
    return s;
  }

  int bwtool(){
    std::string line;

    std::stringstream s;
    while(std::getline(std::cin, line)){
      const auto toks = bib::str::split(line, '\t');
      //const std::string& accession = toks[3];
      const auto vals = bib::str::split(toks[5], ',');

      size_t n_bars = 30;
      if(ZiARG_nbars > 0){
          n_bars = ZiARG_nbars;
      } else {
          n_bars = static_cast<size_t>(std::ceil(vals.size() / 100.));
      }

      a::fvec regions(vals.size());
      for(size_t i = 0; i < vals.size(); ++i){
          if(likely(vals[i] != "NA")){
              regions[i] = std::stof(vals[i]);
          } else{
              regions[i] = 0;
          }
      }

      size_t chunkSize = regions.size() / n_bars;
      std::vector<float> ret;
      for(size_t i = 0; i < n_bars; ++i){
          float sum = 0;
          for(size_t j = chunkSize * i;
              j < chunkSize * (i + 1) && j < regions.size(); ++j){
              sum += regions[j];
          }
          sum /= chunkSize;
          ret.push_back(sum);
      }

      s << std::setprecision(4);
      s << "\"[ "; join(s, ret, ",") << "]\"\n";
    }

    std::cout << s.str();

    return 0;
  }

} // namespace bib

int main(int argc, char* argv[]){
  zi::parse_arguments(argc, argv, true);  // modifies argc and argv
  const auto args = std::vector<std::string>(argv + 1, argv + argc);

  return bib::bwtool();

  return 0;
}


#include <cmath>
#include <iostream>
#include <vector>

#include <boost/align/aligned_allocator.hpp>
#include <boost/align/aligned_delete.hpp>

#include <boost/simd/function/cos.hpp>
#include <boost/simd/function/load.hpp>
#include <boost/simd/function/sin.hpp>
#include <boost/simd/function/sincos.hpp>
#include <boost/simd/function/store.hpp>
#include <boost/simd/pack.hpp>

int main(int argc, char** argv)
{
  namespace bs = boost::simd;
  namespace ba = boost::alignment;

  using pack_t = bs::pack<float>;

  std::size_t num_elements = 1024;
  std::size_t alignment    = pack_t::alignment;
  //! [transcendental-declare]
  std::vector<float, ba::aligned_allocator<float, pack_t::alignment>> X(num_elements);
  std::vector<float, ba::aligned_allocator<float, pack_t::alignment>> sinX(num_elements);
  std::vector<float, ba::aligned_allocator<float, pack_t::alignment>> cosX(num_elements);
  //! [transcendental-declare]

  //! [transcendental-scalar]
  for (int i = 0; i < num_elements; ++i) {
    sinX[i] = std::sin(X[i]);
    cosX[i] = std::cos(X[i]);
  }
  //! [transcendental-scalar]

  //! [transcendental-calc-individ]
  for (int i = 0; i < num_elements; i += pack_t::static_size) {
    pack_t v0 = bs::load<pack_t>(&X[i]);
    bs::store(bs::sin(v0), &sinX[i]);
    bs::store(bs::cos(v0), &sinX[i]);
  }
  //! [transcendental-calc-individ]

  //! [transcendental-calc-combine]
  for (int i = 0; i < num_elements; i += pack_t::static_size) {
    pack_t v0 = bs::load<pack_t>(&X[i]);
    auto res  = bs::sincos(v0);
    bs::store(res.first, &sinX[i]);
    bs::store(res.second, &cosX[i]);
  }
  //! [transcendental-calc-combine]

  //! [transcendental-calc-small]
  for (int i = 0; i < num_elements; i += pack_t::static_size) {
    pack_t v0 = bs::load<pack_t>(&X[i]);
    bs::store(bs::sin(v0, bs::tag::small_), &sinX[i]);
  }
  //! [transcendental-calc-small]
}


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

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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IS_NEGATIVE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_NEGATIVE_HPP_INCLUDED

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

 /*!

    @ingroup group-predicates
    Function object implementing is_negative capabilities

    Returns @ref True if x is negative else @ref False.

    This function differs from @ref is_ltz for floating point arguments,
    because @ref Mzero is negative but not less than zero, and @ref Mzero is
    not positive and not greater than zero, It's probably @ref is_ltz that
    you want.

    @par Semantic:

    @code
    as_logical_t<T> r = is_negative(x);
    @endcode

    is similar to:

    @code
    if T is signed
      as_logical_t<T> r = bitofsign(x) == 1;
    else
      as_logical_t<T> r = @ref False;
    @endcode

    @par Note:

    @ref Mzero is the floating point 'minus zero',
    i.e. all bits are @ref Zero but the sign bit.
    Such a value is treated as @ref Zero by IEEE standards.

    behaviour of is_negative on @ref Nan entry is undefined.

    @par Alias

    signbit

    @see is_positive,  Mzero, bitofsign

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

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

#endif


//
/// \file  convolution_test.cpp
/// \package how_much_data
//
/// \author Created by Joseph Dunn on 12/31/18.
/// \copyright  © 2018, 2019 Joseph Dunn. All rights reserved.
//

#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
using std::ostream;
#include <iomanip>
using std::setw;
using std::setprecision;
using std::right;
using std::fixed;
using std::scientific;
using std::defaultfloat;
#include <string>
using std::string;
using std::to_string;
#include <sstream>
using std::istringstream;
using std::ostringstream;
#include <fstream>
using std::ofstream;
#include <vector>
using std::vector;
#include <array>
using std::array;
#include <algorithm>
using std::max;
using std::min;
using std::max_element;
using std::sort;
#include <utility>
using std::pair;
#include <mutex>
using std::mutex;
using std::unique_lock;
#include <thread>
using std::thread;

#include <complex>
using dcomplex = std::complex<double>;
#include <Eigen/Dense>
using Eigen::Matrix;
using Eigen::Dynamic;
using Eigen::Map;

/// cause Eigen/FFT to use fftw.  delete to avoid GPL
#define EIGEN_FFTW_DEFAULT
#include <unsupported/Eigen/FFT>

Eigen::FFT<double> fft_eng;

#include <boost/timer/timer.hpp>
using boost::timer::auto_cpu_timer;
using boost::timer::cpu_timer;
#include <boost/filesystem.hpp>
using boost::filesystem::path;
#include <boost/math/tools/roots.hpp>
using boost::math::tools::bracket_and_solve_root;
using boost::math::tools::eps_tolerance;
#include <boost/math/constants/constants.hpp>
using boost::math::constants::pi;
using boost::math::constants::one_div_root_two_pi;
using boost::math::constants::one_div_two_pi;

#include "pareto_distribution.h"
#include "student_t_distribution.h"
#include "exponential_distribution.h"
#include "lognormal_distribution.h"
#include "taleb_results.h"

/// sturcture passed by imbue to ostreams to use commas in numbers
struct Numpunct: public std::numpunct<char>{
protected:
  virtual char do_thousands_sep() const{return ',';}
  virtual std::string do_grouping() const{return "\03";}
};

/// return the relative differnece between two numbers
template<typename RealType>
RealType rel_err(RealType a, RealType b) {
  return fabs(a-b)/std::max(a,b);
}

/// structure holding the results of a run for a single alpha
struct KappaResult {
  /// constructor
  KappaResult (vector<int> ns  ///< the durations of the output
               ) : ns(ns) {}
  double param;        ///< the parameter for the run
  int nsize;           ///< the size of the vector pased to fft
  double mad_rel_err;  ///< the relative error of mad vs theory
  vector<int> ns;      ///< the durations saved
  vector<double> mad;  ///< the mean absolute deviation by duration
  vector<double> kappa_mad; /// the kappa_mad by duration
  double ci_rel_err;   ///< the relative error of conf. int. vs theory
  vector<double> ci;   ///< the conficence interval by duration
  vector<double> kappa_ci; ///< the kappa ci by duration
  /// calculate the kappa from the mad and ci variables
  void calc_kappa() {
    for (size_t i = 1; i<ns.size(); ++i) {
      kappa_mad.push_back(2- (log(ns.at(i))-log(ns.at(0)))/
                          (log(mad.at(i))-log(mad.at(0))));
      kappa_ci.push_back(2- (log(ns.at(i))-log(ns.at(0)))/
                         (log(ci.at(i))-log(ci.at(0))));
    }
  }
};

 /// output the results to an ostream
ostream& operator<< (ostream& os, const KappaResult& k) {
  os << fixed << setw(7) << setprecision(2) << k.param
  << setw(14) << k.nsize
  << setw(12) << setprecision(2) << k.mad_rel_err*100 << "%";
  for (size_t i=1; i<k.mad.size(); ++i) {
    os << setw(13) << setprecision(3) << k.kappa_mad.at(i-1);
  }
  os << setw(12) << setprecision(2) << k.ci_rel_err*100 << "%";
  for (size_t i=1; i<k.ci.size(); ++i) {
    os << setw(13) << setprecision(3) << k.kappa_ci.at(i-1);
  }
  os << endl;
  return os;
}

/// structure holding the results of all runs
struct KappaResults {
  /// constructor
  KappaResults(const vector<int>& ns,  ///< the durations
               size_t n_params,       ///< the # of params
               string param_label,     ///< the param_label
               size_t taleb_offset     ///< the column offset into Talebs table
               )
  : ns(ns), param_label(param_label), kr(n_params, ns),
    taleb_offset(taleb_offset) {}
  /// return reference to a particular result
  KappaResult& at(size_t i) {
    unique_lock<mutex> lock(kr_mutex);
    return this->kr.at(i);
  }
  vector<int> ns;           ///< the durations saved
  string param_label;       ///< the name of the parameter
  vector<KappaResult> kr;   ///< the results of the runs by duration
  size_t taleb_offset;      ///< the column offset into Taleb's table
  mutex kr_mutex;           ///< a mutex for writing results
};

/// output the results of all runs to an ostream
ostream& operator<< (ostream& os, KappaResults& ks) {
  os << setw(7) << right << ks.param_label
     << setw(14) << right << "nsize"
     << setw(13) << right << "mad_rel_err";
  for (int i=1; i<ks.ns.size(); ++i)
    os << setw(13) << right << "kappa"+to_string(ks.ns.at(i))+"_mad";
  os << setw(13) << right << "ci_rel_err";
  for (int i=1; i<ks.ns.size(); ++i)
    os << setw(13) << right << "kappa"+to_string(ks.ns.at(i))+"_ci";
  os << endl << endl;
  for (auto kr : ks.kr)
    os << kr;
  os << endl;
  if (ks.taleb_offset >0) {
    os << setw(68) << right << "Error vs Taleb's Results" << endl << endl;
    for (size_t i=0; i<ks.kr.size(); ++i) {
      os << setw(7) << setprecision(2) << ks.kr.at(i).param << setw(27) << " ";
      for (size_t j=0; j<ks.ns.size()-1; ++j) {
        double kappa_mad = ks.kr.at(i).kappa_mad.at(j);
        double err = kappa_mad - taleb_results.at(i).at(j+ks.taleb_offset);
        os << setw(13) << setprecision(3) << err;
      }
      os << endl;
    }
    os << endl;
  }
  return os;
}

/// modulo calculation return nonnegative number less than the modulus
int mod (int a, int b)
{
  int ret = a % b;
  if(ret < 0)
    ret+=b;
  return ret;
}

/// calcuate confidence interval given pdf at points in range
double confidence_interval(int nmin,    ///< [in] the minimum index
                           int nmax,    ///< [in] the maximum index
                           double delta,///< [in] the spacing of the x's
                           const Map<Matrix<double, Dynamic, 1> >& pdf, ///< [in] the calculated pdf's
                           double ci_level  ///< [in] the confidence level
                           ) {
  int nsize = static_cast<int>(pdf.size());
  double ci_lower_left{nmin * delta};
  double ci_lower_right=ci_lower_left;
  double cdf_lower_left=.5*pdf(mod(nmin,nsize))*delta;
  double cdf_lower_right = cdf_lower_left;
  for (int j=nmin; j<=nmax ; ++j) {
    if (cdf_lower_right > ci_level/2) break;
    ci_lower_left = ci_lower_right;
    cdf_lower_left = cdf_lower_right;
    ci_lower_right= (j+1)*delta;
    cdf_lower_right = cdf_lower_left+.5*(pdf(mod(j, nsize))+pdf(mod(j+1,nsize)))*delta;
  }
  double ci_lower;
  if (ci_lower_left == ci_lower_right)
    ci_lower = ci_lower_right;
  else {
    double t = (ci_level/2-cdf_lower_left)/(cdf_lower_right-cdf_lower_left);
    ci_lower = (1-t) * ci_lower_left + t * ci_lower_right;
  }

  double ci_upper_left{nmax * delta};
  double ci_upper_right=ci_upper_left;
  double cdf_upper_left{.5*pdf(mod(nmax,nsize))*delta};
  double cdf_upper_right=cdf_upper_left;
  for (int j=nmax; j>=nmin ; --j) {
    if (cdf_upper_left > ci_level/2) break;
    ci_upper_right = ci_upper_left;
    cdf_upper_right = cdf_upper_left;
    ci_upper_left= (j-1) * delta;
    cdf_upper_left = cdf_upper_right+.5*(pdf(mod(j,nsize))+pdf(mod(j-1, nsize)))*delta;
  }
  double ci_upper;
  if (ci_upper_right == ci_upper_left)
    ci_upper = ci_upper_left;
  else {
    double t = (ci_level/2-cdf_upper_right)/(cdf_upper_left-cdf_upper_right);
    ci_upper = (1-t) * ci_upper_right + t * ci_upper_left;
  }
  return ci_upper-ci_lower;
}

/// functor for determining upper limit for target mad
template<typename Dist>
class Upper {
public:
  /// constructor
  Upper (double delta,  ///< [in] target mad arising from upper tail
         Dist& dist     ///< [in] reference to distribution
         ) : delta(delta), dist(dist) {}
  /// return excess of estimated mad in tail over target
  double operator() (double x   ///< [in] the quantile
                     ) {
    double a = dist.alpha_stable()/(dist.alpha_stable()-1);
    double ret = max(1.,fabs(x-dist.mean()))* a * (dist.cdf(x, false))-delta;
    return ret;
  }
private:
  double delta;
  Dist& dist;
};

/// functor for determining lower limit for target mad
template<typename Dist>
class Lower {
public:
  /// constructor
  Lower (double delta,    ///< [in] the target mad arising from lower tail
         Dist& dist       ///< [in] the distribution
         ) : delta(delta), dist(dist) {}
  /// return excess of estimated mad in tail over target
  double operator() (double x    ///< [in] the trial position
                     ) {
    double a = dist.alpha_stable()/(dist.alpha_stable()-1);
    double ret = max(1.,fabs(x-dist.mean())) * a * (dist.cdf(x, true))-delta;
    return ret;
  }
private:
  double delta;
  Dist& dist;
};

/// Check whether the factorization of n contains only listed primes
bool factor_check(int n,              ///< [in] the number to check
                  vector<int> primes  ///< [in] the array of candidate primes
                  )
{
  int n_current = n;
  for (auto p : primes) {
    while (n_current % p == 0 ) {
      n_current = n_current / p;
    }
  }
  return n_current == 1;
}

/// instances] of Job used to parcel characteristic funciion calculation to threads
struct Job {
  int nmin;          ///< the overall minimum index
  int nmax;          ///< the overall maximum index
  int ncurrent;      ///< the next index to parcel out
  int nchunk;        ///< the size of the range to parcel out
  mutex job_mutex;   ///< to prevent multiple accesses
  /// consturctor
  Job(int nmin,      ///< [in] the overall minimum index
      int nmax,      ///< [in] the overall maximum index
      int nchunk     ///< [in] the size of the range to parcel out
      ) : nmin(nmin), nmax(nmax), ncurrent(nmin),
                                        nchunk(nchunk) {}
  /// get a range and return true if okay
  bool get_next(int& nstart,   ///< [out] start of the assigned range
                int& nend      ///< [out] next index after assigned range
                ) {
    unique_lock<mutex> lock;
    if (ncurrent > nmax) return false;
    nstart = ncurrent;
    ncurrent = min(nmax+1,ncurrent+nchunk);
    nend = ncurrent;
    return true;
  }
};

/// calculate the characteristic function for a range assigned to trhead
template<typename Dist>
void calc_characteristic_function(
                                const Dist& dist, ///< [in] the distribution
                                const double mean,  ///< [in] the mean to remove
                                const int n,        ///< [in] the duration
                                const double delta_omega, ///< [in] step for omega
                                Job *job,           ///< [in,out]ptr to job assigner
                                Matrix<dcomplex, Dynamic, 1> *adj_cf ///< [out] the characeristic
                                  ///< function of the normalized distrbibution
                                  ///> for non-negative frequencies
                                  )
{
  int n_start, n_end;
  while (job->get_next(n_start, n_end)) {
    for (int i=n_start; i<n_end; ++i) {
      double omega = delta_omega*i/pow(n,1/dist.alpha_stable());
      // remove the location parameter from the characteristic function
      dcomplex fac = exp(dcomplex(0,-mean*omega));
      dcomplex cf = pow(fac*dist.characteristic_function(omega),n);
      adj_cf->operator()(i) = cf;
    }
  } // while job
}

/// set up ranges and step sizes for one alpham pass calculation of cf to threads
/// and use fft to estimate distribution
template<typename Dist>
void calculate_kappa(double delta,    ///< [in] the step size in x / dist.mad
                     double delta2,   ///< [in] cap on % mad from the tail
                     int m,           ///< [in] cap on maximum index
                     vector<int> ns,  ///< [in] the durations to calculate
                     Dist dist,       ///< [in] the distribution
                     double ci_level, ///< [in] the confidence level for kappa_ci
                     KappaResult& k,  ///< [out] the results
                     bool verbose = false ///< [in] flag for trace
                     )
{
  double mean = dist.mean();
  delta *= dist.mad();
  double target_mad = delta2 * dist.mad();
  if (verbose) {
    cout << scientific
         << "Rescaled delta  = " << setw(12) << setprecision(3) << delta << endl
         << "Target tail MAD = " << setw(12) << setprecision(3) << target_mad << endl
         << defaultfloat;
  }
  double alpha_stable = dist.alpha_stable();
  if (verbose)
    cout << dist << endl;

  // Calculate xmax0 and xmin0
  // so that the contriubution of the tails to MAD is small

  Upper<Dist> upper(target_mad, dist);
  double guess = dist.quantile(1-delta2/2);
  double factor = 2;
  bool rising = false;
  eps_tolerance<double> tol;
  boost::uintmax_t max_iter{1000};
  pair<double, double> root = bracket_and_solve_root(upper, guess, factor,
                                                     rising, tol, max_iter);
  double xmax0 = root.second - mean;

  Lower<Dist> lower(target_mad, dist);
  guess = dist.quantile(delta2/2);
  rising = true;
  max_iter = 1000;
  root = bracket_and_solve_root(lower, guess, factor, rising, tol, max_iter);
  double xmin0 = root.first - mean;

  // Since we're centering on the mean we'll set abs(xmax)=abs(xmin)
  double xmax = max(10.,max(fabs(xmax0),fabs(xmin0)));
  // Cap the result to the m input on the command line
  int nmax = static_cast<int>(min(double(m), xmax/delta));
  if (nmax < 1000000) {
    // Add extra points spliting between increasing the range and the density
    delta = delta/sqrt(1000000./nmax);
    nmax = 1000000;
    if (verbose)
      cout << "Revising delta to " << delta << endl;
  }
  int nsize = 2*nmax+1;

  // fftw works best for array sizes that are products of small primes
  vector<int> primes{3,5,7,11};
  for (; !factor_check(nsize, primes); nsize+=2) {}
  nmax = ((nsize-1)/2);
  int nmin = -nmax;

  xmax = nmax * delta;
  // The amounts of mad left in the tail after all of the adjustments
  double uppermad = delta2 + upper(xmax);
  double lowermad = delta2 + lower(-xmax);
  k.nsize = nsize;
  if (verbose) {
    cout << scientific
         << setw(12) << "xmin0 = " << setw(15) << setprecision(2) << xmin0 << endl
         << setw(12) << "xmax0 = " << setw(15) << setprecision(2) << xmax0 << endl
         << defaultfloat
         << setw(12) << "nmin = " << setw(15) << nmin << endl
         << setw(12) << "nmax = " << setw(15) << nmax << endl
         << setw(12) << "xmin = " << setw(15) << setprecision(2) << -xmax << endl
         << setw(12) << "xmax = " << setw(15) << setprecision(2) << xmax << endl
         << scientific
         << setw(12) << "mad = " << setw(15) << setprecision(3) << dist.mad() << endl
         << setw(12) << "uppermad = " << setw(15) << setprecision(3) << uppermad << endl
         << setw(12) << "lowermad = " << setw(15) << setprecision(3) << lowermad << endl
         << defaultfloat
         << setw(12) << "nsize = " << setw(15) << nsize << endl << endl;
  }
  // the maximum absolute value of he angular frequency
  double omega_max = pi<double>()/delta;
  double delta_omega = (2*omega_max/nsize);
  // the adjusted characteristic function for the non-negative half spectrum
  // use Eigen::Matrix to assure alignment to 16 butye boundary for fftw
  Matrix<dcomplex,Dynamic,1> adj_cf(nmax+1);

  if (verbose) {
    cout << setw(5) << " "
    << setw(13) << right << "x.at(nmin)"
    << setw(13) << right << "x.at(-1)"
    << setw(13) << right << "x.at(0)"
    << setw(13) << right << "x.at(1)"
    << setw(13) << right << "x.at(nmax)"
    << endl;

    cout << setw(5) << " "
    << setw(13) << fixed << setprecision(3) << nmin * delta
    << setw(13) << fixed << setprecision(3) << -1. * delta
    << setw(13) << fixed << setprecision(3) << 0. * delta
    << setw(13) << fixed << setprecision(3) << 1. * delta
    << setw(13) << fixed << setprecision(3) << nmax * delta
    << endl << endl;
    cout << setw(5) << right << "n"
    << setw(13) << right << "pdf.at(nmin)"
    << setw(13) << right << "pdf.at(-1)"
    << setw(13) << right << "pdf.at(0)"
    << setw(13) << right << "pdf.at(1)"
    << setw(13) << right << "pdf.at(nmax)"
    << setw(13) << right << "p_total"
    << setw(12) << right << "mad"
    << setw(12) << right << "madlower"
    << setw(12) << right << "madupper"
    << endl;
  }
  for (size_t j=0; j<ns.size(); ++j){
    int n = ns.at(j);
    array<thread,8> threads;
    int nchunk = 1000;
    // Just do the positive half spectrum
    Job job(0, nmax, nchunk);
    for (int i=0; i<8; ++i) {
      threads.at(i)=thread{calc_characteristic_function<Dist>, dist,
                           mean, n, delta_omega,
                           &job, &adj_cf};
    }
    for (int i=0; i<8; ++i)
      threads.at(i).join();
    // fft_eng does an in-place DFT if the pointers to the src & dst match
    // In our case the source is adj_cf and the dst is pdf
    Map<Matrix<double, Dynamic, 1> >pdf(reinterpret_cast<double*>(&adj_cf(0)), nsize);
    fft_eng.inv(&pdf(0), &adj_cf(0), nsize);

    for (int i=-nmax; i<=nmax; ++i)
      pdf(mod(i,nsize)) *= delta_omega * nsize * one_div_two_pi<double>();
    double mad = 0;
    for (int i=-nmax; i<=nmax; ++i) {
      mad += pdf(mod(i,nsize)) * fabs(i * delta)*delta;
    }
    // Assume that the relative weight in the tail is the same as the original distribution
    double mad_upper_tail = mad*uppermad/dist.mad();
    double mad_lower_tail = mad*lowermad/dist.mad();;
    mad += mad_upper_tail+mad_lower_tail;
    // Scal mad.  This reverse the scaling that was done in the adj_cf calculation
    mad *= pow(n,1/alpha_stable);
    k.mad.push_back(mad);
    double ci = confidence_interval(nmin, nmax, delta, pdf, ci_level);
    ci *= pow(n,1/alpha_stable);
    k.ci.push_back(ci);
    if (verbose) {
      double p_total = pdf.sum()*delta;
      cout << setw(5) << n
      << setw(13) << fixed << setprecision(8) << pdf(mod(nmin,nsize))
      << setw(13) << fixed << setprecision(8) << pdf(mod(-1,nsize))
      << setw(13) << fixed << setprecision(8) << pdf(mod(0,nsize))
      << setw(13) << fixed << setprecision(8) << pdf(mod(1,nsize))
      << setw(13) << fixed << setprecision(8) << pdf(mod(nmax,nsize))
      << setw(13) << fixed << setprecision(8) << p_total
      << setw(12) << scientific<< setprecision(3) << mad
      << setw(12) << scientific << setprecision(3) << mad_lower_tail*pow(n,1/alpha_stable)
      << setw(12) << scientific << setprecision(3) << mad_upper_tail*pow(n,1/alpha_stable)
      << endl;
    }
  } // j over ns
  k.calc_kappa();
  k.mad_rel_err = rel_err(k.mad.at(0), dist.mad());
  k.ci_rel_err = rel_err(k.ci.at(0), dist.ci(ci_level));
}

/// show proper usage after improper command line argument
void show_usage(path p     ///< [in] the path of the executable
                ) {
  cerr << "Usage: " << p.filename().string() << " delta [delta2=.001] [m=1e7]" << endl
       << "  where delta is the interval of integration/mad" << endl
       << "  and delta2 is the tail contribution to mad" << endl;
}

/// main program for convolution_testl
int main(int argc, const char * argv[]) {
  path p(argv[0]);
  if ( (argc < 2) || (argc > 4) ) {
    show_usage(p);
    return 1;
  }
  istringstream iss1{string(argv[1])};
  double delta;
  if ( !(iss1 >> delta) || ( delta <=0 ))
    show_usage(p);

  double delta2 = .001;
  if (argc >= 3) {
    istringstream iss2{string(argv[2])};
    if ( !(iss2 >> delta2) || (delta2 <=0 ) )
      show_usage(p);
  }
  int m = 1e7;
  if (argc >= 4) {
    istringstream iss3{string(argv[3])};
    if ( !(iss3 >> m) || (m <=0 ) )
      show_usage(p);
  }
  // Add commas to the cout format
  cout.imbue({std::locale(), new Numpunct});

  // for pareto and student t distribution use the same alphas as Taleb
  vector<double> alphas;
  for (size_t i=0; i<taleb_results.size(); ++i)
    alphas.push_back(taleb_results.at(i).at(0));

  // The durations for which MAD will be calculated
  vector<int> ns{1, 2, 30, 100};

  // In addtion to MAD to measure scale we'll use the length of a conficence
  // interval with ci_level confidence
  double ci_level = .05;

  string out_dir{"../output"};
  ostringstream oss;
  oss << out_dir << "/convolution_test_"
      << delta << "_"
      << delta2 << "_"
      << m
      << ".out";
  cout << "Writing to file " << oss.str() << endl << endl;
  cout.flush();

  ofstream out{oss.str()};
  out.imbue({std::locale(), new Numpunct});

  auto_cpu_timer t(out);

#ifdef EIGEN_FFTW_DEFAULT
  if (fftw_init_threads() == 0)
    throw std::runtime_error("fftw_init_threads failed");
  fftw_plan_with_nthreads(8);
#endif

  {
    KappaResults ks_pareto(ns, alphas.size(), "alpha", 1);
    for (size_t i=0; i<alphas.size(); ++i) {
      double alpha=alphas.at(i);
      pareto_distribution<> pd(alpha);
      KappaResult kr(ns);
      kr.param = alpha;
      calculate_kappa(delta, delta2, m, ns, pd, ci_level, kr, true);
      ks_pareto.at(i) = kr;
    }
    out << "Pareto Distribution" << endl << endl;
    out << ks_pareto;
    out.flush();
  }

  {
    KappaResults ks_student(ns, alphas.size(), "alpha",4);
    for (size_t i=0; i<alphas.size(); ++i) {
      double alpha = alphas.at(i);
      student_t_distribution<> td(alpha);
      KappaResult kr(ns);
      kr.param = alpha;
      calculate_kappa(delta, delta2, m, ns, td, ci_level, kr, true);
      ks_student.at(i)=kr;
    }
    out << "Student t Distribution" << endl << endl;
    out << ks_student;
  }

  {
    KappaResults ks_exponential(ns, 1, "lambda", 0);
    exponential_distribution<> ed(1);
    KappaResult kr(ns);
    kr.param = 1;
    calculate_kappa(delta, delta2, m, ns, ed, ci_level, kr, true);
    ks_exponential.at(0) = kr;
    out << ed << endl << endl;
    out << ks_exponential;
  }

  {
    cout << endl;
    vector<double> sigmas = {.01, .1, .5, 1, 2, 5};
    KappaResults ks_lognormal(ns, sigmas.size(), "sigma",0);
    for (size_t i=0; i<sigmas.size(); ++i) {
      double sigma = sigmas.at(i);
      int type = 3;    // Use the Lambert W approximation
      lognormal_distribution<> lnd(0,sigma, type);
      KappaResult kr(ns);
      kr.param = sigma;
      calculate_kappa(delta, delta2, m, ns, lnd, ci_level, kr, true);
      ks_lognormal.at(i)=kr;
    }
    out << "Lognormal Distribution" << endl << endl;
    out << ks_lognormal;
  }
  fftw_cleanup_threads();

  return 0;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef TRACKINGCONTROLIPOPT_HH
#define TRACKINGCONTROLIPOPT_HH

#include <memory>
#include <algorithm>
#include <cstdlib>

#include <boost/timer.hpp>

#include "IpTNLP.hpp"

#include "assemble.hh"
#include "functional_aux.hh"

/**
 * Interface to Ipopt.
 *
 * The VariationalFunctional provided has to implement Ipopt
 * semantics, i.e., in evaluating the right hand side and the
 * functional the value of lambda is implicitly assumed to be zero,
 * independent of the provided value. This means that the gradient and
 * value of the objective have to be computed instead of gradient and
 * value of the Lagrange functional.
 */
template <class VariationalFunctional>
class TrackingControlInterface: public Ipopt::TNLP
{
  typedef VariationalFunctionalAssembler<LinearizationAt<VariationalFunctional> >  Gop;
  typedef typename Gop::RT RT;
  typedef typename Gop::AnsatzVariableSet VarSet;
  typedef typename VarSet::Grid Grid;
  typedef VariationalFunctional Functional;

  typedef Ipopt::Index Index;
  typedef Ipopt::TNLP::IndexStyleEnum IndexStyleEnum;
  typedef Ipopt::SolverReturn SolverReturn;
  typedef Ipopt::Number Number;

  enum { U = 0, Y = 1, LAMBDA = 2, END = 3 };
  typedef int AssemblyParts;

public:
  TrackingControlInterface(GridSignals& signals,
                           VarSet const& varSet_,
                           Functional& f_):
    xx(varSet_),
    varSet(varSet_),
    gop(signals,varSet.spaces),
    f(f_),
    nu(varSet.degreesOfFreedom(U,U+1)),
    ny(varSet.degreesOfFreedom(Y,Y+1)),
    nl(varSet.degreesOfFreedom(LAMBDA,LAMBDA+1)),
    n(nu+ny)
  {
    f.initVars(xx);
  }

  /** overload this method to return the number of variables
   *  and constraints, and the number of non-zeros in the jacobian and
   *  the hessian. The index_style parameter lets you specify C or Fortran
   *  style indexing for the sparse matrix iRow and jCol parameters.
   *  C_STYLE is 0-based, and FORTRAN_STYLE is 1-based.
   */
  virtual bool get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
                            Index& nnz_h_lag, IndexStyleEnum& index_style)
  {

    n = this->n;
    m = nl;

    nnz_jac_g = gop.nnz(LAMBDA,END,U,Y+1,false);

    int nnz = gop.nnz(U,LAMBDA,U,LAMBDA,true);
    std::vector<int> rows(nnz), cols(nnz);
    std::vector<RT> data(nnz);
    typename std::vector<int>::iterator ir = rows.begin(), ic = cols.begin();
    typename std::vector<RT>::iterator id = data.begin();
    gop.toTriplet(U,LAMBDA,U,LAMBDA,ir,ic,id,true);
    nnz_h_lag = 0;
    for (int i=0; i<nnz; ++i)
      if (rows[i]>=cols[i])
        ++nnz_h_lag;

    index_style = C_STYLE;

    std::cout << "nu=" << nu << " ny=" << ny << " nl=" << nl << " nnz=" << nnz << " nnz_jac_g=" << nnz_jac_g << '\n';
    std::cout.flush();

    return true;
  }

  /** overload this method to return the information about the bound
   *  on the variables and constraints. The value that indicates
   *  that a bound does not exist is specified in the parameters
   *  nlp_lower_bound_inf and nlp_upper_bound_inf.  By default,
   *  nlp_lower_bound_inf is -1e19 and nlp_upper_bound_inf is
   *  1e19. (see TNLPAdapter) */
  virtual bool get_bounds_info(Index n, Number* x_l, Number* x_u,
                               Index m, Number* g_l, Number* g_u)
  {
    assert(n == ny+nu);
    assert(m == nl);

    f.boxConstraints(x_l+nu,x_u+nu,x_l,x_u);

    std::fill_n(g_l,nl,0);
    std::fill_n(g_u,nl,0);

    return true;
  }

  /** overload this method to return the starting point. The bools
   *  init_x and init_lambda are both inputs and outputs. As inputs,
   *  they indicate whether or not the algorithm wants you to
   *  initialize x and lambda respectively. If, for some reason, the
   *  algorithm wants you to initialize these and you cannot, set
   *  the respective bool to false.
   */
  virtual bool get_starting_point(Index n, bool init_x, Number* x,
                                  bool init_z, Number* z_L, Number* z_U,
                                  Index m, bool init_lambda,
                                  Number* lambda)
  {
    if (init_x) {
      std::vector<RT> data(n+m);
      xx.write(data.begin());
      // std::cout << "Initial u:\n";
      // std::copy(data.begin(),data.begin()+nu,std::ostream_iterator<RT>(std::cout," "));
      // std::cout << "\n";
      // std::cout.flush();

      std::copy(data.begin(),data.begin()+n,x);
    }
    if (init_z) {
      std::fill_n(z_L,n,1);
      std::fill_n(z_U,n,1);
    }
    if (init_lambda) {
      std::fill_n(lambda,m,0);
    }

    return true;
  }

  /** overload this method to return the value of the objective function */
  virtual bool eval_f(Index n, const Number* x, bool new_x,
                      Number& obj_value)
  {
    update(new_x,x,false,0);
    obj_value = gop.functional();

    return true;
  }

  /** overload this method to return the vector of the gradient of
   *  the objective w.r.t. x */
  virtual bool eval_grad_f(Index n, const Number* x, bool new_x,
                           Number* grad_f)
  {
    update(new_x,x,false,0);
    gop.toSequence(U,LAMBDA,grad_f);

    return true;
  }

  /** overload this method to return the vector of constraint values */
  virtual bool eval_g(Index n, const Number* x, bool new_x,
                      Index m, Number* g)
  {
    update(new_x,x,false,0);
    gop.toSequence(LAMBDA,END,g);

    return true;
  }

  /** overload this method to return the jacobian of the
   *  constraints. The vectors iRow and jCol only need to be set
   *  once. The first call is used to set the structure only (iRow
   *  and jCol will be non-NULL, and values will be NULL) For
   *  subsequent calls, iRow and jCol will be NULL. */
  virtual bool eval_jac_g(Index n, const Number* x, bool new_x,
                          Index m, Index nele_jac, Index* iRow,
                          Index *jCol, Number* values)
  {
    if (iRow != 0) {
      // provide sparsity structure
      std::vector<RT> data(nele_jac);
      typename std::vector<RT>::iterator v = data.begin();
      gop.toTriplet(LAMBDA,END,U,LAMBDA,iRow,jCol,v,false);
    } else {
      assert(values);

      update(new_x,x,false,0);

      std::vector<Index> idx(nele_jac);
      typename std::vector<Index>::iterator i = idx.begin(), j = idx.begin();
      gop.toTriplet(LAMBDA,END,U,LAMBDA,i,j,values,false);
    }

    return true;
  }

  /** overload this method to return the hessian of the
   *  lagrangian. The vectors iRow and jCol only need to be set once
   *  (during the first call). The first call is used to set the
   *  structure only (iRow and jCol will be non-NULL, and values
   *  will be NULL) For subsequent calls, iRow and jCol will be
   *  NULL. This matrix is symmetric - specify the lower diagonal
   *  only.  A default implementation is provided, in case the user
   *  wants to se quasi-Newton approximations to estimate the second
   *  derivatives and doesn't not neet to implement this method. */
  virtual bool eval_h(Index n, const Number* x, bool new_x,
                      Number obj_factor, Index m, const Number* lambda,
                      bool new_lambda, Index nele_hess,
                      Index* iRow, Index* jCol, Number* values)
  {
    int nnz = gop.nnz(U,LAMBDA,U,LAMBDA,true);
    assert(nnz==nele_hess);

    if (values) {
      if (obj_factor != f.ipoptSigma) {
        f.ipoptSigma = obj_factor;
        assert(lambda);
        new_lambda = true;
        std::cout << "new sigma: " << obj_factor << "\n";
      }
      update(new_x,x,new_lambda,lambda);

      std::vector<int> unused(nnz);
      std::vector<int>::iterator ri = unused.begin(), ci = unused.begin();
      gop.toTriplet(U,LAMBDA,U,LAMBDA,ri,ci,values,true);
    } else {
      std::vector<RT> unused(nnz);
      gop.toTriplet(U,LAMBDA,U,LAMBDA,iRow,jCol,unused.begin(),true);
    }

    return true;
  }

  /** This method is called when the algorithm is complete so the TNLP can store/write the solution */
  virtual void finalize_solution(SolverReturn status,
                                 Index n, const Number* x, const Number* z_L, const Number* z_U,
                                 Index m, const Number* g, const Number* lambda,
                                 Number obj_value)
  {
    int nnz = gop.nnz(U,END,U,END,false);
    std::vector<size_t> ri(nnz), ci(nnz);
    std::vector<double> di(nnz);
    gop.toTriplet(U,END,U,END,ri.begin(),ci.begin(),di.begin(),false);

    std::ofstream out("result.m");
    out << "function [A] = result()\n"
        << "ri = [\n";
    std::copy(ri.begin(),ri.end(),std::ostream_iterator<size_t>(out,"\n"));
    out << "];\nci = [\n";
    std::copy(ci.begin(),ci.end(),std::ostream_iterator<size_t>(out,"\n"));
    out << "];\ndi = [\n";
    std::copy(di.begin(),di.end(),std::ostream_iterator<double>(out,"\n"));
    out << "];\n"
        << "A = spconvert([ri+1 ci+1 di]);\n";

    std::vector<RT> data(n+nl,0);
    std::copy(x,x+n,data.begin());
    if (lambda)
      std::copy(lambda,lambda+nl,data.begin()+n);
    xx.read(data.begin());
    std::cout << "\nControl: ";
    std::copy(x,x+24,std::ostream_iterator<double>(std::cout," "));
    std::cout << "\n";

    writeVTKFile(varSet,xx,"solution");
  }

  // /** Intermediate Callback method for the user.  Providing dummy
  //  *  default implementation.  For details see IntermediateCallBack
  //  *  in IpNLP.hpp. */
  // virtual bool intermediate_callback(AlgorithmMode mode,
  //                                    Index iter, Number obj_value,
  //                                    Number inf_pr, Number inf_du,
  //                                    Number mu, Number d_norm,
  //                                    Number regularization_size,
  //                                    Number alpha_du, Number alpha_pr,
  //                                    Index ls_trials,
  //                                    const IpoptData* ip_data,
  //                                    IpoptCalculatedQuantities* ip_cq);

  /**
   * The current iterate.
   */
  typename VarSet::VariableSet xx;

private:
  void update(bool new_x, Number const* x, bool new_lambda, Number const* lambda)
  {
    if (new_x || new_lambda) {
      std::vector<RT> data(n+nl);
      xx.write(data.begin());

      if (new_x) {
        assert(x);
        std::copy(x,x+n,data.begin());
      }

      if (new_lambda) {
        assert(lambda);
        std::copy(lambda,lambda+nl,data.begin()+n);
      }

      xx.read(data.begin());

      // it's more efficient to simultaneously assemble functional/rhs/matrix
      boost::timer timer;
      gop.assemble(linearization(f,xx));
    }
  }

  VarSet const& varSet;
  Gop gop;
  Functional& f;
  int nu, ny, nl, n;
};

#endif


#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/console/parse.h>
#include <boost/filesystem.hpp>
#include <pcl/PCLPointCloud2.h>
#include <boost/thread/thread.hpp>

typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;

using namespace std;
using namespace sensor_msgs;
using namespace message_filters;

ros::Publisher pointcloud_pub;

int counter = 0;

pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");

boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
  viewer->addCoordinateSystem (1.0);
  viewer->initCameraParameters ();
  return (viewer);
}

void callback(const ImageConstPtr& image_color_msg,
		const ImageConstPtr& image_depth_msg,
		const CameraInfoConstPtr& info_color_msg,
		const CameraInfoConstPtr& info_depth_msg) {

        ::counter++;
        boost::mutex::scoped_lock(mutex_);

	cv::Mat image_color = cv_bridge::toCvCopy(image_color_msg)->image;

	//cv::Mat image_depth = cv_bridge::toCvCopy(image_depth_msg)->image;
        cv::Mat image_depth = cv_bridge::toCvShare(image_depth_msg)->image;

        image_depth.convertTo(image_depth, CV_32F, 1.0);

	cvtColor(image_color,image_color, CV_RGB2BGR);

	cout <<"depth[320,240]="<< image_depth.at<uint16_t>(cv::Point(320,240)) / 1000.0 <<"m"<< endl;
	//image_color.at<cv::Vec3b>(cv::Point(320,240)) = cv::Vec3b(255,255,255);

	// get camera intrinsics
	float fx = info_depth_msg->K[0];// 525.0
	float fy = info_depth_msg->K[4]; //525.0
	float cx = info_depth_msg->K[2]; //319.5
	float cy = info_depth_msg->K[5]; //239.5

	// produce a point cloud
	PointCloud::Ptr pointcloud_msg (new PointCloud);
	pointcloud_msg->header = pcl_conversions::toPCL(image_depth_msg->header);

	PointT pt;
	for(int y=0;y<image_depth.rows;y++) {
		for(int x=0;x<image_depth.cols;x++) {
			float depth = image_depth.at<float>(cv::Point(x,y));
                         cv::Vec3b color = image_color.at<cv::Vec3b>(cv::Point(x,y));
			if(depth>0 && depth<10) {
				pt.x = (x - cx) * depth / fx;
				pt.y = (y - cy) * depth / fy;
				pt.z = depth;

                                pt.r = (color[2]);
                                pt.g = (color[1]);
                                pt.b = (color[0]);
				//cout << pt.x<<" "<<pt.y<<" "<<pt.z<<endl;
				pointcloud_msg->points.push_back(pt);
			}
		}
	}

        //viewer = simpleVis(pointcloud_msg);
        ::viewer.showCloud(pointcloud_msg);
	pointcloud_msg->height = 1;
	pointcloud_msg->width = pointcloud_msg->points.size();
	pointcloud_pub.publish (pointcloud_msg);
	cv::imshow("color", image_depth);
        cv::imwrite("/images/1.jpg", image_depth);
	cv::waitKey(3);
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "RGBD_node");

	ros::NodeHandle nh;

	message_filters::Subscriber<Image> image_color_sub(nh,argv[1], 1);
	message_filters::Subscriber<Image> image_depth_sub(nh,argv[2], 1);
	message_filters::Subscriber<CameraInfo> info_color_sub(nh,argv[3], 1);
	message_filters::Subscriber<CameraInfo> info_depth_sub(nh,argv[4], 1);
	pointcloud_pub = nh.advertise<PointCloud> ("/RGBD/point_cloud", 1);

	typedef sync_policies::ApproximateTime<Image, Image, CameraInfo, CameraInfo> MySyncPolicy;
	Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_color_sub, image_depth_sub, info_color_sub, info_depth_sub);

	sync.registerCallback(boost::bind(&callback, _1, _2, _3, _4));

	ros::spin();

	return 0;
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/mnine.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check mnine behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::mnine;
  using boost::simd::Mnine;

  STF_TYPE_IS(decltype(Mnine<T>()), T);
  STF_EQUAL(Mnine<T>(), T(-9));
  STF_EQUAL(mnine( as(T{}) ),T(-9));
}

STF_CASE_TPL( "Check mnine behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::mnine;
  using boost::simd::Mnine;

  STF_TYPE_IS(decltype(Mnine<T>()), T);
  STF_IEEE_EQUAL(Mnine<T>(), T(-9));
  STF_IEEE_EQUAL(mnine( as(T{}) ), T(-9));
}


#include <boost/compute/experimental/tabulate.hpp>


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::data::mngr::responses.hpp                     //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_SURVIVAL_DATA_DATA_MNGR_RESPONSES_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_DATA_MNGR_RESPONSES_HPP_ER_2009
#include <algorithm>
#include <iterator>
#include <vector>
#include <boost/range.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/statistics/survival/data/data/record.hpp>
#include <boost/statistics/survival/data/algorithm/events.hpp>

namespace boost{
namespace statistics{
namespace survival{
namespace data{

    template<typename T>
    class responses_mngr{
        public:

        typedef event<T>                                        response_type;
        typedef std::vector<response_type>                      responses_type;

        // [ Construction ]
        responses_mngr();

        // [ Update ]
        void clear_responses();
        template<typename It>
        void update_responses(
            It b_r,It e_r, // records
            const T& t
        );

        // [ Access ]
        const responses_type&   responses()const;
        const T&                time()const;

        protected:
        // [ Archive ]
        friend class boost::serialization::access;

        template<class Archive>
        void serialize(Archive & ar, const unsigned int version);

        // [ Variables ]
        responses_type  responses_;
        T               t_;
    };

    // Implementation //

    // [ Construction ]
    template<typename T>
    responses_mngr<T>::responses_mngr(){}

    // [ Update ]

    template<typename T>
    void responses_mngr<T>::clear_responses(){
        (this->responses_).clear();
        this->t_ = static_cast<T>(0);
    }

    template<typename T>
    template<typename It>
    void responses_mngr<T>::update_responses(It b_r,It e_r,const T& t){
        this->responses_.clear();
        this->responses_.reserve( std::distance(b_r,e_r) );
        events(
            b_r,
            e_r,
            t,
            std::back_inserter( this->responses_ )
        );
        this->t_ = t;
    }

    // [ Access ]

    template<typename T>
    const typename responses_mngr<T>::responses_type&
    responses_mngr<T>::responses()const{
        return this->responses_;
    }

    template<typename T>
    const T&
    responses_mngr<T>::time()const{
        return this->t_;
    }

    // [ Archive ]

    template<typename T>
        template<class Archive>
    void responses_mngr<T>::serialize(Archive & ar, const unsigned int version){
        ar & ( this->responses_ );
        ar & ( this->t_ );
    }

}// data
}// survival
}// statistics
}// boost

#endif