#include <librealsense2/rs.hpp>
#include <algorithm>            // std::min, std::max
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <iostream>
#include <chrono>

#include <pcl/registration/icp.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>

#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread/thread.hpp>
#include <string>

typedef pcl::PointXYZRGB P_pcl;
typedef pcl::PointCloud<P_pcl> point_cloud;
typedef point_cloud::Ptr ptr_cloud;
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

void print4x4Matrix (const Eigen::Matrix4d & matrix)
{
    printf ("Rotation matrix :\n");
    printf ("    | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2));
    printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2));
    printf ("    | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2));
    printf ("Translation vector :\n");
    printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3));
}

inline void pass_filter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud)
{
    pcl::PassThrough<pcl::PointXYZRGB> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0,9.0);
    pass.filter(*cloud);
}

inline void planeDetect(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, double threshold) {
    pcl::SACSegmentation<pcl::PointXYZRGB> seg;
    seg.setOptimizeCoefficients(true);
    seg.setInputCloud(cloud);
    seg.setModelType(pcl::SACMODEL_PLANE);//モデル
    seg.setMethodType(pcl::SAC_RANSAC);//検出手法
    seg.setDistanceThreshold(threshold); //閾値 0.5とか
    seg.segment(*inliers, *coefficients);
}

inline void planeRemoval(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::PointIndices::Ptr inliers, bool negative) {
    pcl::ExtractIndices<pcl::PointXYZRGB> extract;
    extract.setInputCloud(cloud);
    extract.setIndices(inliers);
    extract.setNegative(negative);
    extract.filter(*cloud);
}

// Get RGB values based on normals - texcoords, normals value [u v]
std::tuple<uint8_t, uint8_t, uint8_t> get_texcolor(rs2::video_frame texture, rs2::texture_coordinate texcoords)
{
    const int w = texture.get_width(), h = texture.get_height();

    // convert normals [u v] to basic coords [x y]
    int x = std::min(std::max(int(texcoords.u*w + .5f), 0), w - 1);
    int y = std::min(std::max(int(texcoords.v*h + .5f), 0), h - 1);

    int idx = x * texture.get_bytes_per_pixel() + y * texture.get_stride_in_bytes();
    const auto texture_data = reinterpret_cast<const uint8_t*>(texture.get_data());
    return std::tuple<uint8_t, uint8_t, uint8_t>(texture_data[idx], texture_data[idx+1], texture_data[idx+2]);
}

auto points_to_pcl(const rs2::points& points, const rs2::video_frame& color){

    // OpenCV Mat for showing the rgb color image, just as part of processing
    cv::Mat colorr(cv::Size(640, 480), CV_8UC3, (void*)color.get_data(), cv::Mat::AUTO_STEP);
    //namedWindow("Display Image", WINDOW_AUTOSIZE );
    //imshow("Display Image", colorr);

    auto sp = points.get_profile().as<rs2::video_stream_profile>();
    ptr_cloud cloud(new point_cloud);

    // Config of PCL Cloud object
    cloud->width = static_cast<uint32_t>(sp.width());
    cloud->height = static_cast<uint32_t>(sp.height());
    cloud->is_dense = false;
    cloud->points.resize(points.size());

    auto tex_coords = points.get_texture_coordinates();
    auto vertices = points.get_vertices();

    // Iterating through all points and setting XYZRGB coordinates
    // and RGB values
    for (int i = 0; i < points.size(); ++i)
    {
        cloud->points[i].x = vertices[i].x;
        cloud->points[i].y = vertices[i].y;
        cloud->points[i].z = vertices[i].z;

        std::tuple<uint8_t, uint8_t, uint8_t> current_color;
        current_color = get_texcolor(color, tex_coords[i]);

        // Reversed order- 2-1-0 because of BGR model used in camera
        cloud->points[i].r = std::get<2>(current_color);
        cloud->points[i].g = std::get<1>(current_color);
        cloud->points[i].b = std::get<0>(current_color);

    }

   return cloud;
}

int main() {
    FILE *fp;
    int count= 0;
    const int markers_x         = 3;   // x方向に配置するマーカー数
    const int markers_y         = 2;   // y方向に配置するマーカー数
    const float marker_length     = 0.2; // マーカーのサイズ
    const float marker_separation = 0.07;  // マーカー間の間隔

    float degree = 180/3.141592;
    int wide = 1280;
    int height = 720;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr model (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB> Final;
    pcl::io::loadPLYFile("model.ply", *model);
    //aruco
    cv::Mat camera_matrix, dist_coeffs;
    std::ostringstream vector_to_marker;
    float actual_marker_length = 0.15;
    //cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
    cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME::DICT_4X4_50);
    cv::FileStorage fs("calibration_params.yml", cv::FileStorage::READ);
    std::vector<int> ids;
    std::vector<std::vector<cv::Point2f> > corners;
    fs["camera_matrix"] >> camera_matrix;
    fs["distortion_coefficients"] >> dist_coeffs;

    cv::Ptr<cv::aruco::GridBoard> grid_board = cv::aruco::GridBoard::create(markers_x, markers_y, marker_length, marker_separation,dictionary);
    cv::Ptr<cv::aruco::Board> board = grid_board.staticCast<cv::aruco::Board>();
    std::cout << "camera_matrix\n" << camera_matrix << std::endl;
    std::cout << "\ndist coeffs\n" << dist_coeffs << std::endl;

    rs2::pointcloud pc;
    // Declare RealSense pipeline, encapsulating the actual device and sensors
    rs2::pipeline pipe;
    // Create a configuration for configuring the pipeline with a non default profile
    rs2::config cfg;

    // Add desired streams to configuration
    cfg.enable_stream(RS2_STREAM_COLOR, wide, height, RS2_FORMAT_BGR8, 30);
    //cfg.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 60);
    cfg.enable_stream(RS2_STREAM_DEPTH, wide, height, RS2_FORMAT_Z16, 30);

    // Start streaming with default recommended configuration
    pipe.start(cfg);

    std::chrono::system_clock::time_point start, end;
    while(1){

        start = std::chrono::system_clock::now();
    auto frames = pipe.wait_for_frames();
    auto depth = frames.get_depth_frame();
    auto colored_frame = frames.get_color_frame();

    // Order here is crucial!
    // map_to() color frame has to be done befor point calculation
    // otherwise texture won't be mapped

    //pc.map_to(colored_frame);
    auto points = pc.calculate(depth);
    // Actual calling of conversion and saving XYZRGBRGB cloud to file

    ptr_cloud cloud = points_to_pcl(points, colored_frame);
    std::cout << cloud->empty() << std::endl;

    if(cloud->size() != false) {

        cv::Mat image(cv::Size(wide, height), CV_8UC3, (void*)colored_frame.get_data(), cv::Mat::AUTO_STEP);
        cv::aruco::detectMarkers(image, dictionary, corners, ids);
        if (ids.size() > 0)
        {
            cv::aruco::drawDetectedMarkers(image, corners, ids);
            //std::vector<cv::Vec3d> rvecs, tvecs;
            cv::Vec3d rvecs, tvecs;
            //cv::aruco::estimatePoseSingleMarkers(corners, actual_marker_length, camera_matrix, dist_coeffs, rvecs, tvecs);
            // draw axis for each marker
            for(int i=0; i < ids.size(); i++)
            {
                cv::aruco::estimatePoseBoard(corners,ids,board,camera_matrix,dist_coeffs,rvecs,tvecs);
                cv::aruco::drawAxis(image, camera_matrix, dist_coeffs, rvecs, tvecs, 0.1);
                //cv::aruco::drawAxis(image, camera_matrix, dist_coeffs, rvecs[i], tvecs[i], 0.1);
               // std::cout <<"x: " << rvecs[i][0]*degree << " y: " << rvecs[i][1]*degree << " z: "<< rvecs[i][2]*degree <<std::endl;
                 std::cout <<"x: " << rvecs[0]*degree << " y: " << rvecs[1]*degree << " z: "<< rvecs[2]*degree <<std::endl;
                if((fp=fopen("save.csv","a"))!=NULL){
                    fprintf(fp,"step %d,\n",count);
                    fprintf(fp,"%f,%f,%f\n",rvecs[0],rvecs[1],rvecs[2]);
                    fclose(fp);
                }
    }

    end = std::chrono::system_clock::now();
    double elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();

    cv::waitKey(0);
    count++;
    }

    return 1;
}


// ****************************************************************************
// test/manual_entity_performance.cpp
//
// Part of the test harness for entity.
//
// Copyright Chris Glover 2014-2016
//
// 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
//
// ****************************************************************************
#define DAILY_ENABLE_INSTRUMENTATION 0
#include <iostream>
#include <vector>
#include <algorithm>
#include "performance_common.hpp"
#include <daily/timer/instrument.h>

static const int kNumEntities = TEST_SIZE;
static const float kTestLength = 10.0f;
static const float kFrameTime = 0.016f;

#define BOOST_TEST_MODULE Performance
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE( manual_entity )
{
	std::vector<float> positions;
	std::vector<float> velocities;
	std::vector<float> accels;

	std::clog << "Created Pools\n";

	{	 DAILY_AUTO_INSTRUMENT_NODE(ComponentCreation);

		for(int i = 0; i < kNumEntities; ++i)
		{
			positions.push_back(0);
			velocities.push_back(0);
			accels.push_back(9.8f);
		}
	}

	std::clog << "Created Components, simulating...";

	// Simulate over some seconds using a fixed step.
	{
		daily::cpu_timer& simulation_node = daily::timer_map::get_default().create_node("Simulation");
		daily::cpu_timer_scope simulation_scope(simulation_node);

		float time_remaining = kTestLength;
		while(time_remaining > 0)
		{
			for(std::size_t i = 0, s = accels.size(); i < s; ++i)
			{
				// Add a little to accel each frame.
				accels[i] += 0.001f;
			}

			for(std::size_t i = 0, s = velocities.size(); i < s; ++i)
			{
				// Compute new velocity.
				velocities[i] += (accels[i]/2.f) * (kFrameTime * kFrameTime);
			}

			for(std::size_t i = 0, s = positions.size(); i < s; ++i)
			{
				// Compute new position.
				positions[i] += velocities[i] * kFrameTime;
			}

			time_remaining -= kFrameTime;
		}
	}

	std::clog << "done.\n";

    std::clog << "Positions: " << positions[0] << std::endl;
    std::clog << "Velocities: " << velocities[0] << std::endl;

	if(!daily::timer_map::get_default().empty())
	{
		std::cout << "---------- Report -----------\n";
		daily::timer_map::get_default().gather_report(
			std::ostream_iterator<daily::timer_map::result_type>(std::cout, "\n")
		);
	}

	std::cout.flush();
}


// Copyright 2016-2018 Doug Moen
// Licensed under the Apache License, version 2.0
// See accompanying file LICENSE or https://www.apache.org/licenses/LICENSE-2.0

#include <libcurv/builtin.h>

#include <libcurv/analyser.h>
#include <libcurv/arg.h>
#include <libcurv/array_op.h>
#include <libcurv/exception.h>
#include <libcurv/function.h>
#include <libcurv/gl_context.h>
#include <libcurv/math.h>
#include <libcurv/program.h>
#include <libcurv/source.h>
#include <libcurv/system.h>

#include <boost/math/constants/constants.hpp>
#include <boost/filesystem.hpp>

#include <cassert>
#include <cmath>
#include <cstdlib>
#include <string>

using namespace std;
using namespace boost::math::double_constants;

namespace curv {

Shared<Meaning>
Builtin_Value::to_meaning(const Identifier& id) const
{
    return make<Constant>(share(id), value_);
}

struct Is_Null_Function : public Legacy_Function
{
    static const char* name() { return "is_null"; }
    Is_Null_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].is_null()};
    }
};
struct Is_Bool_Function : public Legacy_Function
{
    static const char* name() { return "is_bool"; }
    Is_Bool_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].is_bool()};
    }
};
struct Is_Num_Function : public Legacy_Function
{
    static const char* name() { return "is_num"; }
    Is_Num_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].is_num()};
    }
};
struct Is_String_Function : public Legacy_Function
{
    static const char* name() { return "is_string"; }
    Is_String_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].dycast<String>() != nullptr};
    }
};
struct Is_List_Function : public Legacy_Function
{
    static const char* name() { return "is_list"; }
    Is_List_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].dycast<List>() != nullptr};
    }
};
struct Is_Record_Function : public Legacy_Function
{
    static const char* name() { return "is_record"; }
    Is_Record_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].dycast<Structure>() != nullptr};
    }
};
struct Is_Fun_Function : public Legacy_Function
{
    static const char* name() { return "is_fun"; }
    Is_Fun_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].dycast<Function>() != nullptr};
    }
};

struct Bit_Function : public Legacy_Function
{
    static const char* name() { return "bit"; }
    Bit_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {double(args[0].to_bool(At_Arg(*this, args)))};
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        auto arg = f[0];
        if (arg.type != GL_Type::Bool)
            throw Exception(At_GL_Arg(0, f),
                stringify(name(),": argument is not a bool"));
        auto result = f.gl.newvalue(GL_Type::Num);
        f.gl.out << "  float "<<result<<" = float("<<arg<<");\n";
        return result;
    }
};

#define UNARY_NUMERIC_FUNCTION(Class_Name,curv_name,c_name,glsl_name) \
struct Class_Name : public Legacy_Function \
{ \
    static const char* name() { return #curv_name; } \
    Class_Name() : Legacy_Function(1,name()) {} \
    struct Scalar_Op { \
        static double f(double x) { return c_name(x); } \
        static Shared<const String> callstr(Value x) { \
            return stringify(name(),"(",x,")"); \
        } \
    }; \
    static Unary_Numeric_Array_Op<Scalar_Op> array_op; \
    Value call(Frame& args) override \
    { \
        return array_op.op(args[0], At_Frame(&args)); \
    } \
    GL_Value gl_call(GL_Frame& f) const override \
    { \
        return gl_call_unary_numeric(f, #glsl_name); \
    } \
}; \

UNARY_NUMERIC_FUNCTION(Sqrt_Function, sqrt, sqrt, sqrt)
UNARY_NUMERIC_FUNCTION(Log_Function, log, log, log)
UNARY_NUMERIC_FUNCTION(Abs_Function, abs, abs, abs)
UNARY_NUMERIC_FUNCTION(Floor_Function, floor, floor, floor)
UNARY_NUMERIC_FUNCTION(Ceil_Function, ceil, ceil, ceil)
UNARY_NUMERIC_FUNCTION(Trunc_Function, trunc, trunc, trunc)
UNARY_NUMERIC_FUNCTION(Round_Function, round, rint, roundEven)

inline double frac(double n) { return n - floor(n); }
UNARY_NUMERIC_FUNCTION(Frac_Function, frac, frac, fract)

UNARY_NUMERIC_FUNCTION(Sin_Function, sin, sin, sin)
UNARY_NUMERIC_FUNCTION(Cos_Function, cos, cos, cos)
UNARY_NUMERIC_FUNCTION(Tan_Function, tan, tan, tan)
UNARY_NUMERIC_FUNCTION(Acos_Function, acos, acos, acos)
UNARY_NUMERIC_FUNCTION(Asin_Function, asin, asin, asin)
UNARY_NUMERIC_FUNCTION(Atan_Function, atan, atan, atan)

UNARY_NUMERIC_FUNCTION(Sinh_Function, sinh, sinh, sinh)
UNARY_NUMERIC_FUNCTION(Cosh_Function, cosh, cosh, cosh)
UNARY_NUMERIC_FUNCTION(Tanh_Function, tanh, tanh, tanh)
UNARY_NUMERIC_FUNCTION(Acosh_Function, acosh, acosh, acosh)
UNARY_NUMERIC_FUNCTION(Asinh_Function, asinh, asinh, asinh)
UNARY_NUMERIC_FUNCTION(Atanh_Function, atanh, atanh, atanh)

struct Atan2_Function : public Legacy_Function
{
    static const char* name() { return "atan2"; }
    Atan2_Function() : Legacy_Function(2,name()) {}

    struct Scalar_Op {
        static double f(double x, double y) { return atan2(x, y); }
        static const char* name() { return "atan2"; }
        static Shared<const String> callstr(Value x, Value y) {
            return stringify("atan2(",x,",",y,")");
        }
    };
    static Binary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.op(args[0], args[1], At_Arg(*this, args));
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        auto x = f[0];
        auto y = f[1];

        GL_Type rtype = GL_Type::Bool;
        if (x.type == y.type)
            rtype = x.type;
        else if (x.type == GL_Type::Num)
            rtype = y.type;
        else if (y.type == GL_Type::Num)
            rtype = x.type;
        if (rtype == GL_Type::Bool)
            throw Exception(At_GL_Phrase(f.call_phrase_, &f),
                "GL domain error");

        GL_Value result = f.gl.newvalue(rtype);
        f.gl.out <<"  "<<rtype<<" "<<result<<" = atan(";
        gl_put_as(f, x, At_GL_Arg(0, f), rtype);
        f.gl.out << ",";
        gl_put_as(f, y, At_GL_Arg(1, f), rtype);
        f.gl.out << ");\n";
        return result;
    }
};

GL_Value gl_minmax(const char* name, Operation& argx, GL_Frame& f)
{
    auto list = dynamic_cast<List_Expr*>(&argx);
    if (list) {
        std::list<GL_Value> args;
        GL_Type type = GL_Type::Num;
        for (auto op : *list) {
            auto val = op->gl_eval(f);
            args.push_back(val);
            if (val.type == GL_Type::Num)
                ;
            else if (gl_type_count(val.type) >= 2) {
                if (type == GL_Type::Num)
                    type = val.type;
                else if (type != val.type)
                    throw Exception(At_GL_Phrase(op->syntax_, &f), stringify(
                        "GL: ",name,
                        ": vector arguments of different lengths"));
            } else {
                throw Exception(At_GL_Phrase(op->syntax_, &f), stringify(
                    "GL: ",name,": argument has bad type"));
            }
        }
        auto result = f.gl.newvalue(type);
        if (args.size() == 0)
            f.gl.out << "  " << type << " " << result << " = -0.0/0.0;\n";
        else if (args.size() == 1)
            return args.front();
        else {
            f.gl.out << "  " << type << " " << result << " = ";
            int rparens = 0;
            while (args.size() > 2) {
                f.gl.out << name << "(" << args.front() << ",";
                args.pop_front();
                ++rparens;
            }
            f.gl.out << name << "(" << args.front() << "," << args.back() << ")";
            while (rparens > 0) {
                f.gl.out << ")";
                --rparens;
            }
            f.gl.out << ";\n";
        }
        return result;
    } else {
        auto arg = argx.gl_eval(f);
        auto result = f.gl.newvalue(GL_Type::Num);
        f.gl.out << "  float "<<result<<" = ";
        if (arg.type == GL_Type::Vec2)
            f.gl.out << name <<"("<<arg<<".x,"<<arg<<".y);\n";
        else if (arg.type == GL_Type::Vec3)
            f.gl.out << name<<"("<<name<<"("<<arg<<".x,"<<arg<<".y),"
                <<arg<<".z);\n";
        else if (arg.type == GL_Type::Vec4)
            f.gl.out << name<<"("<<name<<"("<<name<<"("<<arg<<".x,"<<arg<<".y),"
                <<arg<<".z),"<<arg<<".w);\n";
        else
            throw Exception(At_GL_Phrase(argx.syntax_, &f), stringify(
                name,": argument is not a vector"));
        return result;
    }
}

struct Max_Function : public Legacy_Function
{
    static const char* name() { return "max"; }
    Max_Function() : Legacy_Function(1,name()) {}

    struct Scalar_Op {
        static double f(double x, double y) {
            // return NaN if either argument is NaN.
            if (x >= y) return x;
            if (x < y) return y;
            return 0.0/0.0;
        }
        static const char* name() { return "max"; }
        static Shared<const String> callstr(Value x, Value y) {
            return stringify("max(",x,",",y,")");
        }
    };
    static Binary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.reduce(-INFINITY, args[0], At_Arg(*this, args));
    }
    GL_Value gl_call_expr(Operation& argx, const Call_Phrase*, GL_Frame& f)
    const override
    {
        return gl_minmax(name(),argx,f);
    }
};

struct Min_Function : public Legacy_Function
{
    static const char* name() { return "min"; }
    Min_Function() : Legacy_Function(1,name()) {}

    struct Scalar_Op {
        static double f(double x, double y) {
            // return NaN if either argument is NaN
            if (x <= y) return x;
            if (x > y) return y;
            return 0.0/0.0;
        }
        static const char* name() { return "min"; }
        static Shared<const String> callstr(Value x, Value y) {
            return stringify("min(",x,",",y,")");
        }
    };
    static Binary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.reduce(INFINITY, args[0], At_Arg(*this, args));
    }
    GL_Value gl_call_expr(Operation& argx, const Call_Phrase*, GL_Frame& f)
    const override
    {
        return gl_minmax("min",argx,f);
    }
};

// Generalized dot product that includes vector dot product and matrix product.
// Same as Mathematica Dot[A,B]. Like APL A+.×B, Python numpy.dot(A,B)
struct Dot_Function : public Legacy_Function
{
    static const char* name() { return "dot"; }
    Dot_Function() : Legacy_Function(2,name()) {}
    Value call(Frame& args) override
    {
        return dot(args[0], args[1], At_Arg(*this, args));
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        auto a = f[0];
        auto b = f[1];
        if (gl_type_count(a.type) < 2)
            throw Exception(At_GL_Arg(0, f), "dot: argument is not a vector");
        if (a.type != b.type)
            throw Exception(At_GL_Arg(1, f), "dot: arguments have different types");
        auto result = f.gl.newvalue(GL_Type::Num);
        f.gl.out << "  float "<<result<<" = dot("<<a<<","<<b<<");\n";
        return result;
    }
};

struct Mag_Function : public Legacy_Function
{
    static const char* name() { return "mag"; }
    Mag_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        // TODO: use hypot() or BLAS DNRM2 or Eigen stableNorm/blueNorm?
        // Avoids overflow/underflow due to squaring of large/small values.
        // Slower.  https://forum.kde.org/viewtopic.php?f=74&t=62402
        auto& list = arg_to_list(args[0], At_Arg(*this, args));
        double sum = 0.0;
        for (auto val : list) {
            double x = val.get_num_or_nan();
            sum += x * x;
        }
        if (sum == sum)
            return {sqrt(sum)};
        throw Exception(At_Arg(*this, args), "domain error");
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        auto arg = f[0];
        if (gl_type_count(arg.type) < 2)
            throw Exception(At_GL_Arg(0, f), "mag: argument is not a vector");
        auto result = f.gl.newvalue(GL_Type::Num);
        f.gl.out << "  float "<<result<<" = length("<<arg<<");\n";
        return result;
    }
};

struct Count_Function : public Legacy_Function
{
    static const char* name() { return "count"; }
    Count_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        if (auto list = args[0].dycast<const List>())
            return {double(list->size())};
        if (auto string = args[0].dycast<const String>())
            return {double(string->size())};
        throw Exception(At_Arg(*this, args), "not a list or string");
    }
};
struct Fields_Function : public Legacy_Function
{
    static const char* name() { return "fields"; }
    Fields_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        if (auto structure = args[0].dycast<const Structure>())
            return {structure->fields()};
        throw Exception(At_Arg(*this, args), "not a record");
    }
};

struct Strcat_Function : public Legacy_Function
{
    static const char* name() { return "strcat"; }
    Strcat_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        if (auto list = args[0].dycast<const List>()) {
            String_Builder sb;
            for (auto val : *list) {
                if (auto str = val.dycast<const String>())
                    sb << str;
                else
                    sb << val;
            }
            return {sb.get_string()};
        }
        throw Exception(At_Arg(*this, args), "not a list");
    }
};
struct Repr_Function : public Legacy_Function
{
    static const char* name() { return "repr"; }
    Repr_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        String_Builder sb;
        sb << args[0];
        return {sb.get_string()};
    }
};
struct Decode_Function : public Legacy_Function
{
    static const char* name() { return "decode"; }
    Decode_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& f) override
    {
        String_Builder sb;
        At_Arg cx(*this, f);
        auto list = f[0].to<List>(cx);
        for (size_t i = 0; i < list->size(); ++i)
            sb << (char)arg_to_int((*list)[i], 1, 127, At_Index(i,cx));
        return {sb.get_string()};
    }
};
struct Encode_Function : public Legacy_Function
{
    static const char* name() { return "encode"; }
    Encode_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& f) override
    {
        List_Builder lb;
        At_Arg cx(*this, f);
        auto str = f[0].to<String>(cx);
        for (size_t i = 0; i < str->size(); ++i)
            lb.push_back({(double)(int)str->at(i)});
        return {lb.get_list()};
    }
};

struct Match_Function : public Legacy_Function
{
    static const char* name() { return "match"; }
    Match_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& f) override
    {
        At_Arg ctx0(*this, f);
        auto list = f[0].to<List>(ctx0);
        std::vector<Shared<Function>> cases;
        for (size_t i = 0; i < list->size(); ++i)
            cases.push_back(list->at(i).to<Function>(At_Index(i,ctx0)));
        auto mf = make<Piecewise_Function>(cases);
        mf->name_ = name_;
        mf->argpos_ = 1;
        return {mf};
    }
};

// The filename argument to "file", if it is a relative filename,
// is interpreted relative to the parent directory of the source file from
// which "file" is called.
//
// Because "file" has this hidden parameter (the name of the source file from
// which it is called), it is not a pure function. For this reason, it isn't
// a function value at all, it's a metafunction.
struct File_Expr : public Just_Expression
{
    Shared<Operation> arg_;
    File_Expr(Shared<const Call_Phrase> src, Shared<Operation> arg)
    :
        Just_Expression(std::move(src)),
        arg_(std::move(arg))
    {}
    virtual Value eval(Frame& f) const override
    {
        // construct argument context
        auto& callphrase = dynamic_cast<const Call_Phrase&>(*syntax_);
        At_Metacall cx("file", 0, *callphrase.arg_, &f);

        // construct file pathname from argument
        Value arg = arg_->eval(f);
        auto argstr = arg.to<String>(cx);
        namespace fs = boost::filesystem;
        fs::path filepath;
        auto caller_filename = syntax_->location().source().name_;
        if (caller_filename->empty()) {
            filepath = fs::path(argstr->c_str());
        } else {
            filepath = fs::path(caller_filename->c_str()).parent_path()
                / fs::path(argstr->c_str());
        }

        // construct filename extension (includes leading '.')
        std::string ext = filepath.extension().string();
        for (char& c : ext)
            c = std::tolower(c);

        // import file based on extension
        auto importp = f.system_.importers_.find(ext);
        if (importp != f.system_.importers_.end())
            return (*importp->second)(filepath, cx);
        else {
            // If extension not recognized, it defaults to a Curv program.
            auto file = make<File_Source>(make_string(filepath.c_str()), cx);
            std::unique_ptr<Frame> f2 =
                Frame::make(0, f.system_, &f, &callphrase, nullptr);
            Program prog{std::move(file), f.system_,
                Program_Opts().parent_frame(&*f2)};
            auto filekey = Filesystem::canonical(filepath);
            auto& active_files = f.system_.active_files_;
            if (active_files.find(filekey) != active_files.end())
                throw Exception{cx,
                    stringify("illegal recursive reference to file ",filepath)};
            Active_File af(active_files, filekey);
            prog.compile();
            return prog.eval();
        }
    }
};
struct File_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<File_Expr>(share(ph), analyse_op(*ph.arg_, env));
    }
};

/// The meaning of a call to `print`, such as `print "foo"`.
struct Print_Action : public Just_Action
{
    Shared<Operation> arg_;
    Print_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Just_Action(std::move(syntax)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f) const override
    {
        Value arg = arg_->eval(f);
        if (auto str = arg.dycast<String>())
            f.system_.console() << *str;
        else
            f.system_.console() << arg;
        f.system_.console() << std::endl;
    }
};
/// The meaning of the phrase `print` in isolation.
struct Print_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Print_Action>(share(ph), analyse_op(*ph.arg_, env));
    }
};

struct Warning_Action : public Just_Action
{
    Shared<Operation> arg_;
    Warning_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Just_Action(std::move(syntax)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f) const override
    {
        Value arg = arg_->eval(f);
        Shared<String> msg;
        if (auto str = arg.dycast<String>())
            msg = str;
        else
            msg = stringify(arg);
        Exception exc{At_Phrase(*syntax_, &f), msg};
        f.system_.message("WARNING: ", exc);
    }
};
/// The meaning of the phrase `warning` in isolation.
struct Warning_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Warning_Action>(share(ph), analyse_op(*ph.arg_, env));
    }
};

/// The meaning of a call to `error`, such as `error("foo")`.
struct Error_Operation : public Operation
{
    Shared<Operation> arg_;
    Error_Operation(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Operation(std::move(syntax)),
        arg_(std::move(arg))
    {}
    [[noreturn]] void run(Frame& f) const
    {
        Value val = arg_->eval(f);
        Shared<const String> msg;
        if (auto s = val.dycast<String>())
            msg = s;
        else
            msg = stringify(val);
        throw Exception(At_Frame(&f), msg);
    }
    virtual void exec(Frame& f) const override
    {
        run(f);
    }
    virtual Value eval(Frame& f) const override
    {
        run(f);
    }
    virtual void generate(Frame& f, List_Builder&) const override
    {
        run(f);
    }
    virtual void bind(Frame& f, Record&) const override
    {
        run(f);
    }
};
/// The meaning of the phrase `error` in isolation.
struct Error_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Error_Operation>(share(ph), analyse_op(*ph.arg_, env));
    }
};

// exec(expr) -- a debug action that evaluates expr, then discards the result.
// It is used to call functions or source files for their side effects.
struct Exec_Action : public Just_Action
{
    Shared<Operation> arg_;
    Exec_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Just_Action(std::move(syntax)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f) const override
    {
        arg_->eval(f);
    }
};
struct Exec_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Exec_Action>(share(ph), analyse_op(*ph.arg_, env));
    }
};

struct Assert_Action : public Just_Action
{
    Shared<Operation> arg_;
    Assert_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Just_Action(std::move(syntax)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f) const override
    {
        At_Metacall cx{"assert", 0, *arg_->syntax_, &f};
        bool b = arg_->eval(f).to_bool(cx);
        if (!b)
            throw Exception(At_Phrase(*syntax_, &f), "assertion failed");
    }
};
struct Assert_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        auto arg = analyse_op(*ph.arg_, env);
        return make<Assert_Action>(share(ph), arg);
    }
};

struct Assert_Error_Action : public Just_Action
{
    Shared<Operation> expected_message_;
    Shared<const String> actual_message_;
    Shared<Operation> expr_;

    Assert_Error_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> expected_message,
        Shared<const String> actual_message,
        Shared<Operation> expr)
    :
        Just_Action(std::move(syntax)),
        expected_message_(std::move(expected_message)),
        actual_message_(std::move(actual_message)),
        expr_(std::move(expr))
    {}

    virtual void exec(Frame& f) const override
    {
        Value expected_msg_val = expected_message_->eval(f);
        auto expected_msg_str = expected_msg_val.to<const String>(
            At_Phrase(*expected_message_->syntax_, &f));

        if (actual_message_ != nullptr) {
            if (*actual_message_ != *expected_msg_str)
                throw Exception(At_Phrase(*syntax_, &f),
                    stringify("assertion failed: expected error \"",
                        expected_msg_str,
                        "\", actual error \"",
                        actual_message_,
                        "\""));
            return;
        }

        Value result;
        try {
            result = expr_->eval(f);
        } catch (Exception& e) {
            if (*e.shared_what() != *expected_msg_str) {
                throw Exception(At_Phrase(*syntax_, &f),
                    stringify("assertion failed: expected error \"",
                        expected_msg_str,
                        "\", actual error \"",
                        e.shared_what(),
                        "\""));
            }
            return;
        }
        throw Exception(At_Phrase(*syntax_, &f),
            stringify("assertion failed: expected error \"",
                expected_msg_str,
                "\", got value ", result));
    }
};
struct Assert_Error_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        auto parens = cast<Paren_Phrase>(ph.arg_);
        Shared<Comma_Phrase> commas = nullptr;
        if (parens) commas = cast<Comma_Phrase>(parens->body_);
        if (parens && commas && commas->args_.size() == 2) {
            auto msg = analyse_op(*commas->args_[0].expr_, env);
            Shared<Operation> expr = nullptr;
            Shared<const String> actual_msg = nullptr;
            try {
                expr = analyse_op(*commas->args_[1].expr_, env);
            } catch (Exception& e) {
                actual_msg = e.shared_what();
            }
            return make<Assert_Error_Action>(share(ph), msg, actual_msg, expr);
        } else {
            throw Exception(At_Phrase(ph, env),
                "assert_error: expecting 2 arguments");
        }
    }
};

struct Defined_Expression : public Just_Expression
{
    Shared<const Operation> expr_;
    Symbol_Expr selector_;

    Defined_Expression(
        Shared<const Phrase> syntax,
        Shared<const Operation> expr,
        Symbol_Expr selector)
    :
        Just_Expression(std::move(syntax)),
        expr_(std::move(expr)),
        selector_(std::move(selector))
    {
    }

    virtual Value eval(Frame& f) const override
    {
        auto val = expr_->eval(f);
        auto s = val.dycast<Structure>();
        if (s) {
            auto id = selector_.eval(f);
            return {s->hasfield(id)};
        } else {
            return {false};
        }
    }
};
struct Defined_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        auto arg = analyse_op(*ph.arg_, env);
        auto dot = cast<Dot_Expr>(arg);
        if (dot != nullptr)
            return make<Defined_Expression>(
                share(ph), dot->base_, dot->selector_);
        throw Exception(At_Phrase(*ph.arg_, env),
            "defined: argument must be `expression.identifier`");
    }
};

const Namespace&
builtin_namespace()
{
    #define FUNCTION(f) {f::name(), make<Builtin_Value>(Value{make<f>()})}

    static const Namespace names = {
    {"pi", make<Builtin_Value>(pi)},
    {"tau", make<Builtin_Value>(two_pi)},
    {"inf", make<Builtin_Value>(INFINITY)},
    {"null", make<Builtin_Value>(Value())},
    {"false", make<Builtin_Value>(Value(false))},
    {"true", make<Builtin_Value>(Value(true))},

    FUNCTION(Is_Null_Function),
    FUNCTION(Is_Bool_Function),
    FUNCTION(Is_Num_Function),
    FUNCTION(Is_String_Function),
    FUNCTION(Is_List_Function),
    FUNCTION(Is_Record_Function),
    FUNCTION(Is_Fun_Function),
    FUNCTION(Bit_Function),
    FUNCTION(Sqrt_Function),
    FUNCTION(Log_Function),
    FUNCTION(Abs_Function),
    FUNCTION(Floor_Function),
    FUNCTION(Ceil_Function),
    FUNCTION(Trunc_Function),
    FUNCTION(Round_Function),
    FUNCTION(Frac_Function),
    FUNCTION(Sin_Function),
    FUNCTION(Cos_Function),
    FUNCTION(Tan_Function),
    FUNCTION(Asin_Function),
    FUNCTION(Acos_Function),
    FUNCTION(Atan_Function),
    FUNCTION(Atan2_Function),
    FUNCTION(Sinh_Function),
    FUNCTION(Cosh_Function),
    FUNCTION(Tanh_Function),
    FUNCTION(Asinh_Function),
    FUNCTION(Acosh_Function),
    FUNCTION(Atanh_Function),
    FUNCTION(Max_Function),
    FUNCTION(Min_Function),
    FUNCTION(Dot_Function),
    FUNCTION(Mag_Function),
    FUNCTION(Count_Function),
    FUNCTION(Fields_Function),
    FUNCTION(Strcat_Function),
    FUNCTION(Repr_Function),
    FUNCTION(Decode_Function),
    FUNCTION(Encode_Function),
    FUNCTION(Match_Function),

    {"file", make<Builtin_Meaning<File_Metafunction>>()},
    {"print", make<Builtin_Meaning<Print_Metafunction>>()},
    {"warning", make<Builtin_Meaning<Warning_Metafunction>>()},
    {"error", make<Builtin_Meaning<Error_Metafunction>>()},
    {"assert", make<Builtin_Meaning<Assert_Metafunction>>()},
    {"assert_error", make<Builtin_Meaning<Assert_Error_Metafunction>>()},
    {"exec", make<Builtin_Meaning<Exec_Metafunction>>()},
    {"defined", make<Builtin_Meaning<Defined_Metafunction>>()},
    };
    return names;
}

} // namespace curv


// The MIT License
// (c) 2019 Daniel Williams

#include <cstring>
#include <memory>

#include <Eigen/Geometry>

#include <nlohmann/json/json.hpp>

#include "geometry/layout.hpp"

using std::string;

using Eigen::Vector2d;

namespace terrain::geometry {

const string x_key("x");
const string y_key("y");
const string precision_key("precision");
const string width_key("width");

std::unique_ptr<Layout> Layout::make_from_json(nlohmann::json& doc){
    if(!doc.contains(precision_key) || !doc[precision_key].is_number()){
        return {};
    }else if(!doc.contains(x_key) || !doc[x_key].is_number()){
        return {};
    }else if(!doc.contains(y_key) || !doc[y_key].is_number()){
        return {};
    }else if(!doc.contains(width_key) || !doc[width_key].is_number()){
        return {};
    }

    const double precision = doc[precision_key].get<double>();
    const double x = doc[x_key].get<double>();
    const double y = doc[y_key].get<double>();
    const double width = doc[width_key].get<double>();

    return std::make_unique<Layout>(precision, x, y, width);
}

double Layout::constrain_x( const double x) const {
    return std::max(get_x_min(), std::min(x, get_x_max()));
}

double Layout::constrain_y( const double y) const {
    return std::max(get_y_min(), std::min(y, get_y_max()));
}

bool Layout::contains(const Vector2d& at) const {
    // outside x-bounds:
    if( (at[0] < x - half_width) || (at[0] > x + half_width) ){
        return false;
    }

    // outside y-bounds:
    if( (at[1] < y - half_width) || (at[1] > y + half_width) ){
        return false;
    }
    return true;
}

const Vector2d Layout::get_anchor() const {
    return {get_x_min(), get_y_min()};
}

double Layout::get_x_max() const {
    return x + half_width;
}
double Layout::get_x_min() const {
    return x - half_width;
}
double Layout::get_y_max() const {
    return y + half_width;
}
double Layout::get_y_min() const {
    return y - half_width;
}

bool Layout::operator!=(const Layout& other) const {
    return ! (this->operator==(other));
}

bool Layout::operator==(const Layout& other) const {
    const double error =  abs(precision - other.precision)
                        + abs(x - other.x)
                        + abs(y - other.y)
                        + abs(width - other.width);

    if( epsilon > error){
        return true;
    }

    return false;
}

nlohmann::json Layout::to_json() const {
    return {{"precision", precision},
            {"x", x},
            {"y", y},
            {"width", width}};
}

std::string Layout::to_string() const {
    return "x: " + std::to_string(x)\
        +  ", y: " + std::to_string(y)\
        +  ", prec: " + std::to_string(precision)\
        +  ", width: " + std::to_string(width);
}

} // namespace terrain::geometry


/*
 * main.cpp
 *
 *  Created on: 21/mag/2012
 *      Author: Fabrizio
 */

#include <boost/iterator/filter_iterator.hpp>
#include <iostream>

int main(int argc, char *argv[]) {
	int numbers[] = {1, 2, 4, 5, 7, 87, 0, 9, 10};
    const int N = sizeof(numbers)/sizeof(int);

	typedef boost::filter_iterator<bool (*)(int), int*> filter_iterator;

	auto is_even = [](int value) -> bool{ return (value % 2) == 0; };
	filter_iterator begin(is_even, numbers);
	filter_iterator end(is_even, numbers + N);

	std::for_each(begin, end, [](int value){ std::cout << "Number " << value << " is even." << std::endl;});

	return EXIT_SUCCESS;
}


#include "vtkBoundingBoxReader.h"

#include <yaml-cpp/yaml.h>

#include <vtkInformationVector.h>
#include <vtkFieldData.h>
#include <vtkNew.h>
#include <vtkMultiBlockDataSet.h>
#include <vtkObjectFactory.h>
#include <vtkPolyData.h>
#include <vtkPolyLine.h>
#include <vtkStringArray.h>
#include <vtkTransform.h>
#include <vtkTransformPolyDataFilter.h>

#include <Eigen/Geometry>

#include "vtkEigenTools.h"
#include "vtkHelper.h"

using namespace std;

namespace {
  /**
 * @brief CreateBoundingBox2D create a polyData with a polyline representing the 2D bounding box.
 * As everything is 3D in vtk, the z coordinate is set to 0.
 * @param x upper left corner x coordinate
 * @param y upper left corner y coordinate
 * @param w width (must be strictly positive)
 * @param h height (must be strictly positive)
 */
vtkSmartPointer<vtkPolyData> CreateBoundingBox2D(double x, double y, double w, double h)
{
  assert(w > 0 && "width must be strictly positive");
  assert(h > 0 && "height must be strictly positive");

  auto bb = vtkSmartPointer<vtkPolyData>::New();

  // Fill points
  vtkNew<vtkPoints> pts;
  bb->SetPoints(pts);
  pts->InsertNextPoint(x, y, 0);
  pts->InsertNextPoint(x+w, y, 0);
  pts->InsertNextPoint(x+w, y+h, 0);
  pts->InsertNextPoint(x, y+h, 0);

  // Fill cells
  vtkNew<vtkCellArray> cell;
  vtkNew<vtkPolyLine> polyLine;
  polyLine->GetPointIds()->SetNumberOfIds(5);
  polyLine->GetPointIds()->SetId(0, 0);
  polyLine->GetPointIds()->SetId(1, 1);
  polyLine->GetPointIds()->SetId(2, 2);
  polyLine->GetPointIds()->SetId(3, 3);
  polyLine->GetPointIds()->SetId(4, 0);
  cell->InsertNextCell(polyLine.Get());
  bb->SetLines(cell);

  return bb;
}

vtkSmartPointer<vtkPolyData> CreateBoundingBox3D(const Eigen::Isometry3d& p, const Eigen::Vector3d& d)
{
  assert(d(0) > 0 && "d1 must be strictly positive");
  assert(d(1) > 0 && "d2 must be strictly positive");
  assert(d(2) > 0 && "d3 must be strictly positive");

  auto bb = vtkSmartPointer<vtkPolyData>::New();

  // Fill points
  vtkNew<vtkPoints> pts;
  bb->SetPoints(pts);
  // top
  pts->InsertNextPoint(-d(0)/2, -d(1)/2, +d(2)/2);
  pts->InsertNextPoint(+d(0)/2, -d(1)/2, +d(2)/2);
  pts->InsertNextPoint(+d(0)/2, +d(1)/2, +d(2)/2);
  pts->InsertNextPoint(-d(0)/2, +d(1)/2, +d(2)/2);
  // bottom
  pts->InsertNextPoint(-d(0)/2, -d(1)/2, -d(2)/2);
  pts->InsertNextPoint(+d(0)/2, -d(1)/2, -d(2)/2);
  pts->InsertNextPoint(+d(0)/2, +d(1)/2, -d(2)/2);
  pts->InsertNextPoint(-d(0)/2, +d(1)/2, -d(2)/2);

  // Fill cells
  vtkNew<vtkCellArray> cell;
  bb->SetLines(cell);
  // top
  vtkNew<vtkPolyLine> polyLineBottom;
  polyLineBottom->GetPointIds()->SetNumberOfIds(5);
  polyLineBottom->GetPointIds()->SetId(0, 0);
  polyLineBottom->GetPointIds()->SetId(1, 1);
  polyLineBottom->GetPointIds()->SetId(2, 2);
  polyLineBottom->GetPointIds()->SetId(3, 3);
  polyLineBottom->GetPointIds()->SetId(4, 0);
  cell->InsertNextCell(polyLineBottom.Get());

  // bottom
  vtkNew<vtkPolyLine> polyLineTop;
  polyLineTop->GetPointIds()->SetNumberOfIds(5);
  polyLineTop->GetPointIds()->SetId(0, 4);
  polyLineTop->GetPointIds()->SetId(1, 5);
  polyLineTop->GetPointIds()->SetId(2, 6);
  polyLineTop->GetPointIds()->SetId(3, 7);
  polyLineTop->GetPointIds()->SetId(4, 4);
  cell->InsertNextCell(polyLineTop.Get());

  // vertical line
  for (int j = 0; j < 4; j++)
  {
    vtkNew<vtkPolyLine> polyLine;
    polyLine->GetPointIds()->SetNumberOfIds(2);
    polyLine->GetPointIds()->SetId(0, j % 8);
    polyLine->GetPointIds()->SetId(1, (j + 4) % 8);
    cell->InsertNextCell(polyLine.Get());
  }

  // construct the transform
  vtkNew<vtkMatrix4x4> m;
  for (int i = 0; i < 4; ++i)
    for (int j = 0; j < 4; ++j)
    {
      m->SetElement(i, j, p(i, j));
    }
  vtkNew<vtkTransform> t;
  t->SetMatrix(m);

  // transform the bounding box
  vtkNew<vtkTransformPolyDataFilter> transformFilter;
  transformFilter->SetInputData(0, bb);
  transformFilter->SetTransform(t);
  transformFilter->Update();
  return transformFilter->GetOutput();
}
}
//-----------------------------------------------------------------------------
vtkStandardNewMacro(vtkBoundingBoxReader)

//-----------------------------------------------------------------------------
vtkBoundingBoxReader::vtkBoundingBoxReader()
{
  this->SetNumberOfInputPorts(0);
}

//-----------------------------------------------------------------------------
int vtkBoundingBoxReader::RequestData(vtkInformation *, vtkInformationVector **, vtkInformationVector *outputVector)
{
  if (this->FileName.empty())
  {
    vtkErrorMacro("Please specify a file name")
    return VTK_ERROR;
  }
  auto *output = vtkMultiBlockDataSet::GetData(outputVector->GetInformationObject(0));

  YAML::Node node = YAML::LoadFile(this->FileName);
  YAML::Node objects = node["objects"];
  if (!objects)
    vtkErrorMacro("In yaml file, no 'objects' key at level 0!")

  output->SetNumberOfBlocks(objects.size());
  for (int i = 0; i < objects.size(); ++i)
  {
    try {
      std::string type = objects[i]["selector"]["type"].as<std::string>();
      vtkSmartPointer<vtkPolyData> bb = nullptr;
      if (type == "2D bounding box")
      {
        std::vector<double> center = objects[i]["selector"]["center"].as<std::vector<double>>();
        center[1] = this->ImageHeight - center[1]; // due to image processing vs vtk convention
        std::vector<double> dimension= objects[i]["selector"]["dimensions"].as<std::vector<double>>();

        bb = CreateBoundingBox2D(center[0] - dimension[0]/2.,center[1] - dimension[1]/2., dimension[0], dimension[1]);
      }
      else if (type == "3D bounding box")
      {
        std::vector<double> center = objects[i]["selector"]["center"].as<std::vector<double>>();
        std::vector<double> rotation = objects[i]["selector"]["rotation"].as<std::vector<double>>();

        Eigen::Matrix3d r = RollPitchYawInDegreeToMatrix(rotation[0], rotation[1], rotation[2]);
        Eigen::Translation3d t(center[0], center[1], center[2]);
        Eigen::Isometry3d pose(t*Eigen::Quaterniond(r));

        std::vector<double> dimension= objects[i]["selector"]["dimensions"].as<std::vector<double>>();
        Eigen::Vector3d d(dimension.data());

        bb = CreateBoundingBox3D(pose, d);
      }
      else
      {
        vtkErrorMacro("Either the 'type' key is missing or this type of bounding box is not supported")
      }

      // Add label
      std::string label = objects[i]["label"].as<std::string>();
      auto labelData = createArray<vtkStringArray>("Label", 1, 1);
      labelData->SetValue(0, label);
      bb->GetFieldData()->AddArray(labelData);

      output->SetBlock(i, bb);

    } catch (YAML::BadConversion& e) {
      vtkErrorMacro(<< "YAML::BadConversion : " << e.what() << "\nThe yaml file is ill-formed")
    }
  }

  return VTK_OK;
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_UNIFORM_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_UNIFORM_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_greater.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>

namespace stan {
namespace math {

/**
 * Return a uniform random variate for the given upper and lower bounds using
 * the specified random number generator.
 *
 * alpha and beta can each be a scalar or a one-dimensional container. Any
 * non-scalar inputs must be the same size.
 *
 * @tparam T_alpha Type of shape parameter
 * @tparam T_beta Type of inverse scale parameter
 * @tparam RNG type of random number generator
 * @param alpha (Sequence of) lower bound parameter(s)
 * @param beta (Sequence of) upper bound parameter(s)
 * @param rng random number generator
 * @return (Sequence of) uniform random variate(s)
 * @throw std::domain_error if alpha or beta are non-finite
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_alpha, typename T_beta, class RNG>
inline typename VectorBuilder<true, double, T_alpha, T_beta>::type uniform_rng(
    const T_alpha& alpha, const T_beta& beta, RNG& rng) {
  using boost::random::uniform_real_distribution;
  using boost::variate_generator;

  static const char* function = "uniform_rng";

  check_finite(function, "Lower bound parameter", alpha);
  check_finite(function, "Upper bound parameter", beta);
  check_consistent_sizes(function, "Lower bound parameter", alpha,
                         "Upper bound parameter", beta);
  check_greater(function, "Upper bound parameter", beta, alpha);

  scalar_seq_view<T_alpha> alpha_vec(alpha);
  scalar_seq_view<T_beta> beta_vec(beta);
  size_t N = max_size(alpha, beta);
  VectorBuilder<true, double, T_alpha, T_beta> output(N);

  variate_generator<RNG&, uniform_real_distribution<> > uniform_rng(
      rng, uniform_real_distribution<>(0.0, 1.0));
  for (size_t n = 0; n < N; ++n) {
    output[n] = (beta_vec[n] - alpha_vec[n]) * uniform_rng() + alpha_vec[n];
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


/* Last Revised Date: 22_MAY_2015 */
/* Lastest status: not checked   */

// Package include header files

#include "insilico-0.25/include/insilico/core.hpp"
#include <boost/numeric/odeint.hpp>

// Global custom header files

#include "insilico-test/Global_variable_declarations.hpp"

// Main header files for Neurons & Astrocyte

#include "insilico-test/ip3_receptor_test/ip3_receptor_test_neuron.hpp"

// C++ Header files

#include <fstream>
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>

namespace gvars=astron::globals;

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

  newinsilico::initialize(argc, argv);

  newinsilico::observe("ca_cyt");
  newinsilico::observe("ca_er");
  newinsilico::observe("ip3_cyt");
  //newinsilico::observe("ip3r_yk_s110");
  //newinsilico::observe("ip3r_yk_flux");
  newinsilico::generate_neuron<ip3_receptor_test_neuron>(1);

  state_type variables = newinsilico::get_variables();
  integrate_const(boost::numeric::odeint::runge_kutta4<state_type>(),
                  newinsilico::driver(), variables,
                  0.0, 100.0, gvars::DELTA_T, newinsilico::observer());

  newinsilico::finalize();
}


#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <iostream>

#include <fstream>
#include <string>
#include <algorithm>
#include <random>
#include <chrono>
#include <memory>
#include <cmath>

#include <unsupported/Eigen/SparseExtra>
#include <Eigen/Sparse>

#include <omp.h>
#include <signal.h>

#include "macau.h"
#include "mvnormal.h"
#include "bpmfutils.h"
#include "latentprior.h"
#include "noisemodels.h"
#include "sparsetensor.h"

using namespace std;
using namespace Eigen;

static volatile bool keepRunning = true;

void intHandler(int dummy) {
  keepRunning = false;
  printf("[Received Ctrl-C. Stopping after finishing the current iteration.]\n");
}

template<class DType, class NType>
void MacauX<DType, NType>::addPrior(unique_ptr<ILatentPrior> & prior) {
  priors.push_back( std::move(prior) );
}

template<class DType, class NType>
void MacauX<DType, NType>::setSamples(int b, int n) {
  burnin = b;
  nsamples = n;
}

template<class DType, class NType>
void MacauX<DType, NType>::setRelationData(int* rows, int* cols, double* values, int N, int nrows, int ncols) {
  data.setTrain(rows, cols, values, N, nrows, ncols);
}

template<class DType, class NType>
void MacauX<DType, NType>::setRelationDataTest(int* rows, int* cols, double* values, int N, int nrows, int ncols) {
  data.setTest(rows, cols, values, N, nrows, ncols);
}

template<class DType, class NType>
void MacauX<DType, NType>::setRelationData(int* idx, int nmodes, double* values, int nnz, int* dims) {
  data.setTrain(idx, nmodes, values, nnz, dims);
}

template<class DType, class NType>
void MacauX<DType, NType>::setCensoringData(int* idx, int nmodes, int* censoring, int nnz, int* dims) {
  data.setCensoring(idx, nmodes, censoring, nnz, dims);
}

template<class DType, class NType>
void MacauX<DType, NType>::setRelationDataTest(int* idx, int nmodes, double* values, int nnz, int* dims) {
  data.setTest(idx, nmodes, values, nnz, dims);
}

template<class DType, class NType>
double MacauX<DType, NType>::getRmseTest() { return rmse_test; }

template<class DType, class NType>
void MacauX<DType, NType>::init() {
  unsigned seed1 = std::chrono::system_clock::now().time_since_epoch().count();
  if (priors.size() < 2) {
    throw std::runtime_error("Less 2 priors are not supported.");
  }
  init_bmrng(seed1);
  VectorXi dims = data.getDims();
  for (int mode = 0; mode < dims.size(); mode++) {
    MatrixXd* U = new MatrixXd(num_latent, dims(mode));
    U->setZero();
    samples.push_back( std::move(std::unique_ptr<MatrixXd>(U)) );
  }
  noise.init(data);
  keepRunning = true;
}

Macau::~Macau() {
}

inline double sqr(double x) { return x*x; }

template<class DType, class NType>
void MacauX<DType, NType>::run() {
  init();
  if (verbose) {
    std::cout << noise.getInitStatus() << endl;
    std::cout << "Sampling" << endl;
  }
  if (save_model) {
    saveGlobalParams();
  }
  signal(SIGINT, intHandler);

  const VectorXi dims = data.getDims();
  predictions     = VectorXd::Zero( data.getTestNonzeros() );
  predictions_var = VectorXd::Zero( data.getTestNonzeros() );

  auto start = tick();
  for (int i = 0; i < burnin + nsamples; i++) {
    if (keepRunning == false) {
      keepRunning = true;
      break;
    }
    if (verbose && i == burnin) {
      printf(" ====== Burn-in complete, averaging samples ====== \n");
    }
    auto starti = tick();

    // sample latent vectors
    for (int mode = 0; mode < dims.size(); mode++) {
      priors[mode]->sample_latents(noise, data, samples, mode, num_latent);
    }

    // Sample hyperparams
    for (int mode = 0; mode < dims.size(); mode++) {
      priors[mode]->update_prior(*samples[mode]);
    }

    noise.update(data, samples);

    noise.evalModel(data, (i < burnin) ? 0 : (i - burnin), predictions, predictions_var, samples);

    auto endi = tick();
    auto elapsed = endi - start;
    double samples_per_sec = (i + 1) * (dims.sum()) / elapsed;
    double elapsedi = endi - starti;

    if (save_model && i >= burnin) {
      saveModel(i - burnin + 1);
    }
    if (verbose) {
      printStatus(i, elapsedi, samples_per_sec);
    }
    rmse_test = noise.getEvalMetric();
  }
}

template<class DType, class NType>
void MacauX<DType, NType>::printStatus(int i, double elapsedi, double samples_per_sec) {
  printf("Iter %3d: %s  ", i, noise.getEvalString().c_str());

  printf("U:[");
  for (unsigned p = 0; p < priors.size(); p++) {
    printf("%1.2e", samples[p]->norm());
    if (p + 1 < priors.size()) {
      printf(", ");
    }
  }
  printf("]");

  printf("\tSide:[");
  for (unsigned p = 0; p < priors.size(); p++) {
    printf("%1.2e", priors[p]->getLinkNorm());
    if (p + 1 < priors.size()) {
      printf(", ");
    }
  }
  printf("]");
  printf("%s [took %0.1fs]\n", noise.getStatus().c_str(), elapsedi);
}

template<class DType, class NType>
Eigen::VectorXd MacauX<DType, NType>::getStds() {
  VectorXd std( data.getTestNonzeros() );
  if (nsamples <= 1) {
    std.setConstant(NAN);
    return std;
  }
  const int n = std.size();
  const double inorm = 1.0 / (nsamples - 1);
#pragma omp parallel for schedule(static)
  for (int i = 0; i < n; i++) {
    std[i] = sqrt(predictions_var[i] * inorm);
  }
  return std;
}

template<class DType, class NType>
void MacauX<DType, NType>::saveModel(int isample) {
  string fprefix = save_prefix + "-sample" + std::to_string(isample) + "-";
  // saving latent matrices
  for (unsigned int i = 0; i < samples.size(); i++) {
    writeToCSVfile(fprefix + "U" + std::to_string(i+1) + "-latents.csv", *samples[i]);
    priors[i]->saveModel(fprefix + "U" + std::to_string(i+1));
  }
}

template<class DType, class NType>
void MacauX<DType, NType>::saveGlobalParams() {
  VectorXd means(1);
  means << data.getMeanValue();
  writeToCSVfile(save_prefix + "-meanvalue.csv", means);
}

Macau* make_macau_probit(int nmodes, int num_latent) {
  switch (nmodes) {
    case 2: return new MacauX<MatrixData,  ProbitNoise>(num_latent, MatrixData(), ProbitNoise());
    default: return new MacauX<TensorData, ProbitNoise>(num_latent, TensorData(nmodes), ProbitNoise());
  }
}

Macau* make_macau_fixed(int nmodes, int num_latent, double precision) {
  FixedGaussianNoise fnoise(precision);
  switch (nmodes) {
    case 2: return new MacauX<MatrixData,  FixedGaussianNoise>(num_latent, MatrixData(), fnoise);
    default: return new MacauX<TensorData, FixedGaussianNoise>(num_latent, TensorData(nmodes), fnoise);
  }
}

Macau* make_macau_adaptive(int nmodes, int num_latent, double sn_init, double sn_max) {
  AdaptiveGaussianNoise anoise(sn_init, sn_max);
  switch (nmodes) {
    case 2: return new MacauX<MatrixData,  AdaptiveGaussianNoise>(num_latent, MatrixData(), anoise);
    default: return new MacauX<TensorData, AdaptiveGaussianNoise>(num_latent, TensorData(nmodes), anoise);
  }
}

Macau* make_macau_tobit(int nmodes, int num_latent, double precision) {
  FixedGaussianNoise fnoise(precision);
  switch (nmodes) {
    case 2: return new MacauX<MatrixDataCensored,  FixedGaussianNoise>(num_latent, MatrixDataCensored(), fnoise);
    default: throw std::runtime_error("Tensor mode not implemented for Censored Data");
  }
}

template class MacauX<MatrixData, FixedGaussianNoise>;
template class MacauX<MatrixData, AdaptiveGaussianNoise>;
template class MacauX<MatrixData, ProbitNoise>;

template class MacauX<MatrixDataCensored, FixedGaussianNoise>;

template class MacauX<TensorData, FixedGaussianNoise>;
template class MacauX<TensorData, AdaptiveGaussianNoise>;
template class MacauX<TensorData, ProbitNoise>;


/* ==============================================================================================
 *
 * 	isotropic_remeshing.cpp
 *
 * 	Performs an isotropic remeshing of a region of a polygon mesh.  Depends on CGAL
 *
 *  Example Usage
 *  -------------
 *  [Fnew, Vnew, Fnnew, Vnnew] = ...
 *     isotropic_remeshing(faces, vertices, targetEdgeLen, numIterations)
 *
 *
 * 	by Dillon Cislo
 * 	02/19/2019
 *
 * 	This is a MEX-File for MATLAB
 *
 * ============================================================================================*/

#include "mex.h" // for MATLAB

#include <vector>
#include <stdexcept>
#include <iostream>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>

#include <CGAL/Polygon_mesh_processing/remesh.h>
#include <CGAL/Polygon_mesh_processing/border.h>
#include <CGAL/Polygon_mesh_processing/orient_polygon_soup.h>
#include <CGAL/Polygon_mesh_processing/polygon_soup_to_polygon_mesh.h>
#include <CGAL/Polygon_mesh_processing/orientation.h>
#include <CGAL/Polygon_mesh_processing/compute_normal.h>

#include <boost/function_output_iterator.hpp>
#include <boost/foreach.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel 	Kernel;
typedef Kernel::Point_3 					Point;
typedef Kernel::Vector_3					Vector;

typedef CGAL::Surface_mesh<Point> 				Mesh;
typedef Mesh::Vertex_index 					Vertex_index;
typedef Mesh::Face_index 					Face_index;

typedef boost::graph_traits<Mesh>::face_descriptor 		face_descriptor;
typedef boost::graph_traits<Mesh>::vertex_descriptor 		vertex_descriptor;
typedef boost::graph_traits<Mesh>::halfedge_descriptor 		halfedge_descriptor;
typedef boost::graph_traits<Mesh>::edge_descriptor 		edge_descriptor;

namespace PMP = CGAL::Polygon_mesh_processing;

///
/// A struct holding a vector of edges associated with certain halfedges
///
struct halfedge2edge {

	halfedge2edge( const Mesh &m, std::vector<edge_descriptor> &edges )
		: m_mesh( m ), m_edges( edges ) {}

	void operator()( const halfedge_descriptor &h ) const {
		m_edges.push_back( edge( h, m_mesh ) );
	}

	const Mesh &m_mesh;

	std::vector<edge_descriptor> &m_edges;

};

///
/// Brief main function to call computational functionalities
///
void mexFunction( int nlhs, mxArray *plhs[],
		int nrhs, const mxArray *prhs[] ) {

	// -------------------------------------------------------------------------------------
	// INPUT PROCESSING
	// -------------------------------------------------------------------------------------

	// Check for proper number of arguments
	if ( nrhs != 4 ) {
		mexErrMsgIdAndTxt( "MATLAB:isotropic_remeshing:nargin",
				"ISOTROPIC_REMESHING requires four input arguments." );
	} else if ( nlhs != 4 ) {
		mexErrMsgIdAndTxt( "MATLAB:isotropic_remeshing:nargout",
				"ISOTROPIC_REMESHING requires four output arguments." );
	}

	double *faces = mxGetPr( prhs[0] );	   // The face connectivity list
	std::size_t numFaces = mxGetM( prhs[0] );  // The number of faces
	std::size_t sizeFaces = mxGetN( prhs[0] ); // The number of vertices in a single face

	double *vertex = mxGetPr( prhs[1] ); 	   // The vertex coordinate list
	std::size_t numVertex = mxGetM( prhs[1] ); // The number of vertices
	std::size_t dim = mxGetN( prhs[1] ); 	   // The dimensionality of the vertex list

	// Check the dimensionality of the vertex list
	if ( dim != 3 ) {
		mexErrMsgIdAndTxt( "MATLAB:isotropic_remeshing:vertex_dim",
				"Vertex coordinates must be 3D." );
	}

	// The target edge length of the final output mesh
	double target_edge_length = *mxGetPr( prhs[2] );

	// Check the sign of the target edge length
	if ( target_edge_length <= 0.0 ) {
		mexErrMsgIdAndTxt( "MATLAB:isotropic_remeshing:edge_sign",
				"Target edge length must be positive." );
	}

	// The number of iterations to be run
	int nb_iter = (int) *mxGetPr( prhs[3] );

	// Check the sign of the number of iterations
	if ( nb_iter < 1 ) {
		mexErrMsgIdAndTxt( "MATLAB:isotropic_remeshing:iter_sign",
				"Number of iterations must be positive." );
	}

	// Create and populate the polyhedral mesh ---------------------------------------------

	// Create vector of 3D point objects
	std::vector<Point> points;
	points.reserve( numVertex );
	for( int i = 0; i < numVertex; i++ ) {

		points.push_back( Point( vertex[i],
					 vertex[i+numVertex],
					 vertex[i+(2*numVertex)] ) );

	}

	// Create vector of polygon objects
	std::vector< std::vector<std::size_t> > polygons;
	polygons.reserve( numFaces );
	for( int i = 0; i < numFaces; i++ ) {

		std::vector<std::size_t> currentPolygon;
		currentPolygon.reserve( sizeFaces );
		for( int j = 0; j < sizeFaces; j++ ) {

			// NOTE: We subtract 1 from the index to account for
			// MATLAB's 1-indexed array structures
			double index = faces[i+(j*numFaces)]-1.0;
			currentPolygon.push_back( (std::size_t) index );

		}

		polygons.push_back( currentPolygon );

	}

	// Populate the mesh
	Mesh mesh;
	PMP::orient_polygon_soup( points, polygons );
	PMP::polygon_soup_to_polygon_mesh( points, polygons, mesh );

	// -------------------------------------------------------------------------------------
	// MESH PROCESSING
	// -------------------------------------------------------------------------------------

	// Split border edges
	std::cout << "Split border... ";

	std::vector<edge_descriptor> border;
	PMP::border_halfedges( mesh.faces(), mesh,
			boost::make_function_output_iterator(halfedge2edge(mesh,border)) );
	PMP::split_long_edges( border, target_edge_length, mesh );

	std::cout << "done." << std::endl;

	// Perform isotropic remeshing
	std::cout << "Start remeshing... ";

	PMP::isotropic_remeshing(
			mesh.faces(),
			target_edge_length,
			mesh,
			PMP::parameters::number_of_iterations(nb_iter)
			.protect_constraints(true) // i.e. protect border
			);

	std::cout << "done." << std::endl;

	// Collect any garbage that may have accumulated in the mesh
	if ( mesh.has_garbage() ) { mesh.collect_garbage(); }

	// Calculate normals
	auto fnormals = mesh.add_property_map<face_descriptor, Vector>
		("f:normals", CGAL::NULL_VECTOR).first;
	auto vnormals = mesh.add_property_map<vertex_descriptor, Vector>
		("v:normals", CGAL::NULL_VECTOR).first;

	PMP::compute_normals( mesh, vnormals, fnormals,
			PMP::parameters::vertex_point_map( mesh.points() ).
			geom_traits( Kernel() ) );

	// -------------------------------------------------------------------------------------
	// OUTPUT PROCESSING
	// -------------------------------------------------------------------------------------

	std::size_t numFacesFinal = mesh.number_of_faces(); // Final number of faces
	std::size_t numVertexFinal = mesh.number_of_vertices(); // Final number of vertices

	// I'm assuming the remeshing process keeps face size the same?
	plhs[0] = mxCreateDoubleMatrix( numFacesFinal, sizeFaces, mxREAL );
	double *facesOut = mxGetPr( plhs[0] );

	plhs[1] = mxCreateDoubleMatrix( numVertexFinal, 3, mxREAL );
	double *vertexOut = mxGetPr( plhs[1] );

	plhs[2] = mxCreateDoubleMatrix( numFacesFinal, 3, mxREAL );
	double *fnOut = mxGetPr( plhs[2] );

	plhs[3] = mxCreateDoubleMatrix( numVertexFinal, 3, mxREAL );
	double *vnOut = mxGetPr( plhs[3] );

	// Collect face quantities
	int i = 0;
	BOOST_FOREACH( Face_index f, mesh.faces() ) {

		// Iterate around the current face to find connectivity
		int j = 0;
		BOOST_FOREACH( Vertex_index v, vertices_around_face(mesh.halfedge(f),mesh) ) {

			// NOTE: We add 1 to the index to account for
			// MATLAB's 1-indexed array structures
			facesOut[i+(j*numFacesFinal)] = (double) v+1.0;
			j++;

		}

		// Face normal vector
		Vector fn = fnormals[f];

		fnOut[i] = fn[0];
		fnOut[i+numFacesFinal] = fn[1];
		fnOut[i+(2*numFacesFinal)] = fn[2];

		i++;

	}

	// Collect vertex quantities
	i = 0;
	BOOST_FOREACH( Vertex_index v, mesh.vertices() ) {

		// Vertex coordinates
		Point pp = mesh.point( v );

		vertexOut[i] = pp[0];
		vertexOut[i+numVertexFinal] = pp[1];
		vertexOut[i+(2*numVertexFinal)] = pp[2];

		// Vertex normal vector
		Vector vn = vnormals[v];

		vnOut[i] = vn[0];
		vnOut[i+numVertexFinal] = vn[1];
		vnOut[i+(2*numVertexFinal)] = vn[2];

		i++;

	}

	return;

};


#include <algorithm>
#include <cmath>
#include <cstdint>
#include <iostream>
#include <stdexcept>
#include <string>

#ifndef CPPDEBUG /* Ubuntu's Boost does not provide binaries compatible with libstdc++'s debug mode so we just reduce functionality here */
#include <boost/program_options.hpp>
#endif

#include "boost_profile.cpp"
#include "libff/algebra/curves/edwards/edwards_pp.hpp"
#include "libff/algebra/curves/alt_bn128/alt_bn128_pp.hpp"

#include "libiop/algebra/fields/utils.hpp"
#include "libiop/algebra/fft.hpp"
#include "libiop/algebra/fields/gf64.hpp"
#include "libiop/algebra/fields/gf128.hpp"
#include "libiop/algebra/fields/gf192.hpp"
#include "libiop/algebra/fields/gf256.hpp"
#include "libiop/algebra/field_subset/subgroup.hpp"

#include "libiop/common/common.hpp"
#include "libiop/iop/iop.hpp"
#include "libiop/protocols/ldt/fri/fri_ldt.hpp"
#include "libiop/snark/fri_snark.hpp"

#ifndef CPPDEBUG
bool process_prover_command_line(const int argc, const char** argv, options &options,
                                 std::size_t localization_parameter,
                                 std::size_t num_localization_steps,
                                 std::size_t num_oracles,
                                 std::size_t num_interactive_repetitions,
                                 std::size_t num_query_repetitions)
{
    namespace po = boost::program_options;

    try
    {
        po::options_description desc = gen_options(options);
        desc.add_options()
            ("localization_parameter", po::value<std::size_t>(&localization_parameter)->default_value(2), "Only used when num_localization_steps is 0")
            ("num_localization_steps", po::value<std::size_t>(&num_localization_steps)->default_value(0))
            ("num_oracles", po::value<std::size_t>(&num_oracles)->default_value(1))
            ("num_interactive_repetitions", po::value<std::size_t>(&num_interactive_repetitions)->default_value(1))
            ("num_query_repetitions", po::value<std::size_t>(&num_query_repetitions)->default_value(64));

        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc), vm);

        if (vm.count("help"))
        {
            std::cout << desc << "\n";
            return false;
        }

        po::notify(vm);
    }
    catch(std::exception& e)
    {
        std::cerr << "Error: " << e.what() << "\n";
        return false;
    }

    return true;
}
#endif

using namespace libiop;

template<typename FieldT, typename hash_type>
void instrument_FRI(options &options,
                    std::size_t localization_parameter,
                    std::size_t num_localization_steps,
                    std::size_t num_oracles,
                    std::size_t num_interactive_repetitions,
                    std::size_t num_query_repetitions)
{
    for (std::size_t log_n = options.log_n_min; log_n <= options.log_n_max; ++log_n)
    {
        print_separator();
        const std::size_t poly_degree_bound = 1ull << log_n;
        const std::size_t RS_extra_dimensions = 2; /* \rho = 2^{-RS_extra_dimensions} */
        const std::size_t codeword_domain_dim = log_n + RS_extra_dimensions;

        std::vector<std::size_t> localization_parameter_array;
        if (num_localization_steps != 0)
        {
            std::size_t remaining = codeword_domain_dim - RS_extra_dimensions - 1;
            std::size_t vals = remaining / num_localization_steps;
            localization_parameter_array = std::vector<std::size_t>(num_localization_steps, vals);
            localization_parameter_array.insert(localization_parameter_array.begin(), 1);
        }

        std::cout << "Codeword domain dimension: " << codeword_domain_dim << "\n"
                << "RS_extra_dimensions: " << RS_extra_dimensions << "\n"
                << "poly_degree_bound: " << poly_degree_bound << "\n"
                << "\n";

        const polynomial<FieldT> poly = polynomial<FieldT>::random_polynomial(poly_degree_bound);

        /* Set up the protocol blueprint */
        iop_protocol<FieldT> IOP;

        const std::size_t codeword_domain_size = 1ull << codeword_domain_dim;

        FRI_snark_parameters<FieldT> params;
        params.codeword_domain_dim_ = codeword_domain_dim;
        params.security_level_ = options.security_level;
        params.hash_enum_ = options.hash_enum;
        params.RS_extra_dimensions_ = RS_extra_dimensions;
        params.localization_parameter_array_ = localization_parameter_array;
        params.localization_parameter_ = localization_parameter;
        params.num_interactive_repetitions_ = num_interactive_repetitions;
        params.num_query_repetitions_ = num_query_repetitions;
        params.field_type_ = get_field_type<FieldT>(FieldT::zero());
        params.num_oracles_ = num_oracles;

        const FRI_snark_proof<FieldT, hash_type> proof = FRI_snark_prover<FieldT, hash_type>(params);
        printf("\n");
        print_indent(); printf("* Argument size in bytes (IOP): %zu\n", proof.IOP_size_in_bytes());
        print_indent(); printf("* Argument size in bytes (BCS): %zu\n", proof.BCS_size_in_bytes());
        print_indent(); printf("* Argument size in bytes (total): %zu\n", proof.size_in_bytes());

        printf("\nIf we were to remove the pruning of BCS merkle tree paths feature,\n"
               "the argument would have the following sizes:\n");
        print_indent(); printf("* Argument size in bytes (BCS, no pruning): %zu\n", proof.BCS_size_in_bytes_without_pruning());
        print_indent(); printf("* Argument size in bytes (total, no pruning): %zu\n", proof.size_in_bytes_without_pruning());
        printf("\n");

        const bool bit = FRI_snark_verifier<FieldT, hash_type>(proof, params);

        libiop::print_indent(); printf("* Verifier satisfied: %s\n", bit ? "true" : "false");
    }
}

int main(int argc, const char * argv[])
{
    options default_vals;

    std::size_t localization_parameter = 2;
    std::size_t num_localization_steps = 0;
    std::size_t num_oracles = 1;
    std::size_t num_interactive_repetitions = 1;
    std::size_t num_query_repetitions = 10;

#ifdef CPPDEBUG
    /* set reasonable defaults */
    if (argc > 1)
    {
        printf("There is no argument parsing in CPPDEBUG mode.");
        exit(1);
    }
    libiop::UNUSED(argv);

#else
    if (!process_prover_command_line(argc, argv, default_vals, localization_parameter,
                                     num_interactive_repetitions, num_query_repetitions,
                                     num_localization_steps, num_oracles))
    {
        return 1;
    }

#endif

    start_profiling();

    printf("Selected parameters:\n");
    printf("* log_n_min = %zu\n", default_vals.log_n_min);
    printf("* log_n_max = %zu\n", default_vals.log_n_max);
    printf("* security_level = %zu\n", default_vals.security_level);

    if (default_vals.is_multiplicative) {
        switch (default_vals.field_size) {
            case 181:
                edwards_pp::init_public_params();
                instrument_FRI<edwards_Fr, binary_hash_digest>(default_vals, localization_parameter,
                                     num_interactive_repetitions, num_query_repetitions,
                                     num_localization_steps, num_oracles);
                break;
            case 256:
                libff::alt_bn128_pp::init_public_params();
                instrument_FRI<alt_bn128_Fr, binary_hash_digest>(default_vals, localization_parameter,
                                     num_interactive_repetitions, num_query_repetitions,
                                     num_localization_steps, num_oracles);
                break;
            default:
                throw std::invalid_argument("Field size not supported.");
        }
    } else {
        switch (default_vals.field_size)
        {
            case 64:
                instrument_FRI<gf64, binary_hash_digest>(default_vals, localization_parameter,
                                     num_interactive_repetitions, num_query_repetitions,
                                     num_localization_steps, num_oracles);
                break;
            case 128:
                instrument_FRI<gf128, binary_hash_digest>(default_vals, localization_parameter,
                                     num_interactive_repetitions, num_query_repetitions,
                                     num_localization_steps, num_oracles);
                break;
            case 192:
                instrument_FRI<gf192, binary_hash_digest>(default_vals, localization_parameter,
                                     num_interactive_repetitions, num_query_repetitions,
                                     num_localization_steps, num_oracles);
                break;
            case 256:
                instrument_FRI<gf256, binary_hash_digest>(default_vals, localization_parameter,
                                     num_interactive_repetitions, num_query_repetitions,
                                     num_localization_steps, num_oracles);
                break;
            default:
                throw std::invalid_argument("Field size not supported.");
        }
    }
}


#ifndef PLANNER_H_INCLUDED
#define PLANNER_H_INCLUDED

#include <iostream>
#include <fstream>
#include <string>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>
#include "pinocchio/math/rpy.hpp"

#define N0_gait 20
// Number of rows in the gait matrix. Arbitrary value that should be set high enough so that there is always at
// least one empty line at the end of the gait matrix

typedef Eigen::MatrixXd matXd;

class TrajGen {
  /* Class that generates a reference trajectory in position, velocity and acceleration that feet it swing phase
     should follow */

 private:
  double lastCoeffs_x[6];  // Coefficients for the X component
  double lastCoeffs_y[6];  // Coefficients for the Y component
  double h = 0.05;  // Apex height of the swinging trajectory
  double time_adaptative_disabled = 0.2;  // Target lock before the touchdown
  double x1 = 0.0;  // Target for the X component
  double y1 = 0.0;  // Target for the Y component
  Eigen::Matrix<double, 11, 1> result = Eigen::Matrix<double, 11, 1>::Zero();  // Output of the generator

  // Coefficients
  double Ax5 = 0.0, Ax4 = 0.0, Ax3 = 0.0, Ax2 = 0.0, Ax1 = 0.0, Ax0 = 0.0, Ay5 = 0.0, Ay4 = 0.0, Ay3 = 0.0, Ay2 = 0.0,
         Ay1 = 0.0, Ay0 = 0.0, Az6 = 0.0, Az5 = 0.0, Az4 = 0.0, Az3 = 0.0;

 public:
  TrajGen();  // Empty constructor
  TrajGen(double h_in, double t_lock_in, double x_in, double y_in);  // Default constructor
  Eigen::Matrix<double, 11, 1> get_next_foot(double x0, double dx0, double ddx0, double y0, double dy0, double ddy0,
                                             double x1_in, double y1_in, double t0, double t1, double dt);
};

class Planner {
  /* Planner that outputs current and future locations of footsteps, the reference trajectory of the base and
     the position, velocity, acceleration commands for feet in swing phase based on the reference velocity given by
     the user and the current position/velocity of the base */

 private:
  // Inputs of the constructor
  double dt;       // Time step of the contact sequence (time step of the MPC)
  double dt_tsid;  // Time step of TSID
  double T_gait;   // Gait period
  double T_mpc;    // MPC period (prediction horizon)
  double h_ref;    // Reference height for the trunk
  int k_mpc;       // Number of TSID iterations for one iteration of the MPC
  bool on_solo8;   //  Whether we are working on solo8 or not

  // Predefined quantities
  double k_feedback = 0.03;  // Feedback gain for the feedback term of the planner
  double g = 9.81;           // Value of the gravity acceleartion
  double L = 0.155;          // Value of the maximum allowed deviation due to leg length
  bool is_static = false;    // Flag for static gait

  // Number of time steps in the prediction horizon
  int n_steps; // T_mpc / time step of the MPC

  // Feet index vector
  std::vector<int> feet;
  std::vector<double> t0s;
  double t_remaining = 0.0;
  double t_swing[4] = {0.0, 0.0, 0.0, 0.0};

  // Constant sized matrices
  Eigen::MatrixXd fsteps = Eigen::MatrixXd::Zero(N0_gait, 13);
  Eigen::Matrix<double, 3, 4> shoulders = Eigen::Matrix<double, 3, 4>::Zero();  // Position of shoulders in local frame
  Eigen::Matrix<double, 19, 1> q_static = Eigen::Matrix<double, 19, 1>::Zero();
  Eigen::Matrix<double, 3, 1> RPY_static = Eigen::Matrix<double, 3, 1>::Zero();
  Eigen::Matrix<double, 1, 12> o_feet_contact = Eigen::Matrix<double, 1, 12>::Zero();  // Feet matrix in world frame
  Eigen::Matrix<double, 3, 4> next_footstep =
      Eigen::Matrix<double, 3, 4>::Zero();  // To store the result of the compute_next_footstep function
  Eigen::Matrix<double, 3, 3> R =
      Eigen::Matrix<double, 3, 3>::Zero();  // Predefined matrices for compute_footstep function
  Eigen::Matrix<double, 3, 3> R_1 =
      Eigen::Matrix<double, 3, 3>::Zero();  // Predefined matrices for compute_next_footstep function
  Eigen::Matrix<double, 3, 3> R_2 = Eigen::Matrix<double, 3, 3>::Zero();
  Eigen::Matrix<double, N0_gait, 1> dt_cum = Eigen::Matrix<double, N0_gait, 1>::Zero();
  Eigen::Matrix<double, N0_gait, 1> angle = Eigen::Matrix<double, N0_gait, 1>::Zero();
  Eigen::Matrix<double, N0_gait, 1> dx = Eigen::Matrix<double, N0_gait, 1>::Zero();
  Eigen::Matrix<double, N0_gait, 1> dy = Eigen::Matrix<double, N0_gait, 1>::Zero();
  Eigen::Matrix<double, 3, 1> q_tmp = Eigen::Matrix<double, 3, 1>::Zero();
  Eigen::Matrix<double, 3, 1> q_dxdy = Eigen::Matrix<double, 3, 1>::Zero();
  Eigen::Matrix<double, 3, 1> RPY = Eigen::Matrix<double, 3, 1>::Zero();
  Eigen::Matrix<double, 3, 1> b_v_cur = Eigen::Matrix<double, 3, 1>::Zero();
  Eigen::Matrix<double, 6, 1> b_v_ref = Eigen::Matrix<double, 6, 1>::Zero();
  Eigen::Matrix<double, 3, 1> cross = Eigen::Matrix<double, 3, 1>::Zero();
  Eigen::Matrix<double, 6, 1> vref_in = Eigen::Matrix<double, 6, 1>::Zero();

  Eigen::Matrix<double, N0_gait, 5> gait_p = Eigen::Matrix<double, N0_gait, 5>::Zero();  // Past gait
  Eigen::MatrixXd gait_f = Eigen::MatrixXd::Zero(N0_gait, 5);                                // Current and future gait
  Eigen::Matrix<double, N0_gait, 5> gait_f_des = Eigen::Matrix<double, N0_gait, 5>::Zero();  // Future desired gait

  // Time interval vector
  Eigen::Matrix<double, 1, Eigen::Dynamic> dt_vector;

  // Reference trajectory matrix of size 12 by (1 + N)  with the current state of
  // the robot in column 0 and the N steps of the prediction horizon in the others
  Eigen::MatrixXd xref;

  // Foot trajectory generator
  double max_height_feet = 0.05;  // * (1000/312.5);  // height * correction coefficient
  double t_lock_before_touchdown = 0.07;
  std::vector<TrajGen> myTrajGen;

  // Variables for foot trajectory generator
  int i_end_gait = 0;
  Eigen::Matrix<double, 1, 4> t_stance =
      Eigen::Matrix<double, 1, 4>::Zero();  // Total duration of current stance phase for each foot
  Eigen::Matrix<double, 2, 4> footsteps_target = Eigen::Matrix<double, 2, 4>::Zero();
  Eigen::MatrixXd goals = Eigen::MatrixXd::Zero(3, 4);   // Store 3D target position for feet
  Eigen::MatrixXd vgoals = Eigen::MatrixXd::Zero(3, 4);  // Store 3D target velocity for feet
  Eigen::MatrixXd agoals = Eigen::MatrixXd::Zero(3, 4);  // Store 3D target acceleration for feet
  Eigen::Matrix<double, 6, 4> mgoals =
      Eigen::Matrix<double, 6, 4>::Zero();  // Storage variable for the trajectory generator

  Eigen::Matrix<double, 11, 4> res_gen = Eigen::Matrix<double, 11, 4>::Zero(); // Result of the generator

 public:
  Planner();
  Planner(double dt_in, double dt_tsid_in, double T_gait_in, double T_mpc_in, int k_mpc_in, bool on_solo8_in,
          double h_ref_in, const Eigen::MatrixXd &fsteps_in);

  void Print();

  int create_walk();
  int create_trot();
  int create_pacing();
  int create_bounding();
  int create_static();
  int create_gait_f();
  int roll(int k);
  int handle_joystick(int code, const Eigen::MatrixXd &q);
  int compute_footsteps(Eigen::MatrixXd q_cur, Eigen::MatrixXd v_cur, Eigen::MatrixXd v_ref);
  double get_stance_swing_duration(int i, int j, double value);
  int compute_next_footstep(int i, int j);
  int getRefStates(Eigen::MatrixXd q, Eigen::MatrixXd v, Eigen::MatrixXd vref, double z_average);
  int update_target_footsteps();
  int update_trajectory_generator(int k, double h_estim);
  int run_planner(int k, const Eigen::MatrixXd &q, const Eigen::MatrixXd &v, const Eigen::MatrixXd &b_vref,
                  double h_estim, double z_average, int joystick_code);

  // Accessors (to retrieve C data from Python)
  Eigen::MatrixXd get_xref();
  Eigen::MatrixXd get_fsteps();
  Eigen::MatrixXd get_gait();
  Eigen::MatrixXd get_goals();
  Eigen::MatrixXd get_vgoals();
  Eigen::MatrixXd get_agoals();
};

#endif  // PLANNER_H_INCLUDED


#include "driver/imu_driver.hpp"

#include <JetsonGPIO.h>
#include <device/imu.h>

#include <boost/bind.hpp>
#include <boost/function.hpp>

#include <sched.h>

#define BETA_IMU 0.75l // Change to IMU gyro error
#define SAMPLE_RATE 1e3l
#define DEG_TO_RAD 0.01745329251l

class Callback
{
public:
    typedef boost::function<void(const std::string &)> Handle;

    Callback(Handle fxn, const std::string &name)
        : fxn_(fxn), name_(name)
    {
    }

    Callback(const Callback &) = default;

    void operator()(const std::string &channel)
    {
        fxn_(channel);
    }

    bool operator==(const Callback &other) const
    {
        return name_ == other.name_;
    }

private:
    boost::function<void(const std::string &)> fxn_;
    std::string name_;
};

IMUDriver::IMUDriver(int interrupt_pin, int sda_pin, int scl_pin, int address)
    : Node("imu_driver"), imu_(new IMU(address, sda_pin, scl_pin)), data_(new DataIMU()), beta_(BETA_IMU), sample_freq_(SAMPLE_RATE)
{
    publisher_ = create_publisher<sensor_msgs::msg::Imu>("imu_data", 10);
    cb_ = new Callback(boost::bind(&IMUDriver::data_callback, this, _1), "data_callback");
    imu_->init();
    imu_->configure(INT_ENABLE, 0x01);
    imu_->configure(SMPLRT_DIV, 0x07);
    imu_->configure(INT_PIN_CFG, 0x00);
    imu_->setGyroRange(IMU::DEG250);
    imu_->setAccelRange(IMU::G2);

    q_.w = 1.0;
    q_.x = 0.0;
    q_.y = 0.0;
    q_.z = 0.0;

    GPIO::setmode(GPIO::BOARD);
    GPIO::setup(interrupt_pin, GPIO::IN);

    GPIO::add_event_detect(interrupt_pin, GPIO::RISING, *cb_, 1);
}

IMUDriver::~IMUDriver()
{
    delete data_;
    GPIO::cleanup();
}

void IMUDriver::data_callback(const std::string &channel)
{
    imu_->getData(data_);

    sensor_msgs::msg::Imu msg;
    msg.angular_velocity.x = data_->gyro().x() * DEG_TO_RAD;
    msg.angular_velocity.y = data_->gyro().y() * DEG_TO_RAD;
    msg.angular_velocity.z = data_->gyro().z() * DEG_TO_RAD;
    msg.linear_acceleration.x = data_->accel().x();
    msg.linear_acceleration.y = data_->accel().y();
    msg.linear_acceleration.z = data_->accel().z();

    madgwickAHRS();

    msg.header.stamp = get_clock()->now();

    msg.orientation = q_;

    publisher_->publish(msg);
}

// https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
void IMUDriver::madgwickAHRS()
{
    double gx = data_->gyro().x() * DEG_TO_RAD;
    double gy = data_->gyro().y() * DEG_TO_RAD;
    double gz = data_->gyro().z() * DEG_TO_RAD;
    double ax = data_->accel().x();
    double ay = data_->accel().y();
    double az = data_->accel().z();
    double _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
    double s0, s1, s2, s3;
    double recip_norm;
    Quaternion qDot;

    // Quaternion derivative using gyro data
    qDot.w = 0.5 * (-q_.x * gx - q_.y * gy - q_.z * gz);
    qDot.x = 0.5 * (q_.w * gx + q_.y * gz - q_.z * gy);
    qDot.y = 0.5 * (q_.w * gy - q_.x * gz + q_.z * gx);
    qDot.z = 0.5 * (q_.w * gz + q_.x * gy - q_.y * gx);

    // http://en.wikipedia.org/wiki/Fast_inverse_square_root
    auto invSqrt = [](const double &x)
    {
        double halfx = 0.5 * x;
        double y = x;
        long i = *(long *)&y;
        i = 0x5fe6eb50c7b537a9 - (i >> 1); // https://stackoverflow.com/questions/11644441/fast-inverse-square-root-on-x64
        y = *(double *)&i;
        y = y * (1.5 - (halfx * y * y));
        return y;
    };

    if (!((ax == 0.0) && (ay == 0.0) && (az == 0.0)))
    {

        // Normalize accelerometer data
        recip_norm = invSqrt(ax * ax + ay * ay + az * az);
        ax *= recip_norm;
        ay *= recip_norm;
        az *= recip_norm;

        // Constants
        _2q0 = 2.0 * q_.w;
        _2q1 = 2.0 * q_.x;
        _2q2 = 2.0 * q_.y;
        _2q3 = 2.0 * q_.z;
        _4q0 = 4.0 * q_.w;
        _4q1 = 4.0 * q_.x;
        _4q2 = 4.0 * q_.y;
        _8q1 = 8.0 * q_.x;
        _8q2 = 8.0 * q_.y;
        q0q0 = q_.w * q_.w;
        q1q1 = q_.x * q_.x;
        q2q2 = q_.y * q_.y;
        q3q3 = q_.z * q_.z;

        // Gradient decent
        s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
        s1 = _4q1 * q3q3 - _2q3 * ax + 4.0l * q0q0 * q_.x - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
        s2 = 4.0l * q0q0 * q_.y + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
        s3 = 4.0l * q1q1 * q_.z - _2q1 * ax + 4.0l * q2q2 * q_.z - _2q2 * ay;
        recip_norm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
        s0 *= recip_norm;
        s1 *= recip_norm;
        s2 *= recip_norm;
        s3 *= recip_norm;

        // Apply feedback
        qDot.w -= beta_ * s0;
        qDot.x -= beta_ * s1;
        qDot.y -= beta_ * s2;
        qDot.z -= beta_ * s3;
    }

    // Integrate quaternion derivative to get update
    q_.w += qDot.w * (1.0 / sample_freq_);
    q_.x += qDot.x * (1.0 / sample_freq_);
    q_.y += qDot.y * (1.0 / sample_freq_);
    q_.z += qDot.z * (1.0 / sample_freq_);

    // Normalize final quaternion
    recip_norm = invSqrt(q_.w * q_.w + q_.x * q_.x + q_.y * q_.y + q_.z * q_.z);
    q_.w *= recip_norm;
    q_.x *= recip_norm;
    q_.y *= recip_norm;
    q_.z *= recip_norm;
}


#include <opencv2/opencv.hpp>

#include <QString>
#include <QDir>
#include <QString>
#include <fstream>

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

#include "qrcode.h"
using namespace cv;
using namespace std;

#define CERR(x) std::cerr<<#x<<" = "<<(x)<<std::endl;//CERR(1<<2)

int test_qrcode_cv(cv::Mat src, std::vector<Point2f> &points) {
#if (CV_VERSION_MAJOR > 3)
    cv::QRCodeDetector qrdc;

    qrdc.detect(src, points);
    std::cerr<<points.size()<<std::endl;
    for (Point2f p : points)
    {
      std::cerr<<p.x<<" "<<p.y<<"\n";
    }
#endif
    return points.size();
}

int test_qrcode(cv::Mat src) {
    pc::QRCodeDetector qrdc;
    std::vector<Point2f> points;
    qrdc.detect(src, points);
    std::cerr<<points.size()<<std::endl;
    for (Point2f p : points)
    {
      std::cerr<<p.x<<" "<<p.y<<"\n";
    }
    return 0;
}

double computeReprojectionErrors(
        const vector<vector<Point3f> >& objectPoints,
        const vector<vector<Point2f> >& imagePoints,
        const vector<Mat>& rvecs, const vector<Mat>& tvecs,
        const Mat& cameraMatrix, const Mat& distCoeffs,
        vector<float>& perViewErrors )
{
    vector<Point2f> imagePoints2;
    int i, totalPoints = 0;
    double totalErr = 0, err;
    perViewErrors.resize(objectPoints.size());

    for( i = 0; i < (int)objectPoints.size(); i++ )
    {
        projectPoints(Mat(objectPoints[i]), rvecs[i], tvecs[i],
                      cameraMatrix, distCoeffs, imagePoints2);
        err = norm(Mat(imagePoints[i]), Mat(imagePoints2), NORM_L2);
        int n = (int)objectPoints[i].size();
        perViewErrors[i] = (float)std::sqrt(err*err/n);
        totalErr += err*err;
        totalPoints += n;
    }

    return std::sqrt(totalErr/totalPoints);
}

int main(int argc, char *argv[]) {
    string filename = "../QRCode-CV4/tys_data.txt";
    string line;
    ifstream myfile (filename.c_str());

    vector<array<float, 6>> allposes;
    if (myfile.is_open())
    {
        std::array<float, 6> pos;
        while(getline(myfile, line))
        {
            if (line.empty()) continue;
            stringstream ss;
            ss<<line;
            for (int i = 0; i<6; ++i){
                ss>>pos[i];
                if (i > 2) pos[i] = pos[i]/180*M_PI;
            }
            allposes.push_back(pos);
        }
        myfile.close();
    }
  string folderPath = "../tys-data3/tys-data2";

  vector<vector<Point2f>> pointslist;
  vector<array<float, 6>> goodposes;
  for (int i  = 1; i<= 18 ; ++i)
  {
//      system(("cp " + folderPath + "/"+std::to_string(i) +"/2.jpg " + "./robot" + std::to_string(i)
//             + ".jpg").c_str());
          std::string one_file = folderPath + "/"+std::to_string(i) +"/rgb_0.png";
          Mat image = imread(one_file);
          std::vector<Point2f> points;
          if (test_qrcode_cv(image, points) == 4) {
              pointslist.push_back(points);
              if (i<allposes.size()) goodposes.push_back(allposes[i]);
              std::cerr<<one_file<<std::endl;

          }
  }
  vector<vector<Point3f> > object_pointsL;
  for (auto x : pointslist)
  {
      float a = 0.5;
//      vector<Point3f> obj({Point3f(0,0,0), Point3f(1,0,0), Point3f(1,1,0), Point3f(0,1,0)});
      vector<Point3f> obj({Point3f(0,0,0), Point3f(0,a,0), Point3f(a,a,0), Point3f(a,0,0)});
      object_pointsL.push_back(obj);
  }

  Mat KL;
  Mat DL(5, 1, CV_32F);
  DL = 0.0;
  vector<Mat> rvecsL, tvecsL;
  int flagL = 0;
  if (1){
      flagL |= CALIB_ZERO_TANGENT_DIST;
      flagL |= CALIB_FIX_K3;
  }

  double errorL;
  errorL = calibrateCamera(object_pointsL, pointslist, Size(1920, 1080), KL, DL, rvecsL, tvecsL, flagL);
  CERR(errorL)
  vector<float> reprojErrsL;
  errorL = computeReprojectionErrors(object_pointsL, pointslist,
              rvecsL, tvecsL, KL, DL, reprojErrsL);

  CERR(KL);
  CERR(DL);
  for (auto x : reprojErrsL)
  {
      CERR(x);
  }

  int n = goodposes.size();

  auto abc2rot = [](array<float, 6> pos)->Eigen::Matrix3f
  {
      Eigen::Vector3f axis(pos[3], pos[4], pos[5]);
//      CERR(axis);
      float angle = axis.norm();
      axis = axis/angle;
//      return (Eigen::AngleAxisf(angle, axis)) .matrix();//mirror
      return (Eigen::AngleAxisf(pos[3], Eigen::Vector3f::UnitX())*
              Eigen::AngleAxisf(pos[4], Eigen::Vector3f::UnitY())*
              Eigen::AngleAxisf(pos[5], Eigen::Vector3f::UnitZ())) .matrix();
  };
  auto rvec2rot = [](Mat rvec)->Eigen::Matrix3f
  {
//      CERR(rvec);
      //why double? how to get scalar type?
      Eigen::Vector3f axis(rvec.at<double>(0,0), rvec.at<double>(1,0), rvec.at<double>(2,0));
//      CERR(axis);
      float angle = axis.norm();
      axis = axis/angle;
      return (Eigen::AngleAxisf(angle, axis)) .matrix();//mirror
  };
  for (int i = 0; i< n; ++i)//robot pose
  {
      Eigen::Matrix3f base2handi = abc2rot(goodposes[i]);
      Eigen::Matrix3f cam2pati = rvec2rot(rvecsL[i]);
      CERR(cam2pati)
      for (int j=0; j<n; ++j)// cam pose
      {
          if (i==j) continue;

          Eigen::Matrix3f base2handj = abc2rot(goodposes[j]);

          Eigen::Matrix3f cam2patj = rvec2rot(rvecsL[j]);

          Eigen::Matrix3f A = cam2patj*cam2pati.inverse();
          Eigen::Matrix3f B = base2handj.inverse()*base2handi;

          Eigen::AngleAxisf ax(A);
          Eigen::AngleAxisf bx(B);

          float x = ax.angle() - floor(ax.angle()/(2*M_PI))*2*M_PI - M_PI;
          float y = bx.angle() - floor(bx.angle()/(2*M_PI))*2*M_PI - M_PI;

          cerr<<i<<" "<<j<<" "<<((fabs(x) - fabs(y))/M_PI*180)<<endl;
      }
  }

    return 0;
}