//
// Created by wei on 2/5/19.
//

#include <Eigen/Eigen>
#include <Open3D/Open3D.h>
#include "DatasetConfig.h"

using namespace open3d;
using namespace open3d::io;
using namespace open3d::utility;
using namespace open3d::geometry;
using namespace open3d::registration;

std::shared_ptr<LineSet> VisualizePoseGraph(PoseGraph &pose_graph) {
    std::shared_ptr<LineSet> pose_graph_vis =
        std::make_shared<LineSet>();

    int cnt = 0;

    const int kPointsPerFrustum = 5;
    const int kEdgesPerFrustum = 8;
    for (auto &node : pose_graph.nodes_) {
        auto pose = node.pose_;

        double norm = 0.1;
        Eigen::Vector4d ph;

        ph = pose * Eigen::Vector4d(0, 0, 0, 1);
        pose_graph_vis->points_.emplace_back(ph.hnormalized());
        ph = pose * (norm * Eigen::Vector4d(1, 1, 2, 1/norm));
        pose_graph_vis->points_.emplace_back(ph.hnormalized());
        ph = pose * (norm * Eigen::Vector4d(1, -1, 2, 1/norm));
        pose_graph_vis->points_.emplace_back(ph.hnormalized());
        ph = pose * (norm * Eigen::Vector4d(-1, -1, 2, 1/norm));
        pose_graph_vis->points_.emplace_back(ph.hnormalized());
        ph = pose * (norm * Eigen::Vector4d(-1, 1, 2, 1/norm));
        pose_graph_vis->points_.emplace_back(ph.hnormalized());

        pose_graph_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 0, cnt + 1));
        pose_graph_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 0, cnt + 2));
        pose_graph_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 0, cnt + 3));
        pose_graph_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 0, cnt + 4));
        pose_graph_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 1, cnt + 2));
        pose_graph_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 2, cnt + 3));
        pose_graph_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 3, cnt + 4));
        pose_graph_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 4, cnt + 1));

        for (int k = 0; k < kEdgesPerFrustum; ++k) {
            pose_graph_vis->colors_.emplace_back(Eigen::Vector3d(1, 0, 0));
        }

        cnt += kPointsPerFrustum;
    }

    for (auto &edge : pose_graph.edges_) {
        int s = edge.source_node_id_;
        int t = edge.target_node_id_;

        if (edge.uncertain_) {
            pose_graph_vis->lines_.emplace_back(
                Eigen::Vector2i(s * kPointsPerFrustum, t * kPointsPerFrustum));
            pose_graph_vis->colors_.emplace_back(Eigen::Vector3d(0, 1, 0));
        } else {
            pose_graph_vis->lines_.emplace_back(
                Eigen::Vector2i(s * kPointsPerFrustum, t * kPointsPerFrustum));
            pose_graph_vis->colors_.emplace_back(Eigen::Vector3d(0, 0, 1));
        }
    }

    return pose_graph_vis;
}

int main(int argc, char **argv) {
    SetVerbosityLevel(utility::VerbosityLevel::Debug);

    DatasetConfig config;

    std::string config_path = argc > 1 ? argv[1] :
        kDefaultDatasetConfigDir + "/stanford/lounge.json";

    bool is_success = ReadIJsonConvertible(config_path, config);
    if (! is_success) return 1;

    PoseGraph pose_graph_cuda;
    const std::string &path_cuda = config.GetPoseGraphFileForRefinedScene(true);
    ReadPoseGraph(path_cuda, pose_graph_cuda);

    auto pose_graph_vis_cuda = VisualizePoseGraph(pose_graph_cuda);
    auto mesh = CreateMeshFromFile(config.GetReconstructedSceneFile());

    visualization::DrawGeometries({pose_graph_vis_cuda, mesh});

    return 0;
}


#include "strategy.hpp"
#include <armadillo>
#include <list>

Strategy::Strategy(VSSSBuffer<GameState> &game_buffer, int *waitkey_buf, const char *serial_port, int baud)
{
	if (serial_port)
	{
		this->serial = new SerialSender(serial_port, baud);
	}
	else
	{
		this->serial = NULL;
	}
	this->robot_control_th = std::thread(&Strategy::robot_control, this, &game_buffer, waitkey_buf);
}

Strategy::~Strategy()
{
	if ((this->robot_control_th).joinable())
	{
		(this->robot_control_th).join();
	}
	this->serial->send(0, 0, 0, 0, 0, 0);
	this->serial->serialclose();
	delete this->serial;
}

arma::vec2 to_target(Robot robot, arma::vec2 target, double distance_to_stop = -1.0);
list<Ball> prev_ball_state;
arma::vec2 future_ball_pos(int n_frames);

list<Ball> ball_history;
list<Robot> mid_history;

void Strategy::robot_control(VSSSBuffer<GameState> *game_buffer, int *waitkey_buf)
{
	vector<arma::vec2> robot_speed(3);
	const arma::vec2 friendly_goal_center_pos = {0, 65};
	const arma::vec2 enemy_goal_center_pos = {150, 65};
	bool game_running = false;
	int last_paused = 0;
	int frame_number = 0;
	while (*waitkey_buf != ESC_CHAR)
	{
		game_running = (*waitkey_buf) == 's' ? true : game_running;
		game_running = (*waitkey_buf) == 'p' ? false : game_running;

		game_buffer->get(this->state);
		frame_number++;
		if (!game_running)
			last_paused = frame_number;

		// Ball prediction
		prev_ball_state.push_back(this->state.ball);
		if (prev_ball_state.size() > 5)
			prev_ball_state.pop_front();

		ball_history.push_back(this->state.ball);
		if (ball_history.size() > 30)
			ball_history.pop_front();

		mid_history.push_back(this->state.robots[0]);
		if (mid_history.size() > 30)
			mid_history.pop_front();

		cout << this->state.robots[0].pos << endl;

		// Mid-field
		// TODO future number of frames reduced when close.
		arma::vec2 enemy_goal_to_ball = (future_ball_pos(0) - enemy_goal_center_pos);
		double scaling_factor = (sqrt(arma::norm(enemy_goal_to_ball, 2)) + 1.5) / sqrt(arma::norm(enemy_goal_to_ball, 2));

		arma::vec2 midfield_target = scaling_factor * enemy_goal_to_ball + enemy_goal_center_pos;
		// TODO verify out of bounds conditions or goalkeeper interference

		if (sqrt(arma::norm(midfield_target - midfield_target, 2)) < 3.0)
		{
			midfield_target = this->state.ball.pos;
		}

		midfield_target[0] = max(6.0, min(144.0, midfield_target[0]));
		midfield_target[1] = min(124.0, max(6.0, midfield_target[1]));

		// Stay outside goal area.
		if (midfield_target[0] < 20 && 25 < midfield_target[1] && midfield_target[1] < 105)
		{
			midfield_target[0] = 30;
			if (this->state.ball.pos[1] < 65)
				midfield_target[1] = 15;
			else
				midfield_target[1] = 115;
		}

		robot_speed[0] = to_target(this->state.robots[0], midfield_target);

		if (!mid_history.front().missing && !mid_history.back().missing && frame_number - last_paused > 60)
		{
			if (sqrt(arma::norm(mid_history.front().pos - mid_history.back().pos, 2)) < 1)
			{
				robot_speed[0] = -2 * robot_speed[0];
				robot_speed[0][(frame_number / 40) % 2] = 0;
			}
		}

		if (!ball_history.front().missing && !ball_history.back().missing && frame_number - last_paused > 60)
		{
			if (sqrt(arma::norm(ball_history.front().pos - ball_history.back().pos, 2)) < 1)
				if (sqrt(arma::norm(this->state.ball.pos - this->state.robots[0].pos, 2)) < 3)
				{
					if (this->state.robots[0].pos[1] > 65)
						robot_speed[0] = {120, -120};
					else
						robot_speed[0] = {-120, 120};
				}
		}

		// Defender
		arma::vec2 defender_target;
		defender_target[0] = 30;
		if (this->state.robots[2].pos[1] < 65)
			defender_target[1] = 80;
		else
			defender_target[1] = 50;

		robot_speed[1] = to_target(this->state.robots[1], defender_target, 2.0);

		double defender_distance_to_ball = sqrt(arma::norm(this->state.ball.pos - this->state.robots[1].pos, 2));
		if (defender_distance_to_ball < 6)
		{
			arma::vec2 goal_to_robot = this->state.robots[1].pos - friendly_goal_center_pos;
			arma::vec2 goal_to_ball = this->state.ball.pos - friendly_goal_center_pos;

			arma::vec3 c_prod = arma::cross(arma::vec3({goal_to_robot[0], goal_to_robot[1], 0.0}), arma::vec3({goal_to_ball[0], goal_to_ball[1], 0.0}));
			double c_prod_sign = c_prod[2] == 0 ? 1 : (c_prod[2] / abs(c_prod[2]));

			robot_speed[1] = {c_prod_sign * 250, c_prod_sign * -250};
		}

		// Goalkeeper
		arma::vec2 goalkeeper_target = {8, max(42.0, min(88.0, future_ball_pos(2)[1]))};
		robot_speed[2] = to_target(this->state.robots[2], goalkeeper_target, 2.0);

		double goalkeeper_distance_to_ball = sqrt(arma::norm(this->state.ball.pos - this->state.robots[2].pos, 2));
		if (goalkeeper_distance_to_ball < 4)
		{
			arma::vec2 goal_to_robot = this->state.robots[2].pos - friendly_goal_center_pos;
			arma::vec2 goal_to_ball = this->state.ball.pos - friendly_goal_center_pos;

			arma::vec3 c_prod = arma::cross(arma::vec3({goal_to_robot[0], goal_to_robot[1], 0.0}), arma::vec3({goal_to_ball[0], goal_to_ball[1], 0.0}));
			double c_prod_sign = c_prod[2] == 0 ? 1 : (c_prod[2] / abs(c_prod[2]));

			robot_speed[2] = {c_prod_sign * 120, c_prod_sign * -120};
		}

		// Missing conditions
		for (int i = 0; i < 3; i++)
			if (this->state.robots[i].missing || this->state.ball.missing || !game_running)
				robot_speed[i] = {0, 0};

		if (this->serial)
			this->serial->send(robot_speed[0][0], robot_speed[0][1], robot_speed[1][0], robot_speed[1][1], robot_speed[2][0], robot_speed[2][1]);
		else
		{
			printf("Speed: %lf %lf %lf %lf %lf %lf\n", robot_speed[0][0], robot_speed[0][1], robot_speed[1][0], robot_speed[1][1], robot_speed[2][0], robot_speed[2][1]);
		}
	}
}

arma::vec2 to_target(Robot robot, arma::vec2 target, double distance_to_stop)
{
	arma::vec2 dir_target = arma::normalise(target - robot.pos);

	arma::vec3 c_prod = arma::cross(arma::vec3({robot.dir[0], robot.dir[1], 0.0}), arma::vec3({dir_target[0], dir_target[1], 0.0}));
	double c_prod_sign = c_prod[2] == 0 ? 1 : (c_prod[2] / abs(c_prod[2]));

	double theta;
	double move_dir;
	if (arma::dot(dir_target, robot.dir) >= 0)
		theta = acos(arma::dot(dir_target, robot.dir)), move_dir = 1.0;
	else
		theta = acos(-1 * arma::dot(dir_target, robot.dir)), move_dir = -1.0;

	double distance_to_target = sqrt(arma::norm(target - robot.pos, 2));

	double diff = min(60.0, theta * 30); // TODO find the best parameters for both these numbers
	double fwd = min(200.0, max(60.0, 40 + 6 * distance_to_target));

	arma::vec2 retv = {move_dir * (fwd - c_prod_sign * diff), move_dir * (fwd + c_prod_sign * diff)}; // TODO maybe use {fwd, fwd + diff}

	if (distance_to_target < distance_to_stop)
		retv = {0.0, 0.0};

	return retv;
}

arma::vec2 future_ball_pos(int n_frames)
{
	if (prev_ball_state.size() > 1 && !prev_ball_state.front().missing && !prev_ball_state.back().missing)
	{
		arma::vec2 diff = (prev_ball_state.back().pos - prev_ball_state.front().pos) / (prev_ball_state.size() - 1);
		return prev_ball_state.back().pos + n_frames * diff;
	}
	else
		return prev_ball_state.back().pos;
}


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-operator
    This function object computes the product of two parameters of the same type.

    Infix notation can be used with operator '*',

    @par Header <boost/simd/function/multiplies.hpp>

    @par Decorators

     - `saturated_` (See @ref group-decorator) computes the saturated product.

    @see minus, divides, plus

    @par Example:

      @snippet multiplies.cpp multiplies

    @par Possible output:

      @snippet multiplies.txt multiplies

    returns the product of @c a and @c b

    @see fma, fms, fnma, fnms

  **/
  Value multiplies(Value const& x, Value const& y);
} }
#endif

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

#endif


/* Desc: narf_interst_image.cpp
  Author: Sasha Sax, CVGL
  Desc: Reads in a depth image and saves out an interest image. An interest image contains
    the 'soft' output of a keypoint detector, in this case NARF
    (paper: https://pdfs.semanticscholar.org/e070/1662a370622a1cdbb7c6a83bbede3d0e6c23.pdf)
    Specifically, the interest image contains intensity values for each pixel before a
    hard yes/no decision is made as to whether that pixel is a keypoint. This will probably
    provide richer signals to a learning model and that is why we use the interest image
    instead of just keypoints.

  Usage: ./narf_interest_image.bin [options] <inputDepthImage.png> <outputInterestImage.png>

    Options:
    -------------------------------------------
    -r <int>     Image resolution in pixels
    -f <float>   Focal length in pixels
    -d <float>   The distance that corresponds to a 1-pixel-intensity difference - default 0.00195312)
    -m           Treat all unseen points to max range
    -s <float>   support size for the interest points (diameter of the used sphere - default 0.2)
    -o <0/1>     switch rotational invariant version of the feature on/off (default 1)
    -v <0/1>     Be verbose and visualize the output -  (default 0)
    -h           this help

  Requires (to be run):
    - generate_points.py
    - create_depth_images.py
*/
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/features/narf_descriptor.h>
// #include <pcl/keypoints/narf_keypoint.h>
#include "pcl_narf.hpp"
#include <pcl/range_image/range_image_planar.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/range_image_visualizer.h>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

// This needs to be included before GIL because cruft
#define png_infopp_NULL (png_infopp)NULL
#define int_p_NULL (int*)NULL
#include <boost/gil/gil_all.hpp>
#include <boost/gil/gray.hpp>
#include <boost/gil/extension/io/png_dynamic_io.hpp>
#include <iostream>
using namespace boost::gil;

// #include <stdint.h>

using boost::property_tree::ptree;

float support_size = 0.2f; // Use points w/in this many meters to detect keypoints
bool set_unseen_to_max_range = false;
bool rotation_invariant = true;
float sensitivity = 1.0f/512.0f;
int resolution = 256;
float focal_length = 242.9311106566421;
bool visualize = false;

void
printUsage (const char* progName)
{
  std::cout << "\n\nUsage: "<<progName<<" [options] <inputDepthImage.png> <outputInterestImage.png>\n\n"
            << "Options:\n"
            << "-------------------------------------------\n"
            << "-r <int>     Image resolution in pixels\n"
            << "-f <float>   Focal length in pixels\n"
            << "-d <float>   The distance that corresponds to a 1-pixel-intensity difference - "
                                                                  "default "<<sensitivity<<")\n"
            << "-m           Treat all unseen points to max range\n"
            << "-s <float>   support size for the interest points (diameter of the used sphere - "
                                                                  "default "<<support_size<<")\n"
            << "-o <0/1>     switch rotational invariant version of the feature on/off"
            <<               " (default "<< (int)rotation_invariant<<")\n"
            << "-v <0/1>     Be verbose and visualize the output - "
            <<               " (default "<< (int)visualize<<")\n"
            << "-h           this help\n"
            << "\n\n";
}

// Uncomment this if building and using visualizations
void
setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
{
  Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f (0, 0, 0);
  Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f (0, 0, -1) + pos_vector;
  Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f (0, 1, 0);
  viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
                            look_at_vector[0], look_at_vector[1], look_at_vector[2],
                            up_vector[0], up_vector[1], up_vector[2]);
}

struct PixelInserter{
        std::vector<uint16_t>* storage;
        PixelInserter(std::vector<uint16_t>* s) : storage(s) {}
        void operator()(boost::gil::gray16_pixel_t p) const {
                storage->push_back(boost::gil::at_c<0>(p));
        }
};

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

    // --------------------------------------
    // -----Parse Command Line Arguments-----
    // --------------------------------------
    if (pcl::console::parse (argc, argv, "-v", visualize) >= 0)
        if(visualize)
            std::cout << "Setting visualization to "<<visualize<<".\n";
    if (pcl::console::find_argument (argc, argv, "-h") >= 0)
    {
        printUsage (argv[0]);
        return 0;
    }
    if (pcl::console::find_argument (argc, argv, "-m") >= 0 && visualize)
    {
        set_unseen_to_max_range = true;
        std::cout << "Setting unseen values in range image to maximum range readings.\n";
    }
    if (pcl::console::parse (argc, argv, "-o", rotation_invariant) >= 0 && visualize)
        std::cout << "Switching rotation invariant feature version "<< (rotation_invariant ? "on" : "off")<<".\n";
    int tmp_coordinate_frame;
    if (pcl::console::parse (argc, argv, "-s", support_size) >= 0 && visualize)
        std::cout << "Setting support size to "<<support_size<<".\n";
    if (pcl::console::parse (argc, argv, "-r", resolution) >= 0 && visualize)
        std::cout << "Setting resolution to "<<resolution<<".\n";
    if (pcl::console::parse (argc, argv, "-f", focal_length) >= 0 && visualize)
        std::cout << "Setting focal length to "<<focal_length<<" pixels.\n";
    if (pcl::console::parse (argc, argv, "-d", sensitivity) >= 0 && visualize)
        std::cout << "Setting distance per pixel to "<<sensitivity<<".\n";

    std::vector<int> png_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "png");
    std::string input_filename = argv[png_filename_indices[0]];
    std::string output_filename = argv[png_filename_indices[1]];
    if(visualize) {
        std::cout << "Input " << input_filename << std::endl;
        std::cout << "Output " << output_filename << std::endl;

    }
    double program_start_time = pcl::getTime();
	// Parameters needed by the planar range image object:

	// Image size. Both Kinect and Xtion work at 640x480.
	int imageSizeX = resolution;
	int imageSizeY = resolution;
	// Center of projection. here, we choose the middle of the image.
	float centerX = ((float)resolution) / 2.0f;
	float centerY = ((float)resolution) / 2.0f;
	float focalLengthX = focal_length, focalLengthY = focalLengthX;

	// Sensor pose. We just set this to empty.
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	Eigen::Affine3f sensorPose = Eigen::Affine3f(Eigen::Translation3f(cloud->sensor_origin_[0],
								 cloud->sensor_origin_[1],
								 cloud->sensor_origin_[2])) *
								 Eigen::Affine3f(cloud->sensor_orientation_);

    // Noise level. If greater than 0, values of neighboring points will be averaged.
	// This would set the search radius (e.g., 0.03 == 3cm).
	float noiseLevel = 0.0f;
	float minimumRange = 0.0f; // Minimum range. If set, any point closer to the sensor than this will be ignored.

    // ------------------------
    // -----Read range png-----
    // ------------------------
    double load_start_time = pcl::getTime();
    boost::gil::gray16_image_t image;
    std::vector<uint16_t> storage;
    storage.reserve(resolution * resolution);
    std::vector<float> depthImage;
    depthImage.reserve(resolution * resolution);

    boost::gil::png_read_and_convert_image(input_filename, image);

    // Save image as array and multiply by the pixel sensitivity
    for_each_pixel(const_view(image), PixelInserter(&storage));
    for ( int i = 0; i < storage.size (); i++ )
        depthImage.push_back( (float) storage[i] * sensitivity );

    double load_time = pcl::getTime()-load_start_time;
    if(visualize) std::cout << "Loading took "<<1000.0*load_time<<"ms.\n";

    // -------------------------
    // -----Set Range Image-----
    // -------------------------
	// Planar range image object.
    boost::shared_ptr<pcl::RangeImagePlanar> range_image_planar_ptr (new pcl::RangeImagePlanar);
    pcl::RangeImagePlanar& range_image_planar = *range_image_planar_ptr;
	range_image_planar.setDepthImage(&depthImage[0], imageSizeX, imageSizeY,
			centerX, centerY, focalLengthX, focalLengthX );

    // --------------------------------
    // -----Extract NARF keypoints-----
    // --------------------------------
    if (set_unseen_to_max_range)
      range_image_planar.setUnseenToMaxRange ();

    pcl::RangeImageBorderExtractor range_image_border_extractor;
    pcl::NarfKeypoint2 narf_keypoint_detector;
    narf_keypoint_detector.setRangeImageBorderExtractor (&range_image_border_extractor);
    narf_keypoint_detector.setRangeImage (&range_image_planar);
    narf_keypoint_detector.getParameters ().support_size = support_size;
    narf_keypoint_detector.getParameters ().calculate_sparse_interest_image = false;
    narf_keypoint_detector.getParameters ().use_recursive_scale_reduction = true;
    // narf_keypoint_detector.getParameters ().do_non_maximum_suppression = false;

    pcl::PointCloud<int> keypoint_indices;
    double keypoint_extraction_start_time = pcl::getTime();
    // narf_keypoint_detector.compute (keypoint_indices);
    // narf_keypoint_detector.calculateInterestImage ();
    float* interest_ptr = narf_keypoint_detector.getInterestImage ();
    double keypoint_extraction_time = pcl::getTime()-keypoint_extraction_start_time;
    if(visualize)
        std::cout << "Found "<<keypoint_indices.points.size ()<<" key points. "
              << "This took "<<1000.0*keypoint_extraction_time<<"ms.\n";

    double program_time = pcl::getTime()-program_start_time;
    if(visualize)
        std::cout <<  "Program took "<<1000.0*program_time<<"ms.\n";

    // -----------------------------
    // -----Save interest image-----
    // -----------------------------
    // One option is to write out the interest points as a json
    // ptree pt;
    // ptree children;
    // for( int i=0; i < keypoint_indices.points.size(); i++ ) {
    //     ptree interest_node;
    //     interest_node.put("", keypoint_indices.points[i]);
    //     children.push_back( std::make_pair("", interest_node) );
    // }
    // pt.add_child("Interests", children);
    // write_json(output_filename + "kps.json", pt);

    // But better to save the interest points as a png
    const gray16_view_t & mViewer = view(image);
    int count_zero = 0;
    int count_nonzero = 0;
    for (int y = 0; y < mViewer.height(); ++y)
    {
        gray16_view_t::x_iterator trIt = mViewer.row_begin(y);
        for (int x = 0; x < mViewer.width(); ++x, ++interest_ptr){
            at_c<0>(trIt[x]) = (*(interest_ptr) * std::pow(2,16)) ;
        }
    }
    boost::gil::png_write_view(output_filename, mViewer);

    if(!visualize)
        return 0;

    // ----------------------------------------------
    // -----Show keypoints in range image widget-----
    // ----------------------------------------------
  	pcl::visualization::RangeImageVisualizer range_image_widget("Planar range image");
	range_image_widget.showRangeImage(range_image_planar);
    for (size_t i=0; i<keypoint_indices.points.size (); ++i){
        range_image_widget.markPoint (keypoint_indices.points[i]%range_image_planar.width,
                                    resolution - 1 - keypoint_indices.points[i]/range_image_planar.width,
                                    pcl::visualization::Vector3ub (0,255,0));
    }

    // --------------------------------------------
    // -----Open 3D viewer and add point cloud-----
    // --------------------------------------------
    pcl::visualization::PCLVisualizer viewer ("3D Viewer");
    viewer.setBackgroundColor (1, 1, 1);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_planar_ptr, 0, 0, 0);
    viewer.addPointCloud ( range_image_planar_ptr, range_image_color_handler, "range image");
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
    viewer.initCameraParameters ();
    setViewerPose (viewer, range_image_planar.getTransformationToWorldSystem ());

    // -------------------------------------
    // -----Show keypoints in 3D viewer-----
    // -------------------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;
    keypoints.points.resize (keypoint_indices.points.size ());
    for (size_t i=0; i<keypoint_indices.points.size (); ++i)
        keypoints.points[i].getVector3fMap () = range_image_planar.points[keypoint_indices.points[i]].getVector3fMap ();
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (keypoints_ptr, 0, 255, 0);
    viewer.addPointCloud<pcl::PointXYZ> (keypoints_ptr, keypoints_color_handler, "keypoints");
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");

	while (!viewer.wasStopped())
	{
		range_image_widget.spinOnce();
		// Sleep 100ms to go easy on the CPU.
		pcl_sleep(0.1);
	}

}


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 **************************************************************************/

#ifndef MFMG_DEALII_UTILS_H
#define MFMG_DEALII_UTILS_H

#include <deal.II/lac/la_parallel_vector.h>
#include <deal.II/lac/trilinos_sparse_matrix.h>
#include <deal.II/lac/trilinos_vector.h>

#include <string>

namespace mfmg
{
dealii::LinearAlgebra::distributed::Vector<double>
extract_row(dealii::TrilinosWrappers::SparseMatrix const &matrix,
            dealii::types::global_dof_index global_j);

// matrix_transpose_matrix_multiply(C, B, A) performs the matrix-matrix
// multiplication with the transpose of B, i.e. C = A * B^T
//
// Note that it is different from deal.II's SparseMatrix::Tmmult(C, B) which
// performs C = A^T * B
template <typename Operator>
std::shared_ptr<dealii::TrilinosWrappers::SparseMatrix>
matrix_transpose_matrix_multiply(
    dealii::IndexSet const &row_index_set,
    dealii::IndexSet const &col_index_set, MPI_Comm const &comm,
    dealii::TrilinosWrappers::SparseMatrix const &B, Operator const &A)
{
  // C = A * B^T
  // C_ij = A_ik * B^T_kj = A_ik * B_jk

  auto C = std::make_shared<dealii::TrilinosWrappers::SparseMatrix>(
      row_index_set, col_index_set, comm);
  auto tmp = A.build_range_vector();
  std::vector<dealii::types::global_dof_index> i_indices;
  tmp->locally_owned_elements().fill_index_vector(i_indices);
  std::vector<dealii::types::global_dof_index> j_indices;
  B.locally_owned_range_indices().fill_index_vector(j_indices);

  int const global_n_rows = tmp->size();
  int const global_n_columns = B.m(); //< number of rows in B
  for (int j = 0; j < global_n_columns; ++j)
  {
    auto const src = extract_row(B, j);

    std::remove_const<decltype(src)>::type dst(tmp->locally_owned_elements(),
                                               tmp->get_mpi_communicator());
    A.apply(src, dst);

    // NOTE: getting an error that the index set is not compressed when calling
    // IndexSet::index_within_set() directly on dst.locally_owned_elements() so
    // ended up making a copy and calling IndexSet::compress() on it.  Not sure
    // it is the best solution but this will do for now.
    auto index_set = dst.locally_owned_elements();
    index_set.compress();
    for (int i = 0; i < global_n_rows; ++i)
    {
      auto const local_i = index_set.index_within_set(i);
      if (local_i != dealii::numbers::invalid_dof_index)
      {
        auto const value = dst[i];
        if (std::abs(value) > 1e-14) // is that an appropriate epsilon?
        {
          C->set(i, j, value);
        }
      }
    }
  }
  C->compress(dealii::VectorOperation::insert);

  return C;

  //--------------------------------------------------------------------------//
  //  THE CODE BELOW CAN BE USED TO GREATLY DECREASE THE MEMORY REQUIREMENT   //
  //  BUT THE CODE ONLY WORKS IN SERIAL                                       //
  //--------------------------------------------------------------------------//
  /*
  auto tmp = A.build_range_vector();

  int const global_n_rows = tmp->size();
  int const global_n_columns = B.m(); //< number of rows in B

  dealii::LinearAlgebra::distributed::Vector<double> dst(
      tmp->locally_owned_elements(), tmp->get_mpi_communicator());
  // NOTE: getting an error that the index set is not compressed when calling
  // IndexSet::index_within_set() directly on dst.locally_owned_elements() so
  // ended up making a copy and calling IndexSet::compress() on it.  Not sure
  // it is the best solution but this will do for now.
  auto index_set = dst.locally_owned_elements();
  index_set.compress();

  auto C = std::make_shared<dealii::TrilinosWrappers::SparseMatrix>(
      row_index_set, col_index_set, comm);

  Epetra_CrsMatrix c_mat(Copy, row_index_set.make_trilinos_map(),
                         col_index_set.make_trilinos_map(), 0 ,
                         true);
  {
    std::vector<std::vector<std::pair<int, double>>> this_is_stupid(
        index_set.size());
    int const eigenvectors_per_aggregate = 1;
    int const max_number_of_aggretes_within_one_hop_of_any_fine_dof =
        9; // 27 in 3D. This also assumes that aggregates have at least 2 cells
           // in each direction.
    std::for_each(this_is_stupid.begin(), this_is_stupid.end(), [](auto &v) {
      v.reserve(max_number_of_aggretes_within_one_hop_of_any_fine_dof *
                eigenvectors_per_aggregate);
    });

    int nnz = 0;
    for (int j = 0; j < global_n_columns; ++j)
    {
      auto const src = extract_row(B, j);
      A.apply(src, dst);

      for (int i = 0; i < global_n_rows; ++i)
      {
        auto const local_i = index_set.index_within_set(i);
        if (local_i != dealii::numbers::invalid_dof_index)
        {
          auto const value = dst[i];
          if (std::abs(value) > 1e-14) // is that an appropriate epsilon?
          {
            this_is_stupid[local_i].push_back(std::make_pair(j, value));
            nnz++;
          }
        }
      }
    }
    auto n = this_is_stupid.size();
    assert(n == index_set.size());

    Epetra_IntSerialDenseVector &c_rowptr = c_mat.ExpertExtractIndexOffset();
    Epetra_IntSerialDenseVector &c_colind = c_mat.ExpertExtractIndices();
    double *&c_values = c_mat.ExpertExtractValues();

    c_rowptr.Resize(n + 1);
    c_colind.Resize(nnz);
    delete[] c_values;
    c_values = new double[nnz];

    int *rowptr_aux = c_rowptr.Values();
    int *colind_aux = c_colind.Values();
    double *values_aux = c_values;

    for (unsigned int i = 0; i < index_set.size(); i++)
    {
      auto &row = this_is_stupid[i];
      rowptr_aux[i + 1] = rowptr_aux[i] + row.size();
      for (unsigned int j = 0; j < row.size(); j++)
      {
        colind_aux[rowptr_aux[i] + j] = row[j].first;
        values_aux[rowptr_aux[i] + j] = row[j].second;
      }
    }
  }

  // c_mat.ExpertStaticFillComplete(C->domain_partitioner(),
  // C->range_partitioner());
  // c_mat.ExpertStaticFillComplete(row_index_set.make_trilinos_map(),
  // row_index_set.make_trilinos_map());
  c_mat.ExpertStaticFillComplete(col_index_set.make_trilinos_map(),
                                 row_index_set.make_trilinos_map());

  C->reinit(c_mat);

  return C;
  */
}

void matrix_market_output_file(
    std::string const &filename,
    dealii::TrilinosWrappers::SparseMatrix const &matrix);

void matrix_market_output_file(std::string const &filename,
                               dealii::SparseMatrix<double> const &matrix);

void matrix_market_output_file(
    std::string const &filename,
    dealii::TrilinosWrappers::MPI::Vector const &vector);
} // namespace mfmg

#endif


#include <boost/graph/create_condensation_graph.hpp>


/* ----------------------------------------------------------------------- *//**
 *
 * @file LinearRegression_impl.hpp
 *
 *//* ----------------------------------------------------------------------- */

#ifndef MADLIB_MODULES_REGRESS_LINEAR_REGRESSION_IMPL_HPP
#define MADLIB_MODULES_REGRESS_LINEAR_REGRESSION_IMPL_HPP

#include <dbconnector/dbconnector.hpp>
#include <boost/math/distributions.hpp>
#include <modules/prob/student.hpp>
#include <modules/prob/boost.hpp>
#include <limits>

namespace madlib {

// Use Eigen
using namespace dbal::eigen_integration;

namespace modules {

namespace regress {

template <class Container>
inline
LinearRegressionAccumulator<Container>::LinearRegressionAccumulator(
    Init_type& inInitialization)
  : Base(inInitialization) {

    this->initialize();
}

/**
 * @brief Bind all elements of the state to the data in the stream
 *
 * The bind() is special in that even after running operator>>() on an element,
 * there is no guarantee yet that the element can indeed be accessed. It is
 * cruicial to first check this.
 *
 * Provided that this methods correctly lists all member variables, all other
 * methods can, however, rely on that fact that all variables are correctly
 * initialized and accessible.
 */
template <class Container>
inline
void
LinearRegressionAccumulator<Container>::bind(ByteStream_type& inStream) {
    inStream
        >> numRows >> widthOfX >> y_sum >> y_square_sum;
    uint16_t actualWidthOfX = widthOfX.isNull()
        ? static_cast<uint16_t>(0)
        : static_cast<uint16_t>(widthOfX);
    inStream
        >> X_transp_Y.rebind(actualWidthOfX)
        >> X_transp_X.rebind(actualWidthOfX, actualWidthOfX);
}

/**
 * @brief Update the accumulation state
 *
 * We update the number of rows \f$ n \f$, the partial
 * sums \f$ \sum_{i=1}^n y_i \f$ and \f$ \sum_{i=1}^n y_i^2 \f$, the matrix
 * \f$ X^T X \f$, and the vector \f$ X^T \boldsymbol y \f$.
 */
template <class Container>
inline
LinearRegressionAccumulator<Container>&
LinearRegressionAccumulator<Container>::operator<<(const tuple_type& inTuple) {
    const MappedColumnVector& x = std::get<0>(inTuple);
    const double& y = std::get<1>(inTuple);

    // The following checks were introduced with MADLIB-138. It still seems
    // useful to have clear error messages in case of infinite input values.
    if (!std::isfinite(y))
        throw std::domain_error("Dependent variables are not finite.");
    else if (!dbal::eigen_integration::isfinite(x))
        throw std::domain_error("Design matrix is not finite.");
    else if (x.size() > std::numeric_limits<uint16_t>::max())
        throw std::domain_error("Number of independent variables cannot be "
            "larger than 65535.");

    // Initialize in first iteration
    if (numRows == 0) {
        widthOfX = static_cast<uint16_t>(x.size());
        this->resize();
    } else if (widthOfX != static_cast<uint16_t>(x.size())) {
        throw std::runtime_error("Inconsistent numbers of independent "
            "variables.");
    }

    numRows++;
    y_sum += y;
    y_square_sum += y * y;
    X_transp_Y.noalias() += x * y;

    // X^T X is symmetric, so it is sufficient to only fill a triangular part
    // of the matrix
    triangularView<Lower>(X_transp_X) += x * trans(x);
    return *this;
}

/**
 * @brief Merge with another accumulation state
 */
template <class Container>
template <class OtherContainer>
inline
LinearRegressionAccumulator<Container>&
LinearRegressionAccumulator<Container>::operator<<(
    const LinearRegressionAccumulator<OtherContainer>& inOther) {

    // Initialize if necessary
    if (numRows == 0) {
        *this = inOther;
        return *this;
    } else if (inOther.numRows == 0)
        return *this;
    else if (widthOfX != inOther.widthOfX)
        throw std::runtime_error("Inconsistent numbers of independent "
            "variables.");

    numRows += inOther.numRows;
    y_sum += inOther.y_sum;
    y_square_sum += inOther.y_square_sum;
    X_transp_Y.noalias() += inOther.X_transp_Y;
    triangularView<Lower>(X_transp_X) += inOther.X_transp_X;
    return *this;
}

template <class Container>
template <class OtherContainer>
inline
LinearRegressionAccumulator<Container>&
LinearRegressionAccumulator<Container>::operator=(
    const LinearRegressionAccumulator<OtherContainer>& inOther) {

    this->copy(inOther);
    return *this;
}

template <class Container>
LinearRegression::LinearRegression(
    const LinearRegressionAccumulator<Container>& inState) {

    compute(inState);
}

/**
 * @brief Transform a linear-regression accumulation state into a result
 *
 * The result of the accumulation phase is \f$ X^T X \f$ and
 * \f$ X^T \boldsymbol y \f$. We first compute the pseudo-inverse, then the
 * regression coefficients, the model statistics, etc.
 *
 * @sa For the mathematical description, see \ref grp_linreg.
 */
template <class Container>
inline
LinearRegression&
LinearRegression::compute(
    const LinearRegressionAccumulator<Container>& inState) {

    Allocator& allocator = defaultAllocator();

    // The following checks were introduced with MADLIB-138. It still seems
    // useful to have clear error messages in case of infinite input values.
    if (!dbal::eigen_integration::isfinite(inState.X_transp_X) ||
            !dbal::eigen_integration::isfinite(inState.X_transp_Y))
        throw std::domain_error("Design matrix is not finite.");

    SymmetricPositiveDefiniteEigenDecomposition<Matrix> decomposition(
        inState.X_transp_X, EigenvaluesOnly, ComputePseudoInverse);

    // Precompute (X^T * X)^+
    Matrix inverse_of_X_transp_X = decomposition.pseudoInverse();
    conditionNo = decomposition.conditionNo();

    // Vector of coefficients: For efficiency reasons, we want to return this
    // by reference, so we need to bind to db memory
    coef.rebind(allocator.allocateArray<double>(inState.widthOfX));
    coef.noalias() = inverse_of_X_transp_X * inState.X_transp_Y;

    // explained sum of squares (regression sum of squares)
    double ess = dot(inState.X_transp_Y, coef)
        - (inState.y_sum * inState.y_sum / static_cast<double>(inState.numRows));

    // total sum of squares
    double tss = inState.y_square_sum
        - (inState.y_sum * inState.y_sum / static_cast<double>(inState.numRows));

    // With infinite precision, the following checks are pointless. But due to
    // floating-point arithmetic, this need not hold at this point.
    // Without a formal proof convincing us of the contrary, we should
    // anticipate that numerical peculiarities might occur.
    if (tss < 0)
        tss = 0;
    if (ess < 0)
        ess = 0;
    // Since we know tss with greater accuracy than ess, we do the following
    // sanity adjustment to ess:
    if (ess > tss)
        ess = tss;

    // coefficient of determination
    // If tss == 0, then the regression perfectly fits the data, so the
    // coefficient of determination is 1.
    r2 = (tss == 0 ? 1 : ess / tss);

    // In the case of linear regression:
    // residual sum of squares (rss) = total sum of squares (tss) - explained
    // sum of squares (ess)
    // Proof: http://en.wikipedia.org/wiki/Sum_of_squares
    double rss = tss - ess;

    // Variance is also called the mean square error
	double variance = rss / static_cast<double>(inState.numRows - inState.widthOfX);

    // Vector of standard errors and t-statistics: For efficiency reasons, we
    // want to return these by reference, so we need to bind to db memory
    stdErr.rebind(allocator.allocateArray<double>(inState.widthOfX));
    tStats.rebind(allocator.allocateArray<double>(inState.widthOfX));
    for (int i = 0; i < inState.widthOfX; i++) {
        // In an abundance of caution, we see a tiny possibility that numerical
        // instabilities in the pinv operation can lead to negative values on
        // the main diagonal of even a SPD matrix
        if (inverse_of_X_transp_X(i,i) < 0) {
            stdErr(i) = 0;
        } else {
            stdErr(i) = std::sqrt( variance * inverse_of_X_transp_X(i,i) );
        }

        if (coef(i) == 0 && stdErr(i) == 0) {
            // In this special case, 0/0 should be interpreted as 0:
            // We know that 0 is the exact value for the coefficient, so
            // the t-value should be 0 (corresponding to a p-value of 1)
            tStats(i) = 0;
        } else {
            // If stdErr(i) == 0 then abs(tStats(i)) will be infinity, which
            // is what we need.
            tStats(i) = coef(i) / stdErr(i);
        }
    }

    // Vector of p-values: For efficiency reasons, we want to return this
    // by reference, so we need to bind to db memory
    pValues.rebind(allocator.allocateArray<double>(inState.widthOfX));
    if (inState.numRows > inState.widthOfX)
        for (int i = 0; i < inState.widthOfX; i++)
            pValues(i) = 2. * prob::cdf(
                boost::math::complement(
                    prob::students_t(
                        static_cast<double>(inState.numRows - inState.widthOfX)
                    ),
                    std::fabs(tStats(i))
                ));
    return *this;
}

/*
	Robust Linear Regression: Huber-White Sandwich estimator
*/

template <class Container>
inline
RobustLinearRegressionAccumulator<Container>::RobustLinearRegressionAccumulator(
    Init_type& inInitialization) : Base(inInitialization) {
    this->initialize();
}

/**
 * @brief Bind all elements of the state to the data in the stream
 *
 * The bind() is special in that even after running operator>>() on an element,
 * there is no guarantee yet that the element can indeed be accessed. It is
 * cruicial to first check this.
 *
 * Provided that this methods correctly lists all member variables, all other
 * methods can, however, rely on that fact that all variables are correctly
 * initialized and accessible.
 */
template <class Container>
inline
void
RobustLinearRegressionAccumulator<Container>::bind(ByteStream_type& inStream) {
    inStream >> numRows >> widthOfX;
    uint16_t actualWidthOfX = widthOfX.isNull() ? 0 : static_cast<uint16_t>(widthOfX);
    inStream	>> ols_coef.rebind(actualWidthOfX)
							>> X_transp_X.rebind(actualWidthOfX, actualWidthOfX)
							>> X_transp_r2_X.rebind(actualWidthOfX, actualWidthOfX);
}

/**
 * @brief Update the accumulation state
 *
 * We update the number of rows \f$ n \f$, the matrix \f$ X^T X \f$,
 * and the matrix \f$ X^T diag(r_1^2, r_2^2 ... r_n^2 X \f$
 */
template <class Container>
inline
RobustLinearRegressionAccumulator<Container>&
RobustLinearRegressionAccumulator<Container>::operator<<(const tuple_type& inTuple) {

		// Inputs
    const MappedColumnVector& x = std::get<0>(inTuple);
    const double& y = std::get<1>(inTuple);
    const MappedColumnVector& coef = std::get<2>(inTuple);

    if (!std::isfinite(y))
        throw std::domain_error("Dependent variables are not finite.");
    else if (x.size() > std::numeric_limits<uint16_t>::max())
        throw std::domain_error("Number of independent variables cannot be "
            "larger than 65535.");

    // Initialize in first iteration
    if (numRows == 0) {
        widthOfX = static_cast<uint16_t>(x.size());
        this->resize();
        ols_coef = coef;
    }

    // dimension check
    if (widthOfX != static_cast<uint16_t>(x.size())) {
        throw std::runtime_error("Inconsistent numbers of independent "
            "variables.");
    }

    numRows++;
    double r = y - trans(ols_coef)*x;

    // The following matrices are symmetric, so it is sufficient to
		// only fill a triangular part
    triangularView<Lower>(X_transp_X) += x * trans(x);
    triangularView<Lower>(X_transp_r2_X) += r * r * x * trans(x);

    return *this;
}

/**
 * @brief Merge with another accumulation state
 */
template <class Container>
template <class OtherContainer>
inline
RobustLinearRegressionAccumulator<Container>&
RobustLinearRegressionAccumulator<Container>::operator<<(
    const RobustLinearRegressionAccumulator<OtherContainer>& inOther) {

    numRows += inOther.numRows;
    triangularView<Lower>(X_transp_X) += inOther.X_transp_X;
    triangularView<Lower>(X_transp_r2_X) += inOther.X_transp_r2_X;
    return *this;
}

template <class Container>
template <class OtherContainer>
inline
RobustLinearRegressionAccumulator<Container>&
RobustLinearRegressionAccumulator<Container>::operator=(
    const RobustLinearRegressionAccumulator<OtherContainer>& inOther) {

    this->copy(inOther);
    return *this;
}

template <class Container>
RobustLinearRegression::RobustLinearRegression(
    const RobustLinearRegressionAccumulator<Container>& inState) {

    compute(inState);
}

/**
 * @brief Transform a robust linear-regression accumulation state into a result
 *
 * The result of the accumulation phase is \f$ X^T X \f$ and
 * \f$ X^T U X \f$. We first compute the pseudo-inverse, then the
 * the robust model statistics.
 *
 * @sa For the mathematical description, see \ref grp_linreg.
 */
template <class Container>
inline
RobustLinearRegression&
RobustLinearRegression::compute(
    const RobustLinearRegressionAccumulator<Container>& inState) {

    Allocator& allocator = defaultAllocator();

    // The following checks were introduced with MADLIB-138. It still seems
    // useful to have clear error messages in case of infinite input values.
    if (!dbal::eigen_integration::isfinite(inState.X_transp_X) ||
            !dbal::eigen_integration::isfinite(inState.X_transp_r2_X))
        throw std::domain_error("Design matrix is not finite.");

    SymmetricPositiveDefiniteEigenDecomposition<Matrix> decomposition(
        inState.X_transp_X, EigenvaluesOnly, ComputePseudoInverse);

    // Precompute (X^T * X)^+
    Matrix inverse_of_X_transp_X = decomposition.pseudoInverse();

		// Calculate the robust variance covariance matrix as:
		// (X^T X)^-1  X^T diag(r1^2,r2^2....rn^2)X  (X^T X)^-1
		// Where r_1, r_2 ... r_n are the residuals
		// Note: X_transp_r2_X calcualtes X^T diag(r1^2,r2^2....rn^2)X

		Matrix robust_var_cov = inState.X_transp_r2_X.triangularView<Eigen::StrictlyLower>();
		robust_var_cov = robust_var_cov + trans(inState.X_transp_r2_X);
		robust_var_cov = inverse_of_X_transp_X * robust_var_cov * inverse_of_X_transp_X;

    // Vector of standard errors and t-statistics: For efficiency reasons, we
    // want to return these by reference, so we need to bind to db memory
    coef.rebind(allocator.allocateArray<double>(inState.widthOfX));
    stdErr.rebind(allocator.allocateArray<double>(inState.widthOfX));
    tStats.rebind(allocator.allocateArray<double>(inState.widthOfX));
    for (int i = 0; i < inState.widthOfX; i++) {
        // In an abundance of caution, we see a tiny possibility that numerical
        // instabilities in the pinv operation can lead to negative values on
        // the main diagonal of even a SPD matrix
        coef(i) = inState.ols_coef(i);

        if (inverse_of_X_transp_X(i,i) < 0) {
            stdErr(i) = 0;
        } else {
            stdErr(i) = std::sqrt(robust_var_cov(i,i));
        }

        if (inState.ols_coef(i) == 0 && stdErr(i) == 0) {
            // In this special case, 0/0 should be interpreted as 0:
            // We know that 0 is the exact value for the coefficient, so
            // the t-value should be 0 (corresponding to a p-value of 1)
            tStats(i) = 0;
        } else {
            // If stdErr(i) == 0 then abs(tStats(i)) will be infinity, which
            // is what we need.
            tStats(i) = inState.ols_coef(i) / stdErr(i);
        }
    }

    // Vector of p-values: For efficiency reasons, we want to return this
    // by reference, so we need to bind to db memory
    pValues.rebind(allocator.allocateArray<double>(inState.widthOfX));
    if (inState.numRows > inState.widthOfX){
        for (int i = 0; i < inState.widthOfX; i++){
            pValues(i) = 2. * prob::cdf(
                boost::math::complement(
                    prob::students_t(
                        static_cast<double>(inState.numRows - inState.widthOfX)
                    ),
                    std::fabs(tStats(i))
                ));
        }
    }
    return *this;
}

/*
  Regression for the tests for heteroskedasticity
*/

template <class Container>
inline
HeteroLinearRegressionAccumulator<Container>::HeteroLinearRegressionAccumulator(
    Init_type& inInitialization)
  : Base(inInitialization) {

    this->initialize();
}

/**
 * @brief Bind all elements of the state to the data in the stream
 *
 * The bind() is special in that even after running operator>>() on an element,
 * there is no guarantee yet that the element can indeed be accessed. It is
 * cruicial to first check this.
 *
 * Provided that this methods correctly lists all member variables, all other
 * methods can, however, rely on that fact that all variables are correctly
 * initialized and accessible.
 */
template <class Container>
inline
void
HeteroLinearRegressionAccumulator<Container>::bind(ByteStream_type& inStream) {
    inStream
        >> numRows >> widthOfX >> a_sum >> a_square_sum;
    uint16_t actualWidthOfX = widthOfX.isNull()
        ? 0
        : static_cast<uint16_t>(widthOfX);
    inStream
        >> X_transp_A.rebind(actualWidthOfX)
        >> X_transp_X.rebind(actualWidthOfX, actualWidthOfX);
}

/**
 * @brief Update the accumulation state
 *
 * We update the number of rows \f$ n \f$, the partial
 * sums \f$ \sum_{i=1}^n y_i \f$ and \f$ \sum_{i=1}^n y_i^2 \f$, the matrix
 * \f$ X^T X \f$, and the vector \f$ X^T \boldsymbol y \f$.
 */
template <class Container>
inline
HeteroLinearRegressionAccumulator<Container>&
HeteroLinearRegressionAccumulator<Container>::operator<<(const hetero_tuple_type& inTuple) {
    const MappedColumnVector& x = std::get<0>(inTuple);
    const double& y = std::get<1>(inTuple);
    const MappedColumnVector& coef = std::get<2>(inTuple);

    if (!std::isfinite(y))
        throw std::domain_error("Dependent variables are not finite.");
    else if (!dbal::eigen_integration::isfinite(x))
        throw std::domain_error("Design matrix is not finite.");
    else if (x.size() > std::numeric_limits<uint16_t>::max())
        throw std::domain_error("Number of independent variables cannot be "
            "larger than 65535.");

    // Initialize in first iteration
    if (numRows == 0) {
        widthOfX = static_cast<uint16_t>(x.size());
        this->resize();
    }

    // dimension check
    if (widthOfX != static_cast<uint16_t>(x.size())) {
        throw std::runtime_error("Inconsistent numbers of independent "
            "variables.");
    }

    double a = y - trans(coef)*x;
    a = a*a;

    numRows++;
    a_sum += a;
    a_square_sum += a*a;
    X_transp_A.noalias() += x * a;

    // X^T X is symmetric, so it is sufficient to only fill a triangular part
    // of the matrix
    triangularView<Lower>(X_transp_X) += x * trans(x);
    return *this;
}

/**
 * @brief Merge with another accumulation state
 */
template <class Container>
template <class OtherContainer>
inline
HeteroLinearRegressionAccumulator<Container>&
HeteroLinearRegressionAccumulator<Container>::operator<<(
    const HeteroLinearRegressionAccumulator<OtherContainer>& inOther) {

    numRows += inOther.numRows;
    a_sum += inOther.a_sum;
    a_square_sum += inOther.a_square_sum;
    X_transp_A.noalias() += inOther.X_transp_A;
    triangularView<Lower>(X_transp_X) += inOther.X_transp_X;
    return *this;
}

template <class Container>
template <class OtherContainer>
inline
HeteroLinearRegressionAccumulator<Container>&
HeteroLinearRegressionAccumulator<Container>::operator=(
    const HeteroLinearRegressionAccumulator<OtherContainer>& inOther) {

    this->copy(inOther);
    return *this;
}

template <class Container>
HeteroLinearRegression::HeteroLinearRegression(
    const HeteroLinearRegressionAccumulator<Container>& inState) {

    compute(inState);
}

/**
 * @brief Transform a linear-regression accumulation state into a result
 *
 * The result of the accumulation phase is \f$ X^T X \f$ and
 * \f$ X^T \boldsymbol y \f$. We first compute the pseudo-inverse, then the
 * regression coefficients, the model statistics, etc.
 *
 * @sa For the mathematical description, see \ref grp_linreg.
 */
template <class Container>
inline
HeteroLinearRegression&
HeteroLinearRegression::compute(
    const HeteroLinearRegressionAccumulator<Container>& inState) {

    // The following checks were introduced with MADLIB-138. It still seems
    // useful to have clear error messages in case of infinite input values.
    if (!dbal::eigen_integration::isfinite(inState.X_transp_X) ||
            !dbal::eigen_integration::isfinite(inState.X_transp_A))
        throw std::domain_error("Design matrix is not finite.");

    SymmetricPositiveDefiniteEigenDecomposition<Matrix> decomposition(
        inState.X_transp_X, EigenvaluesOnly, ComputePseudoInverse);

    // Precompute (X^T * X)^+
    Matrix inverse_of_X_transp_X = decomposition.pseudoInverse();

    ColumnVector coef;
    coef = inverse_of_X_transp_X * inState.X_transp_A;

    // explained sum of squares (regression sum of squares)
    double ess = dot(inState.X_transp_A, coef)
        - (inState.a_sum * inState.a_sum / static_cast<double>(inState.numRows));

    // total sum of squares
    double tss = inState.a_square_sum
        - (inState.a_sum * inState.a_sum / static_cast<double>(inState.numRows));

    // With infinite precision, the following checks are pointless. But due to
    // floating-point arithmetic, this need not hold at this point.
    // Without a formal proof convincing us of the contrary, we should
    // anticipate that numerical peculiarities might occur.
    if (tss < 0) tss = 0;
    if (ess < 0) ess = 0;
    // Since we know tss with greater accuracy than ess, we do the following
    // sanity adjustment to ess:
    if (ess > tss) ess = tss;

    // Test statistic: numRows*Coefficient of determination
    test_statistic = inState.numRows*(tss == 0 ? 1 : ess / tss);
    pValue = prob::cdf(complement(prob::chi_squared(
                                      static_cast<double>(inState.widthOfX-1)), test_statistic));

    return *this;
}

} // namespace regress

} // namespace modules

} // namespace madlib

#endif // defined(MADLIB_MODULES_REGRESS_LINEAR_REGRESSION_IMPL_HPP)


//
// Copyright (c) 2009 Rutger ter Borg
//
// 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_NUMERIC_BINDINGS_BLAS_LEVEL1_HPP
#define BOOST_NUMERIC_BINDINGS_BLAS_LEVEL1_HPP

#include <boost/numeric/bindings/blas/level1/asum.hpp>
#include <boost/numeric/bindings/blas/level1/axpy.hpp>
#include <boost/numeric/bindings/blas/level1/copy.hpp>
#include <boost/numeric/bindings/blas/level1/dotc.hpp>
#include <boost/numeric/bindings/blas/level1/dot.hpp>
#include <boost/numeric/bindings/blas/level1/dotu.hpp>
#include <boost/numeric/bindings/blas/level1/doth.hpp>
#include <boost/numeric/bindings/blas/level1/iamax.hpp>
#include <boost/numeric/bindings/blas/level1/nrm2.hpp>
#include <boost/numeric/bindings/blas/level1/prec_dot.hpp>
#include <boost/numeric/bindings/blas/level1/rotg.hpp>
#include <boost/numeric/bindings/blas/level1/rot.hpp>
#include <boost/numeric/bindings/blas/level1/rotmg.hpp>
#include <boost/numeric/bindings/blas/level1/rotm.hpp>
#include <boost/numeric/bindings/blas/level1/scal.hpp>
#include <boost/numeric/bindings/blas/level1/set.hpp>
#include <boost/numeric/bindings/blas/level1/swap.hpp>

#endif


///////////////////////////////////////////////////////////////////////////////
// distribution::survival::failure_time::meta::random_generator.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_FAILURE_TIME_META_RANDOM_GENERATOR_N_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_FAILURE_TIME_META_RANDOM_GENERATOR_N_HPP_ER_2009
#include <boost/utility/enable_if.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/statistics/detail/distribution/survival/failure_time/meta/random_distribution.hpp>
#include <boost/statistics/detail/distribution/survival/failure_time/meta/detail/traits.hpp>

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

	template<typename M,typename U>
    struct random_generator{

        typedef typename failure_time::random_distribution<M>::type r_ft_;
        typedef boost::variate_generator<U&,r_ft_> type;

        template<typename X>
        static type call(const X& x,const M& m, U& urng)
        {
            return type(
                urng,
                failure_time::make_random_distribution( x, m )
            );
        }
    };

	template<typename X,typename M,typename U>
	typename random_generator<M,U>::type
    make_random_generator(const X& x,const M& model,U& urng)
    {
    	typedef random_generator<M,U> meta_;
		return meta_::call(x,model,urng);
    }

	template<typename Map,typename U>
	typename random_generator<
        typename detail::traits<Map>::model::type,
        U
    >::type
    make_random_generator(const Map& map,U& urng)
    {
    	typedef typename detail::traits<Map>::model::type model_;
    	typedef random_generator<model_,U> meta_;
		return meta_::call(map,urng);
    }

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

#endif


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    Generate the maximum exposant of floating point numbers minus one

    @par Semantic:

    @code
    as_integer<T> r = Maxexponentm1<T>();
    @endcode

    @code
    if T is double
      r =  1022;
    else if T is float
      r =  126;
    @endcode

    @return The Maxexponentm1 constant for the proper type
  */

namespace boost { namespace simd
{
  namespace tag
  {
    struct maxexponentm1_ : boost::dispatch::constant_value_<maxexponentm1_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,maxexponentm1_,boost::dispatch::constant_value_<maxexponentm1_>);
      struct value_map
      {
        template<typename X>
        static std::integral_constant<X,0> value(boost::dispatch::integer_<X> const&);

        template<typename X>
        static std::integral_constant<std::int32_t,126> value(boost::dispatch::single_<X> const&);

        template<typename X>
        static std::integral_constant<std::int64_t,1022> value(boost::dispatch::double_<X> const&);
      };
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,maxexponentm1_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::maxexponentm1_,maxexponentm1);
  }

  template<typename T> BOOST_FORCEINLINE auto Maxexponentm1()
  BOOST_NOEXCEPT_DECLTYPE(detail::maxexponentm1( boost::dispatch::as_<T>{}))
  {
    return detail::maxexponentm1( boost::dispatch::as_<T>{} );
  }
} }

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


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

#include <nt2/sdk/meta/strip.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/include/functions/simd/fma.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/constants/real_splat.hpp>
#include <boost/preprocessor/punctuation/remove_parens.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>
#include <boost/preprocessor/repetition/enum_params.hpp>
#include <boost/preprocessor/tuple/elem.hpp>
#include <boost/preprocessor/tuple/reverse.hpp>

namespace nt2
{
  //////////////////////////////////////////////////////////////////////////////
  // static unrollign of horner scheme
  // Coefficients are given in decreasing powers and hexadecimal form
  // horner< NT2_HORNER_COEFF(float,5,(a0, a1, a2, a3, a4))>(x)
  // means a0*x^4 + a1*x^3 + a2*x^2 + a3*x + a4
  // computed as a4 + x*(a3 + x*(a2 + x*(a1 + x*a0))))
  // aka fma(x, fma(x, fma(x, fma(x, a0, a1), a2), a3), a4)
  //////////////////////////////////////////////////////////////////////////////

  namespace details
  {
    template<class V, V Coeff, class Next>
    struct horner_unroll
    {
      template<class T>
      static BOOST_FORCEINLINE T call(T const& x)
      {
        return fma(x, Next::call(x), Const<T, Coeff>());
      }
    };

    template<class V, V Coeff>
    struct horner_unroll<V, Coeff, void>
    {
      template<class T>
      static BOOST_FORCEINLINE T call(T const&)
      {
        return Const<T, Coeff>();
      }
    };
  }

  //////////////////////////////////////////////////////////////////////////////
  /// @brief Static Horner scheme
  //////////////////////////////////////////////////////////////////////////////
  template<class Coeff, class Type>
  static BOOST_FORCEINLINE Type horner( Type const& x )
  {
    return Coeff::call(x);
  }
}

#define NT2_COEFF_GEN(z,n,text)                                               \
nt2::details::                                                                \
horner_unroll< BOOST_PP_REMOVE_PARENS(BOOST_PP_TUPLE_ELEM(3,0,text))         \
             , BOOST_PP_TUPLE_ELEM( BOOST_PP_TUPLE_ELEM(3,1,text)             \
                                  , n                                         \
                                  , BOOST_PP_TUPLE_ELEM(3,2,text)             \
                                  )                                           \
             ,                                                                \
/**/

#define NT2_COEFF_GEN2(z,n,text) >

#define NT2_HORNER_COEFF(Type, Size, Seq)                                     \
BOOST_PP_REPEAT(Size, NT2_COEFF_GEN, ( (nt2::meta::as_integer<Type, unsigned>::type) \
                                     , Size                                   \
                                     , BOOST_PP_TUPLE_REVERSE(Size, Seq)      \
                                     )                                        \
               )                                                              \
void                                                                          \
BOOST_PP_REPEAT(Size, NT2_COEFF_GEN2, ~)                                      \
/**/

#define NT2_HORNER_COEFF_T(Type, Size, Seq)                                   \
BOOST_PP_REPEAT(Size, NT2_COEFF_GEN, ( (typename nt2::meta::as_integer<Type, unsigned>::type) \
                                     , Size                                   \
                                     , BOOST_PP_TUPLE_REVERSE(Size, Seq)      \
                                     )                                        \
               )                                                              \
void                                                                          \
BOOST_PP_REPEAT(Size, NT2_COEFF_GEN2, ~)                                      \
/**/

////////////////////////////////////////////////////////////////////////////////
// Computing a rationnal fraction P/Q at value x
// with Horner coefs of type "type"
// where P and Q lists of coefficients have respectively n and m elements
// Coefficients are given in decreasing powers and hexadecimal form as in Horner
////////////////////////////////////////////////////////////////////////////////

#define NT2_HORNER_RAT(type, n, m, x, P, Q)      \
  horner < NT2_HORNER_COEFF_T(type, n, P) > (x)/ \
  horner < NT2_HORNER_COEFF_T(type, m, Q) > (x)  \
/**/
#endif


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

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

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

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/function/is_nan.hpp>
#endif
#include <boost/simd/arch/common/detail/generic/pow_kernel.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/log2_em1.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/powlargelim.hpp>
#include <boost/simd/constant/powlowlim.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/fms.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/pow2.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( pow_abs_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A1> >
                          )
  {
    inline A0 operator() ( A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
    {
      const A0 Oneo_16 = Ratio<A0, 1, 16>();
      using i_t = bd::as_integer_t<A0>;
      const i_t Sixteen = Ratio<i_t, 16>();
      A0 x =  bs::abs(a0);
      if (x == One<A0>()) return x;
      if (is_eqz(x)) return is_eqz(a1) ? One<A0>() : is_ltz(a1) ? Inf<A0>() : Zero<A0>();
    #ifndef BOOST_SIMD_NO_INFINITIES
      if(x == a1 && a1 == Inf<A0>())  return Inf<A0>();
      if(x == Inf<A0>() && a1 == Minf<A0>()) return Zero<A0>();
      if(a1 == Inf<A0>()) return (x < One<A0>()) ? Zero<A0>() : Inf<A0>();
      if(a1 == Minf<A0>()) return (x >  One<A0>()) ? Zero<A0>() : Inf<A0>();
      if(x == Inf<A0>()) return (a1 < Zero<A0>()) ? Zero<A0>() : ((a1 == Zero<A0>()) ? One<A0>() : Inf<A0>());
    #endif
    #ifndef BOOST_SIMD_NO_INVALIDS
      if(is_nan(a0)) return is_eqz(a1) ? One<A0>() : a0;
      if(is_nan(a1)) return Nan<A0>();
    #endif
      i_t e;
      x = frexp(x, e);
      i_t i  = detail::pow_kernel<A0>::select(x);
      A0 z = sqr(x);
      A0 w = detail::pow_kernel<A0>::pow1(x, z);
      w = fma(Mhalf<A0>(), z, w);
      w = fma(Log2_em1<A0>(), w, w);
      z = fma(Log2_em1<A0>(), x, w);
      z += x;
      w =  fma(A0(-i), Oneo_16, A0(e));
      A0 ya = reduc(a1);
      A0 yb = a1 - ya;
      A0 W = fma(z, a1, w*yb);
      A0 Wa = reduc(W);
      A0 Wb = W - Wa;
      W = fma(w, ya, Wa);
      Wa = reduc(W);
      A0 u = W - Wa;
      W = Wb + u;
      Wb = reduc(W);
      w = Ratio<A0, 16>()*(Wa + Wb);

      // Test the power of 2 for overflow
      if( w > Powlargelim<A0>() ) return Inf<A0>();
      if( w < Powlowlim<A0>()   ) return Zero<A0>();
      e = w;
      Wb = W - Wb;  //
      if( Wb > 0.0f ) //
      {
        ++e;
        Wb -= Oneo_16; //
      }
      z = detail::pow_kernel<A0>::pow2(Wb)*Wb; //
      i = (e >> 4) + (e>=0);
      e = fms(i, Sixteen, e);
      w =  detail::pow_kernel<A0>::twomio16(e);
      z = fma(w, z, w);
      z = ldexp( z, i );
      return z;
    }
  private :
    static BOOST_FORCEINLINE A0 reduc(A0 x) BOOST_NOEXCEPT
    {
      // Find a multiple of 1/16 that is within 1/16 of x.
      return Ratio<A0, 1, 16>()*floor(Ratio<A0, 16>()*x);
    }
  };
} } }

#endif


#include <Eigen/Dense>
#include <iostream>
#include <limits>
#include <chrono>
#include <vector>
#include "optimization.h"

#include "align/bsfit_pipe.h"
#include "io/ply/meshio.h"

namespace
{
using namespace telef::io;
using namespace telef::align;
using namespace alglib;

const std::vector<int> lmk_selection =
  {
   7, 8, 9,
   //51, 48, 54, 60, 64, 62, 57, 66,
   60, 62, 64, 66,
   37, 38, 40, 41,
   43, 44, 46, 47,
   17, 19, 21,
   22, 24, 26,
  };

boost::shared_ptr<PCANonRigidAlignmentSuite>
step_position(boost::shared_ptr<PCANonRigidAlignmentSuite> in)
{
  const auto landmark3d = in->fittingSuite->landmark3d;
  Eigen::VectorXf ref = in->pca_model->getReferenceVector();
  const auto lmkInds = in->pca_model->getLandmarks();
  const auto expRank = in->pca_model->getExpressionRank();
  Eigen::MatrixXf shapeMat = in->pca_model->getShapeBasisMatrix();
  Eigen::MatrixXf expMat = in->pca_model->getExpressionBasisMatrix();
  Eigen::MatrixXf expLmkMat2d(2*lmk_selection.size(), expMat.cols());
  for (size_t i=0; i<lmk_selection.size(); i++)
    {
      Eigen::Matrix3Xf pos =
        expMat.block(3*lmkInds[lmk_selection[i]],0,3,expMat.cols());
      Eigen::Matrix3Xf tpos =
        (in->transformation * pos.colwise().homogeneous()).colwise().hnormalized();
      expLmkMat2d.block(2*i,0,2,expMat.cols()) = tpos.block(0,0,2,expMat.cols());
    }

  Eigen::VectorXf shape = ref+shapeMat*in->shapeCoeff;
  Eigen::VectorXf b2d = Eigen::VectorXf::Zero(2*lmk_selection.size());
  Eigen::VectorXf lmk = Eigen::VectorXf::Zero(3*lmk_selection.size());
  Eigen::VectorXf lmkref = Eigen::VectorXf::Zero(3*lmk_selection.size());
  for(int i=0; i<lmk_selection.size(); i++)
    {
      Eigen::VectorXf m = shape.segment(3*lmkInds[lmk_selection[i]],3);
      Eigen::VectorXf tm = (in->transformation*m.homogeneous()).hnormalized();
      b2d(2*i+0) = landmark3d->points[lmk_selection[i]].x - tm(0);
      b2d(2*i+1) = landmark3d->points[lmk_selection[i]].y - tm(1);

      lmk(3*i+0) = landmark3d->points[lmk_selection[i]].x;
      lmk(3*i+1) = landmark3d->points[lmk_selection[i]].y;
      lmk(3*i+2) = landmark3d->points[lmk_selection[i]].z;

      lmkref(3*i+0) = tm(0);
      lmkref(3*i+1) = tm(1);
      lmkref(3*i+2) = tm(2);
    }
  ColorMesh mlmk, mref;
  mlmk.position = lmk;
  mref.position = lmkref;

  Eigen::MatrixXd dExpLmkMat2d = expLmkMat2d.cast<double>();
  Eigen::VectorXd db2d = b2d.cast<double>();
  Eigen::MatrixXd Q = dExpLmkMat2d.transpose()*dExpLmkMat2d;
  Eigen::VectorXd c = -dExpLmkMat2d.transpose()*db2d;
  Eigen::VectorXd x0 = Eigen::VectorXd::Zero(expRank);
  //Eigen::VectorXd bndl = -std::numeric_limits<double>::infinity()*Eigen::VectorXd::Ones(expRank);
  Eigen::VectorXd bndl = -0.05*Eigen::VectorXd::Ones(expRank);
  Eigen::VectorXd bndu = std::numeric_limits<double>::infinity()*Eigen::VectorXd::Ones(expRank);
  Eigen::VectorXd scale = Eigen::VectorXd::Ones(expRank);

  real_2d_array algQ;
  real_1d_array algc, algx0, algbndl, algbndu, algx, algscale;
  algQ.setcontent(Q.rows(), Q.cols(), Q.data());
  algc.setcontent(c.size(), c.data());
  algx0.setcontent(x0.size(), x0.data());
  algbndl.setcontent(bndl.size(), bndl.data());
  algbndu.setcontent(bndu.size(), bndu.data());
  algscale.setcontent(scale.size(), scale.data());

  minqpstate state;
  minqpcreate(expRank, state);
  minqpsetquadraticterm(state, algQ);
  minqpsetlinearterm(state, algc);
  minqpsetstartingpoint(state, algx0);
  minqpsetbc(state, algbndl, algbndu);
  minqpsetscale(state, algscale);
  minqpreport rep;
  minqpsetalgoquickqp(state, 0.0, 0.0, 0.0, 0, false);
  minqpoptimize(state);
  minqpresults(state, algx, rep);
  /*
  std::cout << int(rep.terminationtype) << " " <<
    rep.outeriterationscount <<
    " " << rep.inneriterationscount << std::endl;

  std::cout << algx.tostring(expRank) << std::endl;
  */

  Eigen::VectorXf exCoeff =
    Eigen::Map<Eigen::VectorXd>(algx.getcontent(), algx.length()).cast<float>();

  /*
  for(int i=0; i<expMat.cols(); i++)
    {
      Eigen::VectorXf res = shape+expMat.col(i);
      ColorMesh mres;
      mres.position = res;
      mres.triangles = in->pca_model->getReferenceMesh().triangles;
      telef::io::ply::writePlyMesh("cusbs_"+std::to_string(i)+".ply", mres);
    }
  */

  in->expressionCoeff = exCoeff;

  return in;
}
}

namespace telef::align
{
boost::shared_ptr<PCANonRigidAlignmentSuite>
BsFitPipe::_processData(boost::shared_ptr<PCANonRigidAlignmentSuite> in)
{
  auto res = in;
  std::chrono::system_clock::time_point start = std::chrono::system_clock::now();
  res = step_position(res);
  std::chrono::duration<double> dur = std::chrono::system_clock::now()-start;
  std::cout << "Time (BsFit): " <<
    std::chrono::duration_cast<std::chrono::milliseconds>(dur).count() << "ms" << std::endl;
  return res;
}
}


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

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/mzero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( rec_real,  BOOST_SIMD_REAL_TYPES)
{
  using nt2::rec;
  using nt2::tag::rec_;
  typedef typename std::complex<T> cT;
  typedef typename boost::dispatch::meta::call<rec_(cT)>::type r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, cT);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Inf<T>(), nt2::Inf<T>())), cT(nt2::Zero<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Minf<T>(), nt2::Minf<T>())), cT(nt2::Zero<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Inf<T>(), nt2::Zero<T>())), cT(nt2::Zero<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Zero<T>(), nt2::Inf<T>())), cT(nt2::Zero<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Minf<T>(), nt2::Zero<T>())), cT(nt2::Zero<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Zero<T>(), nt2::Minf<T>())), cT(nt2::Zero<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Zero<T>(), nt2::Zero<T>())), cT(nt2::Inf<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Mzero<T>(), nt2::Zero<T>())), cT(nt2::Minf<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Nan<T>(), nt2::Nan<T>())), cT(nt2::Nan<T>(), nt2::Nan<T>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Nan<T>(), nt2::One<T>())), cT(nt2::Nan<T>(), nt2::Nan<T>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::One<T>(), nt2::Nan<T>())), cT(nt2::Nan<T>(), nt2::Nan<T>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Nan<T>(), nt2::Zero<T>())), cT(nt2::Nan<cT>()));
   NT2_TEST_EQUAL(nt2::rec(cT(nt2::Zero<T>(), nt2::Nan<T>())), cT(nt2::Zero<T>(), nt2::Nan<T>()));
#endif
   std::complex < T > a(1, 0);
   NT2_TEST_EQUAL(nt2::rec(a), nt2::One<T>());
   std::complex < T > b(1, 1);
   NT2_TEST_ULP_EQUAL(nt2::rec(b), cT(0.5, -0.5), 0);

}


#include <boost/variant/static_visitor.hpp>
#include <common/Geometry.h>
#include <common/geometry/Object2D.h>

namespace geometry {

Polygon createPolygon(Point const &center, Angle const &angle, PolygonalShape const &shape) {
	Polygon polygon;
	// TODO do some rotation on shape
	for(Vector const & vec : shape)
		polygon.push_back(center + rotate(vec, angle));

	return polygon;
}

Object2D createObject2D(Point const &point, Angle const & angle, Shape const & shape) {

	class CreateObject2D : public boost::static_visitor<Object2D>{
	private:
		Point const &point;
		Angle const ∠
	public:
		CreateObject2D(Point const &point, Angle const &angle) :
			point(point), angle(angle) {
		}

		Object2D operator()(CircleShape const & shape) {
			return {Circle{point, shape}};
		}

		Object2D operator()(PolygonalShape const &shape) {
			return createPolygon(point, angle, shape);
		}
	};

	CreateObject2D co2d{point, angle};
	return boost::apply_visitor(co2d, shape);
}

} // namespace geometry


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Suites
#include <boost/test/unit_test.hpp>
#include"basis.hpp"
#include"operators.hpp"
//#include"diag.hpp"

using namespace boost::unit_test;
using boost::unit_test_framework::test_suite;
using namespace Many_Body;
BOOST_AUTO_TEST_SUITE(basistesting)

BOOST_AUTO_TEST_SUITE(operatortesting)
BOOST_AUTO_TEST_CASE(operatoroperations)
{
  using namespace Eigen;
  using Mat=Operators::Mat;
   const size_t L=3;
   ElectronBasis<L> e(2);
   Mat NR=Operators::NumberOperator(e);
   std::cout << NR;
   Mat EK=Operators::EKinOperator(e);

      std::cout << EK;
   // VectorXd v(5);
  //  MatrixXcd mat = MatrixXcd::Random(5, 5);
  // MatrixXcd mat4= mat+mat.adjoint();
  //  MatrixXcd mat2=mat4;

  //  Many_Body::diagherm(mat4, v);
  //     MatrixXcd mat3=mat4.adjoint();
  //  std::cout<< mat3*mat2*mat4<< std::endl;
  //   std::cout<< v<< std::endl;
  //   numberoperator<ElectronBasis> n2;
  //      kineticoperator<ElectronBasis> cdagc1(e);
  // 	kineticoperator<ElectronBasis> cdagc2;

  // 	double t=3;
  // 	souble l=1;
  // 	hamiltonian<ElectronBasis> H1{t*n1+ l*cdagc1};
  // 		hamiltonian<ElectronBasis > H2{t*n2+ l*cdagc2};
  // 		H2(e);
  // 		cout<< H2.energies();
 // 				cout<< H1.energies();

}
}
// BOOST_AUTO_TEST_CASE(electrondimension)
// {
// }
BOOST_AUTO_TEST_SUITE_END()
// EOF


/**
	\brief command line tool for using the cpu hoNFFT toolbox

	Handles 2D data with the weights provided

	\param d: the kspace data points
	\param k: the kspace trajectory
	\param w: density compensation
	\param s: oversampling factor for gridding recon
	\param q: width of the kernl on oversampled grid
	\param n: non-oversampled size of image
	\param o: output file
*/

#include "hoNDArray.h"
#include "hoNDArray_fileio.h"
#include <iostream>
#include "parameterparser.h"
#include "vector_td.h"
#include "hoNFFT.h"
#include "hoCgSolver.h"
#include <boost/make_shared.hpp>
#include "hoLsqrSolver.h"

using namespace std;
using namespace Gadgetron;

int main(int argc, char** argv){
	ParameterParser parms;
	parms.add_parameter('d', COMMAND_LINE_STRING, 1, "Data (.cplx)", true, "d.cplx");
	parms.add_parameter('k', COMMAND_LINE_STRING, 1, "Trajectories (.real", true, "k.real");
	parms.add_parameter('w', COMMAND_LINE_STRING, 1, "Density compensation (.real)", true, "w.real");
	parms.add_parameter('s', COMMAND_LINE_FLOAT, 1, "Oversampling factor (float)", true, "1.5");
	parms.add_parameter('q', COMMAND_LINE_FLOAT, 1, "Kernel width (float)", true, "7");
	parms.add_parameter('n', COMMAND_LINE_INT, 1, "Image size (int)", true, "128");
	parms.add_parameter('o', COMMAND_LINE_STRING, 1, "Output file (.cplx)", true, "out.cplx");

	parms.parse_parameter_list(argc, argv);
	if(parms.all_required_parameters_set()){
		cout << "Running nfft with the following parameter:" << endl;
		parms.print_parameter_list();
	}else{
		cout << "Some required parameters are missing: " << endl;
		parms.print_parameter_list();
		parms.print_usage();
		return 1;
	}

	// Load data from disk
	boost::shared_ptr<hoNDArray<complext<float>>> data =
		read_nd_array<complext<float>>((char*) parms.get_parameter('d')->get_string_value());
	boost::shared_ptr<hoNDArray<float>> traj =
		read_nd_array<float>((char*) parms.get_parameter('k')->get_string_value());
	boost::shared_ptr<hoNDArray<float>> weights =
		read_nd_array<float>((char*) parms.get_parameter('w')->get_string_value());
	float osf = parms.get_parameter('s')->get_float_value();
	float kernelWidth = parms.get_parameter('q')->get_float_value();
	int n = parms.get_parameter('n')->get_int_value();

	// Print data info
	cout << "data n: " << data->get_number_of_elements() << endl;
	cout << "traj n: " << traj->get_number_of_elements() << endl;
	cout << "weights n: " << weights->get_number_of_elements() << endl;
	cout << "osf: " << osf << endl;
	cout << "Kernel width: " << kernelWidth << endl;
	cout << "Matrix size: " << n << endl;

	// Reformat data for hoNFFT
	hoNDArray<typename reald<float,2>::Type> k(weights->get_number_of_elements());
	for(size_t i = 0; i < traj->get_number_of_elements()/2; i++)
		k[i][0] = (*traj)[i];
	for(size_t i = traj->get_number_of_elements()/2; i < traj->get_number_of_elements(); i++)
		k[i-traj->get_number_of_elements()/2][1] = (*traj)[i];

	typename uint64d<2>::Type matrixSize; matrixSize[0] = n; matrixSize[1] = n;
	typename uint64d<2>::Type matrixSizeOs; matrixSizeOs[0] = n*osf; matrixSizeOs[1] = n*osf;

	hoNFFT_plan<float, 2> plan(matrixSize, osf, kernelWidth);
	hoNDArray<complext<float>> result(matrixSizeOs[0], matrixSizeOs[1]);
	plan.preprocess(k);
	plan.compute((*data), result, (*weights), hoNFFT_plan<float, 2>::NFFT_BACKWARDS_NC2C);

	auto output = boost::make_shared<hoNDArray<complext<float>>>(result);
	write_nd_array<complext<float>>(output.get(), (char*) parms.get_parameter('o')->get_string_value());
}


#include <boost/random/discrete_distribution.hpp>