#include <algorithm>
#include <array>
#include <iostream>
#include <limits>
#include <map>
#include <vector>

#include <boost/range/irange.hpp>

/* ====================
 * SHOP INVENTORY BEGIN
 * ====================
 */
// the dmg and ac values can be represented by the index, since they are sequential
constexpr auto weapons = std::array{ 8, 10, 25, 40, 74 };
constexpr auto armors  = std::array{ 0, 13, 31, 53, 75, 102 }; // zero cost for no armor

constexpr auto rings_dmg = std::array{ 25, 50, 100 };
constexpr auto rings_ac  = std::array{ 20, 40, 80 };

/* ====================
 * SHOP INVENTORY END
 * ====================
 */

/* =====================
 * PRECALCULATIONS BEGIN
 * =====================
 */
constexpr auto MAX_NUM_RINGS = 2;
constexpr auto MIN_WPN_DMG   = 4;
constexpr auto MIN_DMG       = 1;

template<typename T>
constexpr auto factorial(T n) -> T {
    return (n > 1) ? (n * factorial(n-1)) : 1;
}

template<typename N, typename K>
constexpr auto binomial_coefficient(N n, K k) {
  return (factorial(n) / (factorial(k) * factorial(n-k)));
}

constexpr auto num_ring_combos(const int n) {
  auto num_combos = 0;
  for(auto k = 0; k <= MAX_NUM_RINGS; ++k) {
    num_combos += binomial_coefficient(n, k);
  }
  return num_combos;
}

struct RingComboStats {
  int num_rings,
      rings_cost,
      bonuses;
};

template<typename Array>
constexpr auto ring_combo_stats(Array rings) {

  constexpr auto rings_size = rings.size();
  constexpr auto array_size = num_ring_combos(rings_size);

  auto stats = std::array<RingComboStats, array_size>{};

  // first combo is the one with no rings
  stats.front() = {};

  auto current = 1;

  for(auto i = 0; i < static_cast<decltype(i)>(rings_size); ++i) {

    const auto ring_i = rings[i];

    // the i'th ring (0,1,2) has a bonus of (1,2,3) -> (i + 1)
    stats[current++] = { 1 , ring_i, (i + 1) };

    for(auto j = (i + 1); j < static_cast<decltype(j)>(rings_size); ++j) {

      // the i'th + j'th rings have a bonus of (i+1) + (j+1) -> (i + j + 2)
      stats[current++] = { 2, (ring_i + rings[j]), (i + j + 2) };
    }
  }

  return stats;
}

constexpr auto ring_dmg_combos = ring_combo_stats(rings_dmg);
constexpr auto ring_ac_combos  = ring_combo_stats(rings_ac);

template<typename T, typename U>
constexpr auto costs_size(const T& items, const U& ring_combos) {
  const auto max_combo_cost = std::max_element(ring_combos.begin(), ring_combos.end(), [] (const auto& lhs, const auto& rhs) {
    return lhs.bonuses < rhs.bonuses;
  });
  return (items.size() + max_combo_cost->bonuses);
}

constexpr auto dmg_costs_size = costs_size(weapons, ring_dmg_combos);
constexpr auto ac_costs_size  = costs_size(armors, ring_ac_combos);

struct ComboCost {
  int total_cost,
      num_rings;
};

struct CostLookup {
  std::array<std::vector<ComboCost>, dmg_costs_size> dmg_combo_costs;
  std::array<std::vector<ComboCost>, ac_costs_size>  ac_combo_costs;
};

auto precalc_combo_costs() {

  auto lookup = CostLookup{};

  const auto precalc = [] (const auto& items, const auto& ring_combos, auto& lookup) {
    for(const auto i : boost::irange(items.size())) {
      const auto item_cost = items[i];
      for(const auto& combo_stats : ring_combos) {
        lookup[i+combo_stats.bonuses].push_back({
          (item_cost + combo_stats.rings_cost),
          combo_stats.num_rings
        });
      }
    }
  };

  precalc(weapons, ring_dmg_combos, lookup.dmg_combo_costs);
  precalc(armors, ring_ac_combos, lookup.ac_combo_costs);

  return lookup;
}

/* =====================
 * PRECALCULATIONS END
 * =====================
 */

struct Entity {
  int health,
      damage,
      armor;
};

auto run_game(const CostLookup& lookup, const Entity& boss, const int player_hp) {

  const auto min_armor_to_lose = [&] (const int player_dmg) {
    const auto player_real_dmg = std::max((player_dmg - boss.armor), MIN_DMG);
    const auto num_rounds_to_win = ((boss.health / player_real_dmg) + ((boss.health % player_real_dmg) > 0));
    const auto num_rounds_to_last = (num_rounds_to_win - 1);
    const auto max_health_loss_per_round = (player_hp / num_rounds_to_last);

    // if player has no hp left after num_rounds_to_last, then they need extra armor
    const auto extra_armor = ((player_hp % num_rounds_to_last) == 0);
    const auto min_armor_to_win = (boss.damage - max_health_loss_per_round + extra_armor);

    // if min_armor is below minimum required for a win, then that's a loss
    return (min_armor_to_win - 1);
  };

  auto max_cost = std::numeric_limits<int>::min();

  const auto& dmg_combo_costs = lookup.dmg_combo_costs;
  const auto& ac_combo_costs  = lookup.ac_combo_costs;

  for(const auto i : boost::irange(dmg_combo_costs.size())) {
    /*
     * MIN_WPN_DMG is there because we need to adjust the damage value
     * represented by the index of the array we are iterating over,
     * which was started from 0 for convenience in the precalculations
     */
    const auto player_dmg = (i + MIN_WPN_DMG);

    const auto min_armor = min_armor_to_lose(player_dmg);

    using Type = std::decay_t<decltype(min_armor)>;

    // suppress gcc warning
    if(min_armor < static_cast<Type>(ac_combo_costs.size())) {

      for(const auto& ac_combo_cost : ac_combo_costs[min_armor]) {

        for(const auto& dmg_combo_cost : dmg_combo_costs[i]) {

          const auto num_rings = (ac_combo_cost.num_rings + dmg_combo_cost.num_rings);

          if(num_rings <= MAX_NUM_RINGS) {

            const auto new_cost = (ac_combo_cost.total_cost + dmg_combo_cost.total_cost);

            max_cost = std::max(new_cost, max_cost);
          }
        }
      }
    }
  }

  return max_cost;
}

auto solution(const Entity& boss, const int player_hp) {
  const auto& lookup = precalc_combo_costs();
  const auto max_cost = run_game(lookup, boss, player_hp);
  return max_cost;
}

int main() {

  const auto boss = Entity{103, 9, 2};
  const auto player_hp = 100;

  std::cout << solution(boss, player_hp) << std::endl;

}


#include <ros/ros.h>
#include "std_msgs/String.h"
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <stdlib.h>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/QR>
#include <Eigen/LU>
#include <math.h>
#include <stdio.h>
//#include <turtlesim/Pose.h>
#include <Eigen/Geometry>
#include "gazebo_msgs/LinkStates.h"
// #include <LinkStates.h>

using namespace Eigen;

//For geometry_msgs::Twist using:
// 		dummy.linear.x
// 		dummy.linear.y
// 		dummy.angular.z
geometry_msgs::Twist robot_position;
geometry_msgs::Twist target_position;

//rate_hz assignment
double rate_hz = 30;

//Assign the position of the robot (from other topic) to robot_position.
//Assuming the topic that generate the robot position uses geometry_msgs::Twist
//and the information is in *.linear. This might need to be modifed
void getRobotPose(const gazebo_msgs::LinkStates& msg) {

	// msg.name[1]
	robot_position.linear.x = msg.pose[1].position.x;
	robot_position.linear.y = msg.pose[1].position.y;
    robot_position.angular.z = 0;//msg.pose[1].angular.z;
}

//Assign the position of the target (from other topic) to target_position.
//Assuming the topic that generate the robot position uses geometry_msgs::Twist
//and the information is in *.linear. This might need to be modifed
void getTargetPose(const geometry_msgs::Twist& msg) {
	target_position.linear.x = msg.linear.x;
	target_position.linear.y = msg.linear.y;
    target_position.angular.z = msg.angular.z;
}

//Function to determine if the goal is still far.
//Variables epsilon_x and epsilon_y used to determined if the goal has been reach
bool isGoalFar(geometry_msgs::Twist p_start, geometry_msgs::Twist p_goal) {
	double epsilon_x, epsilon_y;
	epsilon_x = .2;
	epsilon_y = .2;
	double distance_x = abs(p_goal.linear.x - p_start.linear.x);
	double distance_y = abs(p_goal.linear.y - p_start.linear.y);
	if (distance_x > epsilon_x || distance_y > epsilon_y)
		return true;
	else
		return false;
}

//Function to generate a Linear Constant Velocity from robot's position to the target's position
geometry_msgs::Twist generateConstantVelocity(double constant_speed, geometry_msgs::Twist p_start, geometry_msgs::Twist p_goal){

    // Compute direction to goal
	Vector3d p_start_vector(p_start.linear.x,p_start.linear.y,p_start.angular.z);
	Vector3d p_goal_vector(p_goal.linear.x, p_goal.linear.y, p_goal.angular.z);
	// Vector3d goal_direction_vector = p_goal_vector-p_start_vector;
	Vector3d goal_direction_vector = p_start_vector - p_goal_vector;

    // Compute speed in the direction to goal
	Vector3d velocity_vector = constant_speed * (goal_direction_vector/ goal_direction_vector.norm());

	geometry_msgs::Twist velocity;

	velocity.linear.x = velocity_vector.x();
	velocity.linear.y = velocity_vector.y();
	velocity.angular.z = velocity_vector.z();

	return velocity;
}

// Function to keep velocity under the allowed robot limits
geometry_msgs::Twist boundVelocity(geometry_msgs::Twist velocity) {

	double max_linear_speed = 30;
	double min_linear_speed = 0;
	double max_angular_speed = M_PI*4;
	double min_angular_speed = M_PI/16;

	if (velocity.linear.x > max_linear_speed)
		velocity.linear.x = max_linear_speed;
	else if (velocity.linear.x < -max_linear_speed)
		velocity.linear.x = -max_linear_speed;
	if (velocity.linear.y > max_linear_speed)
		velocity.linear.y = max_linear_speed;
	else if (velocity.linear.y < -max_linear_speed)
		velocity.linear.y = -max_linear_speed;
	if (velocity.angular.z > max_angular_speed)
		velocity.angular.z = max_angular_speed;
	else if (velocity.angular.z < -max_angular_speed)
		velocity.angular.z = -max_angular_speed;

    // Lower speed bounds
	if (velocity.linear.x > 0 && velocity.linear.x < min_linear_speed)
		velocity.linear.x = min_linear_speed;
	else if (velocity.linear.x < 0 && velocity.linear.x > -min_linear_speed)
		velocity.linear.x = -min_linear_speed;
	if (velocity.linear.y > 0 && velocity.linear.y<min_linear_speed)
		velocity.linear.y = min_linear_speed;
	else if (velocity.linear.y < 0 && velocity.linear.y > -min_linear_speed)
		velocity.linear.y = -min_linear_speed;
	if (velocity.angular.z >0 && velocity.angular.z < min_angular_speed)
		velocity.angular.z = min_angular_speed;
	else if (velocity.angular.z < 0 && velocity.linear.z > - min_angular_speed )
		velocity.angular.z = -min_angular_speed;

}

int main(int argc, char **argv){
	ros::init(argc,argv,"turtle_trajectory_node");
	ros::NodeHandle nh;
	ROS_INFO_STREAM("turtle_trajectory_node initialized");
	ROS_INFO_STREAM(ros::this_node::getName());

	//Topic to publish velocity command, queue size equals rate_hz to keep up with the rate at which messages are generated,

    //Publish to the turtle topic "/turtle1/cmd_vel at rate_hz"
	ros::Publisher pub_vel_turtle = nh.advertise<geometry_msgs::Twist>("/target_vel_topic", rate_hz);

	//Topics to acquire robot and target position (from the vision node)
	ros::Subscriber sub_robot_pos = nh.subscribe("/gazebo/model_states", 1, &getRobotPose);
	ros::Subscriber sub_ball_pos = nh.subscribe("/target_position_topic", 1, &getTargetPose);

    //Twist variable to publish velocity (trajectories)
	geometry_msgs::Twist desired_velocity;
	double tiempo = 0;

    //define the max speed
	double cruise_speed = 30;

    //define the rate
	ros::Rate rate(rate_hz);

	while (ros::ok())
	{
        //ROS_INFO_STREAM use for debugging
		ROS_INFO_STREAM("Robot Position:"
			<<" X="<<robot_position.linear.x
			<<",Y="<<robot_position.linear.y
			<<",W="<<robot_position.angular.z);
		ROS_INFO_STREAM("Target position:"
			<<" X="<<target_position.linear.x
			<<",Y="<<target_position.linear.y
			<<",W="<<target_position.angular.z);

		if (isGoalFar(robot_position, target_position)) {

			desired_velocity = generateConstantVelocity(cruise_speed, robot_position, target_position);
			desired_velocity = boundVelocity(desired_velocity);
		} else {
            // Goal has been reach ==> dont move
			desired_velocity.linear.x = 0;
			desired_velocity.linear.y = 0;
			desired_velocity.angular.z = 0;
		}
		//ROS_INFO_STREAM use for debugging
		ROS_INFO_STREAM("Desired Velocity:"
			<<"X:"<<desired_velocity.linear.x
			<<",Y:"<<desired_velocity.linear.y
			<<",W:"<<desired_velocity.angular.z);

		//publish the new velocity
		pub_vel_turtle.publish(desired_velocity);

		ros::spinOnce();
		rate.sleep();
        tiempo+=(1/rate_hz);
    }
    return 0;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "STFT.hpp"
#include "../util/AlgorithmUtils.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cmath>

namespace fluid {
namespace algorithm {

class GriffinLim
{

public:
  void process(ComplexMatrixView in, index nSamples, index nIter, index winSize,
               index fftSize, index hopSize)
  {
    using namespace Eigen;
    using namespace _impl;
    using namespace std::complex_literals;
    double    momentum = 0.9;
    auto      stft = STFT(winSize, fftSize, hopSize);
    auto      istft = ISTFT(winSize, fftSize, hopSize);
    ArrayXd   tmp = ArrayXd::Zero(nSamples);
    ArrayXXcd magnitude = asEigen<Array>(in).abs();
    ArrayXXcd phase =
        ArrayXXcd::Random(magnitude.rows(), magnitude.cols()) * 2 * 1i * pi;
    phase = phase.exp();
    ArrayXXcd estimate = ArrayXXcd::Zero(magnitude.rows(), magnitude.cols());
    ArrayXXcd prev = ArrayXXcd::Zero(magnitude.rows(), magnitude.cols());
    for (index i = 0; i < nIter; i++)
    {
      prev = estimate;
      ArrayXXcd spectrogram = magnitude * phase;
      istft.process(asFluid(spectrogram), asFluid(tmp));
      stft.process(asFluid(tmp), asFluid(estimate));
      phase = estimate - (momentum / (1 + momentum)) * prev;
      phase = phase / (phase.abs() + epsilon);
    }
    estimate = magnitude * phase;
    in <<= asFluid(estimate);
  }
};
} // namespace algorithm
} // namespace fluid


#include <ros/ros.h>
#include <vector>
#include <math.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <boost/foreach.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/kdtree/kdtree.h>
#include <std_msgs/String.h>
#include <sstream>

typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;

void callback(const PointCloud::ConstPtr& msg)
{
  ros::NodeHandle n;
  ros::Publisher pub = n.advertise<std_msgs::String>("/kinect_data",100);
  ROS_INFO("I heard: [%d] [%d]",msg->width, msg->height);
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
  pcl::VoxelGrid<pcl::PointXYZRGB> vg;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered (new pcl::PointCloud<pcl::PointXYZRGB>());
  vg.setInputCloud(msg);
  vg.setLeafSize(0.03f, 0.03f, 0.03f);
  vg.filter(*filtered);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
  std::vector<int> inl;
  pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZRGB> (filtered));
  pcl::RandomSampleConsensus<pcl::PointXYZRGB> ransac(model_p);
  ransac.setDistanceThreshold(0.05);
  ransac.computeModel();
  ransac.getInliers(inl);
  pcl::copyPointCloud<pcl::PointXYZRGB>(*filtered, inl, *cloud);
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ()), cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  //seg.setMaxIterations (100);
  seg.setDistanceThreshold(0.05);
  int i = 0, nr_points = (int)filtered->points.size();
  while(filtered->points.size() > 0.4*nr_points)
  {
    seg.setInputCloud(filtered);
    seg.segment(*inliers, *coefficients);
    pcl::ExtractIndices<pcl::PointXYZRGB> extract;
    extract.setInputCloud(filtered);
    extract.setIndices(inliers);
    extract.setNegative(false);
    extract.filter (*cloud_plane);
    extract.setNegative (true);
    extract.filter (*cloud_f);
    *filtered = *cloud_f;
  }
  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
  tree->setInputCloud (filtered);
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
  ec.setClusterTolerance (0.03); // 2cm
  ec.setMinClusterSize (50);
  ec.setMaxClusterSize (10000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (filtered);
  ec.extract (cluster_indices);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
  int j = 0;
  std::vector<std::vector<float> > moi, xcor;
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    j++;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr clusters (new pcl::PointCloud<pcl::PointXYZRGB>);
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
    {
      clusters->points.push_back (filtered->points[*pit]); //*
      cloud_cluster->points.push_back (filtered->points[*pit]); //*
    }
    pcl::MomentOfInertiaEstimation <pcl::PointXYZRGB> feature_extractor;
    feature_extractor.setInputCloud (clusters);
    feature_extractor.compute ();
    std::vector <float> moment_of_inertia;
    std::vector <float> eccentricity;
    pcl::PointXYZRGB min_point_AABB;
    pcl::PointXYZRGB max_point_AABB;
    pcl::PointXYZRGB min_point_OBB;
    pcl::PointXYZRGB max_point_OBB;
    pcl::PointXYZRGB position_OBB;
    Eigen::Matrix3f rotational_matrix_OBB;
    float major_value, middle_value, minor_value;
    Eigen::Vector3f major_vector, middle_vector, minor_vector;
    Eigen::Vector3f mass_center;
    feature_extractor.getMomentOfInertia (moment_of_inertia);
    feature_extractor.getEccentricity (eccentricity);
    feature_extractor.getAABB (min_point_AABB, max_point_AABB);
    feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
    feature_extractor.getEigenValues (major_value, middle_value, minor_value);
    feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
    feature_extractor.getMassCenter (mass_center);
    std::vector<float> row1;
    for(int k=0;k<2;k++)
      row1.push_back(mass_center[k]);
    moi.push_back(row1);
    std::vector<float> row2;
    row2.push_back(min_point_AABB.x);
    row2.push_back(max_point_AABB.x);
    row2.push_back(min_point_AABB.z);
    row2.push_back(max_point_AABB.z);
    row2.push_back(min_point_AABB.y);
    row2.push_back(max_point_AABB.y);
    xcor.push_back(row2);
  }
  std::cout<<endl<<j<<endl;
  std::stringstream ss;
  std_msgs::String kin_val;
  for(int i=0; i<j; i++)
      {
        if(xcor[i][2]<2.0 && xcor[i][5]-xcor[i][4]>0.15)
        {
          if(moi[i][0]>=0)
	  {
            std::cout<<"left\n";
	    ss << "left";
            kin_val.data = ss.str();
	    goto label;
	  }
          else
	  {
            std::cout<<"right\n";
	    ss << "right";
            kin_val.data = ss.str();
	    goto label;
	  }
        }
       }
	std::cout<<"straight\n";
	ss << "straight";
  	kin_val.data = ss.str();
      label:
	pub.publish(kin_val);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "obs_av");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("/camera/depth/color/points", 1, callback);
  ros::spin();
  return 0;
}


//
// Created by h on 20/07/18.
//

#include "species.h"

#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>

Species::Species(const boost::python::object &species, double B) {
    charge = p::extract<double>(species.attr("charge"));
    mass = p::extract<double>(species.attr("mass"));
    density = p::extract<double>(species.attr("density"));
    wc = B * abs(charge) / mass;
    wp = pow(density * charge * charge / (e0 * mass), 0.5);

    vpara_h = arrayutils::extract1d(species.attr("vpara_h"));
    vperp_h = arrayutils::extract1d(species.attr("vperp_h"));

    npara = vpara_h.size();
    nperp = vperp_h.size();

    df_dvpara_h = arrayutils::extract2d(species.attr("df_dvpara_h"), npara, nperp);
    df_dvperp_h = arrayutils::extract2d(species.attr("df_dvperp_h"), npara, nperp);

    p::list pyns = p::extract<p::list>(species.attr("ns"));
    for (int i = 0; i < len(pyns); ++i) {
        int n{p::extract<int>(pyns[i])};
        ns.push_back(n);
        jns.push_back(arr1d(nperp));
        jnps.push_back(arr1d(nperp));
        qs.push_back({arr1d(npara), arr1d(npara), arr1d(npara), arr1d(npara), arr1d(npara), arr1d(npara)});
        q_ms.push_back({arr1d(npara - 1), arr1d(npara - 1), arr1d(npara - 1), arr1d(npara - 1), arr1d(npara - 1),
                        arr1d(npara - 1)});
        q_cs.push_back({arr1d(npara - 1), arr1d(npara - 1), arr1d(npara - 1), arr1d(npara - 1), arr1d(npara - 1),
                        arr1d(npara - 1)});
    }

    dvperp_h = arr1d(nperp - 1);
    vpara_hp = arr1d(npara - 1);
    vpara_hm = arr1d(npara - 1);
    dv1 = arr1d(npara - 1);
    dv2 = arr1d(npara - 1);
    dv3 = arr1d(npara - 1);
    dv4 = arr1d(npara - 1);
    dv5 = arr1d(npara - 1);
    dv6 = arr1d(npara - 1);
    dv7 = arr1d(npara - 1);
    dv8 = arr1d(npara - 1);

    for (size_t i = 0; i < nperp - 1; i++) {
        dvperp_h[i] = vperp_h[i + 1] - vperp_h[i];
    }

    for (size_t i = 0; i < npara - 1; i++) {
        vpara_hm[i] = vpara_h[i];
        vpara_hp[i] = vpara_h[i + 1];
        dv1[i] = pow(vpara_hp[i], 1) - pow(vpara_hm[i], 1);
        dv2[i] = pow(vpara_hp[i], 2) - pow(vpara_hm[i], 2);
        dv3[i] = pow(vpara_hp[i], 3) - pow(vpara_hm[i], 3);
        dv4[i] = pow(vpara_hp[i], 4) - pow(vpara_hm[i], 4);
        dv5[i] = pow(vpara_hp[i], 5) - pow(vpara_hm[i], 5);
        dv6[i] = pow(vpara_hp[i], 6) - pow(vpara_hm[i], 6);
        dv7[i] = pow(vpara_hp[i], 7) - pow(vpara_hm[i], 7);
        dv8[i] = pow(vpara_hp[i], 8) - pow(vpara_hm[i], 8);
    }

}

void Species::push_kperp(const double kperp) {
    for (size_t ni = 0; ni < ns.size(); ni++) {
        int n{ns[ni]};
        double jns0 = bm::cyl_bessel_j(n, -(kperp / wc) * vperp_h[0]);
        double jnps0 = bm::cyl_bessel_j(n, -(kperp / wc) * vperp_h[0]);
        for (size_t j = 0; j < nperp; j++) {
            jns[ni][j] = bm::cyl_bessel_j(n, (kperp / wc) * vperp_h[j]);
            jnps[ni][j] = bm::cyl_bessel_j_prime(n, (kperp / wc) * vperp_h[j]);
        }
        for (size_t i = 0; i < npara; i++) {
            double qh[6]{};
            qh[0] = 0.5 * jns0 * jns0 * df_dvperp_h[i][0] * dvperp_h[0];
            qh[1] = 0.5 * jns0 * jns0 * vperp_h[0] * df_dvpara_h[i][0] * dvperp_h[0];
            qh[2] = 0.5 * jns0 * jnps0 * vperp_h[0] * df_dvperp_h[i][0] * dvperp_h[0];
            qh[3] = 0.5 * jns0 * jnps0 * vperp_h[0] * vperp_h[0] * df_dvpara_h[i][0] * dvperp_h[0];
            qh[4] = 0.5 * jnps0 * jnps0 * vperp_h[0] * vperp_h[0] * df_dvperp_h[i][0] * dvperp_h[0];
            qh[5] = 0.5 * jnps0 * jnps0 * vperp_h[0] * vperp_h[0] * vperp_h[0] * df_dvpara_h[i][0] * dvperp_h[0];
            for (size_t j = 0; j < nperp - 1; j++) {
                qh[0] += jns[ni][j] * jns[ni][j] * df_dvperp_h[i][j] * dvperp_h[j];
                qh[1] += jns[ni][j] * jns[ni][j] * vperp_h[j] * df_dvpara_h[i][j] * dvperp_h[j];
                qh[2] += jns[ni][j] * jnps[ni][j] * vperp_h[j] * df_dvperp_h[i][j] * dvperp_h[j];
                qh[3] += jns[ni][j] * jnps[ni][j] * vperp_h[j] * vperp_h[j] * df_dvpara_h[i][j] * dvperp_h[j];
                qh[4] += jnps[ni][j] * jnps[ni][j] * vperp_h[j] * vperp_h[j] * df_dvperp_h[i][j] * dvperp_h[j];
                qh[5] += jnps[ni][j] * jnps[ni][j] * vperp_h[j] * vperp_h[j] * vperp_h[j] * df_dvpara_h[i][j] *
                         dvperp_h[j];
            }
            for (size_t k = 0; k < 6; k++) {
                qs[ni][k][i] = qh[k];
            }
        }
        for (size_t i = 0; i < npara - 1; i++) {
            for (size_t k = 0; k < 6; k++) {
                q_ms[ni][k][i] = (qs[ni][k][i + 1] - qs[ni][k][i]) / dv1[i];
                q_cs[ni][k][i] = qs[ni][k][i] - q_ms[ni][k][i] * vpara_hm[i];
            }
        }
    }
}

array<array<cdouble, 3>, 3> Species::push_omega(const double kpara, const double kperp, const double wr, const double wi) {
    array<array<cdouble, 3>, 3> XP{};
    for (size_t ni = 0; ni < ns.size(); ni++) {
        int n{ns[ni]};
        double a{wr};
        double b{wi};
        double b2{b * b};
        double d{n * wc};
        cdouble apb{a, b};
        cdouble apbmd{a - d, b};
        cdouble apbmd2 = apbmd*apbmd;
        cdouble apbmd3 = apbmd2*apbmd;
        cdouble iapb{1.0 / apb};
        cdouble iapbmd{1.0 / apbmd};

        array<cdouble, 6> s_L0_q_m{};
        array<cdouble, 6> s_L0_q_c{};
        array<cdouble, 6> s_L1_q_m{};
        array<cdouble, 6> s_L1_q_c{};
        array<cdouble, 6> s_L2_q_m{};
        array<cdouble, 6> s_L2_q_c{};
        array<cdouble, 6> s_L3_q_m{};

        for (size_t i = 0; i < npara - 1; i++) {
            //cdouble clogfac{log((apb - d - vpara_hp[i]*kpara)/(apb - d - vpara_hm[i]*kpara))};
            //cdouble clogfac{log((apb - d - vpara_hm[i]*kpara - dv1[i]*kpara)/(apb - d - vpara_hm[i]*kpara))};
            //cdouble clogfac{log(1.0 - (dv1[i]*kpara)/(apb - d - vpara_hm[i]*kpara))};

            //Wish to taylor expand in kpara.
            //cdouble cx = (-dv1[i]*kpara)/(apbmd - vpara_hm[i]*kpara);
            cdouble cx = kpara*iapbmd;
            cdouble L0, L1, L2, L3;
            const double narr[]{1./1., 1./2., 1./3., 1./4., 1./5., 1./5., 1./7., 1./8.};
            if ((vpara_hm[i]+vpara_hp[i])*(vpara_hm[i]+vpara_hp[i])*(cx.real()*cx.real() + cx.imag()*cx.imag()) < -0.000001){
                cdouble logfacsum{0.0, 0.0};
                cdouble powarr[8];
                const cdouble dvarr[]{dv1[i], dv2[i], dv3[i], dv4[i], dv5[i], dv6[i],dv7[i], dv8[i]};
                powarr[0] = cx;
                for (size_t i = 1; i < 8;i++){
                    powarr[i] = powarr[i-1]*(cx);
                }
                for (size_t i = 8; i > 3; i--) {
                    logfacsum -= powarr[i-1]*(narr[i-1]*dvarr[i-1]);
                }
                L3 = logfacsum;
                L2 = L3 - powarr[2]*narr[2]*dvarr[2];
                L1 = L2 - powarr[1]*narr[1]*dvarr[1];
                L0 = L1 - powarr[0]*narr[0]*dvarr[0];
                if (wi<0.0){
                    cdouble di = I*L0.imag();
                    L0 -= di;
                    L1 -= di;
                    L2 -= di;
                    L3 -= di;
                }
            }else{
                double advkm = a - d - vpara_hm[i] * kpara;
                double rlogfac = 0.5 * log1p(dv1[i] * kpara * (dv1[i] * kpara - 2.0 * advkm) / (advkm * advkm + b2));
                double ilogfac = atan2(abs(b)*dv1[i]*kpara, (advkm + dv1[i]*kpara)*advkm + b2);
                L0 = cdouble(rlogfac, ilogfac);
                cdouble powarr[3];
                const cdouble dvarr[]{dv1[i], dv2[i], dv3[i]};
                powarr[0] = cx;
                for (size_t i = 1; i < 3;i++){
                    powarr[i] = powarr[i-1]*(cx);
                }
                L1 = L0 + powarr[0]*narr[0]*dvarr[0];
                L2 = L1 + powarr[1]*narr[1]*dvarr[1];
                L3 = L2 + powarr[2]*narr[2]*dvarr[2];
            }

            for (size_t k = 0; k < 6; k++) {
                s_L0_q_m[k] += L0 * q_ms[ni][k][i];
                s_L0_q_c[k] += L0 * q_cs[ni][k][i];
                s_L1_q_m[k] += L1 * q_ms[ni][k][i];
            }

            for (size_t k = 0; k < 5; k++) {
                s_L1_q_c[k] += L1 * q_cs[ni][k][i];
                s_L2_q_m[k] += L2 * q_ms[ni][k][i];
            }

            s_L2_q_c[0] += L2 * q_cs[ni][0][i];
            s_L2_q_c[2] += L2 * q_cs[ni][2][i];
            s_L3_q_m[0] += L3 * q_ms[ni][0][i];
            s_L3_q_m[2] += L3 * q_ms[ni][2][i];
        }

        double kpara2 = kpara*kpara;
        double kpara3 = kpara2*kpara;
        double kpara4 = kpara3*kpara;
        double kperp2 = kperp*kperp;

        cdouble c00 = M_PI * 2.0 * wp * wp * wc * wc * n * n * iapb * iapb;
        XP[0][0] -= c00*apb*(kpara2*apbmd*s_L1_q_m[0] + kpara3*s_L0_q_c[0]);
        XP[0][0] -= c00*(kpara3*apbmd*s_L1_q_m[1] + kpara4*s_L0_q_c[1]);
        XP[0][0] += c00*(kpara2*apbmd2*s_L2_q_m[0] + kpara3*apbmd*s_L1_q_c[0]);

        cdouble c01 = M_PI * 2.0 * wp * wp * n * wc * I * iapb * iapb * kperp;
        XP[0][1] -= c01*apb*(kpara2*apbmd*s_L1_q_m[2] + kpara3*s_L0_q_c[2]);
        XP[0][1] -= c01*(kpara3*apbmd*s_L1_q_m[3] + kpara4*s_L0_q_c[3]);
        XP[0][1] += c01*(kpara2*apbmd2*s_L2_q_m[2] + kpara3*apbmd*s_L1_q_c[2]);

        cdouble c02 = M_PI * 2.0 * wp * wp * n * wc * iapb * iapb * kperp;
        XP[0][2] -= c02*apb*(kpara*apbmd2*s_L2_q_m[0] + kpara2*apbmd*s_L1_q_c[0]);
        XP[0][2] -= c02*(kpara2*apbmd2*s_L2_q_m[1] + kpara3*apbmd*s_L1_q_c[1]);
        XP[0][2] += c02*(kpara*apbmd3*s_L3_q_m[0] + kpara2*apbmd2*s_L2_q_c[0]);

        cdouble c11 = M_PI * 2.0 * wp * wp * iapb * iapb * kperp2;
        XP[1][1] -= c11*apb*(kpara2*apbmd*s_L1_q_m[4] + kpara3*s_L0_q_c[4]);
        XP[1][1] -= c11*(kpara3*apbmd*s_L1_q_m[5] + kpara4*s_L0_q_c[5]);
        XP[1][1] += c11*(kpara2*apbmd2*s_L2_q_m[4] + kpara3*apbmd*s_L1_q_c[4]);

        cdouble c12 = M_PI * 2.0 * wp * wp * -I * iapb * iapb * kperp2;
        XP[1][2] -= c12*apb*(kpara*apbmd2*s_L2_q_m[2] + kpara2*apbmd*s_L1_q_c[2]);
        XP[1][2] -= c12*(kpara2*apbmd2*s_L2_q_m[3] + kpara3*apbmd*s_L1_q_c[3]);
        XP[1][2] += c12*(kpara*apbmd3*s_L3_q_m[2] + kpara2*apbmd2*s_L2_q_c[2]);

        cdouble c22 = M_PI * 2.0 * wp * wp * iapb * iapb * kperp2;
        XP[2][2] -= c22*apbmd*(kpara*apbmd2*s_L2_q_m[1] + kpara2*apbmd*s_L1_q_c[1]);
        XP[2][2] -= c22*d*(apbmd3*s_L2_q_m[0] + kpara*apbmd2*s_L2_q_c[0]);
    }

    XP[1][0] = -XP[0][1];
    XP[2][0] = XP[0][2];
    XP[2][1] = -XP[1][2];

    return XP;
}


#include <Engine/MeshEdit/Simulate_fast.h>

#include <math.h>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Dense>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

void Simulate::Clear() {
	this->positions.clear();
}

bool Simulate::Init() {
	m = positions.size() / 3; // number of vertices
	s = edgelist.size() / 2;  // number of springs
	g = 9.8;
	h = 1 / 30;
	iteration = 10;

	// init q_n-1, q_n, q_n+1
	x.resize(3 * m); // vector x, initialized to be y
	y.resize(3 * m); // vector y = 2q_n - q_n-1
	for (int i = 0; i < m; i++) {
		x.segment(3 * i, 3) << positions[i][0]
			<< positions[i][1]
			<< positions[i][2];
		y.segment(3 * i, 3) = x.segment(3 * i, 3);
	}

	// init Mass
	M.resize(3 * m);
	M.setOnes();

	// init force_ext, i.e. gravity
	f_ext.resize(3 * m);
	for (int i = 0; i < m; i++)
		f_ext.segment(3 * i, 3) = Vector3d(0, 0, -1);

	L = MatrixXd::Zero(m * 3, m * 3);
	buildL();
	J = MatrixXd::Zero(m * 3, m * 3);
	buildJ();

	FixPoint();
	buildK();
	getb();

	// prefactorization
	MatrixXd A_ = K * (M + h * h * L) * K.transpose() * K;
	A = A_.sparseView();
	LLT_.compute(A);

	return true;
}

void Simulate::SetLeftFix()
{
	//固定网格x坐标最小点
	fixed_id.clear();
	double x = 100000;
	for (int i = 0; i < positions.size(); i++)
	{
		if (positions[i][0] < x)
		{
			x = positions[i][0];
		}
	}

	for (int i = 0; i < positions.size(); i++)
	{
		if (abs(positions[i][0] - x) < 1e-5)
		{
			fixed_id.push_back(i);
		}
	}

	Init();
}

void Simulate::FixPoint() {
	fixed_id.push_back(0);
	fixed_id.push_back(m / 4);
}

void Simulate::buildK() {
	K = MatrixXd::Ones(m * 3 - 3 * fixed_id.size(), m * 3);

	for (int i = 0, j = 0; i < xk.size(); i++) {
		if (fix.find(i) == fix.end()) {
			K(j++, i) = 1;
		}
	}

	// Eigen::MatrixXd xt;
	// xt.resize((x.size()),1);
	// b.resize(x.size());

	// for (int i = 0; i < x.size(); i++)
	// 	xt(i, 0) = x[i];
	// Eigen::MatrixXd t = K.transpose() * K * xt;

	// for (int i = 0; i < xk.size(); i++)
	// 	b[i] = x[i] - t(i, 0);
}

void Simulate::buildL() {
	size_t m = positions.size() / 3;
	size_t s = edgelist.size() / 2;

	MatrixXd temp = MatrixXd::Zero(m, m);

	for (size_t i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];

		VectorXd Ai = VectorXd::Zero(m);
		Ai(index1 - 1) += 1;
		Ai(index2 - 1) -= 1;
		temp += stiff * Ai * Ai.transpose();
	}

	Matrix3d I3 = Matrix3d::Identity();

	// kronecker product, L = kronecker(temp, I3)
	for (int i = 0; i < m; i++) {
		L.block(i * 3, i * 3, 3, 3) = temp(i, i) * I3;
	}
}

void Simulate::buildJ() {
	MatrixXd temp = MatrixXd::Zero(m, s);

	for (size_t i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];

		VectorXd Ai = VectorXd::Zero(m);
		VectorXd Si = VectorXd::Zero(s);
		Ai(index1 - 1) += 1;
		Ai(index2 - 1) -= 1;
		Si(i) += 1;
		temp += stiff * Ai * Si.transpose();
	}

	Matrix3d I3 = Matrix3d::Identity();

	// kronecker product, L = kronecker(temp, I3)
	for (int i = 0; i < m; i++) {
		L.block(i * 3, i * 3, 3, 3) = temp(i, i) * I3;
	}

}

void Simulate::local() {
	d = VectorXd::Ones(s * 3);

	for (size_t i = 0; i < s; i++) {
		size_t index1 = edgelist[2 * i];
		size_t index2 = edgelist[2 * i + 1];
		pointf3 p1 = positions[index1];
		pointf3 p2 = positions[index2];
		vecf3 r = p1 - p2;

		VectorXd di(3);
		di << r[0], r[1], r[2];

		d.segment(3 * i, 3) = l[i] * di / pointf3::distance(p1, p2);
	}

}

void Simulate::global() {
	VectorXd RHS = K * (h * h * J * d + M * y - h * h * f_ext - (M + h * h * L) * b);

	VectorXd xf = LLT_.solve(RHS);

	x = K.transpose() * xf + b;

}

void Simulate::getb() {
	b = x - K.transpose() * K * x;
}

void Simulate::UpdatePos() {
	for (int i = 0; i < m * 3; i++)
		positions[i / 3][i % 3] = x(i);
}

void Simulate::SimulateOnce() {
	//update y, y = 2q_n - q_n-1
	y = 2 * x - y;
	size_t step = 0;
	while (step < iteration) {
		local();
		global();
	}
	UpdatePos();
}

bool Simulate::Run() {
	SimulateOnce();
	return true;
}


#pragma once

#include <Eigen/Core>
#include <autodiff/forward/eigen.hpp>

namespace samson::types
{
    using autodiff::Vector3dual;
    using Vector6dual = Eigen::Matrix<dual, 6, 1>;
    using Matrix6dual = Eigen::Matrix<dual, 6, 6>;

} // namespace samson::types


// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Paul Ferrand

#include "helpers.h"
#define EIGEN_MPL2_ONLY
#include <Eigen/Dense>
#include <numeric>

using namespace std::complex_literals;

template<class T>
constexpr T pi = T { 3.14159265358979323846 };

std::pair<float, std::complex<float>> frequencyPeakSearch(float* signal, size_t size, float coarseFrequency,
    float sampleRate, float range, int resolution)
{
    using namespace Eigen;

    // Build the super-resolution time-frequency matrix around the coarse frequency
    VectorXf freq = VectorXf::LinSpaced(2 * resolution + 1, coarseFrequency - range, coarseFrequency + range);
    VectorXf time = VectorXf::LinSpaced(size, 0, static_cast<float>(size - 1)) / sampleRate;

    MatrixXcf projectionMatrix =
        exp(2.0if * pi<float> * (freq * time.transpose()).array());

    // Project the signal on the matrix
    VectorXcf projected = projectionMatrix * Map<VectorXf>(signal, size);

    // Find the highest harmonic
    unsigned maxIdx = 0;
    float maxHarmonic = 0.0f;
    for (unsigned i = 0; i < projected.size(); ++i) {
        float harmonic = std::abs(projected[i]);
        if (harmonic > maxHarmonic) {
            maxIdx = i;
            maxHarmonic = harmonic;
        }
    }

    return std::make_pair(freq[maxIdx], projected[maxIdx]);
}

std::vector<float> buildWavetable(const HarmonicVector& harmonics, int size, bool normalizePower)
{
    using namespace Eigen;

    std::vector<double> table;
    std::vector<float> output;
    table.resize(size);
    output.reserve(size);
    std::fill(table.begin(), table.end(), 0.0);

    if (harmonics.empty()) {
        std::fill_n(std::back_inserter(output), size, 0.0f);
        return output;
    }

    using RowArrayXd = Array<double, 1, Dynamic>;
    ArrayXd time = ArrayXd::LinSpaced(size, 0, static_cast<double>(size - 1));
    time /= static_cast<double>(size);
    Map<ArrayXd> mappedTable { table.data(), size };

    for (const auto& [f, h] : harmonics) {
        double freqIndex = std::round(f / harmonics.front().first);
        double phase = std::arg(h);
        double magnitude = std::abs(h);
        // fmt::print("Harmonic at {:.2f} ({}) Hz: {:.3f} exp (i pi {:.3f})\n", f, freqIndex, magnitude, phase);
        mappedTable += magnitude * (2.0 * pi<double> * freqIndex * time + phase).sin();
    }

    // Normalize the overall power
    if (normalizePower) {
        double squaredNorm = std::accumulate(harmonics.begin(), harmonics.end(), 0.0,
            [] (double lhs, const auto& rhs) { return lhs + std::pow(std::abs(rhs.second), 2); });
        double norm = std::sqrt(squaredNorm);
        mappedTable /= norm;
    }

    // Roll the wavetable to start around 0
    size_t zeroIndex = 0;
    double zeroValue = mappedTable.maxCoeff();
    for (int i = 0; i < size; ++i) {
        double absValue = std::abs(mappedTable[i]);
        if (absValue < zeroValue) {
            zeroIndex = i;
            zeroValue = absValue;
        }
    }
    ArrayXd head = mappedTable.head(zeroIndex);
    ArrayXd tail = mappedTable.tail(size - zeroIndex);
    mappedTable << tail, head;

    std::transform(table.begin(), table.end(), std::back_inserter(output),
            [](double x) { return static_cast<float>(x); });

    return output;
}

std::vector<float> extractSignalRange(const float* source, double regionStart, double regionEnd,
    double samplePeriod, int stride, int offset)
{
    std::vector<float> signal;

    if (regionStart > regionEnd)
        std::swap(regionStart, regionEnd);

    int rangeStart = static_cast<int>(regionStart / samplePeriod);
    int rangeEnd = static_cast<int>(regionEnd / samplePeriod);
    int rangeSize = rangeEnd - rangeStart;
    if (rangeSize == 0)
        return signal;

    signal.resize(rangeSize);
    for (int t = 0, s = rangeStart; s < rangeEnd; ++t, ++s)
        signal[t] = source[stride * s + offset];

    return signal;
}


#include <string>
#include "Encryptor.h"
#include "RSA.h"
#include <boost/multiprecision/cpp_int.hpp>

using namespace boost::multiprecision;
using namespace Crypto;
using std::string;

Encryptor::Encryptor(const PublicKey* key) : text(""), public_key(key)
{};

CryptoString Encryptor::encryptString(string &input)
{
  CryptoString str;

  int256_t tmp;
  int it;
  for (it = 0; it < input.length(); it++)
  {
    tmp = powm((int256_t)input.at(it), public_key->r, public_key->m);
    str.push_back(tmp);
  }

  return str;
}

CryptoChar Encryptor::encryptChar(char c) const
{
  CryptoChar res = powm((CryptoChar)c, public_key->r, public_key->m);
  return res.convert_to<int>();
}


#include <Eigen/Cholesky>
/*
 * Member function definitions of Kalman template class.
 * See kalman.h for template class declaration.
 */

namespace observer {

template <typename T>
Kalman<T>::Kalman(T& system) : Observer<T>(system, state_t::Zero()) {
    reset();
}

template <typename T>
Kalman<T>::Kalman(T& system, const state_t& x0) : Observer<T>(system, x0) {
    reset();
}

template <typename T>
Kalman<T>::Kalman(T& system, const state_t& x0,
        const process_noise_covariance_t& Q,
        const measurement_noise_covariance_t& R,
        const error_covariance_t& P0) : Observer<T>(system, x0),
            m_P(P0), m_Q(Q), m_R(R) {
    m_K.setZero();
}

template <typename T>
void Kalman<T>::reset() {
    m_x.setZero();
    m_P.setIdentity();
    m_Q.setIdentity();
    m_R.setIdentity();
    m_K.setZero();
}

template <typename T>
void Kalman<T>::update_state(const input_t& u, const measurement_t& z) {
    time_update(u);
    measurement_update(z);
}

template <typename T>
void Kalman<T>::time_update() {
    time_update_state();
    time_update_error_covariance();
}

template <typename T>
void Kalman<T>::time_update(const process_noise_covariance_t& Q) {
    time_update_state();
    time_update_error_covariance(Q);
}

template <typename T>
void Kalman<T>::time_update(const input_t& u) {
    time_update_state(u);
    time_update_error_covariance();
}

template <typename T>
void Kalman<T>::time_update(const input_t& u, const process_noise_covariance_t& Q) {
    time_update_state(u);
    time_update_error_covariance(Q);
}

template <typename T>
void Kalman<T>::measurement_update(const measurement_t& z) {
    measurement_update_kalman_gain();
    measurement_update_state(z);
    measurement_update_error_covariance();
}

template <typename T>
void Kalman<T>::measurement_update(const measurement_t& z, const measurement_noise_covariance_t& R) {
    measurement_update_kalman_gain(R);
    measurement_update_state(z);
    measurement_update_error_covariance();
}

template <typename T>
void Kalman<T>::time_update_state() {
    m_x = m_system.normalize_state(m_system.Ad()*m_x);
}

template <typename T>
void Kalman<T>::time_update_state(const input_t& u) {
    m_x = m_system.normalize_state(m_system.Ad()*m_x + m_system.Bd()*u);
}

template <typename T>
void Kalman<T>::time_update_error_covariance() {
    m_P = m_system.Ad()*m_P*m_system.Ad().transpose() + m_Q;
}

template <typename T>
void Kalman<T>::time_update_error_covariance(const process_noise_covariance_t& Q) {
    m_P = m_system.Ad()*m_P*m_system.Ad().transpose() + Q;
}

template <typename T>
void Kalman<T>::measurement_update_kalman_gain() {
    // S = C*P*C' + R
    // K = P*C'*S^-1 - > K' = S^-1*C*P'
    Eigen::LDLT<measurement_noise_covariance_t> S_ldlt(
            m_system.Cd()*m_P*m_system.Cd().transpose() + m_R);
    m_K.noalias() = S_ldlt.solve(m_system.Cd()*m_P.transpose()).transpose();
}

template <typename T>
void Kalman<T>::measurement_update_kalman_gain(const measurement_noise_covariance_t& R) {
    // S = C*P*C' + R
    // K = P*C'*S^-1 - > K' = S^-1*C*P'
    Eigen::LDLT<measurement_noise_covariance_t> S_ldlt(
            m_system.Cd()*m_P*m_system.Cd().transpose() + R);
    m_K.noalias() = S_ldlt.solve(m_system.Cd()*m_P.transpose()).transpose();
}

template <typename T>
void Kalman<T>::measurement_update_state(const measurement_t& z) {
    m_x = m_system.normalize_state(m_x + m_K*(
                m_system.normalize_output(z - m_system.Cd()*m_x)));
}

template <typename T>
void Kalman<T>::measurement_update_error_covariance() {
    m_P = (error_covariance_t::Identity() - m_K*m_system.Cd())*m_P;
}

} // namespace observer


#include <gtest/gtest.h>
#include <fstream>
#include <iostream>
#include <cmath>
#include <Eigen/Dense>

// testing following API
#include "estimation/ParticleFilterSIR.h"
#include "probability/pdfs.h"
#include "probability/sampling.h"

using namespace std;
using namespace estimation;
using namespace probability;

namespace logTest
{
  // -----------------------------------------
  // logging for nice pictures
  // -----------------------------------------
  void f_log(VectorXd& x, const VectorXd& u)
  {
    x[0] = x[0];
  }
  void h_log(VectorXd& z, const VectorXd& x)
  {
    z[0] = x[0];
  }

  TEST(LogTest, logEstimationOfConstantNoisySignal)
  {
    // example 1: constant signal, but gaussian noisy with mean 0,
    // variance 1

    // create and open log file(s): signal, particles
    ofstream logSignal;
    logSignal.open("logEstimationOfConstantNoisySignal_signal.dat");
    ofstream logParticles;
    logParticles.open("logEstimationOfConstantNoisySignal_particles.dat");
    ofstream logWeights;
    logWeights.open("logEstimationOfConstantNoisySignal_weights.dat");
    ofstream logNeff;
    logNeff.open("logEstimationOfConstantNoisySignal_Neff.dat");
    ofstream logSignalEstimated;
    logSignalEstimated.open("logEstimationOfConstantNoisySignal_signalEstimated.dat");

    // init particle filter
    ParticleFilterSIR pf(1000);

    pf.setStateTransitionModel(f_log);
    pf.setObservationModel(h_log);
    MatrixXd Q(1,1); Q << 0.01;
    pf.setProcessNoiseCovariance(Q);
    MatrixXd R(1,1); R << 1;
    pf.setMeasurementNoiseCovariance(R);
    VectorXd x0(1); x0 << 1;
    pf.setInitialState(x0);

    pf.validate();

    // measurements
    VectorXd mean = VectorXd::Ones(1);		// mean = 1
    MatrixXd var = MatrixXd::Identity(1,1);	// variance = 1

    Input signal(InputValue(1));
    Output signalEstimated;

    // start filtering and log
    for (int i = 0; i < 40; i++)
    {
      // create new measurement
      double sample = sampleNormalDistribution(mean,var)[0];
      signal[0].setValue(sample);

      // estimate
      signalEstimated = pf.estimate(signal);

      // log
      logSignal << (i+1) << " "
		<< sample << endl;
      pf.log(logParticles, ParticleFilterSIR::PARTICLES, 0);
      pf.log(logWeights, ParticleFilterSIR::WEIGHTS, 0);
      logNeff << (i+1) << " ";
      pf.log(logNeff, ParticleFilterSIR::NEFF, 0);
      logSignalEstimated << (i+1) << " "
			 << signalEstimated[0].getValue() << " "
			 << sqrt(signalEstimated[0].getVariance())
			 << endl;
    }

    logSignal.close();
    logParticles.close();
    logWeights.close();
    logNeff.close();
    logSignalEstimated.close();
  }

  TEST(LogTest, logSampleNormalDistribution)
  {
    int N = 10000;
    VectorXd mean = VectorXd::Zero(1);
    MatrixXd cov = MatrixXd::Identity(1,1);

    ofstream logFile;
    logFile.open("logSampleNormalDistribution.dat");
    for (int i = 0; i < N; i++)
      logFile << sampleNormalDistribution(mean, cov)
    	      << endl;

    logFile.close();
  }

  TEST(LogTest, logSampleUniformDistribution)
  {
    int N = 10000;
    VectorXd a = VectorXd::Zero(1);
    VectorXd b = VectorXd::Ones(1);

    ofstream logFile;
    logFile.open("logSampleUniformDistribution.dat");
    for (int i = 0; i < N; i++)
      logFile << sampleUniformDistribution(a, b)
    	      << endl;

    logFile.close();
  }

  TEST(LogTest, logPdfNormalDistribution)
  {
    VectorXd x = VectorXd::Zero(1);
    VectorXd mean = VectorXd::Zero(1);
    MatrixXd cov = MatrixXd::Identity(1,1);

    ofstream logFile;
    logFile.open("logPdfNormalDistribution.dat");
    for (float i = -2.0; i < 2.0; i=i+0.1)
    {
      x[0] = i;
      logFile << i << " "
	      << pdfNormalDistribution(x, mean, cov)
    	      << endl;
    }
    logFile.close();
  }
}


#include <iostream>
#include <Eigen/Dense>
#include "util.h"
#include "basic_bpnn.h"

using Eigen::MatrixXd;

int main() {
    using namespace std;

	cout.sync_with_stdio(false);

	/* inputs matrix: [n * m], m is the pixels number of one image, n is the number of examples.
	 * labels matrix: [m * 1], m is the number of labels.
	 */
    auto train_inputs = util::read_mnist_matrix("../../../data/mnist/train-images.idx3-ubyte", true);
    auto train_labels = util::read_mnist_matrix("../../../data/mnist/train-labels.idx1-ubyte", false);
	auto test_inputs = util::read_mnist_matrix("../../../data/mnist/t10k-images.idx3-ubyte", true);
	auto test_labels = util::read_mnist_matrix("../../../data/mnist/t10k-labels.idx1-ubyte", false);

    CppMLNN bpnn;
    bpnn.set_hidden_layers(vector<size_t> ({256}));
    bpnn.set_output_size(10);
    bpnn.set_learn_rate(1);
    bpnn.set_max_epoch(5);
	bpnn.set_minibatch_size(256);
    bpnn.run_train(train_inputs, train_labels);
	bpnn.run_test(test_inputs, test_labels);
    return 0;
}


#include "GLKinectFrame.h"
#include "KinectFrame.h"
#include <iostream>
#include <QVector2D>
#include <QVector3D>

#include <Eigen/Dense>
#include "Projection.h"
#include "KinectFrame.h"

GLKinectFrame::GLKinectFrame()
{
}

GLKinectFrame::~GLKinectFrame()
{
}

GLuint GLKinectFrame::vertexBufferId() const
{
	return vertexBuf.bufferId();
}

void GLKinectFrame::initGL()
{
	initializeOpenGLFunctions();

	// Generate VBOs
	vertexBuf.create();
	colorBuf.create();
	normalBuf.create();

	//KinectFrame frame;
	//QKinectIO::loadFrame("../../data/room.knt", frame);
	//setFrame(&frame);
}

void GLKinectFrame::cleanupGL()
{
	vertexBuf.destroy();
	colorBuf.destroy();
	normalBuf.destroy();
}

void GLKinectFrame::setFrame(KinectFrame* frame)
{
	try
	{
		const int color_map_width = 1920;
		const int color_map_height = 1080;
		const int depth_map_width = 512;
		const int depth_map_height = 424;
		const float fovy = 70.0f;
		const float aspect_ratio = static_cast<float>(depth_map_width) / static_cast<float>(depth_map_height);
		const float near_plane = 0.1f;
		const float far_plane = 10240.0f;

		std::vector<Eigen::Vector3f> vertices;
		std::vector<Eigen::Vector3f> normals;
		std::vector<Eigen::Vector3f> colors;

		const float depth_to_color_width = color_map_width / depth_map_width;
		const float depth_to_color_height = color_map_height / depth_map_height;

		for (int x = 1; x < depth_map_width - 1; ++x)
		{
			for (int y = 1; y < depth_map_height - 1; ++y)
			{
				const float depth = static_cast<float>(frame->depth[y * depth_map_width + x]) / 100.f;
				const Eigen::Vector3f vert_uv = window_coord_to_3d(Eigen::Vector2f(x, y), depth, fovy, aspect_ratio, near_plane, far_plane, depth_map_width, depth_map_height);
				const Eigen::Vector3f vert_u1v = window_coord_to_3d(Eigen::Vector2f(x + 1, y), depth, fovy, aspect_ratio, near_plane, far_plane, depth_map_width, depth_map_height);
				const Eigen::Vector3f vert_uv1 = window_coord_to_3d(Eigen::Vector2f(x, y + 1), depth, fovy, aspect_ratio, near_plane, far_plane, depth_map_width, depth_map_height);

				float x_color = x * depth_to_color_width;
				float y_color = y * depth_to_color_height;

				if (!vert_uv.isZero() && !vert_u1v.isZero() && !vert_uv1.isZero())
				{
					const Eigen::Vector3f n1 = vert_u1v - vert_uv;
					const Eigen::Vector3f n2 = vert_uv1 - vert_uv;
					const Eigen::Vector3f n = n1.cross(n2).normalized();

					vertices.push_back(vert_uv);
					normals.push_back(n);

					const uchar r = static_cast<uchar>(frame->color[4 * y_color * color_map_width + x_color + 0]);
					const uchar g = static_cast<uchar>(frame->color[4 * y_color * color_map_width + x_color + 1]);
					const uchar b = static_cast<uchar>(frame->color[4 * y_color * color_map_width + x_color + 2]);

					//colors.push_back((n * 0.5f + Eigen::Vector3f(0.5, 0.5, 0.5)) * 255.0f);
					//colors.push_back(Eigen::Vector3f(0, 1, 0));

					colors.push_back(Eigen::Vector3f(static_cast<float>(r) / 255.f, static_cast<float>(g) / 255.f, static_cast<float>(b) / 255.f));
				}
			}
		}

		vertexBuf.bind();
		vertexBuf.allocate(&vertices[0][0], vertices.size() * sizeof(Eigen::Vector3f));

		colorBuf.bind();
		colorBuf.allocate(&colors[0][0], colors.size() * sizeof(Eigen::Vector3f));

		normalBuf.bind();
		normalBuf.allocate(&normals[0][0], normals.size() * sizeof(Eigen::Vector3f));
	}
	catch (const std::exception& ex)
	{
		std::cerr << "Error: " << ex.what() << std::endl;
	}
}

void GLKinectFrame::render(QOpenGLShaderProgram *program)
{
    vertexBuf.bind();
    int vertexLocation = program->attributeLocation("in_position");
    program->enableAttributeArray(vertexLocation);
	program->setAttributeBuffer(vertexLocation, GL_FLOAT, 0, 3, sizeof(Eigen::Vector3f));

	colorBuf.bind();
	int colorLocation = program->attributeLocation("in_color");
	program->enableAttributeArray(colorLocation);
	program->setAttributeBuffer(colorLocation, GL_FLOAT, 0, 3, sizeof(Eigen::Vector3f));

	normalBuf.bind();
	int normalLocation = program->attributeLocation("in_normal");
	program->enableAttributeArray(normalLocation);
	program->setAttributeBuffer(normalLocation, GL_FLOAT, 0, 3, sizeof(Eigen::Vector3f));

    // Draw geometry
	glDrawArrays(GL_POINTS, 0, vertexBuf.size() / sizeof(Eigen::Vector3f));
}


#include <Eigen/Dense>
#include <iostream>

Eigen::Matrix3d getRotMatrix(const Eigen::Vector3d eulers) {
    Eigen::AngleAxisd Rx(eulers(0), Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd Ry(eulers(1), Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd Rz(eulers(2), Eigen::Vector3d::UnitZ());
    return Rx.toRotationMatrix() * Ry.toRotationMatrix() * Rz.toRotationMatrix();
}

int main() {
    std::cout << getRotMatrix(Eigen::Vector3d(M_PI / 4, M_PI / 2, 0)) << std::endl;
    printf("%lf\n", atan2(0.0, 0.0));
}


#ifndef MATH_QUAT_HPP
#define MATH_QUAT_HPP

#include <boost/operators.hpp>

namespace Math {

	template<typename R = float>
	struct quat: public boost::operators<quat<R>> {
		R w, x, y, z;

		/* Unary operators */

		/** Multiplicative inverse */
		quat<R> operator!(void) const;
		/** Additive inverse */
		quat<R> operator-(void) const;
		/** Conjugate */
		quat<R> operator~(void) const;
		/** Cast operator, as Euclidean norm */
		explicit operator R(void) const;
		/** Squared Euclidean norm */
		R operator()(void) const;
		/** Simple promotion */
		explicit operator dual<R>(void) const;

		/** Distributes equality test */
		bool operator==(quat<R> const& rhs) const;
		/** Apply (lhs * rhs * ~lhs) */
		quat<R> operator()(quat<R> const& rhs) const;
		/** Apply (lhs * rhs * ~lhs) */
		dual<R> operator()(dual<R> const& rhs) const;

		/*quat<R> operator+(quat<R> const& rhs) const;
		quat<R> operator-(quat<R> const& rhs) const;
		quat<R> operator/(quat<R> const& rhs) const;
		quat<R> operator/(R const& rhs) const;*/

		quat<R>& operator+=(quat<R> const& rhs);
		quat<R>& operator-=(quat<R> const& rhs);
		quat<R>& operator*=(R const& rhs);
		quat<R>& operator*=(quat<R> const& rhs);
		quat<R>& operator/=(R const& rhs);
		quat<R>& operator/=(quat<R> const& rhs);

		quat(void) = default;
		quat(const R w, const R x = 0, const R y = 0, const R z = 0):
			w(w), x(x), y(y), z(z) {}
	};
	template<typename R>
	quat<R> quat<R>::operator-(void) const {
		return {-w,-x,-y,-z};
	}
	template<typename R>
	quat<R> quat<R>::operator!(void) const {
		return ~(*this)/((*this)());
	}
	template<typename R>
	quat<R> quat<R>::operator~(void) const {
		return {w,-x,-y,-z};
	}
	template<typename R>
	quat<R>::operator R(void) const {
		return sqrt((*this)());
	}
	template<typename R>
	R quat<R>::operator()(void) const {
		return w*w + x*x + y*y + z*z;
	}
	template<typename R>
	quat<R>::operator dual<R>(void) const {
		return {*this, 0};
	}

	template<typename R>
	bool quat<R>::operator==(quat<R> const& rhs) const {
		return w == rhs.w && x == rhs.x
			&& y == rhs.y && z == rhs.z;
	}
	template<typename R>
	quat<R> quat<R>::operator()(quat<R> const& rhs) const {
		return *this * rhs * ~*this;
	}

	template<typename R>
	quat<R>& quat<R>::operator+=(quat<R> const& rhs) {
		w += rhs.w; x += rhs.x; y += rhs.y; z += rhs.z;
		return *this;
	}
	template<typename R>
	quat<R>& quat<R>::operator-=(quat<R> const& rhs) {
		w -= rhs.w; x -= rhs.x; y -= rhs.y; z -= rhs.z;
		return *this;
	}
	template<typename R>
	quat<R>& quat<R>::operator*=(R const& rhs) {
		w *= rhs; x *= rhs;
		y *= rhs; z *= rhs;
		return *this;
	}
	template<typename R>
	quat<R>& quat<R>::operator*=(quat<R> const& r) {
		R lw = w, lx = x, ly = y, lz = z,
		  rw = r.w, rx = r.x, ry = r.y, rz = r.z;
		w = lw*rw - lx*rx - ly*ry - lz*rz;
		x = lw*rx + lx*rw + ly*rz - lz*ry;
		y = lw*ry - lx*rz + ly*rw + lz*rx;
		z = lw*rz + lx*ry - ly*rx + lz*rw;
		return *this;
	}
}

#endif


/* $Id: RescalePseudoLog.cpp,v 1.3 2013/04/08 07:36:16 severin Exp $ */

/***

NAME - EEDB::SPStreams::RescalePseudoLog

SYNOPSIS

DESCRIPTION

  A simple signal procesor which rescale expression level as pseudo log
Log transformation is convenient to vizualize data whose expression levels
varies in a wide range of values, but zero values are common places and
log(base,0) is not defined and we would thus need to recurse to pseudocount
(typically arbitrarily adding 0.5).

Alternatively we can use pseudolog defined as asinh(x/2) / log(base), which
has the following nice properties
   * is defined for all real x values
   * pseudolog(base, 0) = 0
   * pseudolog(base, -x) = -1* pseudolog(base, x)
   * pseudolog(base, x) ~ log(base, x) for x > base values
           [ For information :                                         ]
           [       pseudolog(10,1)  = 0.2089876; log10(1)  = 0         ]
           [       pseudolog(10,2)  = 0.3827757; log10(2)  = 0.3010300 ]
           [       pseudolog(10,3)  = 0.5188791; log10(3)  = 0.4771213 ]
           [       pseudolog(10,4)  = 0.6269629; log10(4)  = 0.6020600 ]
           [       pseudolog(10,5)  = 0.7153834; log10(5)  = 0.6989700 ]
           [       pseudolog(10,10) = 1.0042792; log10(10) = 1         ]
           [       pseudolog(10,100)= 2.0000430; log10(100)= 2         ]
           [       pseudolog(2,1)  = 0.6942419; log2(1)  = 0           ]
           [       pseudolog(2,2)  = 1.2715533; log2(2)  = 1           ]
           [       pseudolog(2,3)  = 1.7236790; log2(3)  = 1.584963    ]
           [       pseudolog(2,4)  = 2.0827257; log2(4)  = 2           ]
           [       pseudolog(2,5)  = 2.3764522; log2(5)  = 2.321928    ]
           [       pseudolog(2,10) = 3.3361433; log2(10) = 3.321928    ]
           [       pseudolog(2,100)= 6.6440004; log2(100)= 6.643856    ]

CONTACT

Nicolas Bertin <nbertin@gsc.riken.jp>
Jessica Severin <severin@gsc.riken.jp>

LICENSE

 * Software License Agreement (BSD License)
 * EdgeExpressDB [eeDB] system
 * copyright (c) 2007-2013 Jessica Severin RIKEN OSC
 * 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 Jessica Severin RIKEN OSC 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 ''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 COPYRIGHT HOLDERS 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.

APPENDIX

The rest of the documentation details each of the object methods. Internal methods are usually preceded with a _

***/

#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <string>
#include <stdarg.h>
#include <rapidxml.hpp>  //rapidxml must be include before boost
#include <boost/algorithm/string.hpp>
#include <EEDB/Feature.h>
#include <EEDB/Expression.h>
#include <EEDB/Experiment.h>
#include <EEDB/SPStream.h>
#include <EEDB/SPStreams/RescalePseudoLog.h>

using namespace std;
using namespace MQDB;

const char*  EEDB::SPStreams::RescalePseudoLog::class_name = "EEDB::SPStreams::RescalePseudoLog";

//function prototypes
void _spstream_rescalepseudolog_delete_func(MQDB::DBObject *obj) {
  delete (EEDB::SPStreams::RescalePseudoLog*)obj;
}
MQDB::DBObject* _spstream_rescalepseudolog_next_in_stream_func(EEDB::SPStream* node) {
  return ((EEDB::SPStreams::RescalePseudoLog*)node)->_next_in_stream();
}
void _spstream_rescalepseudolog_xml_func(MQDB::DBObject *obj, string &xml_buffer) {
  ((EEDB::SPStreams::RescalePseudoLog*)obj)->_xml(xml_buffer);
}
string _spstream_rescalepseudolog_display_desc_func(MQDB::DBObject *obj) {
  return ((EEDB::SPStreams::RescalePseudoLog*)obj)->_display_desc();
}

EEDB::SPStreams::RescalePseudoLog::RescalePseudoLog() {
  init();
}

EEDB::SPStreams::RescalePseudoLog::~RescalePseudoLog() {
}

void EEDB::SPStreams::RescalePseudoLog::init() {
  EEDB::SPStream::init();
  _classname                 = EEDB::SPStreams::RescalePseudoLog::class_name;
  _module_name               = "RescalePseudoLog";
  _funcptr_delete            = _spstream_rescalepseudolog_delete_func;
  _funcptr_xml               = _spstream_rescalepseudolog_xml_func;
  _funcptr_simple_xml        = _spstream_rescalepseudolog_xml_func;
  _funcptr_display_desc      = _spstream_rescalepseudolog_display_desc_func;

  //function pointer code
  _funcptr_next_in_stream         = _spstream_rescalepseudolog_next_in_stream_func;

  //attribute variables
  // default to base 10 (aka pseudolog10)
  base(10);
}

void EEDB::SPStreams::RescalePseudoLog::base(long value) {
  _base = value;
  char buffer[17];
  snprintf(buffer, 16, "_pseudolog%ld", value);
  _base_str = buffer;
}

void EEDB::SPStreams::RescalePseudoLog::base(char* value) {
  if(value==NULL) { return; }
  _base_str = string("_pseudolog") + value;
  _base = strtol(value, NULL, 10);
}

////////////////////////////////////////////////////////////////////////////
//
//  creation from XML section
//
////////////////////////////////////////////////////////////////////////////

EEDB::SPStreams::RescalePseudoLog::RescalePseudoLog(void *xml_node) {
  //constructor using a rapidxml <spstream> description
  init();
  if(xml_node==NULL) { return; }

  rapidxml::xml_node<>      *root_node = (rapidxml::xml_node<>*)xml_node;
  rapidxml::xml_node<>      *node;

  if(string(root_node->name()) != "spstream") { return; }

  if((node = root_node->first_node("base")) != NULL) {
    //base(strtol(node->value(), NULL, 10));
    base(node->value());
  }
}

string EEDB::SPStreams::RescalePseudoLog::_display_desc() {
  char buffer[256];
  snprintf(buffer, 256, "RescalePseudoLog%ld", _base);
  return buffer;
}

void EEDB::SPStreams::RescalePseudoLog::_xml(string &xml_buffer) {
  _xml_start(xml_buffer);  //from SPStream superclass

  char buffer[256];
  snprintf(buffer, 256, "<base>%ld</base>", _base);
  xml_buffer.append(buffer);

  _xml_end(xml_buffer);  //from superclass
}

////////////////////////////////////////////////////////////////////////////
//
// callback methods
//
////////////////////////////////////////////////////////////////////////////

MQDB::DBObject* EEDB::SPStreams::RescalePseudoLog::_next_in_stream() {
  if(_source_stream == NULL) { return NULL; }

  MQDB::DBObject *obj = _source_stream->next_in_stream();
  if(obj == NULL) { return NULL; }

  if(obj->classname() == EEDB::Expression::class_name) {
    EEDB::Expression *express = (EEDB::Expression*)obj;
    _process_expression(express);
  }

  else if(obj->classname() == EEDB::Feature::class_name) {
    EEDB::Feature *feature = (EEDB::Feature*)obj;
    vector<EEDB::Expression*>  expression = feature->expression_array();
    for(unsigned int i=0; i<expression.size(); i++) {
      _process_expression(expression[i]);
    }
    feature->rebuild_expression_hash();
  }
  //other classes are not modified

  //everything is just passed through
  return obj;
}

void  EEDB::SPStreams::RescalePseudoLog::_process_expression(EEDB::Expression *express) {
  if(express == NULL) { return; }
  EEDB::Experiment *exp = express->experiment();
  if(exp == NULL) { return; }
  EEDB::Datatype *dtype = express->datatype();
  if(dtype == NULL) { return; }

  double tval =  asinh( express->value() / 2) / log(_base) ;
  express->value(tval);
  express->datatype(dtype->type() + _base_str);
}


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_REFINE_REC_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_REFINE_REC_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/refine_rec.hpp>
#include <boost/simd/include/functions/simd/fnms.hpp>
#include <boost/simd/include/functions/simd/fma.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( refine_rec_, tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0,A0 const& a1) const
    {
      // Newton-Raphson: 1/X ~= x*(1-a0*x) + x
      return fma(fsm(One<A0>(), a0, a1), a1, a1);
    }
  };
} } }

#endif


#include <iostream>
#include <fstream>
#include <cstdio>
#include <complex>
#include <set>
#include <cmath>
#include <map>
#include <ctime>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <gflags/gflags.h>
#include <redsvd/redsvd.hpp>
#include <boost/math/special_functions/bessel.hpp>

using namespace Eigen;
using namespace REDSVD;
using namespace boost;
using namespace std;

const float EPS = 0.00000000001f;

DEFINE_string(filename, "test.ungraph", "Filename for edgelist file.");
DEFINE_string(emb1, "sparse.emb", "Filename for svd results.");
DEFINE_string(emb2, "spectral.emb", "Filename for svd results.");
DEFINE_int32(num_node, 4, "Number of node in the graph.");
DEFINE_int32(num_rank, 2, "Embedding dimension.");
DEFINE_int32(num_step, 5, "Number of order for recursion.");
DEFINE_int32(num_iter, 2, "Number of iter in randomized svd.");
DEFINE_int32(num_thread, 10, "Number of threads.");
DEFINE_double(theta, 0.5, "Parameter of ProNE");
DEFINE_double(mu, 0.1, "Parameter of ProNE");

SMatrixXf readGraph(string filename, int num_node){
    SMatrixXf A(num_node, num_node);
    typedef Eigen::Triplet<float> T;
    vector<T> tripletList;
    ifstream fin(filename.c_str());
    while (1)
    {
        string x, y;
        if (!(fin >> x >> y))
            break;
        int a = atoi(x.c_str()), b = atoi(y.c_str());
        if (a==b) continue;
        tripletList.push_back(T(a, b, 1));
        tripletList.push_back(T(b, a, 1));
    }
    A.setFromTriplets(tripletList.begin(), tripletList.end());
    return A;
}

void saveEmbedding(MatrixXf &data, string output){
    int m = data.rows(), d = data.cols();
    FILE *emb = fopen(output.c_str(), "wb");
    fprintf(emb, "%d %d\n", m, d);
    for (int i = 0; i < m; i++)
    {
        fprintf(emb, "%d", i);
        for (int j = 0; j < d; j++)
            fprintf(emb, " %f", data(i, j));
        fprintf(emb, "\n");
    }
    fclose(emb);
}

SMatrixXf l1Normalize(SMatrixXf & mat){
    SMatrixXf mat2(mat.rows(), mat.cols());
    for (int k=0; k<mat.outerSize(); ++k){
        int num_neighbor = mat.row(k).sum();
        for (SMatrixXf::InnerIterator it(mat,k); it; ++it)
            mat2.insert(k, it.col()) = it.value()/num_neighbor;
    }
    return mat2;
}

MatrixXf & l2Normalize(MatrixXf & mat){
    for (int i = 0; i < mat.rows(); ++i){
        float ssn = sqrt(mat.row(i).squaredNorm());
        if (ssn < EPS) ssn = EPS;
        mat.row(i) = mat.row(i) / ssn;
      }
    return mat;
}

SMatrixXf & validate(SMatrixXf & mat){
    for (int k=0; k<mat.outerSize(); ++k)
          for (SMatrixXf::InnerIterator it(mat,k); it; ++it)
              if (it.value() <=0)
                mat.coeffRef(k, it.col()) = 1;
    return mat;
}

SMatrixXf & smfLog(SMatrixXf & mat){
    for (int k=0; k<mat.outerSize(); ++k)
          for (SMatrixXf::InnerIterator it(mat,k); it; ++it)
              mat.coeffRef(it.row(), it.col()) = log(it.value());
    return mat;
}

float bessel(int a, float b){
    return boost::math::cyl_bessel_i(a, b);
}

void printSmf(SMatrixXf & mat){
    for (int k=0; k<mat.outerSize(); ++k)
          for (SMatrixXf::InnerIterator it(mat,k); it; ++it)
            cout <<"(" <<k << ", "<<it.col()<<", "<<it.value()<<")"<<endl;
}

MatrixXf & svdFlip(MatrixXf & mat){
    VectorXf max_abs_num = mat.cwiseAbs().colwise().maxCoeff();
    for (int i = 0; i < mat.cols(); ++i){
        float sign = max_abs_num(i) >= 0? 1.0:-1.0;
        mat.col(i) = mat.col(i) * sign;
      }
    return mat;
}

MatrixXf randomizedRangeFinder(SMatrixXf &A, int size, int num_iter){
    int n_samples = A.rows(), n_features= A.cols();
    MatrixXf Q = MatrixXf::Random(n_features, size), L(n_samples, size);
    Eigen::FullPivLU<MatrixXf> lu1(n_samples, size);
    Eigen::FullPivLU<MatrixXf> lu2(n_features, size);
    for(int i=0; i<num_iter;i++)
    {
        lu1.compute(A * Q);
        L.setIdentity();
        L.block(0, 0, n_samples, size).triangularView<Eigen::StrictlyLower>() = lu1.matrixLU();
        L = lu1.permutationP().inverse() * L;

        lu2.compute(A.transpose() * L);
        Q.setIdentity();
        Q.block(0, 0, n_features, size).triangularView<Eigen::StrictlyLower>() = lu2.matrixLU();
        Q = lu2.permutationP().inverse() * Q;
    }
    Eigen::ColPivHouseholderQR<MatrixXf> qr(A * Q);
    // return qr.colsPermutation().inverse() * qr.householderQ();
    return qr.householderQ() * MatrixXf::Identity(n_samples, size);
}

MatrixXf randomizedSvd(SMatrixXf &data, int rank, int num_iter){
    int n_oversamples = 10;
    int n_random = rank + n_oversamples;
    int n_samples = data.rows(), n_features= data.cols();
    if(n_random > min(n_samples, n_features))
        n_random = min(n_samples, n_features);

    MatrixXf Q = randomizedRangeFinder(data, n_random, num_iter);
    cout <<"Q computed done"<<endl;
    MatrixXf B = Q.transpose() * data;

    // Eigen::JacobiSVD<MatrixXf> svdOfB(B, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::BDCSVD<Eigen::MatrixXf> svdOfB(B, Eigen::ComputeThinU);

    VectorXf s = svdOfB.singularValues();
    // MatrixXf V = svdOfB.matrixV();
    MatrixXf U = Q * svdOfB.matrixU();

    U = svdFlip(U);

    MatrixXf newU = U.block(0, 0, n_samples, rank);
    // MatrixXf V = svdOfB.matrixV().block(0, 0, n_samples, rank);
    VectorXf newS = s.head(rank);

    MatrixXf emb = newU * newS.cwiseSqrt().asDiagonal();

    emb = l2Normalize(emb);
    return emb;
}

MatrixXf getEmbbeddingViaSvd(SMatrixXf &data, int rank){
    RedSVD redsvd;
    redsvd.run(data, rank);
    MatrixXf emb = redsvd.matrixU() * redsvd.singularValues().cwiseSqrt().asDiagonal();
    emb = l2Normalize(emb);
    return emb;
}

MatrixXf getEmbbeddingViaDenseSvd(MatrixXf &data, int rank){
    // Eigen::JacobiSVD<Eigen::MatrixXf> svdOfC(data, Eigen::ComputeThinU | Eigen::ComputeThinV);
    // Eigen::BDCSVD<Eigen::MatrixXf> svdOfC(data, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::BDCSVD<Eigen::MatrixXf> svdOfC(data, Eigen::ComputeThinU);
    MatrixXf emb = svdOfC.matrixU() * svdOfC.singularValues().cwiseSqrt().asDiagonal();
    emb = l2Normalize(emb);
    return emb;
}

MatrixXf getSparseEmbedding(SMatrixXf & A, int rank, int num_iter){
    time_t t1 = time(NULL);
    // cout << "number of nnz: "<< A.nonZeros() <<endl;
    int row = A.rows(), col = A.cols();
    SMatrixXf B = l1Normalize(A);
    SMatrixXf C = B.transpose();
    SMatrixXf D(col, col), E(row, col), F(row, col);
    for (int i = 0; i < row; ++i){
        D.insert(i, i) = pow(C.row(i).sum(), 0.75);
    }

    D = D / D.sum();
    E = A * D;

    B = validate(B);
    E = validate(E);

    B = smfLog(B);
    E = smfLog(E);
    F = B - E;
    cout << "preprocess time: "<< (time(NULL) - t1 + 0.0) << endl;
    // printSmf(F);
    //cout << "number of nnz: "<< F.nonZeros() <<endl;

    MatrixXf emb = getEmbbeddingViaSvd(F, rank);
    //MatrixXf emb = randomizedSvd(F, rank, num_iter);
    return emb;
}

MatrixXf getSpectralEmbedding(SMatrixXf & A, MatrixXf & a, int step, float theta, float mu){
    time_t t1 = time(NULL);
    cout << "Chebyshev series --------------- " << endl;
    if (step==1) return a;
    int num_node = a.rows(), rank = a.cols();
    SMatrixXf I(num_node, num_node);
    for (int i = 0; i < num_node; ++i)
        I.insert(i, i) = 1;
    A = A + I;
    SMatrixXf B = l1Normalize(A);
    SMatrixXf L = I - B;
    SMatrixXf M = L - mu * I;
    // cout << "number of nnz: "<< M.nonZeros() <<endl;
    // printSmf(M);

    MatrixXf Lx0 = a;
    MatrixXf Lx1 = M * a, Lx2;
    Lx1 = 0.5 * M * Lx1 - a;

    MatrixXf conv = bessel(0, theta)* Lx0;
    conv -= 2 * bessel(1, theta)* Lx1;
    for(int i=2; i<step; i++){
        Lx2 = M * Lx1;
        Lx2 = (M * Lx2 - 2 * Lx1) - Lx0;

        if (i % 2 == 0)
            conv += 2 * bessel(i, theta) * Lx2;
        else
            conv -= 2 * bessel(i, theta) * Lx2;
        Lx0 = Lx1;
        Lx1 = Lx2;
        cout << "Bessell time: " << i <<"\t"<< (time(NULL) - t1 + 0.0) << endl;
    }
    MatrixXf F = A * (a - conv);
    cout << "Chebyshev time: "<< (time(NULL) - t1 + 0.0) << endl;
    time_t t2 = time(NULL);

    MatrixXf emb = getEmbbeddingViaDenseSvd(F, rank);
    cout << "dense svd time: "<< (time(NULL) - t2 + 0.0) << endl;
    return emb;
}

int main(int argc, char** argv)
{
    gflags::ParseCommandLineFlags(&argc, &argv, true);
    time_t start_time = time(NULL);
    Eigen::setNbThreads(FLAGS_num_thread);

    SMatrixXf A = readGraph(FLAGS_filename, FLAGS_num_node);
    time_t t1 = time(NULL);
    cout << "Running time of read graph: " << (t1 - start_time + 0.0)<< endl;

    MatrixXf feature = getSparseEmbedding(A, FLAGS_num_rank, FLAGS_num_iter);
    time_t t2 = time(NULL);
    cout << "Running time of get sparse embedding: " << (t2 - t1 + 0.0) << endl;

    MatrixXf embedding = getSpectralEmbedding(A, feature, FLAGS_num_step, FLAGS_theta, FLAGS_mu);
    time_t t3 = time(NULL);
    cout << "Running time of get spectral embedding: " << (t3 - t2 + 0.0)  << endl;
    cout << "Running time of ProNE: " << (t3 - start_time + 0.0) << endl;
    saveEmbedding(feature, FLAGS_emb1);
    saveEmbedding(feature, FLAGS_emb2);

}


/*
(c) 2019 M. Werner - Part of the GIS++ tutorial
- https://www.martinwerner.de/teaching/spatial-cpp
- https://github.com/mwernerds/spatial-cpp

Program: Points
Compile: g++ -I $(BOOST_DIR) -Wall -std=c++11  -o 00_points 00_points.cpp
*/

#include<iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

using namespace boost::geometry;

// declare some points. There are type templates in the library.

typedef model::d2::point_xy<double> point_a;

// but what if we have old c-style libraries around? The array case:

#include <boost/geometry/geometries/adapted/c_array.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian) //<-- this tags the type with a coordinate system! nice.

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

    point_a p(42,11), q(43,12);
    std::cout << "The distance of P and Q is: " << distance(p,q) << std::endl;

    int p2[2]={42,11}, q2[2]={43,12};
    std::cout << "The distance of P2 and Q2 is: " << distance(p2,q2) << std::endl;

    // but what if we use a spheroid Earth model?

    return 0;
}


#include <signal.h>

// ROS
#include <ros/ros.h>
// ROS msgs
#include <vive_bridge/TrackedDevicesStamped.h>
#include <sensor_msgs/JoyFeedback.h>

// tf2
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>

// RViz
#include <rviz_visual_tools/rviz_visual_tools.h>

// Eigen
#include "tf2_eigen/tf2_eigen.h"
#include <Eigen/Dense>

// Sophus - C++ implementation of Lie Groups using Eigen
#include <sophus/se3.hpp>

#include "sophus_ros_conversions/eigen.hpp"
#include "sophus_ros_conversions/geometry.hpp"

#include "test/ceres/local_parameterization_se3.hpp"

// Ceres NLS solver solver
#include <ceres/ceres.h>

// Handle signal [ctrl + c]
bool sigint_flag = true;

void IntHandler(int signal) {
    sigint_flag = false;
}

enum E_CalibrationStates {
    STATE_SPHERE_POINTS = 0,
    STATE_CHECKERBOARD_POINTS
};

namespace Eigen {
    namespace internal {
        template <class T, int N, typename NewType>
            struct cast_impl<ceres::Jet<T, N>, NewType> {
            EIGEN_DEVICE_FUNC

            static inline NewType run(ceres::Jet<T, N> const& x) {
                return static_cast<NewType>(x.a);
            }
        };
    }  // namespace internal
}  // namespace Eigen

struct SphereCostFunctor {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    SphereCostFunctor(Eigen::Vector3d SurfacePoint) : SurfacePoint(SurfacePoint) {}

    template <class T>
    bool operator()(T const* const sCenterPoint,
                    T const* const sRadius,
                    T* sResiduals) const
    {
        using Vector3T = Eigen::Matrix<T, 3, 1>;
        Eigen::Map<Vector3T const> const CenterPoint(sCenterPoint);

        sResiduals[0] = (SurfacePoint.cast<T>() - CenterPoint).squaredNorm() - T(sRadius[0])*T(sRadius[0]);

        return true;
    }

    Eigen::Vector3d SurfacePoint;
};

struct CheckerboardCostFunctor {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CheckerboardCostFunctor(Sophus::SE3d T_s, Sophus::SE3d::Point p_t) : T_s(T_s), p_t(p_t) {}

    template <class T>
    bool operator()(T const* const sT_x, T* sResiduals) const
    {
        Eigen::Map<Sophus::SE3<T> const> const T_x(sT_x);
        Eigen::Map<Eigen::Matrix<T, 3, 1> > residuals(sResiduals);

        // residuals = ((T_a.cast<T>()*T_x).inverse()*(T_x*T_b.cast<T>() ) ).log();
        residuals = T_s.cast<T>() * T_x * p_t.cast<T>() - T_x.inverse() * T_s.translation().cast<T>();

        return true;
    }

    Sophus::SE3d T_s;
    Sophus::SE3d::Point p_t;
};

class ToolCalibratingNode {
    ros::NodeHandle nh_;
    ros::Rate loop_rate_;

    // Publishers
    ros::Publisher joy_feedback_pub_;

    // Subscribers
    ros::Subscriber devices_sub_;
    ros::Subscriber joy_sub_;

    void DevicesCb(const vive_bridge::TrackedDevicesStamped& msg_);
    void JoyCb(const sensor_msgs::Joy& msg_);

    // ROS msgs
    geometry_msgs::TransformStamped tf_msg_, tf_msg_tool_;
    sensor_msgs::JoyFeedback joy_feedback_msg_;

    // RViz
    rviz_visual_tools::RvizVisualToolsPtr rviz_tools_, rviz_mesh_tools_;

    // tf2
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener *tf_listener_;
    tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;

    std::string controller_frame, tool_frame, tracker_frame, world_frame;
    unsigned int controller_id, tracker_id;

    // Eigen
    Eigen::Affine3d eigen_msg_, eigen_tool_;

    std::vector<Eigen::Vector3d> points_;
    Eigen::Vector3d eigen_point_, eigen_c_, vecs_[3], basis_[3];
    double radius;

    Sophus::SE3d T_x;
    std::vector<Sophus::SE3d> tracker_poses_;
    std::vector<Sophus::SE3d::Point> checkerboard_points_;

    unsigned int state;

    public:
        ToolCalibratingNode(int frequency);
        ~ToolCalibratingNode();

        bool Init();
        void Loop();
        void Shutdown();
};

void ToolCalibratingNode::DevicesCb(const vive_bridge::TrackedDevicesStamped& msg_) {
    /**
     * Update information about the currently tracked devices.
     */

    for (int i = 0; i < msg_.device_count; i++) {
        if (msg_.device_classes[i] == msg_.CONTROLLER) {
            controller_frame = msg_.device_frames[i];
            controller_id = i;
        }
        if (msg_.device_classes[i] == msg_.TRACKER) {
            tracker_frame = msg_.device_frames[i];
            tracker_id = i;
        }
    }
}

ToolCalibratingNode::ToolCalibratingNode(int frequency)
    : loop_rate_(frequency),
      static_tf_broadcaster_(),
      tf_listener_(new tf2_ros::TransformListener(tf_buffer_) )
{
    // Publishers
    joy_feedback_pub_ = nh_.advertise<sensor_msgs::JoyFeedback>("/vive_node/joy/haptic_feedback", 10, true);
    // Subscribers
    devices_sub_ = nh_.subscribe("/vive_node/tracked_devices", 1, &ToolCalibratingNode::DevicesCb, this);

    // RViz
    rviz_tools_.reset(new rviz_visual_tools::RvizVisualTools(world_frame, "/rviz_visual_markers") );
    rviz_mesh_tools_.reset(new rviz_visual_tools::RvizVisualTools(world_frame, "/vive_node/rviz_mesh_markers") );

    // Define joy feedback message
    joy_feedback_msg_.type = joy_feedback_msg_.TYPE_RUMBLE;
    joy_feedback_msg_.intensity = .1;

    state = STATE_SPHERE_POINTS;

    eigen_tool_.setIdentity();

    const double W = 0.02;
    const double L = 0.02;
    const int m = 3;
    const int n = 9;

    checkerboard_points_.resize(m*n);
    for (int j = 0; j < m; j++) {
        for (int i = 0; i < n; i++) {
            checkerboard_points_[j*n + i] << L*i, W*j, 0;
        }
    }
}
ToolCalibratingNode::~ToolCalibratingNode() {
}

void ToolCalibratingNode::JoyCb(const sensor_msgs::Joy& msg_) {
      /**
     * Handle VIVE Controller inputs
     */

    if (msg_.axes[0] + msg_.axes[1] + msg_.axes[2] == 0.) {
        if (msg_.buttons[1]) { // Grip button
            std::string pError;
            if (tf_buffer_.canTransform(world_frame, tracker_frame, ros::Time(0), &pError) ) {
                // Get tracked device location from tf server
                tf_msg_ = tf_buffer_.lookupTransform(world_frame, tracker_frame, ros::Time(0) );
                eigen_msg_ = tf2::transformToEigen(tf_msg_);
                eigen_point_ = eigen_msg_.translation();

                // Trigger controller haptic feedback
                joy_feedback_pub_.publish(joy_feedback_msg_);

                // Publish sphere at sampled point
                rviz_tools_->publishSphere(eigen_point_, rviz_visual_tools::BLUE, rviz_visual_tools::MEDIUM);
                rviz_tools_->trigger();

                if (state == STATE_SPHERE_POINTS) {
                    points_.push_back(eigen_point_);
                    int n_points = points_.size();
                    ROS_INFO_STREAM("Point " << n_points << ": \n" << eigen_point_.matrix() );

                    if (n_points >= 4) {
                        // Compute solver seed from linear solution
                        if (n_points == 4) {
                            Eigen::Matrix4d eigen_A_;
                            eigen_A_ << points_[0].transpose(), 1,
                                        points_[1].transpose(), 1,
                                        points_[2].transpose(), 1,
                                        points_[3].transpose(), 1;

                            Eigen::Vector4d eigen_b_;
                            eigen_b_ << points_[0].squaredNorm(),
                                        points_[1].squaredNorm(),
                                        points_[2].squaredNorm(),
                                        points_[3].squaredNorm();

                            Eigen::Vector4d eigen_x_ = eigen_A_.fullPivHouseholderQr().solve(eigen_b_);
                            eigen_c_ = -0.5*eigen_x_.head<3>();
                            radius = 0.5*std::sqrt(eigen_c_.squaredNorm() - 4*eigen_x_(3) );
                        }

                        // Ceres NLS solver
                        ceres::Problem ceres_problem;
                        ceres::Solver::Options ceres_options;
                        ceres::Solver::Summary ceres_summary;

                        // Residual blocks
                        for (std::vector<Eigen::Vector3d>::iterator it_ = points_.begin(); it_ != points_.end(); ++it_) {
                            ceres::CostFunction* cost_function =
                                new ceres::AutoDiffCostFunction<SphereCostFunctor, 1, 3, 1>
                                                                (new SphereCostFunctor(*it_) );
                            ceres_problem.AddResidualBlock(cost_function, NULL, eigen_c_.data(), &radius);
                        }

                        // Set solver options
                        ceres_options.linear_solver_type = ceres::DENSE_SCHUR;

                        // Solve NLS problem
                        Solve(ceres_options, &ceres_problem, &ceres_summary);
                        // ROS_INFO_STREAM(ceres_summary.FullReport() << std::endl);

                        // for (std::vector<Eigen::Vector3d>::iterator it_ = points_.begin(); it_ != points_.end(); ++it_) {
                        //     eigen_tool_.translation() += eigen_msg_.rotation().inverse()*(eigen_c_ - *it_);
                        // }
                        // eigen_tool_.translation() /= n_points;

                        if (n_points > 4) {
                            eigen_tool_.translation() += (eigen_msg_.rotation().inverse()*(eigen_c_ - eigen_point_) -
                                                          eigen_tool_.translation() ) / n_points;
                        } else {
                            eigen_tool_.translation() = eigen_msg_.rotation().inverse()*(eigen_c_ - eigen_point_);
                        }

                        // eigen_tool_.translation() = Eigen::Vector3d(0., 0., std::abs(radius) );
                        tf_msg_tool_.transform = tf2::eigenToTransform(eigen_tool_).transform;
                        static_tf_broadcaster_.sendTransform(tf_msg_tool_);

                        T_x = sophus_ros_conversions::transformMsgToSophus(tf_msg_tool_.transform).cast<double>();

                        ROS_INFO_STREAM("Sphere center point (relative to tracker): \n" << eigen_tool_.translation().matrix() );
                        ROS_INFO_STREAM("Sphere radius: " << eigen_tool_.translation().norm() );
                        ROS_INFO_STREAM("rosrun tf2_ros static_transform_publisher " << tf_msg_tool_.transform.translation.x << " "
                                                                                << tf_msg_tool_.transform.translation.y << " "
                                                                                << tf_msg_tool_.transform.translation.z << " "
                                                                                << tf_msg_tool_.transform.rotation.x << " "
                                                                                << tf_msg_tool_.transform.rotation.y << " "
                                                                                << tf_msg_tool_.transform.rotation.z << " "
                                                                                << tf_msg_tool_.transform.rotation.w << " "
                                                                                << tracker_frame << " " << tool_frame);

                        rviz_tools_->deleteAllMarkers();
                        rviz_tools_->publishSphere(eigen_c_, rviz_visual_tools::BLUE, 2*radius);
                        rviz_tools_->trigger();

                        if (n_points == 4) {
                            // Publish tool mesh
                            rviz_mesh_tools_->publishMesh(Eigen::Affine3d::Identity(),
                                                        "package://vive_calibrating/meshes/spike.dae",
                                                        rviz_visual_tools::BLACK,
                                                        1,
                                                        tool_frame);
                            rviz_mesh_tools_->trigger();
                        }
                    }
                }

                // if (state == STATE_CHECKERBOARD_POINTS) {
                //     tracker_poses_.push_back(sophus_ros_conversions::transformMsgToSophus(tf_msg_.transform).cast<double>() );
                //     ROS_INFO_STREAM("Pose " << tracker_poses_.size() << "/" << checkerboard_points_.size() << ":");

                //     rviz_tools_->publishSphere((eigen_msg_ * eigen_tool_).translation(), rviz_visual_tools::BLUE, rviz_visual_tools::XLARGE);

                //     if (tracker_poses_.size() == checkerboard_points_.size() ) {
                //         // Ceres NLS solver
                //         ceres::Problem ceres_problem;
                //         ceres::Solver::Options ceres_options;
                //         ceres::Solver::Summary ceres_summary;

                //         ceres_problem.AddParameterBlock(T_x.data(), Sophus::SE3d::num_parameters,
                //                                         new Sophus::test::LocalParameterizationSE3);

                //         // Residual blocks
                //         for (int i = 0; i < tracker_poses_.size(); i++) {
                //             ceres::CostFunction* cost_function =
                //                 new ceres::AutoDiffCostFunction<CheckerboardCostFunctor, 3, Sophus::SE3d::num_parameters>
                //                                                 (new CheckerboardCostFunctor(tracker_poses_[i],
                //                                                                              checkerboard_points_[i]) );
                //             ceres_problem.AddResidualBlock(cost_function, NULL, T_x.data() );
                //         }

                //         // Set solver options
                //         ceres_options.linear_solver_type = ceres::DENSE_SCHUR;

                //         // Solve NLS problem
                //         Solve(ceres_options, &ceres_problem, &ceres_summary);
                //         ROS_INFO_STREAM(ceres_summary.FullReport() << std::endl);

                //         tf_msg_tool_.transform = sophus_ros_conversions::sophusToTransformMsg(T_x.cast<float>() );
                //         tf_msg_tool_.header.stamp = ros::Time::now();

                //         static_tf_broadcaster_.sendTransform(tf_msg_tool_);
                //         ROS_INFO_STREAM(tf_msg_tool_);
                //     }
                // }
            } else {
                ROS_WARN_STREAM("Can't transform from " + world_frame + " to " + tracker_frame + ": " + pError);
            }
        }
    }

    // if (msg_.buttons[0]) { // Menu button
    //     if (state == STATE_SPHERE_POINTS) {
    //         if (points_.size() >= 4) {
    //             ROS_INFO_STREAM("Define checkerboard points");
    //             state = STATE_CHECKERBOARD_POINTS;
    //         }
    //     }
    // }

    // if (msg_.buttons[3]) { // Trigger button
    //     joy_feedback_pub_.publish(joy_feedback_msg_);

    //     points_.push_back(eigen_point_);
    //     ROS_INFO_STREAM("Point " << n_points << ": \n" << eigen_point_.matrix() );
    // }
}

bool ToolCalibratingNode::Init() {
      /**
     * Check if the necessary transforms are available and initialize the node
     */

    nh_.param<std::string>("/vive_node/world_frame", world_frame, "root");

    // Get available controller
    while ((controller_frame.empty() || tracker_frame.empty() ) && sigint_flag) {
        ROS_INFO("Waiting for controller and tracker...");

        ros::spinOnce();
        ros::Duration(3.0).sleep();
    }

    // Handle sigint
    if (!sigint_flag) {
        return false;
    }

    ROS_INFO_STREAM("Using " + controller_frame + " and " + tracker_frame + " for calibration");

    // Subscribe to joy topic
    joy_sub_ = nh_.subscribe("/vive_node/joy/" + controller_frame, 1, &ToolCalibratingNode::JoyCb, this);

    // Check if transforms are available
    std::string pError;
    if (!tf_buffer_.canTransform(controller_frame,
                                 world_frame, ros::Time(0),
                                 ros::Duration(10.0), &pError) )
    {
        ROS_ERROR_STREAM("Can't transform from " + world_frame + " to " + controller_frame + ": " + pError);

        return false;
    }
    if (!tf_buffer_.canTransform(tracker_frame,
                                    world_frame, ros::Time(0),
                                    ros::Duration(0.), &pError) )
    {
        ROS_WARN_STREAM("Can't transform from " + world_frame + " to " + tracker_frame + ": " + pError);

        return false;
    }

    joy_feedback_msg_.id = controller_id;

    tool_frame = tracker_frame + "_tool0";
    tf_msg_tool_.header.frame_id = tracker_frame;
    tf_msg_tool_.child_frame_id = tool_frame;

    // Limited marker lifetime
    rviz_tools_->setAlpha(0.5);
    rviz_tools_->setLifetime(0.);

    rviz_tools_->setBaseFrame(tracker_frame);
    rviz_tools_->enableFrameLocking();

    // Publish red sphere at tracker's position
    rviz_tools_->publishSphere(Eigen::Vector3d(0., 0., 0.), rviz_visual_tools::RED, rviz_visual_tools::XLARGE);
    rviz_tools_->trigger();

    rviz_tools_->setBaseFrame(world_frame);
    rviz_tools_->enableFrameLocking(false);

    rviz_mesh_tools_->loadMarkerPub(false, true);
    rviz_mesh_tools_->enableFrameLocking();
    rviz_mesh_tools_->setBaseFrame(tool_frame);
    rviz_mesh_tools_->setLifetime(0.);

    return true;
}

void ToolCalibratingNode::Loop() {
    ros::spinOnce();
    loop_rate_.sleep();
}
void ToolCalibratingNode::Shutdown() {
      /**
     * Runs before shutting down the node
     */

    ros::shutdown();
}

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

    // Handle signal [ctrl + c]
    signal(SIGINT, IntHandler);

    ToolCalibratingNode node_(60);

    if (!node_.Init() ) {
        node_.Shutdown();

        // Handle sigint
        if (sigint_flag) {
            exit(EXIT_SUCCESS);
        } else {
            exit(EXIT_FAILURE);
        }
    }

    while (ros::ok() && sigint_flag) {
        node_.Loop();
    }

    node_.Shutdown();
    exit(EXIT_SUCCESS);
}


//////////////////////////////////////////////////////////////////////////////////
// survival::modelss::exponential::scalar::meta::failure_time_distribution.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_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_EXPONENTIAL_SCALAR_META_FAILURE_TIME_DISTRIBUTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_EXPONENTIAL_SCALAR_META_FAILURE_TIME_DISTRIBUTION_HPP_ER_2009
#include <cmath>
#include <boost/math/distributions/exponential.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/exponential/is_math_distribution.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/exponential/random.hpp>
#include <boost/statistics/detail/distribution/survival/failure_time/meta/distribution.hpp>
#include <boost/statistics/detail/distribution/survival/models/exponential/scalar/model.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace survival{
namespace failure_time{

    template<typename T,typename L>
    struct distribution< exponential_model<T,L> >
    {

        typedef exponential_model<T,L> model_;
        typedef boost::math::exponential_distribution<T> type;

        template<typename X>
        static type call(const X& x, const model_& mo){
            T lambda = mo.log_rate(x);
            lambda = exp( lambda );
            return type(
                lambda
            );
        }
    };

}// failure_time
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


///////////////////////////////////////////////////////////////////////////////
// random::example::poisson_speed.cpp     						     		 //
//                                                                           //
//  Copyright 2010 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)         //
///////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <algorithm>
#include <iterator>

#include <boost/timer.hpp>

#include <boost/random/poisson_distribution.hpp>
#include <boost/random/poisson_ext/devroye/detail/step4/standard.hpp>
#include <boost/random/poisson_ext/devroye/detail/step4/squeeze.hpp>
#include <boost/random/poisson_ext/devroye/sampler/meta_int_mean.hpp>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

template<typename Int>
void example_poisson_speed(
	std::ostream& os,
    std::size_t n, 			//sample size
    int n_loops,
    int mean,
    int factor 			// mean *= factor at each loop
){
	os << "poisson_speed{" << std::endl;

	namespace devroye = boost::random::poisson::devroye;
	typedef boost::timer timer_;
    typedef Int 	int_;
    typedef double 	val_;
    timer_ timer;
	typedef boost::mt19937 urng_;
    typedef boost::poisson_distribution<int_> random1_;
    typedef devroye::tag::standard tag3_;
    typedef devroye::tag::squeeze tag4_;
    typedef typename devroye::sampler::meta_int_mean<tag3_,int_>::type random3_;
    typedef typename devroye::sampler::meta_int_mean<tag4_,int_>::type random4_;

    urng_ urng;
	urng_ urng1 = urng;
    urng_ urng3 = urng;
    urng_ urng4 = urng;

	typedef boost::variate_generator<urng_&,random1_> vg1_;
	typedef boost::variate_generator<urng_&,random3_> vg3_;
	typedef boost::variate_generator<urng_&,random4_> vg4_;

	val_ t1, t3, t4 = 0;
	os << "n = " << n << std::endl;
	os << "(mean,{time[j] : j = default,devroye-st,devroye-sq})" << std::endl;
	for(int_ i1 = 0; i1<n_loops; ++i1){
		vg1_ vg1(urng1,random1_(mean));
		vg3_ vg3(urng3,random3_(mean));
		vg4_ vg4(urng4,random4_(mean));

        for(int_ i2 = 0; i2<n; i2++){
            timer.restart(); vg1();	t1 += timer.elapsed();
            timer.restart(); vg3();	t3 += timer.elapsed();
            timer.restart(); vg4();	t4 += timer.elapsed();
        }
        mean *= factor;
        os
        	<< '('
            << mean
            << ','
            << t1
            << ','
            << t3
            << ','
            << t4
            << ')'
            << std::endl;
    }

	os << "}" << std::endl;

}