#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include "pcl_ros/point_cloud.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/impl/kdtree.hpp>
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>

#define TINYCOLORMAP_WITH_EIGEN

#include "tinycolormap.hpp"

using namespace std;

ros::Publisher color_pc_pub;
typedef pcl::PointXYZRGB PointType;
pcl::PointCloud<PointType> color_pc;
struct ColorMapParams {
    double max_height;
    double min_height;
    double range;
    string map_type_name, frame_id;
    bool init_map_success{false};
    bool static_pc_mode,inverse_color{false};
    double publish_rate;
} cmp_;

void PubColorCloudCallback(const ros::TimerEvent &e) {
    if (!cmp_.init_map_success) {
        return;
    }
    color_pc_pub.publish(color_pc);
}

void DoubleCalp(double &data_in, const double min_value, const double max_value) {
    data_in = data_in > max_value ? max_value : data_in;
    data_in = data_in < min_value ? min_value : data_in;
}

void PointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &msg) {
    if (cmp_.init_map_success) {
        return;
    }
    tinycolormap::ColormapType ct;
    if (cmp_.map_type_name == "Parula") {
        ct = tinycolormap::ColormapType::Parula;
    }
    if (cmp_.map_type_name == "Heat") {
        ct = tinycolormap::ColormapType::Heat;
    }
    if (cmp_.map_type_name == "Jet") {
        ct = tinycolormap::ColormapType::Jet;
    }
    if (cmp_.map_type_name == "Turbo") {
        ct = tinycolormap::ColormapType::Turbo;
    }
    if (cmp_.map_type_name == "Hot") {
        ct = tinycolormap::ColormapType::Hot;
    }
    if (cmp_.map_type_name == "Gray") {
        ct = tinycolormap::ColormapType::Gray;
    }
    if (cmp_.map_type_name == "Magma") {
        ct = tinycolormap::ColormapType::Magma;
    }
    if (cmp_.map_type_name == "Inferno") {
        ct = tinycolormap::ColormapType::Inferno;
    }
    if (cmp_.map_type_name == "Plasma") {
        ct = tinycolormap::ColormapType::Plasma;
    }
    if (cmp_.map_type_name == "Viridis") {
        ct = tinycolormap::ColormapType::Viridis;
    }
    if (cmp_.map_type_name == "Cividis") {
        ct = tinycolormap::ColormapType::Cividis;
    }
    if (cmp_.map_type_name == "Github") {
        ct = tinycolormap::ColormapType::Github;
    }
    pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
    pcl_conversions::toPCL(*msg, *cloud);
    pcl::fromPCLPointCloud2( *cloud, color_pc);

    for (size_t i = 0; i < color_pc.size(); i++) {
        double color_id = color_pc.points[i].z;
        DoubleCalp(color_id, cmp_.min_height, cmp_.max_height);
        color_id -= cmp_.min_height;
        color_id /= cmp_.range;
        if(cmp_.inverse_color){
            color_id = 1.0 - color_id;
        }
        Eigen::Vector3d color_mag = tinycolormap::GetColor(color_id, ct).ConvertToEigen();
        color_pc.points[i].r = static_cast<uint8_t>((color_mag[0] * 255));
        color_pc.points[i].g = static_cast<uint8_t>((color_mag[1] * 255));;
        color_pc.points[i].b = static_cast<uint8_t>((color_mag[2] * 255));;
    }

    std_msgs::Header hd;
    hd.frame_id = cmp_.frame_id;
    pcl_conversions::toPCL(hd, color_pc.header);

    cmp_.init_map_success = true;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "color_map_pc2");
    ros::NodeHandle n("~");

    ros::Subscriber pc_sub = n.subscribe("cloud", 1, PointCloudCallback);

    color_pc_pub = n.advertise<pcl::PointCloud<pcl::PointXYZRGB>>("color_cloud", 1);
    n.param("color/min_height", cmp_.min_height, 0.0);
    n.param("color/max_height", cmp_.max_height, 5.0);
    n.param("color/map_type_name", cmp_.map_type_name, std::string("Turbo"));
    n.param("color/frame_id", cmp_.frame_id, std::string("world"));
    n.param("color/publish_rate", cmp_.publish_rate, 1.0);
    n.param("color/inverse_color", cmp_.inverse_color, false);
    cmp_.range = cmp_.max_height - cmp_.min_height;
    double dt = 1.0 / cmp_.publish_rate;
    ros::Timer pub_pc_timer = n.createTimer(ros::Duration(dt), &PubColorCloudCallback);

    ros::AsyncSpinner spinner(0);
    spinner.start();

    ros::waitForShutdown();
}


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, University of Stuttgart
 *  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 University of Stuttgart 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 OWNER 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.
 *********************************************************************/

/* Author: Andreas Orthey */

#include "MultiLevelPlanningCommon.h"
#include "MultiLevelPlanningHyperCubeCommon.h"
#include <ompl/base/spaces/RealVectorStateSpace.h>

#include <ompl/multilevel/planners/qrrt/QRRT.h>

#include <ompl/tools/benchmark/Benchmark.h>
#include <ompl/util/String.h>

#include <boost/math/constants/constants.hpp>
#include <boost/format.hpp>
#include <fstream>

const unsigned int ndim = 100;

int main()
{
    auto space(std::make_shared<ompl::base::RealVectorStateSpace>(ndim));
    ompl::base::RealVectorBounds bounds(ndim);
    ompl::geometric::SimpleSetup ss(space);
    ompl::base::ScopedState<> start(space), goal(space);

    ob::SpaceInformationPtr si = ss.getSpaceInformation();

    bounds.setLow(0.);
    bounds.setHigh(1.);
    space->setBounds(bounds);
    ss.setStateValidityChecker(std::make_shared<HyperCubeValidityChecker>(si, ndim));
    si->setStateValidityCheckingResolution(0.001);
    for (unsigned int i = 0; i < ndim; ++i)
    {
        start[i] = 0.;
        goal[i] = 1.;
    }
    ss.setStartAndGoalStates(start, goal);

    std::vector<int> admissibleProjection = getHypercubeAdmissibleProjection(ndim);
    ob::PlannerPtr planner =
      GetMultiLevelPlanner<ompl::multilevel::QRRT>(admissibleProjection, si, "QRRT");
    ss.setPlanner(planner);

    bool solved = ss.solve(1.0);

    double timeToCompute = ss.getLastPlanComputationTime();

    if (solved)
    {
        const ob::ProblemDefinitionPtr pdef = planner->getProblemDefinition();
        std::cout << std::string(80, '-') << std::endl;
        pdef->getSolutionPath()->print(std::cout);
        std::cout << std::string(80, '-') << std::endl;
        OMPL_INFORM("Solved hypercube with %d dimensions after %f seconds.", ndim, timeToCompute);
    }else
    {
      OMPL_ERROR("Failed finding solution after %f seconds.", timeToCompute);
    }

    return 0;
}


#include <boost/numeric/conversion/cast.hpp>

#include <iostream>
#include <climits>
#include <cfloat>   // for DBL_MAX (Peter Schmid)

#include "../third-party/catch.h"
#include "../third-party/half.hpp"

TEST_CASE("boost numeric_cast", "[boost::numeric_cast]")
{

    using namespace std;
    SECTION("Test user-defined type, half_float::half")
    {
        std::cout << "=========" << "half float" << "==============\n";
        using namespace half_float;

        int8_t v1 = boost::numeric_cast<int8_t>(half{1});
        half h1 = boost::numeric_cast<half>(100);
        std::cout << "boost::numeric_cast<half>(100) = " << h1 << std::endl;

        REQUIRE_THROWS_AS( boost::numeric_cast<int8_t>(half{1000}), boost::numeric::bad_numeric_cast);
        // equal expression
        try{
            int8_t v1 = boost::numeric_cast<int8_t>(half{1000});
            std::cout << "boost::numeric_cast<int8_t>(half{1000}) = " << v1 << std::endl;
        }
        catch(boost::numeric::bad_numeric_cast& e)
        {
            std::cout << "`boost::numeric_cast<int8_t>(half{1000})` error : " << e.what() << '\n';
        }

    }

    SECTION("boost::numeric_cast for builtin types")
    {

        REQUIRE_THROWS_AS( boost::numeric_cast<int>( DBL_MAX ) == 0, boost::numeric::bad_numeric_cast);

    }

}


//=======================================================================
// Copyright (c) 2013 Robert Rosolek
//
// 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)
//=======================================================================
/**
 * @file demand_query_example.cpp
 * @brief
 * @author Robert Rosolek
 * @version 1.0
 * @date 2014-6-9
 */

#include "paal/auctions/auction_components.hpp"

#include <boost/optional/optional.hpp>
#include <boost/range/algorithm/copy.hpp>

#include <algorithm>
#include <cassert>
#include <vector>

//! [Demand Query Auction Components Example]

namespace pa = paal::auctions;
namespace pds = paal::data_structures;

using Bidder = std::string;
using Item = std::string;
using Items = std::vector<Item>;
using Value = int;

const std::vector<Bidder> bidders {"Pooh Bear", "Rabbit"};

const Items items {"honey", "baby carrot", "carrot", "jam"};

struct demand_query_func {
   template <class GetPrice>
   std::pair<Items, Value>
   operator()(Bidder bidder, GetPrice get_price) const
   {
      if (bidder == "Pooh Bear") {
         const Value util = 10 - get_price("honey");
         if (util <= 0) return std::make_pair(Items{}, 0);
         return std::make_pair(Items{"honey"}, util);
      }

      assert(bidder == "Rabbit");

      const Value baby_val = 2, val = 3;
      auto const baby_price = get_price("baby carrot"), price = get_price("carrot");

      const Value baby_util = baby_val - baby_price,
         util = val - price, both_util = baby_val + val - baby_price - price;

      if (baby_util <= 0 && util <= 0 && both_util <= 0) return std::make_pair(Items{}, 0);

      if (baby_util >= util && baby_util >= both_util)
         return std::make_pair(Items{"baby carrot"}, baby_util);

      if (util >= both_util)
         return std::make_pair(Items{"carrot"}, util);

      return std::make_pair(Items{"baby carrot", "carrot"}, both_util);
   }
};
//! [Demand Query Auction Components Example]

int main() {
   //! [Demand Query Auction Create Example]
   auto const auction = pa::make_demand_query_auction_components(
      bidders, items, demand_query_func()
   );
   //! [Demand Query Auction Create Example]

   //! [Demand Query Auction Use Example]
   auto get_price = [](Item item) { return item == "honey" ? 5 : 2; };

   std::cout << "pooh bear buys: ";
   auto got_pooh_bear = auction.call<pa::demand_query>("Pooh Bear", get_price);
   boost::copy(got_pooh_bear.first, std::ostream_iterator<Item>(std::cout, ", "));
   std::cout << std::endl;

   std::cout << "rabbit oracle buys: ";
   auto got_rabbit = auction.call<pa::demand_query>("Rabbit", get_price);
   boost::copy(got_rabbit.first, std::ostream_iterator<Item>(std::cout, ", "));
   std::cout << std::endl;
   //! [Demand Query Auction Use Example]
   return 0;
}


#include <app/duplicate_instance_renderer.h>
#include <glb/camera.h>
#include <glb/framebuffer.h>
#include <glb/shader_program_builder.h>
#include <glb/opengl.h>
#include <tess/tessellator.h>
#include <Eigen/Eigenvalues>
#include <rvm/MaterialTable.h>

namespace app
{
	// ---------------------------------------------------------------------------------------------------------------------------------------------------------
	// global definitions
	// ---------------------------------------------------------------------------------------------------------------------------------------------------------

	struct EigenOBB
	{
		EigenVec3 center;
		EigenVec3 half_extents;
		EigenMat3 basis;
	};

	struct bbox
	{
		vec3 min = vec3(math::limit_posf());
		float pad0 = 1.0f;
		vec3 max = vec3(math::limit_negf());
		float pad1 = 1.0f;

		void expand(const vec3& v)
		{
			min.x = math::min(min.x, v.x);
			min.y = math::min(min.y, v.y);
			min.z = math::min(min.z, v.z);
			max.x = math::max(max.x, v.x);
			max.y = math::max(max.y, v.y);
			max.z = math::max(max.z, v.z);
		}
	};

	// ---------------------------------------------------------------------------------------------------------------------------------------------------------
	// global constants
	// ---------------------------------------------------------------------------------------------------------------------------------------------------------

	static const unsigned int MAX_BUFFER_SIZE_BYTES = 5 * 1024 * 1024;
	static const double EPSILON = 1e-3;
	static const int TRANSFORM_TEX_UNIT = 0;
	static const int TEX_OFFSET_ATTRIB = 5;
	static const int COLOR_IDS_TEX_UNIT = 1;
	static const int COLORS_TEX_UNIT = 2;

	// ---------------------------------------------------------------------------------------------------------------------------------------------------------
	// helper functions
	// ---------------------------------------------------------------------------------------------------------------------------------------------------------

	static bool vec3_equal(const vec3& a, const vec3& b)
	{
		const auto err_x = math::abs(a.x - b.x);
		const auto err_y = math::abs(a.y - b.y);
		const auto err_z = math::abs(a.z - b.z);
		return err_x <= EPSILON && err_y <= EPSILON && err_z <= EPSILON;
	}

	static bool eigen3_equal(const EigenVec3& a, const EigenVec3& b)
	{
		const auto diff = (a-b).cwiseAbs();
		return diff.coeff(0) <= EPSILON && diff.coeff(1) <= EPSILON && diff.coeff(2) <= EPSILON;
	}

	static bool eigen3_dist_equal(const EigenVec3& a, const EigenVec3& b)
	{
		return (a-b).norm() <= EPSILON;
	}

	static bool eigen4_equal(const EigenVec4& a, const EigenVec4& b)
	{
		const auto diff = (a-b).cwiseAbs();
		return diff.coeff(0) <= EPSILON && diff.coeff(1) <= EPSILON && diff.coeff(2) <= EPSILON && a.coeff(3) == 1.0 && b.coeff(3) == 1.0;
	}

	static EigenVec3Array to_eigen(const vector<vec3>& input)
	{
		EigenVec3Array output(3, input.size());

		for(unsigned int i = 0; i < input.size(); ++i)
		{
			output.col(i) = EigenVec3(input[i].x, input[i].y, input[i].z);
		}

		return output;
	}

	static vector<vec3> to_bl(const EigenVec3Array& input)
	{
		vector<vec3> output(input.cols());

		for(unsigned int i = 0; i < input.cols(); ++i)
		{
			output[i] = vec3(input.col(i).coeff(0), input.col(i).coeff(1), input.col(i).coeff(2));
		}

		return output;
	}

	static vec3 vec3_to_bl(const EigenVec3& input)
	{
		return vec3(input.coeff(0), input.coeff(1), input.coeff(2));
	}

	static mat4 mat3_to_bl(const EigenMat3& input)
	{
		return mat4(input.coeff(0,0), input.coeff(0,1), input.coeff(0,2), 0.0f,
					input.coeff(1,0), input.coeff(1,1), input.coeff(1,2), 0.0f,
					input.coeff(2,0), input.coeff(2,1), input.coeff(2,2), 0.0f,
					0.0f, 0.0f, 0.0f, 1.0f);
	}

	static mat4 mat4_to_bl(const EigenMat4& input)
	{
		return mat4(input.coeff(0,0), input.coeff(0,1), input.coeff(0,2), input.coeff(0,3),
					input.coeff(1,0), input.coeff(1,1), input.coeff(1,2), input.coeff(1,3),
					input.coeff(2,0), input.coeff(2,1), input.coeff(2,2), input.coeff(2,3),
					input.coeff(3,0), input.coeff(3,1), input.coeff(3,2), input.coeff(3,3));
	}

	static EigenMat4 to_mat4(const EigenMat3& input)
	{
		EigenMat4 m;
		m <<	input.coeff(0,0), input.coeff(0,1), input.coeff(0,2), 0.0f,
				input.coeff(1,0), input.coeff(1,1), input.coeff(1,2), 0.0f,
				input.coeff(2,0), input.coeff(2,1), input.coeff(2,2), 0.0f,
				0.0f, 0.0f, 0.0f, 1.0f;
		return m;
	}

	static bool is_identity(const mat4& i)
	{
		static const float EPS = 1e-6f;
		return  abs(i.at(0,0) - 1.0) <= EPS && abs(i.at(0,1) - 0.0) <= EPS && abs(i.at(0,2) - 0.0) <= EPS && abs(i.at(0,3) - 0.0) <= EPS &&
				abs(i.at(1,0) - 0.0) <= EPS && abs(i.at(1,1) - 1.0) <= EPS && abs(i.at(1,2) - 0.0) <= EPS && abs(i.at(1,3) - 0.0) <= EPS &&
				abs(i.at(2,0) - 0.0) <= EPS && abs(i.at(2,1) - 0.0) <= EPS && abs(i.at(2,2) - 1.0) <= EPS && abs(i.at(2,3) - 0.0) <= EPS &&
				abs(i.at(3,0) - 0.0) <= EPS && abs(i.at(3,1) - 0.0) <= EPS && abs(i.at(3,2) - 0.0) <= EPS && abs(i.at(3,3) - 1.0) <= EPS;
	}

	static bool is_identity(const EigenMat3& i)
	{
		static const float EPS = 1e-12;
		return  abs(i.coeff(0,0) - 1.0) <= EPS && abs(i.coeff(0,1) - 0.0) <= EPS && abs(i.coeff(0,2) - 0.0) <= EPS &&
				abs(i.coeff(1,0) - 0.0) <= EPS && abs(i.coeff(1,1) - 1.0) <= EPS && abs(i.coeff(1,2) - 0.0) <= EPS &&
				abs(i.coeff(2,0) - 0.0) <= EPS && abs(i.coeff(2,1) - 0.0) <= EPS && abs(i.coeff(2,2) - 1.0) <= EPS;
	}

	static bool is_identity(const EigenMat4& i)
	{
		static const float EPS = 1e-12;
		return  abs(i.coeff(0,0) - 1.0) <= EPS && abs(i.coeff(0,1) - 0.0) <= EPS && abs(i.coeff(0,2) - 0.0) <= EPS && abs(i.coeff(0,3) - 0.0) <= EPS &&
				abs(i.coeff(1,0) - 0.0) <= EPS && abs(i.coeff(1,1) - 1.0) <= EPS && abs(i.coeff(1,2) - 0.0) <= EPS && abs(i.coeff(1,3) - 0.0) <= EPS &&
				abs(i.coeff(2,0) - 0.0) <= EPS && abs(i.coeff(2,1) - 0.0) <= EPS && abs(i.coeff(2,2) - 1.0) <= EPS && abs(i.coeff(2,3) - 0.0) <= EPS &&
				abs(i.coeff(3,0) - 0.0) <= EPS && abs(i.coeff(3,1) - 0.0) <= EPS && abs(i.coeff(3,2) - 0.0) <= EPS && abs(i.coeff(3,3) - 1.0) <= EPS;
	}

	static EigenOBB compute_obb(const EigenVec3Array& src)
	{
		EigenOBB obb;

		// 1. compute geometric center
		EigenVec3 mean = src.rowwise().mean();

		// 2. use PCA to find orthonormal basis centered in average
		EigenVec3Array centered = src.colwise() - mean;
		EigenMat3 covariance_matrix = centered * centered.transpose();

		Eigen::SelfAdjointEigenSolver<EigenMat3> eig(covariance_matrix);
		obb.basis = eig.eigenvectors().rightCols(3);

		// 3. describe src points in local basis: subtract by center and project onto basis vectors
		EigenVec3Array local_src = obb.basis * centered;

		// 4. find minimum and maximum coordinates in local basis
		EigenVec3 local_min = local_src.rowwise().minCoeff();
		EigenVec3 local_max = local_src.rowwise().maxCoeff();

		// 5. compute oriented box center
		EigenVec3 local_center = (local_min + local_max) * 0.5f;
		obb.center = mean + obb.basis * local_center;

		// 6. compute oriented box half-extents (aka half-scale along each local axis)
		obb.half_extents = (local_max - local_min) * 0.5f;

		return obb;
	}

	static mat4 estimate_transform_similarity(const vector<vec3>& src_points, const vector<vec3>& dst_points)
	{
		return mat4_to_bl(Eigen::umeyama(to_eigen(src_points), to_eigen(dst_points), true));
	}

	static mat4 estimate_transform_3x3(const EigenVec3Array& src,
									   const EigenVec3& src_mean, const EigenVec3Array& src_local, const EigenMat3& Aqq,
									   const EigenVec3Array& dst,
									   const EigenVec3& dst_mean, const EigenVec3Array& dst_local,
									   double& error)
	{
		EigenMat3 Apq = dst_local * src_local.transpose();

		EigenMat3 A = Apq * Aqq;
		EigenMat4 A4 = to_mat4(A);

		EigenMat4 M = Eigen::Affine3d(Eigen::Translation3d(dst_mean)).matrix() * A4 * Eigen::Affine3d(Eigen::Translation3d(-src_mean)).matrix();

		error = 0.0;
		for(unsigned int i = 0; i < src.cols(); ++i)
		{
			EigenVec3 s3 = src.col(i);
			EigenVec4 s4(s3.coeff(0), s3.coeff(1), s3.coeff(2), 1.0);
			EigenVec4 ts = M * s4;
			EigenVec3 s(ts.coeff(0), ts.coeff(1), ts.coeff(2));
			EigenVec3 d = dst.col(i);
			error += (d - s).squaredNorm();
		}

		return mat4_to_bl(M);
	}

	static mat4 estimate_transform_4x4(const vector<vec3>& src_pts, const vector<vec3>& dst_pts, double& error)
	{
		EigenVec3Array src = to_eigen(src_pts);
		EigenVec3Array dst = to_eigen(dst_pts);

		EigenVec3 src_mean = src.rowwise().mean();
		EigenVec3Array src_local = src.colwise() - src_mean;

		EigenVec3 dst_mean = dst.rowwise().mean();
		EigenVec3Array dst_local = dst.colwise() - dst_mean;

		EigenVec4Array src_local4(4, src_local.cols());
		src_local4.row(0) = src_local.row(0);
		src_local4.row(1) = src_local.row(1);
		src_local4.row(2) = src_local.row(2);
		src_local4.row(3).setConstant(1.0);

		EigenVec4Array dst_local4(4, dst_local.cols());
		dst_local4.row(0) = dst_local.row(0);
		dst_local4.row(1) = dst_local.row(1);
		dst_local4.row(2) = dst_local.row(2);
		dst_local4.row(3).setConstant(1.0);

		EigenMat4 Apq = dst_local4 * src_local4.transpose();
		EigenMat4 Aqq = (src_local4 * src_local4.transpose()).inverse();

		EigenMat4 A = Apq * Aqq;

		EigenMat4 M = Eigen::Affine3d(Eigen::Translation3d(dst_mean)).matrix() * A * Eigen::Affine3d(Eigen::Translation3d(-src_mean)).matrix();

		error = 0.0;
		for(unsigned int i = 0; i < src.cols(); ++i)
		{
			EigenVec3 s3 = src.col(i);
			EigenVec4 s4(s3.coeff(0), s3.coeff(1), s3.coeff(2), 1.0);
			EigenVec4 ts = M * s4;
			EigenVec3 s(ts.coeff(0), ts.coeff(1), ts.coeff(2));
			EigenVec3 d = dst.col(i);
			error += (d - s).squaredNorm();
		}

		return mat4_to_bl(M);
	}

	// ---------------------------------------------------------------------------------------------------------------------------------------------------------
	// public
	// ---------------------------------------------------------------------------------------------------------------------------------------------------------

	void duplicate_instance_renderer::set_current_color(unsigned char color_id)
	{
		_current_color_id = color_id;
	}

	void duplicate_instance_renderer::add_box(const box& b, const mat4& transform)
	{
		_add_mesh(tess::tessellate_box(b.extents), transform);
	}

	void duplicate_instance_renderer::add_circular_torus(const circular_torus& c, const mat4& transform)
	{
		_add_mesh(tess::tessellate_circular_torus(c.in_radius, c.out_radius, c.sweep_angle), transform);
	}

	void duplicate_instance_renderer::add_cone(const cone& c, const mat4& transform)
	{
		_add_mesh(tess::tessellate_cone(c.top_radius, c.bottom_radius, c.height), transform);
	}

	void duplicate_instance_renderer::add_cone_offset(const cone_offset& c, const mat4& transform)
	{
		_add_mesh(tess::tessellate_cone_offset(c.top_radius, c.bottom_radius, c.height, c.offset), transform);
	}

	void duplicate_instance_renderer::add_cylinder(const cylinder& c, const mat4& transform)
	{
		_add_mesh(tess::tessellate_cylinder(c.radius, c.height), transform);
	}

	void duplicate_instance_renderer::add_cylinder_offset(const cylinder_offset& c, const mat4& transform)
	{
		_add_mesh(tess::tessellate_cylinder_offset(c.radius, c.height, c.offset), transform);
	}

	void duplicate_instance_renderer::add_cylinder_slope(const cylinder_slope& c, const mat4& transform)
	{
		_add_mesh(tess::tessellate_cylinder_slope(c.radius, c.height, c.top_slope_angles, c.bottom_slope_angles), transform);
	}

	void duplicate_instance_renderer::add_dish(const dish& d, const mat4& transform)
	{
		_add_mesh(tess::tessellate_dish(d.radius, d.height), transform);
	}

	void duplicate_instance_renderer::add_mesh(const tess::triangle_mesh& m, const mat4& transform)
	{
		_add_mesh(m, transform, true);
	}

	void duplicate_instance_renderer::add_pyramid(const pyramid& p, const mat4& transform)
	{
		_add_mesh(tess::tessellate_pyramid(p.top_extents, p.bottom_extents, p.height, p.offset), transform);
	}

	void duplicate_instance_renderer::add_rectangular_torus(const rectangular_torus& rt, const mat4& transform)
	{
		_add_mesh(tess::tessellate_rectangular_torus(rt.in_radius, rt.out_radius, rt.in_height, rt.sweep_angle), transform);
	}

	void duplicate_instance_renderer::add_sphere(const sphere& s, const mat4& transform)
	{
		_add_mesh(tess::tessellate_sphere(s.radius), transform);
	}

	void duplicate_instance_renderer::end_upload()
	{
//		auto aspect = 1.0f;
//		auto fovy = math::to_radians(60.0f);
//		auto yscale = 1.0f / math::tan(fovy*0.5f);
//		auto xscale = yscale / aspect;
//		auto znear = 2.0f;
//		auto zfar = 1000.0f;

//		auto t0 = mat4::translation({0,0,-1});
//		auto t1 = mat4::translation({5,0,0});
//		auto glp = mat4::perspective(fovy, aspect, znear, zfar);
////		glp.at(3,2) = 1.0f;

//		auto dxp = mat4(xscale,   0.0f,                      0.0f,  0.0f,
//						  0.0f, yscale,                      0.0f,  0.0f,
//						  0.0f,   0.0f,         zfar/(znear-zfar), -1.0f,
//						  0.0f,   0.0f, (znear*zfar)/(znear-zfar),  0.0f);

//		add_box({{1,1,1}}, glp.mul(t0));
//		add_pyramid({{1,1}, {2,2}, {0,0}, 1}, t1);

		const auto unique_mesh_count = _unique_meshes.size();
		_instance_sets.reserve(unique_mesh_count);

		glb::vertex_specification spec;
		spec.setup_vertex_buffer(glb::usage_static_draw, _total_vbo_size_bytes);
		spec.setup_element_buffer(glb::usage_static_draw, _total_ebo_size_bytes);
		spec.add_vertex_attrib({3, glb::type_float, false, sizeof(tess::vertex), 0});
		spec.add_vertex_attrib({3, glb::type_float, false, sizeof(tess::vertex), sizeof(vec3)});

		if(!_vao_builder.initialize(spec, glb::mode_triangles, glb::type_uint))
		{
			return;
		}

		_vao_builder.begin();

		_transform_buffer.create(glb::target_texture_buffer, glb::usage_static_draw, _total_geometries * sizeof(mat34));
		_transform_texture.create(TRANSFORM_TEX_UNIT, glb::target_texture_buffer);
		_transform_texture.set_data_source(glb::internal_format_rgba32f, _transform_buffer);

		_color_id_buffer.create(glb::target_texture_buffer, glb::usage_static_draw, _total_geometries * sizeof(unsigned char));
		_color_ids_texture.create(COLOR_IDS_TEX_UNIT, glb::target_texture_buffer);
		_color_ids_texture.set_data_source(glb::internal_format_r8ui, _color_id_buffer);

//		map<int, int> histogram;
//		vector<int> instance_count;

		auto rc = make_random(0.2f, 0.7f);

//#define WRITE(c) {decltype(c) x = c; file.write((char*)&x, sizeof(x));}

//		std::ofstream file("caos.xfm", std::ios::out | std::ios::binary);

//		WRITE(_unique_meshes.size());
//		file.flush();

		for(auto& itr : _unique_meshes)
		{
			auto& ps = itr.second;

//			std::vector<mat34> xfms;

//			for(const auto& t : ps.transforms)
//			{
//				bbox b;
//				for(const auto& v : ps.mesh.vertices)
//				{
//					b.expand(t.as_mat4().mul(v.position));
//				}
//				mat4 m = mat4::translation((b.min+b.max)*0.5f).mul(mat4::scale(b.max-b.min));
//				xfms.push_back(mat34(m));
//			}

//			WRITE(xfms.size());
//			file.flush();
//			file.write((char*)xfms.data(), sizeof(mat34)*xfms.size());
//			file.flush();

			instance_set instances;
			instances.element_count = ps.mesh.elements.size();
			instances.element_byte_offset = _vao_builder._spec.get_element_buffer().get_size_bytes();
			instances.tex_offset = _transform_buffer.get_count();
			instances.count = ps.transforms.size();
			instances.color = vec3(rc(), rc(), rc());
			_instance_sets.push_back(instances);

			_vao_builder.add_mesh(ps.mesh.vertices.data(), ps.mesh.vertices.size(), ps.mesh.elements.data(), ps.mesh.elements.size());

			_transform_buffer.add(ps.transforms.data(), ps.transforms.size());
			_color_id_buffer.add(ps.color_ids.data(), ps.color_ids.size());

			_cpu_transform_buffer.insert(_cpu_transform_buffer.end(), ps.transforms.begin(), ps.transforms.end());
			_cpu_color_id_buffer.insert(_cpu_color_id_buffer.end(), ps.color_ids.begin(), ps.color_ids.end());

			_total_memory += ps.transforms.size() * sizeof(mat34) + ps.color_ids.size() * sizeof(unsigned char);

//			histogram[ps.transforms.size()]++;
//			instance_count.push_back(ps.transforms.size());
		}

//		file.close();

//		_cpu_transform_buffer2.resize(_cpu_transform_buffer.size());

		_vao_builder.end();

		_main_vao = _vao_builder.get_vertex_arrays()[0];

//		int max_count = 0;
//		for(auto c : instance_count)
//		{
//			io::print(c);
//			max_count = math::max(max_count, c);
//		}
//		io::print("max:", max_count);

//		for(auto c : histogram)
//		{
//			io::print("[#instances,#ocurrences]:", c.first, c.second);
//		}

		// free memory
		_unique_meshes = decltype(_unique_meshes)();

		// CAD color table
		rvm::MaterialTable color_table;

		glb::buffer colors_buffer;
		colors_buffer.create(glb::target_texture_buffer, glb::usage_static_draw, color_table.getNumberMaterials() * sizeof(color));

		for(int i = 0; i < color_table.getNumberMaterials(); ++i)
		{
			auto diffuse = vec3(color_table.getMaterial(i).diffuseColor);
			color c;
			c.rgba[0] = 255 * diffuse.x;
			c.rgba[1] = 255 * diffuse.y;
			c.rgba[2] = 255 * diffuse.z;
			if(c.rgba[0] == 0 && c.rgba[1] == 0 && c.rgba[2] == 0)
			{
				c.rgba[0] = 64;
				c.rgba[1] = 64;
				c.rgba[2] = 64;
			}
			colors_buffer.add(c);
		}
		_colors_texture.create(COLORS_TEX_UNIT, glb::target_texture_buffer);
		_colors_texture.set_data_source(glb::internal_format_rgba8ui, colors_buffer);

		io::print("unique meshes:", unique_mesh_count);
		io::print("geometries:", _total_geometries);
		io::print("triangles:", _total_triangles);
		io::print("-- memory matching:", _total_memory / 1024.0f / 1024.0f, "MB");
	}

	bool duplicate_instance_renderer::initialize(glb::framebuffer& fbuffer, glb::camera& cam)
	{
		fbuffer.set_clear_color(0, 1.0f, 1.0f, 1.0f);

		glb::shader_program_builder shader_builder;
		shader_builder.begin();
		if(!shader_builder.add_file(glb::shader_vertex, "../shaders/duplicate_instance.vert"))
		{
			return false;
		}
//		if(!shader_builder.add_file(glb::shader_vertex, "../shaders/pass_through.vert"))
//		{
//			return false;
//		}
//		if(!shader_builder.add_file(glb::shader_geometry, "../shaders/duplicate_instance.geom"))
//		{
//			return false;
//		}
		if(!shader_builder.add_file(glb::shader_fragment, "../shaders/per_pixel_lighting_color.frag"))
		{
			return false;
		}
		shader_builder.bind_vertex_attrib("in_position", 0);
		shader_builder.bind_vertex_attrib("in_normal", 1);
		shader_builder.bind_vertex_attrib("in_tex_offset", TEX_OFFSET_ATTRIB);
		shader_builder.bind_vertex_attrib("in_color", 7);
		shader_builder.bind_draw_buffer("out_color", fbuffer.get_color_buffer_to_display());
		if(!shader_builder.end())
		{
			return false;
		}
		_shader = shader_builder.get_shader_program();
		_shader.bind_uniform_buffer("camera_uniform_block", cam.get_uniform_buffer());
		_shader.set_uniform("tex_transforms", TRANSFORM_TEX_UNIT);
		_shader.set_uniform("tex_colorIDs", COLOR_IDS_TEX_UNIT);
		_shader.set_uniform("tex_colors", COLORS_TEX_UNIT);

		return true;
	}

	bool duplicate_instance_renderer::finalize()
	{
		return true;
	}

	void duplicate_instance_renderer::render()
	{
//		static auto rid = make_random(0, 255);
//		static auto rt = make_random(-1.0f, 1.0f);

//		for(auto& id : _cpu_color_id_buffer)
//		{
//			id = rid();
//		}
//		_color_id_buffer.replace(_cpu_color_id_buffer.data(), _cpu_color_id_buffer.size());

//		for(unsigned int i = 0; i < _cpu_transform_buffer.size(); ++i)
//		{
//			const auto& t = _cpu_transform_buffer[i];
//			_cpu_transform_buffer2[i] = mat34(mat4::translation({rt(), rt(), rt()}).mul(t.as_mat4()));
//		}
//		_transform_buffer.replace(_cpu_transform_buffer2.data(), _cpu_transform_buffer2.size());

		_shader.bind();
		_transform_texture.bind();
		_color_ids_texture.bind();
		_colors_texture.bind();
		_main_vao.bind();
		for(const auto& instances : _instance_sets)
		{
			glVertexAttribI1i(TEX_OFFSET_ATTRIB, instances.tex_offset);
//			glVertexAttrib3fv(7, instances.color.data());
			glDrawElementsInstanced(GL_TRIANGLES, instances.element_count, GL_UNSIGNED_INT, GLB_BYTE_OFFSET(instances.element_byte_offset), instances.count);
		}
	}

	void duplicate_instance_renderer::render_color(const vec3& color)
	{
		_shader.bind();
		_transform_texture.bind();
		_color_ids_texture.bind();
		_colors_texture.bind();
		_main_vao.bind();
		glVertexAttrib3fv(7, color.data());
		for(const auto& instances : _instance_sets)
		{
			glVertexAttribI1i(TEX_OFFSET_ATTRIB, instances.tex_offset);
			glDrawElementsInstanced(GL_TRIANGLES, instances.element_count, GL_UNSIGNED_INT, GLB_BYTE_OFFSET(instances.element_byte_offset), instances.count);
		}
	}

	// ---------------------------------------------------------------------------------------------------------------------------------------------------------
	// private
	// ---------------------------------------------------------------------------------------------------------------------------------------------------------

	void duplicate_instance_renderer::_add_mesh(const tess::triangle_mesh& mesh, const mat4& transform, bool remove_duplicate_vertices /*= false*/)
	{
		// 1. apply transform to mesh
		auto new_mesh = mesh;
		const auto ntransform = transform.to_normal_matrix();
		vector<vec3> all_points;
		for(auto& v : new_mesh.vertices)
		{
			v.position = transform.mul(v.position);
			v.normal = ntransform.mul3x3(v.normal);
			all_points.push_back(v.position);
		}

		// 2. get unique positions from transformed mesh
		vector<vec3> new_points;
		if(remove_duplicate_vertices)
		{
			hash_set<vec3> point_hash;
			point_hash.reserve(all_points.size());
			new_points.reserve(all_points.size());
			for(const auto& p : all_points)
			{
				if(point_hash.find(p) == end(point_hash))
				{
					new_points.push_back(p);
					point_hash.emplace(p);
				}
			}
		}
		else
		{
			new_points = all_points;
		}

		EigenVec3Array dst = to_eigen(new_points);
		EigenVec3 dst_mean = dst.rowwise().mean();
		EigenVec3Array dst_local = dst.colwise() - dst_mean;

		// 3. search for candidate mesh with the same number of reference points
		auto range = _unique_meshes.equal_range(new_points.size());

		// 4. for each candidate mesh
		auto best_match = range.second;
		double best_error = EPSILON;
		mat4 best_matrix = mat4::IDENTITY;
		for(auto itr = range.first; itr != range.second; ++itr)
		{
			// 4.1 estimate transformation from candidate mesh to new mesh
			double error = 0.0;
			auto m = estimate_transform_3x3(itr->second.src, itr->second.src_mean, itr->second.src_local, itr->second.Aqq,
											dst, dst_mean, dst_local, error);

			// 4.2 save best match so far
			if(error < best_error)
			{
				best_match = itr;
				best_error = error;
				best_matrix = m;
			}
		}

		if(best_match != range.second)
		{
			best_match->second.transforms.push_back(mat34(best_matrix));
			best_match->second.color_ids.push_back(_current_color_id);
		}
		else
		{
			// 5. if no candidate matched, add new mesh as a new candidate
			_total_vbo_size_bytes += new_mesh.vertices.size() * sizeof(tess::vertex);
			_total_ebo_size_bytes += new_mesh.elements.size() * sizeof(tess::element);

			point_set ps;
			ps.mesh = new_mesh;
			ps.transforms.push_back(mat34(mat4::IDENTITY));
			ps.color_ids.push_back(_current_color_id);

			ps.src = dst;
			ps.src_mean = dst.rowwise().mean();
			ps.src_local = dst.colwise() - ps.src_mean;
			ps.Aqq = (ps.src_local * ps.src_local.transpose()).inverse();

			_unique_meshes.emplace(new_points.size(), ps);

			io::print("unique meshes:", _unique_meshes.size());

			_total_memory += new_mesh.vertices.size() * sizeof(tess::vertex) + new_mesh.elements.size() * sizeof(tess::element);
		}

		++_total_geometries;
		_total_triangles += new_mesh.elements.size()/3;
	}
} // namespace app


#include "TimeCorr.hxx"
#include <string>
#include <iostream>
#include <boost/format.hpp>
#include <cmath>
#include <ctype.h>
#include <time.h>

// offset from UTC for the specified grid
float TimeCorr::localTimeOffset(const std::string & grid) {
  char d1 = grid[0];
  char d2 = grid[2];

  float hrs_per_20deg = 20.0 * 12.0 / 180.0;
  float fhrs = 0.0;
  int d1_steps = (int) (toupper(d1) - 'J'); // approximately.. GMT
  float mult = (d1_steps < 0) ? -1.0 : 1.0;
  int d2_steps = (int) (d2 - '0');

  float fhrs1 = hrs_per_20deg * ((float) d1_steps);
  float fhrs2 = 0.1 * hrs_per_20deg * ((float) d2_steps);

  return fhrs1 + fhrs2;
}

float TimeCorr::circularMean(float maxpos, float a, float b)
{
  if(b < a) return circularMean(maxpos, b, a);

  if(((a < 0.0) && (b < 0.0)) ||
     ((a > 0.0) && (b > 0.0)) ||
     (a * b == 0)) {
    return 0.5 * (a + b);
  }
  else {
    float m1, m2, ap;
    m1 = 0.5 * (a + b);
    ap = a + maxpos;
    m2 = 0.5 * (ap + b);
    if (m2 > (0.5 * maxpos)) {
      m2 = m2 - maxpos;
    }

    float lim = maxpos * 0.25;
    if(fabs(a - m1) <= lim) {
      return m1;
    }
    else return m2;
  }
}

float TimeCorr::localTimeOffset(const std::string & from, const std::string & to)
{
  float f_off = localTimeOffset(from);  // !
  float t_off = localTimeOffset(to);

  return circularMean(24.0, f_off, t_off);
}

float TimeCorr::localTime(const std::string & grid,
			  long epoch_time)
{
  double h_off = (double) localTimeOffset(grid);

  struct tm ts;
  gmtime_r(&epoch_time, &ts);

  float fhr = (float) ts.tm_hour;
  float fmin = (float) ts.tm_min;
  float res = h_off + fhr + fmin / 60.0; // return hours + fraction
  if(res < -24.0) {
    std::cerr << boost::format("What is going on here.  Grid = [%s] epoch = %ld h_off = %f ts.tm_hour = %d  hr = %f ts.tm_min = %d fmin = %f res = %f\n")
      % grid % epoch_time % h_off % ts.tm_hour % fhr % ts.tm_min % fmin % res;
  }
  while(res > 24.0) res -= 24.0;
  while(res < 0.0) res += 24.0;
  return res;
}

float TimeCorr::localTime(const std::string & from,
			  const std::string & to,
			  long epoch_time)
{
  double h_off = (double) localTimeOffset(from, to);

  struct tm ts;
  gmtime_r(&epoch_time, &ts);

  float fhr = (float) ts.tm_hour;
  float fmin = (float) ts.tm_min;
  float res = h_off + fhr + fmin / 60.0; // return hours + fraction
  while(res > 24.0) res -= 24.0;
  while(res < 0.0) res += 24.0;
  return res;
}


// Testing PPR
#include "pauli_product.cpp"
#include <armadillo>
#include <chrono>
#include <iostream>
#include <stdlib.h>

using namespace std;
using namespace arma;

// Checking that apply_ppr and apply_ppr_slow yields the same result.
int main()
{
  int n = 1024;
  int n_itt = 100;

  double theta_lower, theta_upper;
  theta_lower = 0; theta_upper = 3.14159265358979;
  uniform_real_distribution<double> unif(theta_lower, theta_upper);
  default_random_engine re;

  for (int i=0; i<n_itt; i++)
    {
      cx_vec psi1, psi2;
      psi1.randn(n);
      psi1 = normalise(psi1, 2);
      psi2 = psi1;

      double theta = unif(re);
      unsigned x = rand()%n;
      unsigned z = rand()%n;

      /*      cout << "Norms before mult" << endl;
      cout << norm(psi1, 2) << endl;
      cout << norm(psi2, 2) << endl;

      cout << "psi1 before mult" << endl;
      cout << psi1 << endl;

      cout << "psi2 before mult" << endl;
      cout << psi2 << endl;*/

      apply_ppr(x, z, theta, psi1);
      apply_ppr_slow(x, z, theta, psi2);

      /*      cout << "Norms after mult" << endl;
      cout << norm(psi1, 2) << endl;
      cout << norm(psi2, 2) << endl;*/

      //      cout << "theta = " << theta << endl;
      cout << "Difference = " << norm(psi1-psi2, 2) << endl;

      /*      cout << "psi1" << endl;
      cout << psi1<< endl;

      cout << "psi2" << endl;
      cout << psi2<< endl;

      cout << "psi1-psi2" << endl;
      cout << psi1-psi2 << endl;*/
    }
  cx_vec psi1, psi2;
  psi1.randn(n);
  psi1 = normalise(psi1, 2);
  psi2 = psi1;
  std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
  for (int i=0; i<n_itt; i++)
    {
      double theta = unif(re);
      unsigned x = rand()%n;
      unsigned z = rand()%n;

      apply_ppr(x, z, theta, psi1);
    }
  std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();

  std::cout << "PPR Time (fast) = " << std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count()/n_itt << "[µs]" << std::endl;
  std::cout << "PPR Time (fast) = " << std::chrono::duration_cast<std::chrono::nanoseconds> (end - begin).count()/n_itt << "[ns]" << std::endl;

  psi1.randn(n);
  psi1 = normalise(psi1, 2);
  psi2 = psi1;
  std::chrono::steady_clock::time_point begin2 = std::chrono::steady_clock::now();
  for (int i=0; i<n_itt; i++)
    {
      double theta = unif(re);
      unsigned x = rand()%n;
      unsigned z = rand()%n;

      apply_ppr_slow(x, z, theta, psi1);
    }
  std::chrono::steady_clock::time_point end2 = std::chrono::steady_clock::now();

  std::cout << "PPR Time (slow) = " << std::chrono::duration_cast<std::chrono::microseconds>(end2 - begin2).count()/n_itt << "[µs]" << std::endl;
  std::cout << "PPR Time (slow) = " << std::chrono::duration_cast<std::chrono::nanoseconds> (end2 - begin2).count()/n_itt << "[ns]" << std::endl;

}


/**
 * @file Data.hpp
 * @author Giulio Romualdi
 * @copyright  Released under the terms of the BSD 3-Clause License
 * @date 2018
 */

#ifndef OSQPEIGEN_DATA_HPP
#define OSQPEIGEN_DATA_HPP

// Eigen
#include <Eigen/Dense>

// OSQP
#include <osqp.h>

// OsqpEigen
#include <OsqpEigen/SparseMatrixHelper.hpp>

/**
 * OsqpEigen namespace.
 */
namespace OsqpEigen
{
    /**
     * Data class is a wrapper of the OSQP OSQPData struct.
     */
    class Data
    {
        OSQPData *m_data; /**< OSQPData struct. */
        bool m_isNumberOfVariablesSet; /**< Boolean true if the number of variables is set. */
        bool m_isNumberOfConstraintsSet; /**< Boolean true if the number of constraints is set. */
        bool m_isHessianMatrixSet;  /**< Boolean true if the hessian matrix is set. */
        bool m_isGradientSet; /**< Boolean true if the gradient vector is set. */
        bool m_isLinearConstraintsMatrixSet; /**< Boolean true if the linear constrain matrix is set. */
        bool m_isLowerBoundSet; /**< Boolean true if the lower bound vector is set. */
        bool m_isUpperBoundSet; /**< Boolean true if the upper bound vector is set. */

    public:
        /**
         * Constructor.
         */
        Data();

        /**
         * Constructor.
         * @param n is the number of variables;
         * @param m is the number of constraints.
         */
        Data(int n, int m);

        /**
         * Deconstructor.
         */
        ~Data();

        /**
         * Clear the hessian matrix.
         */
        void clearHessianMatrix();

        /**
         * Clear the linear constraints matrix.
         */
        void clearLinearConstraintsMatrix();

        /**
         * Set the number of variables.
         * @param n is the number of variables.
         */
        void setNumberOfVariables(int n);

        /**
         * Set the number of constraints.
         * @param m is the number of constraints.
         */
        void setNumberOfConstraints(int m);

        /**
         * Set the quadratic part of the cost function (Hessian).
         * It is assumed to be a simmetric matrix.
         * @param hessianMatrix is the Hessian matrix.
         * @return true/false in case of success/failure.
         */
        template<typename Derived>
        bool setHessianMatrix(const Eigen::SparseCompressedBase<Derived> &hessianMatrix);

        /**
         * Set the linear part of the cost function (Gradient).
         * @param gradientVector is the Gradient vector.
         * @note the elements of the gradient are not copied inside the library.
         * The user has to guarantee that the lifetime of the object passed is the same of the
         * OsqpEigen object
         * @return true/false in case of success/failure.
         */
        bool setGradient(Eigen::Ref<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> gradientVector);

        Eigen::Matrix<c_float, Eigen::Dynamic, 1> getGradient();

        /**
         * Set the linear constraint matrix A (size m x n)
         * @param linearConstraintsMatrix is the linear constraints matrix A.
         * @return true/false in case of success/failure.
         */
        template<typename Derived>
        bool setLinearConstraintsMatrix(const Eigen::SparseCompressedBase<Derived> &linearConstraintsMatrix);

        /**
         * Set the array for lower bound (size m).
         * @param lowerBoundVector is the lower bound constraint.
         * @note the elements of the lowerBoundVector are not copied inside the library.
         * The user has to guarantee that the lifetime of the object passed is the same of the
         * OsqpEigen object
         * @return true/false in case of success/failure.
         */
        bool setLowerBound(Eigen::Ref<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> lowerBoundVector);

        /**
         * Set the array for upper bound (size m).
         * @param upperBoundVector is the upper bound constraint.
         * @note the elements of the upperBoundVector are not copied inside the library.
         * The user has to guarantee that the lifetime of the object passed is the same of the
         * OsqpEigen object.
         * @return true/false in case of success/failure.
         */
        bool setUpperBound(Eigen::Ref<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> upperBoundVector);

        /**
         * Set the array for upper and lower bounds (size m).
         * @param lowerBound is the lower bound constraint.
         * @param upperBound is the upper bound constraint.
         * @note the elements of the upperBound and lowerBound are not copied inside the library.
         * The user has to guarantee that the lifetime of the object passed is the same of the
         * OsqpEigen object.
         * @return true/false in case of success/failure.
         */
        bool setBounds(Eigen::Ref<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> lowerBound,
                       Eigen::Ref<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> upperBound);

        /**
         * Get the OSQPData struct.
         * @return a const point to the OSQPData struct.
         */
        OSQPData *const & getData() const;

        /**
         * Verify if all the matrix and vectors are already set.
         * @return true if all the OSQPData struct are set.
         */
        bool isSet() const;
    };
}

#include <OsqpEigen/Data.tpp>

#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2006, 2008, 2010, 2018 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
*/

/*
 QuantLib Benchmark Suite

 Measures the performance of a preselected set of numerically intensive
 test cases. The overall QuantLib Benchmark Index is given by the average
 performance in mflops.

 The number of floating point operations of a given test case was measured
 using the perfex library, http://user.it.uu.se/~mikpe/linux/perfctr
 and PAPI, http://icl.cs.utk.edu/papi

 Example results: 1. i7 7820X@3.6GHz        :24192.2 mflops
                  2. i7 4702HQ@2.2GHz       : 6524.9 mflops
                  3. i7 870@2.93GHz         : 4759.2 mflops
                  4. Core2 Q9300@2.5Ghz     : 2272.6 mflops
                  5. Core2 Q6600@2.4Ghz     : 1984.0 mflops
                  6. i3 540@3.1Ghz          : 1755.3 mflops
                  7. Core2 Dual@2.0Ghz      :  835.9 mflops
                  8. Athlon 64 X2 4400+     :  824.2 mflops
                  9. Core2 Dual@2.0Ghz      :  754.1 mflops
                 10. Pentium4 Dual@2.8Ghz   :  423.8 mflops
                 11. Raspberry Pi3@1.2GHz   :  309.2 mflops
                 12. Pentium4@3.0Ghz        :  266.3 mflops
                 13. PentiumIII@1.1Ghz      :  146.2 mflops
                 14. Alpha 2xEV68@833Mhz    :  184.6 mflops
                 15. Wii PowerPC 750@729MHz :   46.1 mflops
                 16. Raspberry Pi ARM@700Mhz:   28.3 mflops
                 17. Strong ARM@206Mhz      :    1.4 mflops

 Remarks: OS: Linux, static libs
  2. g++-6.3.0 -O3 -ffast-math -march=core-avx2
      Remark: 16 processes
  2. g++-4.8.1 -O3 -ffast-math -march=core-avx2
      Remark: eight processes
  3. gcc-4.6.3, -O3 -ffast-math -mfpmath=sse,387 -march=corei7
      Remark: eight processes
  4. icc-11.0,  -gcc-version=420 -fast -fp-model fast=2 -ipo-jobs2
      Remark: four processes
  5. icc-11.0,  -gcc-version=420 -fast -fp-model fast=2 -ipo-jobs2
      Remark: four processes
  6. gcc-4.4.5, -O3 -ffast-math -mfpmath=sse,387 -msse4.2 -march=core2
      Remark: four processes
  7. icc-11.0,  -gcc-version=420 -fast -fp-model fast=2 -ipo-jobs2
      Remark: two processes
  8. icc-11.0,  -gcc-version=420 -xSSSE3 -O3 -ipo -no-prec-div -static
                -fp-model fast=2 -ipo-jobs2, Remark: two processes
  9. gcc-4.2.1, -O3 -ffast-math -mfpmath=sse,387 -msse3 -funroll-all-loops
      Remark: two processes
 10. gcc-4.0.1, -O3 -march=pentium4 -ffast-math
      -mfpmath=sse,387 -msse2 -funroll-all-loops, Remark: two processes
 11. gcc-4.9.2  -O2, Remark: four processes
 12. gcc-4.0.1, -O3 -march=pentium4 -ffast-math
                -mfpmath=sse,387 -msse2 -funroll-all-loops
 13. gcc-4.1.1, -O3 -march=pentium3 -ffast-math
                -mfpmath=sse,387 -msse -funroll-all-loops
 14. gcc-3.3.5, -O3 -mcpu=e67 -funroll-all-loops, Remark: two processes
 15. gcc-4.9.2, -O2 -g on a Nintendo Wii
 16. gcc-4.6.3, -O3
 17. gcc-3.4.3, -O2 -g on a Zaurus PDA

  This benchmark is derived from quantlibtestsuite.cpp. Please see the
  copyrights therein.
*/

#include <ql/types.hpp>
#include <ql/version.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/timer.hpp>
#include <iostream>
#include <iomanip>
#include <list>
#include <string>

/* PAPI code
#include <stdio.h
#include <papi.h>
*/

/* Use BOOST_MSVC instead of _MSC_VER since some other vendors (Metrowerks,
   for example) also #define _MSC_VER
*/
#ifdef BOOST_MSVC
#  include <ql/auto_link.hpp>
#  define BOOST_LIB_NAME boost_unit_test_framework
#  include <boost/config/auto_link.hpp>
#  undef BOOST_LIB_NAME

/* uncomment the following lines to unmask floating-point exceptions.
   See http://www.wilmott.com/messageview.cfm?catid=10&threadid=9481
*/
//#  include <float.h>
//   namespace { unsigned int u = _controlfp(_EM_INEXACT, _MCW_EM); }

#endif
#include "utilities.hpp"

#include "americanoption.hpp"
#include "asianoptions.hpp"
#include "barrieroption.hpp"
#include "basketoption.hpp"
#include "batesmodel.hpp"
#include "convertiblebonds.hpp"
#include "digitaloption.hpp"
#include "dividendoption.hpp"
#include "europeanoption.hpp"
#include "fdheston.hpp"
#include "hestonmodel.hpp"
#include "interpolations.hpp"
#include "jumpdiffusion.hpp"
#include "marketmodel_smm.hpp"
#include "marketmodel_cms.hpp"
#include "lowdiscrepancysequences.hpp"
#include "quantooption.hpp"
#include "riskstats.hpp"
#include "shortratemodels.hpp"

using namespace boost::unit_test_framework;

namespace {

    class Benchmark {
      public:
        typedef void (*fct_ptr)();
        Benchmark(const std::string& name, fct_ptr f, double mflop)
        : f_(f), name_(name), mflop_(mflop) {
        }

        test_case* getTestCase() const {
            return QUANTLIB_TEST_CASE(f_);
        }
        double getMflop() const {
            return mflop_;
        }
        std::string getName() const {
            return name_;
        }
      private:
        fct_ptr f_;
        const std::string name_;
        const double mflop_; // total number of mega floating
                             // point operations (not per sec!)
    };

    boost::timer t;
    std::list<double> runTimes;
    std::list<Benchmark> bm;

    /* PAPI code
    float real_time, proc_time, mflops;
    long_long lflop, flop=0;
    */

    void startTimer() {
        t.restart();

        /* PAPI code
        lflop = flop;
        PAPI_flops(&real_time, &proc_time, &flop, &mflops);
        */
    }

    void stopTimer() {
        runTimes.push_back(t.elapsed());

        /* PAPI code
        PAPI_flops(&real_time, &proc_time, &flop, &mflops);
        printf("Real_time: %f Proc_time: %f Total mflop: %f\n",
               real_time, proc_time, (flop-lflop)/1e6);
        */
    }

    void printResults() {
        std::string header = "Benchmark Suite "
        #ifdef BOOST_MSVC
        QL_LIB_NAME;
        #else
        "QuantLib " QL_VERSION;
        #endif

        std::cout << std::endl
                  << std::string(56,'-') << std::endl;
        std::cout << header << std::endl;
        std::cout << std::string(56,'-')
                  << std::endl << std::endl;

        double sum=0;
        std::list<double>::const_iterator iterT = runTimes.begin();
        std::list<Benchmark>::const_iterator iterBM = bm.begin();

        while (iterT != runTimes.end()) {
            const double mflopsPerSec = iterBM->getMflop()/(*iterT);
            std::cout << iterBM->getName()
                      << std::string(42-iterBM->getName().length(),' ') << ":"
                      << std::fixed << std::setw(6) << std::setprecision(1)
                      << mflopsPerSec
                      << " mflops" << std::endl;

            sum+=mflopsPerSec;
            ++iterT;
            ++iterBM;
        }
        std::cout << std::string(56,'-') << std::endl
                  << "QuantLib Benchmark Index                  :"
                  << std::fixed << std::setw(6) << std::setprecision(1)
                  << sum/runTimes.size()
                  << " mflops" << std::endl;
    }
}

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {
    Integer sessionId() { return 0; }
}
#endif

test_suite* init_unit_test_suite(int, char*[]) {

    bm.push_back(Benchmark("AmericanOption::FdAmericanGreeks",
        &AmericanOptionTest::testFdAmericanGreeks, 518.31));
    bm.push_back(Benchmark("AmericanOption::FdShoutGreeks",
        &AmericanOptionTest::testFdShoutGreeks, 546.58));
    bm.push_back(Benchmark("AsianOption::MCArithmeticAveragePrice",
        &AsianOptionTest::testMCDiscreteArithmeticAveragePrice, 5186.13));
    bm.push_back(Benchmark("BarrierOption::BabsiriValues",
        &BarrierOptionTest::testBabsiriValues, 880.8));
    bm.push_back(Benchmark("BasketOption::EuroTwoValues",
        &BasketOptionTest::testEuroTwoValues, 340.04));
    bm.push_back(Benchmark("BasketOption::TavellaValues",
        &BasketOptionTest::testTavellaValues, 933.80));
    bm.push_back(Benchmark("BasketOption::OddSamples",
        &BasketOptionTest::testOddSamples, 642.46));
    bm.push_back(Benchmark("BatesModel::DAXCalibration",
        &BatesModelTest::testDAXCalibration, 1993.35));
    bm.push_back(Benchmark("ConvertibleBondTest::testBond",
        &ConvertibleBondTest::testBond, 159.85));
    bm.push_back(Benchmark("DigitalOption::MCCashAtHit",
        &DigitalOptionTest::testMCCashAtHit,995.87));
    bm.push_back(Benchmark("DividendOption::FdEuropeanGreeks",
        &DividendOptionTest::testFdEuropeanGreeks, 949.52));
    bm.push_back(Benchmark("DividendOption::FdAmericanGreeks",
        &DividendOptionTest::testFdAmericanGreeks, 1113.74));
    bm.push_back(Benchmark("EuropeanOption::FdMcEngines",
        &EuropeanOptionTest::testMcEngines, 1988.63));
    bm.push_back(Benchmark("EuropeanOption::ImpliedVol",
        &EuropeanOptionTest::testImpliedVol, 131.51));
    bm.push_back(Benchmark("EuropeanOption::FdEngines",
        &EuropeanOptionTest::testFdEngines, 148.43));
    bm.push_back(Benchmark("EuropeanOption::PriceCurve",
        &EuropeanOptionTest::testPriceCurve, 414.76));
    bm.push_back(Benchmark("FdHestonTest::testFdmHestonAmerican",
        &FdHestonTest::testFdmHestonAmerican, 234.21));
    bm.push_back(Benchmark("HestonModel::DAXCalibration",
        &HestonModelTest::testDAXCalibration, 555.19));
    bm.push_back(Benchmark("InterpolationTest::testSabrInterpolation",
        &InterpolationTest::testSabrInterpolation, 2266.06));
    bm.push_back(Benchmark("JumpDiffusion::Greeks",
        &JumpDiffusionTest::testGreeks, 433.77));
    bm.push_back(Benchmark("MarketModelCmsTest::testCmSwapsSwaptions",
        &MarketModelCmsTest::testMultiStepCmSwapsAndSwaptions,
        11497.73));
    bm.push_back(Benchmark("MarketModelSmmTest::testMultiSmmSwaptions",
        &MarketModelSmmTest::testMultiStepCoterminalSwapsAndSwaptions,
        11244.95));
    bm.push_back(Benchmark("QuantoOption::ForwardGreeks",
        &QuantoOptionTest::testForwardGreeks, 90.98));
    bm.push_back(Benchmark("RandomNumber::MersenneTwisterDescrepancy",
        &LowDiscrepancyTest::testMersenneTwisterDiscrepancy, 951.98));
    bm.push_back(Benchmark("RiskStatistics::Results",
        &RiskStatisticsTest::testResults, 300.28));
    bm.push_back(Benchmark("ShortRateModel::Swaps",
        &ShortRateModelTest::testSwaps, 454.73));

    test_suite* test = BOOST_TEST_SUITE("QuantLib benchmark suite");

    for (std::list<Benchmark>::const_iterator iter = bm.begin();
         iter != bm.end(); ++iter) {
        test->add(QUANTLIB_TEST_CASE(startTimer));
        test->add(iter->getTestCase());
        test->add(QUANTLIB_TEST_CASE(stopTimer));
    }

    test->add(QUANTLIB_TEST_CASE(printResults));

    return test;
}


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

  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/fact_10.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check fact_10 behavior for integral types"
            , (std::uint32_t)(std::uint64_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::fact_10;
  using boost::simd::Fact_10;

  STF_TYPE_IS(decltype(Fact_10<T>()), T);
  STF_EQUAL(Fact_10<T>(), T(3628800));
  STF_EQUAL(fact_10( as(T{}) ),T(3628800));
}

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

  STF_TYPE_IS(decltype(Fact_10<T>()), T);
  STF_IEEE_EQUAL(Fact_10<T>(), T(3628800));
  STF_IEEE_EQUAL(fact_10( as(T{}) ), T(3628800));
}


#ifndef ALEPH_TOPOLOGY_PARTITIONS_HH__
#define ALEPH_TOPOLOGY_PARTITIONS_HH__

#include <aleph/config/Eigen.hh>

#ifdef ALEPH_WITH_EIGEN
  #include <Eigen/Core>
  #include <Eigen/Eigenvalues>
#endif

#include <aleph/geometry/HeatKernel.hh>

#include <aleph/math/Quantiles.hh>

#include <unordered_map>
#include <stdexcept>
#include <vector>

namespace aleph
{

namespace topology
{

template <class SimplicialComplex> std::vector<SimplicialComplex> bisect( const SimplicialComplex& K )
{
#ifdef ALEPH_WITH_EIGEN

  auto L = aleph::geometry::weightedLaplacianMatrix( K );

  Eigen::SelfAdjointEigenSolver< decltype(L) > solver;
  solver.compute( L );

  using Simplex    = typename SimplicialComplex::ValueType;
  using VertexType = typename Simplex::VertexType;
  using DataType   = typename Simplex::DataType;

  auto&& eigenvectors = solver.eigenvectors().template cast<DataType>();

  if( eigenvectors.size() < 2 )
    throw std::runtime_error( "Laplacian matrix dimensions are insufficient for bisection" );

  std::vector<DataType> fiedlerVector;

  {
    auto fiedlerVector_ = eigenvectors.col(1);

    fiedlerVector.assign( fiedlerVector_.data(),
                          fiedlerVector_.data() + fiedlerVector_.size() );
  }

  auto median     = aleph::math::median( fiedlerVector.begin(), fiedlerVector.end() );
  using IndexType = typename std::vector<DataType>::size_type;

  // Prepare map from index to vertex ----------------------------------

  std::unordered_map<IndexType, VertexType> index_to_vertex;

  {
    std::vector<VertexType> vertices;
    K.vertices( std::back_inserter( vertices ) );

    IndexType index = IndexType();

    for( auto&& vertex : vertices )
      index_to_vertex[index++] = vertex;
  }

  // Partition vertices ------------------------------------------------

  std::unordered_map<VertexType, bool> partition;

  for( IndexType i = 0; i < fiedlerVector.size(); i++ )
  {
    auto vertex = index_to_vertex.at(i);

    if( fiedlerVector[i] < median )
      partition[vertex] = true;
    else
      partition[vertex] = false;
  }

  std::vector<Simplex> simplices( K.begin(), K.end() );

  auto itLeft = std::stable_partition( simplices.begin(), simplices.end(),
    [&partition] ( const Simplex& s )
    {
      // All vertices of the simplex need to be part of the same
      // partition with respect to the matrix.
      return s.size() == IndexType( std::count_if( s.begin(), s.end(),
        [&partition] ( VertexType v )
        {
          return partition.at(v);
        }
      ) );
    }
  );

  auto itRight = std::stable_partition( itLeft, simplices.end(),
    [&partition] ( const Simplex& s )
    {
      // All vertices of the simplex need to be part of the same
      // partition with respect to the matrix.
      return s.size() == IndexType( std::count_if( s.begin(), s.end(),
        [&partition] ( VertexType v )
        {
          return !partition.at(v);
        }
      ) );
    }
  );

  std::vector<SimplicialComplex> complexes;
  complexes.push_back( SimplicialComplex( simplices.begin(), itLeft ) );
  complexes.push_back( SimplicialComplex( itLeft, itRight ) );

  return complexes;

#else
  (void) K;
  return {};
#endif
}

} // namespace topology

} // namespace aleph

#endif


#pragma once
#include <boost/python.hpp>
#include "datatypes.hpp"
#include "vectorize.hpp"

namespace gd {

using namespace boost::python;

template<class ProfileType>
void fill(ProfileType *profile, double_vector r, double_vector target) {
	double* rp = r.data().begin();
	double* tp = target.data().begin();
	int size = r.size();
	for(int i = 0; i < size; i++) {
		tp[i] = profile->dphidr(rp[i]);
	}

}

template<class ProfileType, class Ctor>
	void py_export_profile_profile(string name) {
		class_<ProfileType, bases<Profile, Density> >(name.c_str(), Ctor())
			.def("potentialr", vectorize(&ProfileType::potentialr))
			.def("densityr", vectorize(&ProfileType::densityr))
			.def("densityR", vectorize(&ProfileType::densityR))
			.def("dphidr", vectorize(&ProfileType::dphidr))
			.def("fill", (&fill<ProfileType>))
		//.def("dphidr2", (&Plummer::dphidr2))
			;
	}

template<class ProfileType, class Ctor>
	void py_export_profile_profile_kw(string name, Ctor ctor) {
		class_<ProfileType, bases<Profile, Density> >(name.c_str(), ctor)
			.def("potentialr", vectorize(&ProfileType::potentialr))
			.def("densityr", vectorize(&ProfileType::densityr))
			.def("densityR", vectorize(&ProfileType::densityR))
			.def("dphidr", vectorize(&ProfileType::dphidr))
			.def("enclosed_mass", vectorize(&ProfileType::enclosed_mass))
			.def("fill", (&fill<ProfileType>))
		//.def("dphidr2", (&Plummer::dphidr2))
			;
	}

}


/*
 * test_MACD.cpp
 *
 *  Created on: 2013-4-11
 *      Author: fasiondog
 */

#ifdef TEST_ALL_IN_ONE
    #include <boost/test/unit_test.hpp>
#else
    #define BOOST_TEST_MODULE test_hikyuu_indicator_suite
    #include <boost/test/unit_test.hpp>
#endif

#include <fstream>
#include <hikyuu/StockManager.h>
#include <hikyuu/indicator/crt/KDATA.h>
#include <hikyuu/indicator/crt/MACD.h>
#include <hikyuu/indicator/crt/PRICELIST.h>
#include <hikyuu/indicator/crt/EMA.h>

using namespace hku;

/**
 * @defgroup test_indicator_MACD test_indicator_MACD
 * @ingroup test_hikyuu_indicator_suite
 * @{
 */

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_MACD ) {
    PriceList d;
    for (size_t i = 0; i < 20; ++i) {
        d.push_back(i);
    }

    Indicator ind = PRICELIST(d);
    Indicator macd, bar, diff, dea;
    Indicator ema1, ema2, fast, slow, bmacd;

    /** @arg 源数据为空 */
    macd = MACD(Indicator(), 12, 26, 9);
    BOOST_CHECK(macd.size() == 0);
    BOOST_CHECK(macd.empty() == true);

    /** @arg n1 = n2 = n3 = 1*/
    macd = MACD(ind, 1, 1, 1);
    BOOST_CHECK(macd.getResultNumber() == 3);
    bar = macd.getResult(0);
    diff = macd.getResult(1);
    dea = macd.getResult(2);
    BOOST_CHECK(bar.size() == 20);
    BOOST_CHECK(diff.size() == 20);
    BOOST_CHECK(dea.size() == 20);

    BOOST_CHECK(diff[0] == 0);
    BOOST_CHECK(diff[1] == 0);
    BOOST_CHECK(diff[19] == 0);

    BOOST_CHECK(dea[0] == 0);
    BOOST_CHECK(dea[1] == 0);
    BOOST_CHECK(dea[19] == 0);

    BOOST_CHECK(bar[0] == 0);
    BOOST_CHECK(bar[1] == 0);
    BOOST_CHECK(bar[19] == 0);

    /** @arg n1 = 1 n2 = 2 n3 = 3*/
    macd = MACD(ind, 1, 2, 3);
    BOOST_CHECK(macd.size() == 20);
    BOOST_CHECK(macd.discard() == 0);
    bar = macd.getResult(0);
    diff = macd.getResult(1);
    dea = macd.getResult(2);
    ema1 = EMA(ind, 1);
    ema2 = EMA(ind, 2);
    fast = ema1 - ema2;
    slow = EMA(fast, 3);
    bmacd = fast - slow;
    BOOST_CHECK(bar.size() == 20);
    BOOST_CHECK(diff.size() == 20);
    BOOST_CHECK(dea.size() == 20);

    BOOST_CHECK(diff[0] == fast[0]);
    BOOST_CHECK(diff[1] == fast[1]);
    BOOST_CHECK(diff[19] == fast[19]);

    BOOST_CHECK(dea[0] == slow[0]);
    BOOST_CHECK(std::fabs(dea[1] - slow[1]) < 0.0001);
    BOOST_CHECK(dea[19] == slow[19]);

    BOOST_CHECK(bar[0] == bmacd[0]);
    BOOST_CHECK(bar[1] == bmacd[1]);
    BOOST_CHECK(bar[19] == bmacd[19]);

    /** @arg n1 = 3 n2 = 2 n3 = 1*/
    macd = MACD(ind, 3, 2, 1);
    BOOST_CHECK(macd.size() == 20);
    BOOST_CHECK(macd.discard() == 0);
    bar = macd.getResult(0);
    diff = macd.getResult(1);
    dea = macd.getResult(2);
    ema1 = EMA(ind, 3);
    ema2 = EMA(ind, 2);
    fast = ema1 - ema2;
    slow = EMA(fast, 1);
    bmacd = fast - slow;
    BOOST_CHECK(bar.size() == 20);
    BOOST_CHECK(diff.size() == 20);
    BOOST_CHECK(dea.size() == 20);

    BOOST_CHECK(diff[0] == fast[0]);
    BOOST_CHECK(diff[1] == fast[1]);
    BOOST_CHECK(diff[19] == fast[19]);

    BOOST_CHECK(dea[0] == slow[0]);
    BOOST_CHECK(dea[1] == slow[1]);
    BOOST_CHECK(dea[19] == slow[19]);

    BOOST_CHECK(bar[0] == bmacd[0]);
    BOOST_CHECK(bar[1] == bmacd[1]);
    BOOST_CHECK(bar[19] == bmacd[19]);

    /** @arg n1 = 3 n2 = 5 n3 = 2*/
    macd = MACD(ind, 3, 5, 2);
    BOOST_CHECK(macd.size() == 20);
    BOOST_CHECK(macd.discard() == 0);
    bar = macd.getResult(0);
    diff = macd.getResult(1);
    dea = macd.getResult(2);
    ema1 = EMA(ind, 3);
    ema2 = EMA(ind, 5);
    fast = ema1 - ema2;
    slow = EMA(fast, 2);
    bmacd = fast - slow;
    BOOST_CHECK(bar.size() == 20);
    BOOST_CHECK(diff.size() == 20);
    BOOST_CHECK(dea.size() == 20);

    BOOST_CHECK(diff[0] == fast[0]);
    BOOST_CHECK(diff[1] == fast[1]);
    BOOST_CHECK(diff[19] == fast[19]);

    BOOST_CHECK(dea[0] == slow[0]);
    BOOST_CHECK(dea[1] == slow[1]);
    BOOST_CHECK(dea[19] == slow[19]);

    BOOST_CHECK(bar[0] == bmacd[0]);
    BOOST_CHECK(bar[1] == bmacd[1]);
    BOOST_CHECK(bar[19] == bmacd[19]);

    /** @arg operator() */
    Indicator expect = MACD(ind, 3, 5, 2);
    Indicator tmp = MACD(3, 5, 2);
    Indicator result = tmp(ind);
    BOOST_CHECK(result.size() == expect.size());
    for (size_t i = 0; i < expect.size(); ++i) {
        BOOST_CHECK(result.get(i, 0) == expect.get(i, 0));
        BOOST_CHECK(result.get(i, 1) == expect.get(i, 1));
        BOOST_CHECK(result.get(i, 2) == expect.get(i, 2));
    }
}

//-----------------------------------------------------------------------------
// test export
//-----------------------------------------------------------------------------
#if HKU_SUPPORT_SERIALIZATION

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_MACD_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/MACD.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator ma1 = MACD(CLOSE(kdata));
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(ma1);
    }

    Indicator ma2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(ma2);
    }

    BOOST_CHECK(ma1.size() == ma2.size());
    BOOST_CHECK(ma1.discard() == ma2.discard());
    BOOST_CHECK(ma1.getResultNumber() == ma2.getResultNumber());
    for (size_t i = 0; i < ma1.size(); ++i) {
        BOOST_CHECK_CLOSE(ma1.get(i,0), ma2.get(i,0), 0.00001);
        BOOST_CHECK_CLOSE(ma1.get(i,1), ma2.get(i,1), 0.00001);
        BOOST_CHECK_CLOSE(ma1.get(i,2), ma2.get(i,2), 0.00001);
    }
}
#endif /* #if HKU_SUPPORT_SERIALIZATION */

/** @} */


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

  @copyright 2016 NumScale SAS

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

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

  Generates the @c 5! constant

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

  @par Description

  1.  @code
      template<typename T> T Fact_5();
      @endcode

  2.  @code
      template<typename T> T Fact_5( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T that evaluates to 5!.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to @c T(120).

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

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

#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
//==============================================================================
#ifndef NT2_STATISTICS_FUNCTIONS_GENERIC_LOGNINV_HPP_INCLUDED
#define NT2_STATISTICS_FUNCTIONS_GENERIC_LOGNINV_HPP_INCLUDED
#include <nt2/statistics/functions/logninv.hpp>
#include <boost/assert.hpp>
#include <boost/dispatch/attributes.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/sqrt_2.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/functions/fma.hpp>
#include <nt2/include/functions/globalall.hpp>
#include <nt2/include/functions/simd/erfcinv.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/fma.hpp>
#include <nt2/include/functions/is_gtz.hpp>
#include <nt2/include/functions/is_nltz.hpp>
#include <nt2/include/functions/norminv.hpp>
#include <nt2/include/functions/sqrt.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( logninv0_, tag::cpu_
                            , (A0)
                            , (generic_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      return  exp(-Sqrt_2<A0>()*erfcinv( nt2::Two<A0>()*a0));
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( logninv0_, tag::cpu_
                            , (A0)(A1)
                            , (generic_<floating_<A0> > )
                              (generic_<floating_<A1> >)
                             )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(2)
    {

      return  exp(-Sqrt_2<A0>()*erfcinv( nt2::Two<A0>()*a0)+a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( logninv0_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (generic_< floating_<A0> >)
                              (generic_< floating_<A1> >)
                              (generic_< floating_<A2> >)
                            )
  {
    typedef A0 result_type;

    NT2_FUNCTOR_CALL(3)
    {
      BOOST_ASSERT_MSG(nt2::globalall(nt2::is_gtz(a2)), "sigma(s) must be positive");
      return  exp(-Sqrt_2<A0>()*a2*erfcinv( nt2::Two<A0>()*a0)+a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( logninv_, tag::cpu_
                              , (A0)(N0)(A1)(N1)
                              , ((node_<A0, nt2::tag::logninv_, N0, nt2::container::domain>))
                                ((node_<A1, nt2::tag::tie_ , N1, nt2::container::domain>))
                            )
  {
    typedef void                                                    result_type;
    typedef typename boost::proto::result_of::child_c<A1&,0>::type         Out0;
    typedef typename boost::proto::result_of::child_c<A0&,0>::type          In0;
    typedef typename boost::proto::result_of::child_c<A0&,1>::type          In1;
    typedef typename boost::proto::result_of::child_c<A0&,2>::type          In2;
    typedef typename A0::value_type                                  value_type;
    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      doit(a0, a1, N0(), N1());
    }
    ////////////////////////////////////////////
    // No enough inputs to computes all ouputs
    ////////////////////////////////////////////
    BOOST_FORCEINLINE static void doit(const A0&, A1&,
                                       boost::mpl::long_<1> const &, boost::mpl::long_<3> const & )
    {
      BOOST_ASSERT_MSG(false, "Must provide parameter variance to compute confidence bounds.");
    }
    BOOST_FORCEINLINE static void doit(const A0& a0,  A1& a1,
                                       boost::mpl::long_<2> const &, boost::mpl::long_<3> const & )
    {
      BOOST_ASSERT_MSG(false, "Must provide parameter variance to compute confidence bounds.");
      boost::proto::child_c<0>(a1) =  nt2::logninv(boost::proto::child_c<0>(a0),
                                                   boost::proto::child_c<1>(a0));
    }
    BOOST_FORCEINLINE static void doit(const A0&,  A1&,
                                       boost::mpl::long_<3> const &, boost::mpl::long_<3> const & )
    {
      BOOST_ASSERT_MSG(false, "Must provide parameter variance to compute confidence bounds.");

    }
    ////////////////////////////////////////////
    // No enough output to computes all ouputs
    ////////////////////////////////////////////
    template < class T >
    BOOST_FORCEINLINE static void doit(const A0& a0, A1& a1,
                                       boost::mpl::long_<4> const &, T const & )
    {
      boost::proto::child_c<0>(a1) =  nt2::logninv(boost::proto::child_c<0>(a0),
                                                   boost::proto::child_c<1>(a0),
                                                   boost::proto::child_c<2>(a0));
    }
    template < class T >
    BOOST_FORCEINLINE static void doit(const A0& a0, A1& a1,
                                       boost::mpl::long_<5> const &, T const & )
    {
      boost::proto::child_c<0>(a1) =  nt2::logninv(boost::proto::child_c<0>(a0),
                                                   boost::proto::child_c<1>(a0),
                                                   boost::proto::child_c<2>(a0));
    }
    ////////////////////////////////////////////
    // Regular cases
    ////////////////////////////////////////////
    BOOST_FORCEINLINE static void doit(const A0& a0, A1& a1,
                                       boost::mpl::long_<4> const &, boost::mpl::long_<3> const & )
    {
      conf_bounds(a0, a1, value_type(0.05));
    }
    BOOST_FORCEINLINE static void doit(const A0& a0, A1& a1,
                                       boost::mpl::long_<5> const &, boost::mpl::long_<3> const & )
    {
      conf_bounds(a0, a1,boost::proto::child_c<4>(a0));
    }

    BOOST_FORCEINLINE static void conf_bounds(const A0& a0, A1& a1,
                                              const value_type& alpha )
    {
      typedef nt2::memory::container<tag::table_, value_type, nt2::_2D>  semantic;
      NT2_AS_TERMINAL_IN(semantic, pcov, boost::proto::child_c<3>(a0));
      const In0& p  = boost::proto::child_c<0>(a0);
      const In1& mu = boost::proto::child_c<1>(a0);
      const In2& sigma = boost::proto::child_c<2>(a0);
      auto logx0 = -Sqrt_2<A0>()*erfcinv( nt2::Two<A0>()*p);
      auto xvar =   fma(fma(pcov(2,2), logx0, Two<value_type>()*pcov(1,2)), logx0, pcov(1,1));
      BOOST_ASSERT_MSG(nt2::globalall(nt2::is_nltz(xvar)), "Covariance matrix must be positive");
      value_type normz = -nt2::norminv(alpha*nt2::Half<value_type>());
      auto halfwidth = normz*nt2::sqrt(xvar);
      boost::proto::child_c<0>(a1) = exp(fma(sigma, logx0, mu));
      auto coef = exp(-halfwidth);
      boost::proto::child_c<1>(a1) = boost::proto::child_c<0>(a1)*coef;
      boost::proto::child_c<2>(a1) = boost::proto::child_c<0>(a1)/coef;
    }

  };

} }

#endif


/*
 * RobotMotionMapUpdater.hpp
 *
 *  Created on: Feb 5, 2014
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, ANYbotics
 */

#pragma once

// Elevation Mapping
#include "elevation_mapping/ElevationMap.hpp"

// Eigen
#include <Eigen/Core>

// Kindr
#include <kindr/Core>

// ROS
#include <ros/ros.h>

namespace elevation_mapping {

/*!
 * Computes the map variance update from the pose covariance of the robot.
 */
class RobotMotionMapUpdater {
 public:
  using Pose = kindr::HomogeneousTransformationPosition3RotationQuaternionD;
  using Covariance = Eigen::Matrix<double, 3, 3>;
  using PoseCovariance = Eigen::Matrix<double, 6, 6>;
  using ReducedCovariance = Eigen::Matrix<double, 4, 4>;
  using Jacobian = Eigen::Matrix<double, 4, 4>;

  /*!
   * Constructor.
   */
  explicit RobotMotionMapUpdater(ros::NodeHandle nodeHandle);

  /*!
   * Destructor.
   */
  virtual ~RobotMotionMapUpdater();

  /*!
   * Reads and verifies the ROS parameters.
   * @return true if successful.
   */
  bool readParameters();

  /*!
   * Computes the model update for the elevation map based on the pose covariance and
   * adds the update to the map.
   * @param[in] map the elevation map to be updated.
   * @param[in] robotPose the current pose.
   * @param[in] robotPoseCovariance the current pose covariance matrix.
   * @param[in] time the time of the current update.
   * @return true if successful.
   */
  bool update(ElevationMap& map, const Pose& robotPose, const PoseCovariance& robotPoseCovariance, const ros::Time& time);

 private:
  /*!
   * Computes the reduced covariance (4x4: x, y, z, yaw) from the full pose covariance (6x6: x, y, z, roll, pitch, yaw).
   * @param[in] robotPose the robot pose.
   * @param[in] robotPoseCovariance the full pose covariance matrix (6x6).
   * @param[out] reducedCovariance the reduced covariance matrix (4x4);
   * @return true if successful.
   */
  static bool computeReducedCovariance(const Pose& robotPose, const PoseCovariance& robotPoseCovariance,
                                       ReducedCovariance& reducedCovariance);

  /*!
   * Computes the covariance between the new and the previous pose.
   * @param[in] robotPose the current robot pose.
   * @param[in] reducedCovariance the current robot pose covariance matrix (reduced).
   * @param[out] relativeRobotPoseCovariance the relative covariance between the current and the previous robot pose (reduced form).
   * @return true if successful.
   */
  bool computeRelativeCovariance(const Pose& robotPose, const ReducedCovariance& reducedCovariance, ReducedCovariance& relativeCovariance);

  //! ROS nodehandle.
  ros::NodeHandle nodeHandle_;

  //! Time of the previous update.
  ros::Time previousUpdateTime_;

  //! Previous robot pose.
  Pose previousRobotPose_;

  //! Robot pose covariance (reduced) from the previous update.
  ReducedCovariance previousReducedCovariance_;

  //! Scaling factor for the covariance matrix (default 1).
  double covarianceScale_;
};

}  // namespace elevation_mapping


#pragma once

#include <Eigen/Core>
#include <functional>
#include <memory>

#include "../AgentPosition/agentposition.hh"
#include "../filters/Filter/filter.hh"
#include "../stats/movingaverage.hh"
#include "../stats/lowpassfilter.hh"

namespace bold
{
  template<typename> class Setting;

  enum class FilterType
  {
    Particle = 0,
    Kalman = 1,
    UnscentedKalman = 2
  };

  class Localiser
  {
  public:
    Localiser();

    void update();

    AgentPosition position() const { return d_pos; }
    AgentPosition smoothedPosition() const { return d_smoothedPos; }
    double uncertainty() const { return d_uncertainty; }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  private:
    typedef Eigen::Vector4d FilterState;

    std::pair<FilterState, double> generateState();

    void predict();
    void updateSmoothedPos();
    void updateStateObject();

    bool d_haveLastAgentTransform;
    Eigen::Affine3d d_lastAgentTransform;
    Eigen::Quaterniond d_lastQuaternion;
    double d_preNormWeightSum;
    LowPassFilter d_preNormWeightSumFilter;

    bool d_shouldRandomise;

    AgentPosition d_pos;
    AgentPosition d_smoothedPos;
    MovingAverage<Eigen::Vector4d> d_avgPos;
    double d_uncertainty;

//    Setting<bool>* d_useLines;
//    Setting<int>* d_minGoalsNeeded;
    Setting<double>* d_defaultKidnapWeight;
    Setting<double>* d_penaltyKidnapWeight;
    Setting<bool>* d_enablePenaltyRandomise;
//    Setting<bool>* d_enableDynamicError;

    FilterType d_filterType;
    std::shared_ptr<Filter<4>> d_filter;

    std::function<double()> d_fieldXRng;
    std::function<double()> d_fieldYRng;
    std::function<double()> d_goalAreaXRng;
    std::function<double()> d_goalAreaYRng;
    std::function<double()> d_thetaRng;
  };
}


#include <iostream>
#include <random>

#include <Eigen/Dense>

#include <h5.hpp>

int main()
{
    // Create some random matrix.
    using Matrix = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
    Matrix matrix(1000, 1000);

    std::mt19937 random;
    std::normal_distribution<double> normal;
    for (int i = 0; i < matrix.rows(); i++) {
        for (int j = 0; j < matrix.cols(); j++) {
            matrix(i, j) = normal(random);
        }
    }

    matrix = Matrix(matrix.transpose() * matrix);

    // Store the matrix in an HDF5 file.
    h5::file file("dump.h5", "w");

    file.dataset<h5::f32, 2>("eigen/matrix").write(
        &matrix(0, 0),
        {1000, 1000},
        {.compression = 1, .scaleoffset = 3}
    );
    std::clog << "Matrix is written to dump.h5\n";

    // Read back into another matrix.
    auto dataset = file.dataset<h5::f32, 2>("eigen/matrix");
    auto shape = dataset.shape();

    Matrix buffer(Matrix::Index(shape.dims[0]), Matrix::Index(shape.dims[1]));
    dataset.read(&buffer(0, 0), shape);

    std::clog << "Matrix is read back from the file\n";
}


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

#ifndef MTL_BASE_CASE_MATRIX_INCLUDE
#define MTL_BASE_CASE_MATRIX_INCLUDE

#include <boost/numeric/mtl/utility/static_assert.hpp>
#include <boost/numeric/meta_math/is_power_of_2.hpp>
#include <boost/numeric/meta_math/log_2.hpp>
#include <boost/numeric/mtl/recursion/base_case_test.hpp>
#include <boost/numeric/mtl/recursion/bit_masking.hpp>

namespace mtl { namespace recursion {

template <typename Matrix, typename BaseCaseTest>
struct base_case_matrix
{
    typedef Matrix type;
};

template <typename Elt, unsigned long Mask, typename Parameters, typename BaseCaseTest>
struct base_case_matrix<mtl::mat::morton_dense<Elt, Mask, Parameters>, BaseCaseTest>
{
    MTL_STATIC_ASSERT(meta_math::is_power_of_2<BaseCaseTest::base_case_size>::value, "Static base case size must be power of two");
    static const unsigned long base_case_bits= meta_math::log_2<BaseCaseTest::base_case_size>::value;

    typedef typename boost::mpl::if_<
	is_k_power_base_case_row_major<base_case_bits, Mask>
      , mtl::mat::dense2D<Elt, mat::parameters<row_major> >
      , typename boost::mpl::if_<
	    is_k_power_base_case_col_major<base_case_bits, Mask>
	  , mtl::mat::dense2D<Elt, mat::parameters<col_major> >
          , mtl::mat::morton_dense<Elt, Mask, Parameters>
        >::type
    >::type type;
};

}} // namespace mtl::recursion

#endif // MTL_BASE_CASE_MATRIX_INCLUDE


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

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

#include "filelib.h"
#include "fdict.h"
#include "weights.h"
#include "sparse_vector.h"
#include "em_utils.h"

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

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("optimization_method,m", po::value<string>()->default_value("em"), "Optimization method (em, vb)")
        ("input_format,f",po::value<string>()->default_value("b64"),"Encoding of the input (b64 or text)");
  po::options_description clo("Command line options");
  clo.add_options()
        ("config", po::value<string>(), "Configuration file")
        ("help,h", "Print this help message and exit");
  po::options_description dconfig_options, dcmdline_options;
  dconfig_options.add(opts);
  dcmdline_options.add(opts).add(clo);

  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("config")) {
    ifstream config((*conf)["config"].as<string>().c_str());
    po::store(po::parse_config_file(config, dconfig_options), *conf);
  }
  po::notify(*conf);

  if (conf->count("help")) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
}

double NoZero(const double& x) {
  if (x) return x;
  return 1e-35;
}

void Maximize(const bool use_vb,
              const double& alpha,
              const int total_event_types,
              SparseVector<double>* pc) {
  const SparseVector<double>& counts = *pc;

  if (use_vb)
    assert(total_event_types >= counts.size());

  double tot = 0;
  for (SparseVector<double>::const_iterator it = counts.begin();
       it != counts.end(); ++it)
    tot += it->second;
//  cerr << " = " << tot << endl;
  assert(tot > 0.0);
  double ltot = log(tot);
  if (use_vb)
    ltot = digamma(tot + total_event_types * alpha);
  for (SparseVector<double>::const_iterator it = counts.begin();
       it != counts.end(); ++it) {
    if (use_vb) {
      pc->set_value(it->first, NoZero(digamma(it->second + alpha) - ltot));
    } else {
      pc->set_value(it->first, NoZero(log(it->second) - ltot));
    }
  }
#if 0
  if (counts.size() < 50) {
    for (SparseVector<double>::const_iterator it = counts.begin();
         it != counts.end(); ++it) {
      cerr << " p(" << FD::Convert(it->first) << ")=" << exp(it->second);
    }
    cerr << endl;
  }
#endif
}

int main(int argc, char** argv) {
  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);

  const bool use_b64 = conf["input_format"].as<string>() == "b64";
  const bool use_vb = conf["optimization_method"].as<string>() == "vb";
  const double alpha = 1e-09;
  if (use_vb)
    cerr << "Using variational Bayes, make sure alphas are set\n";

  const string s_obj = "**OBJ**";
  // E-step
  string cur_key = "";
  SparseVector<double> acc;
  double logprob = 0;
  while(cin) {
    string line;
    getline(cin, line);
    if (line.empty()) continue;
    int feat;
    double val;
    size_t i = line.find("\t");
    const string key = line.substr(0, i);
    assert(i != string::npos);
    ++i;
    if (key != cur_key) {
      if  (cur_key.size() > 0) {
        // TODO shouldn't be num_active, should be total number
        // of events
        Maximize(use_vb, alpha, acc.size(), &acc);
        cout << cur_key << '\t';
        if (use_b64)
          B64::Encode(0.0, acc, &cout);
        else
          cout << acc;
        cout << endl;
        acc.clear();
      }
      cur_key = key;
    }
    if (use_b64) {
      SparseVector<double> g;
      double obj;
      if (!B64::Decode(&obj, &g, &line[i], line.size() - i)) {
        cerr << "B64 decoder returned error, skipping!\n";
        continue;
      }
      logprob += obj;
      acc += g;
    } else {       // text encoding - your counts will not be accurate!
      while (i < line.size()) {
        size_t start = i;
        while (line[i] != '=' && i < line.size()) ++i;
        if (i == line.size()) { cerr << "FORMAT ERROR\n"; break; }
        string fname = line.substr(start, i - start);
        if (fname == s_obj) {
          feat = -1;
        } else {
          feat = FD::Convert(line.substr(start, i - start));
        }
        ++i;
        start = i;
        while (line[i] != ';' && i < line.size()) ++i;
        if (i - start == 0) continue;
        val = atof(line.substr(start, i - start).c_str());
        ++i;
        if (feat == -1) {
          logprob += val;
        } else {
          acc.add_value(feat, val);
        }
      }
    }
  }
  // TODO shouldn't be num_active, should be total number
  // of events
  Maximize(use_vb, alpha, acc.size(), &acc);
  cout << cur_key << '\t';
  if (use_b64)
    B64::Encode(0.0, acc, &cout);
  else
    cout << acc;
  cout << endl << flush;

  cerr << "LOGPROB: " << logprob << endl;

  return 0;
}


#pragma once

#include <Eigen/Core>
#ifndef _MSC_VER
extern "C"{
#include "type.h"
}
#else
#include "type.h"
#endif

DllExport void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const CTYPE* matrix, CTYPE* state, ITYPE dim);
DllExport void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const Eigen::MatrixXcd& eigen_matrix, CTYPE* state, ITYPE dim);
DllExport void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const Eigen::Matrix<std::complex<double>,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>& eigen_matrix, CTYPE* state, ITYPE dim);