/**
 * Created by beck on 24/4/2018.
 * Extended Kalman Filter for 16 states, with quaternion for the orientation
 * Fused A3 flight controller IMU with UWB x and y position
 * With IMU raw reading comes in 400Hz and UWB raw reading in 50Hz
 */
#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <uwb_msgs/uwb.h>
#include <Eigen/Eigen>
#include <queue>

// #define INIT_Q_R_BY_MEASUREMENT

using namespace std;
using namespace Eigen;
ros::Publisher odom_pub;
string imu_topic, uwb_topic, publisher_topic;
double uwb_weight, acc_angle_weight, mag_weight;
double acc_weight, gyro_weight, acc_bias_weight, gyro_bias_weight;

/**
 * Define states:
 *      x = [rotation_quaternion, position, velocity, bias_accel, bias_gyro]
 * Define inputs:
 *      u = [omg, accel], all imu measurement
 * Define noises:
 *      n = [n_gyro, n_acc, n_bias_acc, n_bias_gyro]
 */
VectorXd x(16);                             // state
MatrixXd P = MatrixXd::Zero(15, 15);        // covariance
MatrixXd Q = MatrixXd::Identity(12, 12);    // prediction noise covariance
MatrixXd R = MatrixXd::Identity(6, 6);      // observation noise covariance

// buffers to save imu and uwb reading for time synchronization
queue <sensor_msgs::Imu::ConstPtr> imu_buf;
queue <nav_msgs::Odometry::ConstPtr> odom_buf;
queue <Matrix<double, 16, 1>> x_history;
queue <Matrix<double, 15, 15>> P_history;

// previous propagated time
double t_prev;

// Initialization and covariance estimation
// UWB wait until imu is initialized
const int IMU_INIT_COUNT = 40;
int imu_count = 0;
Vector3d g_init = Vector3d::Zero();
Vector3d G = Vector3d::Zero();
Vector3d accl_init_buf[IMU_INIT_COUNT];
Vector3d gyro_init_buf[IMU_INIT_COUNT];
queue<double> uwb_init_buf_w;
queue<double> uwb_init_buf_x;
queue<double> uwb_init_buf_y;
double theta_bias = 0;
bool imu_initialized = false;
bool odom_initialized = false;

// TODO: Calibrate sensor position
// imu frame to world frame rotation matrix, {[0, 0, -1], pi/2}
//Matrix3d imu_R_world = Quaterniond(sqrt(1/2), 0, 0, -sqrt(1/2)).toRotationMatrix();
Matrix3d imu_R_world;

// Matrix3d init_robot_pose = Quaterniond(sqrt(1/2), 0, 0, -sqrt(1/2)).toRotationMatrix();
// Matrix3d init_robot_pose = Quaterniond(0, 0, 0, 1).toRotationMatrix();

void pub_odom_ekf(std_msgs::Header header) {
    nav_msgs::Odometry odom;
    odom.header.stamp = header.stamp;
    odom.header.frame_id = "world";
    odom.pose.pose.orientation.w = x(0);
    odom.pose.pose.orientation.x = x(1);
    odom.pose.pose.orientation.y = x(2);
    odom.pose.pose.orientation.z = x(3);
    odom.pose.pose.position.x = x(4);
    odom.pose.pose.position.y = x(5);
    odom.pose.pose.position.z = x(6);
    odom.twist.twist.linear.x = x(7);
    odom.twist.twist.linear.y = x(8);
    odom.twist.twist.linear.z = x(9);

    odom_pub.publish(odom);
}

// imu propagate in the world frame
void propagate(const sensor_msgs::Imu::ConstPtr &imu_msg) {
    double cur_t = imu_msg->header.stamp.toSec();
    double dt = cur_t - t_prev;
    Vector3d w_raw;
    Vector3d a_raw;
    a_raw(0) = imu_msg->linear_acceleration.x;
    a_raw(1) = imu_msg->linear_acceleration.y;
    a_raw(2) = imu_msg->linear_acceleration.z;
    w_raw(0) = imu_msg->angular_velocity.x;
    w_raw(1) = imu_msg->angular_velocity.y;
    w_raw(2) = imu_msg->angular_velocity.z;

    Vector3d a = a_raw - x.segment<3>(10);
    Vector3d omg = w_raw - x.segment<3>(13);
    Vector3d domg = 0.5 * dt * omg;

    // propagate the state with quaternion calculus
    Quaterniond dR(sqrt(1 - domg.squaredNorm()), domg(0), domg(1), domg(2));
    Quaterniond Rt(x(0), x(1), x(2), x(3));
    Quaterniond R_t = (Rt * dR).normalized();

    x.segment<4>(0) << R_t.w(), R_t.x(), R_t.y(), R_t.z();
    x.segment<3>(4) += x.segment<3>(7) * dt + (Rt * (a - G)) * 0.5 * dt * dt;
    x.segment<3>(7) += (Rt * (a - G)) * dt;

    // propagate the covariance with skew-symmetric matrix
    MatrixXd I = MatrixXd::Identity(3, 3);
    Matrix3d R_omg, R_a;
    R_omg << 0, -omg(2), omg(1),
            omg(2), 0, -omg(0),
            -omg(1), omg(0), 0;
    R_a << 0, -a(2), a(1),
            a(2), 0, -a(0),
            -a(1), a(0), 0;

    MatrixXd A = MatrixXd::Zero(15, 15);
    A.block<3, 3>(0, 0) = -R_omg;
    A.block<3, 3>(0, 12)= -1 * I;
    A.block<3, 3>(3, 6) = I;
    A.block<3, 3>(6, 0) = (-1 * Rt.toRotationMatrix()) * R_a;
    A.block<3, 3>(6, 9) = (-1 * Rt.toRotationMatrix());
    // cout << "DEBUG:: propagate A" << endl << A << endl;

    MatrixXd U = MatrixXd::Zero(15, 12);
    U.block<3, 3>(0, 0) = -1 * I;
    U.block<3, 3>(6, 3) = -1 * Rt.toRotationMatrix();
    U.block<3, 3>(9, 6) = I;
    U.block<3, 3>(12, 9)= I;
    // cout << "DEBUG:: propagate U" << endl << U << endl;

    MatrixXd F, V;
    F = MatrixXd::Identity(15, 15) + dt * A;
    V = dt * U;

    P = F * P * F.transpose() + V * Q * V.transpose();
    // cout << "DEBUG:: P after propagate" << endl << P << endl;

    t_prev = cur_t;
}

// Loosely coupled update in world frame, fusing the global position of UWB and angle
void update_loosely(const uwb_msgs::uwb &msg,
                    const sensor_msgs::Imu::ConstPtr &imu_msg) {
    Quaterniond q_g(1, 0, 0, 0);
/*
    if (imu_msg != nullptr && &imu_msg != NULL) {
        Vector3d a_raw(imu_msg->linear_acceleration.x,
                       imu_msg->linear_acceleration.y,
                       imu_msg->linear_acceleration.z);
        Vector3d a_norm = a_raw.normalized();
        // cout << "DEBUG:: raw accel" << endl << a_norm << endl;
        Vector3d q_vec(a_norm(1), -a_norm(0), 0);
        Vector3d q_norm = q_vec.normalized();
        q_g.w() = acos(a_norm(2));
        q_g.x() = q_norm(0);
        q_g.y() = q_norm(1);
        q_g.z() = q_norm(2);
    }
*/

    double theta_z = msg.pos_theta - theta_bias;
    VectorXd T(3);
    T(0) = msg.pos_x;
    T(1) = msg.pos_y;
    T(2) = 0;

    MatrixXd C = MatrixXd::Zero(6, 15);
    C.block<3, 3>(0, 0) = Matrix3d::Identity();
//    C(2, 2) = 1;
    C.block<3, 3>(3, 3) = Matrix3d::Identity();

    MatrixXd K(15, 6);
    K = P * C.transpose() * (C * P * C.transpose() + R).inverse();
//    cout << "DEBUG:: update K" << endl << K << endl;

    // Matrix3d uwb_R_world = AngleAxisd(theta_z, Vector3d::UnitZ()) * imu_R_world * q_g.toRotationMatrix();
	Matrix3d uwb_R_world = AngleAxisd(theta_z, Vector3d::UnitZ()) * q_g.toRotationMatrix();
//    cout << "DEBUG:: measured angle" << endl << uwb_R_world << endl;

    VectorXd r(6);
    Quaterniond qm(uwb_R_world);
    Quaterniond q = Quaterniond(x(0), x(1), x(2), x(3));
    Quaterniond dq = q.conjugate() * qm; // Hamilton style
    r.head(3) = 2 * dq.vec();
    r.tail(3) = T - x.segment<3>(4);
    VectorXd _r = K * r;
    Vector3d dw(0.5 * _r(0), 0.5 * _r(1), 0.5 * _r(2));
    Quaterniond _dq = Quaterniond(1, dw(0), dw(1), dw(2)).normalized();
    q = q * _dq;

    x(0) = q.w();
    x(1) = q.x();
    x(2) = q.y();
    x(3) = q.z();
    x.segment<12>(4) += _r.segment<12>(3);
    P = P - K * C * P;
//    cout << "DEBUG:: P after update" << endl << P << endl;
}

/**
 * Initialize, handle and save imu messages
 * @param imu_msg
 */
void imu_callback(const sensor_msgs::Imu::ConstPtr &imu_msg) {
    if (!imu_initialized && imu_count < IMU_INIT_COUNT) {
        accl_init_buf[imu_count](0) = imu_msg->linear_acceleration.x;
        accl_init_buf[imu_count](1) = imu_msg->linear_acceleration.y;
        accl_init_buf[imu_count](2) = imu_msg->linear_acceleration.z;
        gyro_init_buf[imu_count](0) = imu_msg->angular_velocity.x;
        gyro_init_buf[imu_count](1) = imu_msg->angular_velocity.y;
        gyro_init_buf[imu_count](2) = imu_msg->angular_velocity.z;
        g_init(0) += accl_init_buf[imu_count](0);
        g_init(1) += accl_init_buf[imu_count](1);
        g_init(2) += accl_init_buf[imu_count](2);
        imu_count++;
    } else if (!imu_initialized && imu_count == IMU_INIT_COUNT) {
#ifdef INIT_Q_R_BY_MEASUREMENT
        double accl_mean[3] = {0};
        double gyro_mean[3] = {0};
        double accl_cova[3] = {0};
        double gyro_cova[3] = {0};
        for (int i = 0; i < IMU_INIT_COUNT; ++i) {
            for (int j = 0; j < 3; ++j) {
                accl_mean[j] += accl_init_buf[i](j);
                gyro_mean[j] += gyro_init_buf[i](j);
                accl_cova[j] += pow(accl_init_buf[i](j), 2);
                gyro_cova[j] += pow(gyro_init_buf[i](j), 2);
            }
        }
        for (int j = 0; j < 3; ++j) {
            accl_mean[j] /= IMU_INIT_COUNT;
            gyro_mean[j] /= IMU_INIT_COUNT;
            accl_cova[j] /= IMU_INIT_COUNT;
            gyro_cova[j] /= IMU_INIT_COUNT;

            // Q omg first, Q acceleration second
            Q(j, j)         = gyro_cova[j] - pow(gyro_mean[j], 2);
            Q(j + 3, j + 3) = accl_cova[j] - pow(accl_mean[j], 2);
        }
        // Add the ratio
        Q.block<3, 3>(0, 0) = gyro_weight * Q.block<3, 3>(0, 0);
        Q.block<3, 3>(3, 3) = acc_weight  * Q.block<3, 3>(3, 3);

        // Hardcoded initial IMU bias_a, bias_g
        Q.block<3, 3>(6, 6) = acc_bias_weight  * Q.block<3, 3>(6, 6);     // IMU bias_a, bias_g
        Q.block<3, 3>(9, 9) = gyro_bias_weight * Q.block<3, 3>(9, 9);     // IMU bias_a, bias_g
        cout << "DEBUG: measured Q" << endl << Q << endl;
#else
        // Initialize the covariance
        Q.block<3, 3>(0, 0) = gyro_weight * Q.block<3, 3>(0, 0);     // IMU omg, accel
        Q.block<3, 3>(3, 3) = acc_weight  * Q.block<3, 3>(3, 3);     // IMU omg, accel
        Q.block<3, 3>(6, 6) = acc_bias_weight  * Q.block<3, 3>(6, 6);     // IMU bias_a, bias_g
        Q.block<3, 3>(9, 9) = gyro_bias_weight * Q.block<3, 3>(9, 9);     // IMU bias_a, bias_g
        cout << "DEBUG: hardcoded Q" << endl << Q << endl;
#endif
        g_init /= IMU_INIT_COUNT;

        // Initialize the state and the gravity
        x.setZero();
        // Quaterniond init_pose(init_robot_pose);
        Quaterniond init_pose(imu_R_world);
        x(0) = init_pose.w();
        x(1) = init_pose.x();
        x(2) = init_pose.y();
        x(3) = init_pose.z();

        G = imu_R_world * g_init;
        cout << "DEBUG: init state x" << x(0) << x(1) << x(2) << x(3) << endl;
		cout << "DEBUG: measured G" << endl << G << endl;
        t_prev = imu_msg->header.stamp.toSec();
        imu_initialized = true;
    } else if (imu_initialized && odom_initialized) {
        propagate(imu_msg);
        pub_odom_ekf(imu_msg->header);

        imu_buf.push(imu_msg);
        x_history.push(x);
        P_history.push(P);
    }
}

/**
 * initialize, handle and save uwb messages
 * Also doing time synchronization
 * @param uwb msg
 */
void odom_callback(const uwb_msgs::uwb &msg) {
    if (!imu_initialized && !odom_initialized) {
        uwb_init_buf_x.push(msg.pos_x);
        uwb_init_buf_y.push(msg.pos_y);
        uwb_init_buf_w.push(msg.pos_theta);
    } else if (imu_initialized && !odom_initialized) {
#ifdef INIT_Q_R_BY_MEASUREMENT
        double uwb_mean[3] = {0, 0, 0};
        double uwb_cova[3] = {0, 0, 0};
        int uwb_count = (int) uwb_init_buf_x.size();
        for (int i = 0; i < uwb_count; ++i) {
            uwb_mean[0] += uwb_init_buf_x.front();
            uwb_mean[1] += uwb_init_buf_y.front();
            uwb_mean[2] += uwb_init_buf_w.front();
            uwb_cova[0] += pow(uwb_init_buf_x.front(), 2);
            uwb_cova[1] += pow(uwb_init_buf_y.front(), 2);
            uwb_cova[2] += pow(uwb_init_buf_w.front(), 2);
            uwb_init_buf_x.pop();
            uwb_init_buf_y.pop();
            uwb_init_buf_w.pop();
        }
        uwb_mean[0] /= uwb_count;
        uwb_mean[1] /= uwb_count;
        uwb_mean[2] /= uwb_count;
        uwb_cova[0] /= uwb_count;
        uwb_cova[1] /= uwb_count;
        uwb_cova[2] /= uwb_count;

        // Initialize the position and bias
        x(4) = uwb_mean[0];
        x(5) = uwb_mean[1];
        x(6) = 0;
        theta_bias = uwb_mean[2];

        // Initialize the covariance R
        R.topLeftCorner(2, 2) = acc_angle_weight * R.topLeftCorner(2, 2);
        R(2, 2) = (uwb_cova[2] - pow(uwb_mean[2], 2)) * mag_weight;
        R(3, 3) = (uwb_cova[0] - pow(uwb_mean[0], 2)) * uwb_weight;
        R(4, 4) = (uwb_cova[1] - pow(uwb_mean[1], 2)) * uwb_weight;
        R(5, 5) = 0.01 * R(5, 5) * uwb_weight;
        cout << "DEBUG: measured R" << endl << R << endl;
#else
        x(4) = msg.pos_x;
        x(5) = msg.pos_y;
        x(6) = 0;

		x(0) = 1;
		x(1) = 0;
		x(2) = 0;
		x(3) = 0;

        theta_bias = msg.pos_theta;
        R.topLeftCorner(2, 2) = acc_angle_weight * R.topLeftCorner(2, 2);
        R(2, 2) = mag_weight * R(2, 2);
        R.bottomRightCorner(3, 3) = uwb_weight * R.bottomRightCorner(3, 3); // Measure x, y
        cout << "DEBUG: hardcoded R" << endl << R << endl;
#endif
        t_prev = msg.header.stamp.toSec();
        odom_initialized = true;
    } else {
        // throw the state and covariance history before the uwb time
        while (!imu_buf.empty() && imu_buf.front()->header.stamp < msg.header.stamp) {
            // trace the time backwards to imu time
            t_prev = imu_buf.front()->header.stamp.toSec();
//            ROS_INFO("throw state with time: %f", t_prev);
            imu_buf.pop();
            x_history.pop();
            P_history.pop();
        }
        // If x_history is empty then the uwb reading is the same as the last imu reading
        // And the current estimated x could be used.
        // If not, use the oldest time in the x_history
        if (!x_history.empty()) {
            x = x_history.front();
            P = P_history.front();
            t_prev = imu_buf.front()->header.stamp.toSec();
            imu_buf.pop();
            x_history.pop();
            P_history.pop();
        }

//        ROS_INFO("update state with time: %f", msg.header.stamp.toSec());
        if (imu_buf.empty()){
            update_loosely(msg, NULL);
        } else {
            update_loosely(msg, imu_buf.front());
        }

        // clean the x and P history since the new update corrects the previous propagate
        while (!x_history.empty()) x_history.pop();
        while (!P_history.empty()) P_history.pop();

        queue <sensor_msgs::Imu::ConstPtr> temp_imu_buf;
        while (!imu_buf.empty()) {
//            ROS_INFO("propagate state with time: %f", imu_buf.front()->header.stamp.toSec());
            propagate(imu_buf.front());
            temp_imu_buf.push(imu_buf.front());
            x_history.push(x);
            P_history.push(P);
            imu_buf.pop();
        }
        std::swap(imu_buf, temp_imu_buf);
    }
}

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

    // sleep for 10 seconds in order to launch both sensors
    ros::Duration(10).sleep();

    n.param("imu_topic", imu_topic, string("/dji_sdk/imu"));
    n.param("uwb_topic", uwb_topic, string("/uwb_driver/info"));
    n.param("publisher_topic", publisher_topic, string("/ekf_odom"));
    n.param("uwb_weight", uwb_weight, 0.01);
    n.param("acc_angle_weight", acc_angle_weight, 0.01);
    n.param("magnetometer_weight" , mag_weight, 0.01);
    n.param("accelerometer_weight", acc_weight, 0.01);
    n.param("gyroscope_weight", gyro_weight, 0.01);
    n.param("acc_bias_weight" , acc_bias_weight, 0.01);
    n.param("gyro_bias_weight", gyro_bias_weight, 0.01);

    ros::Subscriber s1 = n.subscribe(imu_topic, 100, imu_callback);
    ros::Subscriber s2 = n.subscribe(uwb_topic, 10, odom_callback);
    odom_pub = n.advertise<nav_msgs::Odometry>(publisher_topic, 100);

    // Running the odometry in 400Hz as the IMU update
    ros::Rate r(400);

//    Rimu = Quaterniond(0.7071, 0, 0, -0.7071).toRotationMatrix();
    imu_R_world << 1, 0, 0,
            0, 1,  0,
            0, 0,  1;

    cout << "imu_R_world" << endl << imu_R_world << endl;

    ros::spin();
}

/**
 *  0   q_w     body frame --> world frame
 *  1   q_x
 *  2   q_y
 *  3   q_z
 *  4   p_x     world frame
 *  5   p_y
 *  6   p_z
 *  7   v_x     world frame
 *  8   v_y
 *  9   v_z
 *  10  ba_x    accel_bias      body frame
 *  11  ba_y
 *  12  ba_z
 *  13  bw_x    gyro_bias       body frame
 *  14  bw_y
 *  15  bw_z
 */


///////////////////////////////////////////////////////////////////////////////////////////////////
// OpenGL Mathematics Copyright (c) 2005 - 2013 G-Truc Creation (www.g-truc.net)
///////////////////////////////////////////////////////////////////////////////////////////////////
// Created : 2011-01-15
// Updated : 2011-09-13
// Licence : This source is under MIT licence
// File    : test/core/func_common.cpp
///////////////////////////////////////////////////////////////////////////////////////////////////

//#include <boost/array.hpp>
//#include <boost/date_time/posix_time/posix_time.hpp>
//#include <boost/thread/thread.hpp>
#include <glm/gtc/constants.hpp>
#include <glm/gtc/epsilon.hpp>
#include <cstdio>
#include <cmath>

int test_modf()
{
	int Error(0);

	{
		float X(1.5f);
		float I(0.0f);
		float A = glm::modf(X, I);

		Error += I == 1.0f ? 0 : 1;
		Error += A == 0.5f ? 0 : 1;
	}

	{
		glm::vec4 X(1.1f, 1.2f, 1.5f, 1.7f);
		glm::vec4 I(0.0f);
		glm::vec4 A = glm::modf(X, I);

		Error += I == glm::vec4(1.0f) ? 0 : 1;
		Error += glm::all(glm::epsilonEqual(A, glm::vec4(0.1f, 0.2f, 0.5f, 0.7f), 0.00001f)) ? 0 : 1;
	}

	{
		glm::dvec4 X(1.1, 1.2, 1.5, 1.7);
		glm::dvec4 I(0.0);
		glm::dvec4 A = glm::modf(X, I);

		Error += I == glm::dvec4(1.0) ? 0 : 1;
		Error += glm::all(glm::epsilonEqual(A, glm::dvec4(0.1, 0.2, 0.5, 0.7), 0.000000001)) ? 0 : 1;
	}

	{
		double X(1.5);
		double I(0.0);
		double A = glm::modf(X, I);

		Error += I == 1.0 ? 0 : 1;
		Error += A == 0.5 ? 0 : 1;
	}

	return Error;
}

int test_floatBitsToInt()
{
	int Error = 0;

	{
		float A = 1.0f;
		int B = glm::floatBitsToInt(A);
		float C = glm::intBitsToFloat(B);
		int D = *(int*)&A;
		Error += B == D ? 0 : 1;
		Error += A == C ? 0 : 1;
	}

	{
		glm::vec2 A(1.0f, 2.0f);
		glm::ivec2 B = glm::floatBitsToInt(A);
		glm::vec2 C = glm::intBitsToFloat(B);
		Error += B.x == *(int*)&(A.x) ? 0 : 1;
		Error += B.y == *(int*)&(A.y) ? 0 : 1;
		Error += A == C? 0 : 1;
	}

	{
		glm::vec3 A(1.0f, 2.0f, 3.0f);
		glm::ivec3 B = glm::floatBitsToInt(A);
		glm::vec3 C = glm::intBitsToFloat(B);
		Error += B.x == *(int*)&(A.x) ? 0 : 1;
		Error += B.y == *(int*)&(A.y) ? 0 : 1;
		Error += B.z == *(int*)&(A.z) ? 0 : 1;
		Error += A == C? 0 : 1;
	}

	{
		glm::vec4 A(1.0f, 2.0f, 3.0f, 4.0f);
		glm::ivec4 B = glm::floatBitsToInt(A);
		glm::vec4 C = glm::intBitsToFloat(B);
		Error += B.x == *(int*)&(A.x) ? 0 : 1;
		Error += B.y == *(int*)&(A.y) ? 0 : 1;
		Error += B.z == *(int*)&(A.z) ? 0 : 1;
		Error += B.w == *(int*)&(A.w) ? 0 : 1;
		Error += A == C? 0 : 1;
	}

	return Error;
}

int test_floatBitsToUint()
{
	int Error = 0;

	{
		float A = 1.0f;
		glm::uint B = glm::floatBitsToUint(A);
		float C = glm::intBitsToFloat(B);
		Error += B == *(glm::uint*)&A ? 0 : 1;
		Error += A == C? 0 : 1;
	}

	{
		glm::vec2 A(1.0f, 2.0f);
		glm::uvec2 B = glm::floatBitsToUint(A);
		glm::vec2 C = glm::uintBitsToFloat(B);
		Error += B.x == *(glm::uint*)&(A.x) ? 0 : 1;
		Error += B.y == *(glm::uint*)&(A.y) ? 0 : 1;
		Error += A == C ? 0 : 1;
	}

	{
		glm::vec3 A(1.0f, 2.0f, 3.0f);
		glm::uvec3 B = glm::floatBitsToUint(A);
		glm::vec3 C = glm::uintBitsToFloat(B);
		Error += B.x == *(glm::uint*)&(A.x) ? 0 : 1;
		Error += B.y == *(glm::uint*)&(A.y) ? 0 : 1;
		Error += B.z == *(glm::uint*)&(A.z) ? 0 : 1;
		Error += A == C? 0 : 1;
	}

	{
		glm::vec4 A(1.0f, 2.0f, 3.0f, 4.0f);
		glm::uvec4 B = glm::floatBitsToUint(A);
		glm::vec4 C = glm::uintBitsToFloat(B);
		Error += B.x == *(glm::uint*)&(A.x) ? 0 : 1;
		Error += B.y == *(glm::uint*)&(A.y) ? 0 : 1;
		Error += B.z == *(glm::uint*)&(A.z) ? 0 : 1;
		Error += B.w == *(glm::uint*)&(A.w) ? 0 : 1;
		Error += A == C? 0 : 1;
	}

	return Error;
}

namespace test_mix
{
	template <typename T, typename B>
	struct test
	{
		T x;
		T y;
		B a;
		T Result;
	};

	test<float, bool> TestBool[] =
	{
		{0.0f, 1.0f, false, 0.0f},
		{0.0f, 1.0f, true, 1.0f},
		{-1.0f, 1.0f, false, -1.0f},
		{-1.0f, 1.0f, true, 1.0f}
	};

	test<float, float> TestFloat[] =
	{
		{0.0f, 1.0f, 0.0f, 0.0f},
		{0.0f, 1.0f, 1.0f, 1.0f},
		{-1.0f, 1.0f, 0.0f, -1.0f},
		{-1.0f, 1.0f, 1.0f, 1.0f}
	};

	test<glm::vec2, bool> TestVec2Bool[] =
	{
		{glm::vec2(0.0f), glm::vec2(1.0f), false, glm::vec2(0.0f)},
		{glm::vec2(0.0f), glm::vec2(1.0f), true, glm::vec2(1.0f)},
		{glm::vec2(-1.0f), glm::vec2(1.0f), false, glm::vec2(-1.0f)},
		{glm::vec2(-1.0f), glm::vec2(1.0f), true, glm::vec2(1.0f)}
	};

	test<glm::vec2, glm::bvec2> TestBVec2[] =
	{
		{glm::vec2(0.0f), glm::vec2(1.0f), glm::bvec2(false), glm::vec2(0.0f)},
		{glm::vec2(0.0f), glm::vec2(1.0f), glm::bvec2(true), glm::vec2(1.0f)},
		{glm::vec2(-1.0f), glm::vec2(1.0f), glm::bvec2(false), glm::vec2(-1.0f)},
		{glm::vec2(-1.0f), glm::vec2(1.0f), glm::bvec2(true), glm::vec2(1.0f)},
		{glm::vec2(-1.0f), glm::vec2(1.0f), glm::bvec2(true, false), glm::vec2(1.0f, -1.0f)}
	};

	test<glm::vec3, bool> TestVec3Bool[] =
	{
		{glm::vec3(0.0f), glm::vec3(1.0f), false, glm::vec3(0.0f)},
		{glm::vec3(0.0f), glm::vec3(1.0f), true, glm::vec3(1.0f)},
		{glm::vec3(-1.0f), glm::vec3(1.0f), false, glm::vec3(-1.0f)},
		{glm::vec3(-1.0f), glm::vec3(1.0f), true, glm::vec3(1.0f)}
	};

	test<glm::vec3, glm::bvec3> TestBVec3[] =
	{
		{glm::vec3(0.0f), glm::vec3(1.0f), glm::bvec3(false), glm::vec3(0.0f)},
		{glm::vec3(0.0f), glm::vec3(1.0f), glm::bvec3(true), glm::vec3(1.0f)},
		{glm::vec3(-1.0f), glm::vec3(1.0f), glm::bvec3(false), glm::vec3(-1.0f)},
		{glm::vec3(-1.0f), glm::vec3(1.0f), glm::bvec3(true), glm::vec3(1.0f)},
		{glm::vec3(1.0f, 2.0f, 3.0f), glm::vec3(4.0f, 5.0f, 6.0f), glm::bvec3(true, false, true), glm::vec3(4.0f, 2.0f, 6.0f)}
	};

	test<glm::vec4, bool> TestVec4Bool[] =
	{
		{glm::vec4(0.0f), glm::vec4(1.0f), false, glm::vec4(0.0f)},
		{glm::vec4(0.0f), glm::vec4(1.0f), true, glm::vec4(1.0f)},
		{glm::vec4(-1.0f), glm::vec4(1.0f), false, glm::vec4(-1.0f)},
		{glm::vec4(-1.0f), glm::vec4(1.0f), true, glm::vec4(1.0f)}
	};

	test<glm::vec4, glm::bvec4> TestBVec4[] =
	{
		{glm::vec4(0.0f), glm::vec4(1.0f), glm::bvec4(false), glm::vec4(0.0f)},
		{glm::vec4(0.0f), glm::vec4(1.0f), glm::bvec4(true), glm::vec4(1.0f)},
		{glm::vec4(-1.0f), glm::vec4(1.0f), glm::bvec4(false), glm::vec4(-1.0f)},
		{glm::vec4(-1.0f), glm::vec4(1.0f), glm::bvec4(true), glm::vec4(1.0f)},
		{glm::vec4(1.0f, 2.0f, 3.0f, 4.0f), glm::vec4(5.0f, 6.0f, 7.0f, 8.0f), glm::bvec4(true, false, true, false), glm::vec4(5.0f, 2.0f, 7.0f, 4.0f)}
	};

	int run()
	{
		int Error = 0;

		// Float with bool
		{
			for(std::size_t i = 0; i < sizeof(TestBool) / sizeof(test<float, bool>); ++i)
			{
				float Result = glm::mix(TestBool[i].x, TestBool[i].y, TestBool[i].a);
				Error += glm::epsilonEqual(Result, TestBool[i].Result, glm::epsilon<float>()) ? 0 : 1;
			}
		}

		// Float with float
		{
			for(std::size_t i = 0; i < sizeof(TestFloat) / sizeof(test<float, float>); ++i)
			{
				float Result = glm::mix(TestFloat[i].x, TestFloat[i].y, TestFloat[i].a);
				Error += glm::epsilonEqual(Result, TestFloat[i].Result, glm::epsilon<float>()) ? 0 : 1;
			}
		}

		// vec2 with bool
		{
			for(std::size_t i = 0; i < sizeof(TestVec2Bool) / sizeof(test<glm::vec2, bool>); ++i)
			{
				glm::vec2 Result = glm::mix(TestVec2Bool[i].x, TestVec2Bool[i].y, TestVec2Bool[i].a);
				Error += glm::epsilonEqual(Result.x, TestVec2Bool[i].Result.x, glm::epsilon<float>()) ? 0 : 1;
				Error += glm::epsilonEqual(Result.y, TestVec2Bool[i].Result.y, glm::epsilon<float>()) ? 0 : 1;
			}
		}

		// vec2 with bvec2
		{
			for(std::size_t i = 0; i < sizeof(TestBVec2) / sizeof(test<glm::vec2, glm::bvec2>); ++i)
			{
				glm::vec2 Result = glm::mix(TestBVec2[i].x, TestBVec2[i].y, TestBVec2[i].a);
				Error += glm::epsilonEqual(Result.x, TestBVec2[i].Result.x, glm::epsilon<float>()) ? 0 : 1;
				Error += glm::epsilonEqual(Result.y, TestBVec2[i].Result.y, glm::epsilon<float>()) ? 0 : 1;
			}
		}

		// vec3 with bool
		{
			for(std::size_t i = 0; i < sizeof(TestVec3Bool) / sizeof(test<glm::vec3, bool>); ++i)
			{
				glm::vec3 Result = glm::mix(TestVec3Bool[i].x, TestVec3Bool[i].y, TestVec3Bool[i].a);
				Error += glm::epsilonEqual(Result.x, TestVec3Bool[i].Result.x, glm::epsilon<float>()) ? 0 : 1;
				Error += glm::epsilonEqual(Result.y, TestVec3Bool[i].Result.y, glm::epsilon<float>()) ? 0 : 1;
				Error += glm::epsilonEqual(Result.z, TestVec3Bool[i].Result.z, glm::epsilon<float>()) ? 0 : 1;
			}
		}

		// vec3 with bvec3
		{
			for(std::size_t i = 0; i < sizeof(TestBVec3) / sizeof(test<glm::vec3, glm::bvec3>); ++i)
			{
				glm::vec3 Result = glm::mix(TestBVec3[i].x, TestBVec3[i].y, TestBVec3[i].a);
				Error += glm::epsilonEqual(Result.x, TestBVec3[i].Result.x, glm::epsilon<float>()) ? 0 : 1;
				Error += glm::epsilonEqual(Result.y, TestBVec3[i].Result.y, glm::epsilon<float>()) ? 0 : 1;
				Error += glm::epsilonEqual(Result.z, TestBVec3[i].Result.z, glm::epsilon<float>()) ? 0 : 1;
			}
		}

		// vec4 with bool
		{
			for(std::size_t i = 0; i < sizeof(TestVec4Bool) / sizeof(test<glm::vec4, bool>); ++i)
			{
				glm::vec4 Result = glm::mix(TestVec4Bool[i].x, TestVec4Bool[i].y, TestVec4Bool[i].a);
				Error += glm::epsilonEqual(Result.x, TestVec4Bool[i].Result.x, glm::epsilon<float>()) ? 0 : 1;
				Error += glm::epsilonEqual(Result.y, TestVec4Bool[i].Result.y, glm::epsilon<float>()) ? 0 : 1;
				Error += glm::epsilonEqual(Result.z, TestVec4Bool[i].Result.z, glm::epsilon<float>()) ? 0 : 1;
				Error += glm::epsilonEqual(Result.w, TestVec4Bool[i].Result.w, glm::epsilon<float>()) ? 0 : 1;
			}
		}

		// vec4 with bvec4
		{
			for(std::size_t i = 0; i < sizeof(TestBVec4) / sizeof(test<glm::vec4, glm::bvec4>); ++i)
			{
				glm::vec4 Result = glm::mix(TestBVec4[i].x, TestBVec4[i].y, TestBVec4[i].a);
				Error += glm::epsilonEqual(Result.x, TestBVec4[i].Result.x, glm::epsilon<float>()) ? 0 : 1;
				Error += glm::epsilonEqual(Result.y, TestBVec4[i].Result.y, glm::epsilon<float>()) ? 0 : 1;
				Error += glm::epsilonEqual(Result.z, TestBVec4[i].Result.z, glm::epsilon<float>()) ? 0 : 1;
				Error += glm::epsilonEqual(Result.w, TestBVec4[i].Result.w, glm::epsilon<float>()) ? 0 : 1;
			}
		}

		return Error;
	}
}//namespace test_mix

namespace test_step
{
	template <typename EDGE, typename VEC>
	struct test
	{
		EDGE edge;
		VEC x;
		VEC result;
	};

	test<float, glm::vec4> TestVec4Scalar [] =
	{
		{ 0.0f, glm::vec4(1.0f, 2.0f, 3.0f, 4.0f), glm::vec4(1.0f) },
		{ 1.0f, glm::vec4(1.0f, 2.0f, 3.0f, 4.0f), glm::vec4(1.0f) },
		{ 0.0f, glm::vec4(-1.0f, -2.0f, -3.0f, -4.0f), glm::vec4(0.0f) }
	};

	test<glm::vec4, glm::vec4> TestVec4Vector [] =
	{
		{ glm::vec4(-1.0f, -2.0f, -3.0f, -4.0f), glm::vec4(-2.0f, -3.0f, -4.0f, -5.0f), glm::vec4(0.0f) },
		{ glm::vec4( 0.0f, 1.0f, 2.0f, 3.0f), glm::vec4( 1.0f, 2.0f, 3.0f, 4.0f), glm::vec4(1.0f) },
		{ glm::vec4( 2.0f, 3.0f, 4.0f, 5.0f), glm::vec4( 1.0f, 2.0f, 3.0f, 4.0f), glm::vec4(0.0f) },
		{ glm::vec4( 0.0f, 1.0f, 2.0f, 3.0f), glm::vec4(-1.0f,-2.0f,-3.0f,-4.0f), glm::vec4(0.0f) }
	};

	int run()
	{
		int Error = 0;

		// vec4 and float
		{
			for (std::size_t i = 0; i < sizeof(TestVec4Scalar) / sizeof(test<float, glm::vec4>); ++i)
			{
				glm::vec4 Result = glm::step(TestVec4Scalar[i].edge, TestVec4Scalar[i].x);
				Error += glm::all(glm::epsilonEqual(Result, TestVec4Scalar[i].result, glm::epsilon<float>())) ? 0 : 1;
			}
		}

		// vec4 and vec4
		{
			for (std::size_t i = 0; i < sizeof(TestVec4Vector) / sizeof(test<glm::vec4, glm::vec4>); ++i)
			{
				glm::vec4 Result = glm::step(TestVec4Vector[i].edge, TestVec4Vector[i].x);
				Error += glm::all(glm::epsilonEqual(Result, TestVec4Vector[i].result, glm::epsilon<float>())) ? 0 : 1;
			}
		}

		return Error;
	}
}//namespace test_step

int test_round()
{
	int Error = 0;

	{
		float A = glm::round(0.0f);
		Error += A == 0.0f ? 0 : 1;
		float B = glm::round(0.5f);
		Error += B == 1.0f ? 0 : 1;
		float C = glm::round(1.0f);
		Error += C == 1.0f ? 0 : 1;
		float D = glm::round(0.1f);
		Error += D == 0.0f ? 0 : 1;
		float E = glm::round(0.9f);
		Error += E == 1.0f ? 0 : 1;
		float F = glm::round(1.5f);
		Error += F == 2.0f ? 0 : 1;
		float G = glm::round(1.9f);
		Error += G == 2.0f ? 0 : 1;

#if GLM_LANG >= GLM_LANG_CXX11
		float A1 = std::round(0.0f);
		Error += A1 == A ? 0 : 1;
		float B1 = std::round(0.5f);
		Error += B1 == B ? 0 : 1;
		float C1 = std::round(1.0f);
		Error += C1 == C ? 0 : 1;
		float D1 = std::round(0.1f);
		Error += D1 == D ? 0 : 1;
		float E1 = std::round(0.9f);
		Error += E1 == E ? 0 : 1;
		float F1 = std::round(1.5f);
		Error += F == F ? 0 : 1;
		float G1 = std::round(1.9f);
		Error += G1 == G ? 0 : 1;
#endif // GLM_LANG >= GLM_CXX0X
	}

	{
		float A = glm::round(-0.0f);
		Error += A ==  0.0f ? 0 : 1;
		float B = glm::round(-0.5f);
		Error += B == -1.0f ? 0 : 1;
		float C = glm::round(-1.0f);
		Error += C == -1.0f ? 0 : 1;
		float D = glm::round(-0.1f);
		Error += D ==  0.0f ? 0 : 1;
		float E = glm::round(-0.9f);
		Error += E == -1.0f ? 0 : 1;
		float F = glm::round(-1.5f);
		Error += F == -2.0f ? 0 : 1;
		float G = glm::round(-1.9f);
		Error += G == -2.0f ? 0 : 1;
	}

	return Error;
}

int test_roundEven()
{
	int Error = 0;

	{
		float A = glm::roundEven(-1.5f);
		Error += glm::epsilonEqual(A, -2.0f, 0.0001f) ? 0 : 1;
		Error += 0;
	}
	{
		float A = glm::roundEven(1.5f);
		Error += glm::epsilonEqual(A, 2.0f, 0.0001f) ? 0 : 1;
		Error += 0;
	}

	{
		float A = glm::roundEven(-3.5f);
		Error += glm::epsilonEqual(A, -4.0f, 0.0001f) ? 0 : 1;
		Error += 0;
	}
	{
		float A = glm::roundEven(3.5f);
		Error += glm::epsilonEqual(A, 4.0f, 0.0001f) ? 0 : 1;
		Error += 0;
	}

	{
		float A = glm::roundEven(-2.5f);
		Error += glm::epsilonEqual(A, -2.0f, 0.0001f) ? 0 : 1;
		Error += 0;
	}
	{
		float A = glm::roundEven(2.5f);
		Error += glm::epsilonEqual(A, 2.0f, 0.0001f) ? 0 : 1;
		Error += 0;
	}

	{
		float A = glm::roundEven(-2.4f);
		Error += glm::epsilonEqual(A, -2.0f, 0.0001f) ? 0 : 1;
		Error += 0;
	}
	{
		float A = glm::roundEven(2.4f);
		Error += glm::epsilonEqual(A, 2.0f, 0.0001f) ? 0 : 1;
		Error += 0;
	}

	{
		float A = glm::roundEven(-2.6f);
		Error += glm::epsilonEqual(A, -3.0f, 0.0001f) ? 0 : 1;
		Error += 0;
	}
	{
		float A = glm::roundEven(2.6f);
		Error += glm::epsilonEqual(A, 3.0f, 0.0001f) ? 0 : 1;
		Error += 0;
	}

	{
		float A = glm::roundEven(-2.0f);
		Error += glm::epsilonEqual(A, -2.0f, 0.0001f) ? 0 : 1;
		Error += 0;
	}
	{
		float A = glm::roundEven(2.0f);
		Error += glm::epsilonEqual(A, 2.0f, 0.0001f) ? 0 : 1;
		Error += 0;
	}

	{
		float A = glm::roundEven(0.0f);
		Error += A == 0.0f ? 0 : 1;
		float B = glm::roundEven(0.5f);
		Error += B == 0.0f ? 0 : 1;
		float C = glm::roundEven(1.0f);
		Error += C == 1.0f ? 0 : 1;
		float D = glm::roundEven(0.1f);
		Error += D == 0.0f ? 0 : 1;
		float E = glm::roundEven(0.9f);
		Error += E == 1.0f ? 0 : 1;
		float F = glm::roundEven(1.5f);
		Error += F == 2.0f ? 0 : 1;
		float G = glm::roundEven(1.9f);
		Error += G == 2.0f ? 0 : 1;
	}

	{
		float A = glm::roundEven(-0.0f);
		Error += A ==  0.0f ? 0 : 1;
		float B = glm::roundEven(-0.5f);
		Error += B == -0.0f ? 0 : 1;
		float C = glm::roundEven(-1.0f);
		Error += C == -1.0f ? 0 : 1;
		float D = glm::roundEven(-0.1f);
		Error += D ==  0.0f ? 0 : 1;
		float E = glm::roundEven(-0.9f);
		Error += E == -1.0f ? 0 : 1;
		float F = glm::roundEven(-1.5f);
		Error += F == -2.0f ? 0 : 1;
		float G = glm::roundEven(-1.9f);
		Error += G == -2.0f ? 0 : 1;
	}

	{
		float A = glm::roundEven(1.5f);
		Error += A == 2.0f ? 0 : 1;
		float B = glm::roundEven(2.5f);
		Error += B == 2.0f ? 0 : 1;
		float C = glm::roundEven(3.5f);
		Error += C == 4.0f ? 0 : 1;
		float D = glm::roundEven(4.5f);
		Error += D == 4.0f ? 0 : 1;
		float E = glm::roundEven(5.5f);
		Error += E == 6.0f ? 0 : 1;
		float F = glm::roundEven(6.5f);
		Error += F == 6.0f ? 0 : 1;
		float G = glm::roundEven(7.5f);
		Error += G == 8.0f ? 0 : 1;
	}

	{
		float A = glm::roundEven(-1.5f);
		Error += A == -2.0f ? 0 : 1;
		float B = glm::roundEven(-2.5f);
		Error += B == -2.0f ? 0 : 1;
		float C = glm::roundEven(-3.5f);
		Error += C == -4.0f ? 0 : 1;
		float D = glm::roundEven(-4.5f);
		Error += D == -4.0f ? 0 : 1;
		float E = glm::roundEven(-5.5f);
		Error += E == -6.0f ? 0 : 1;
		float F = glm::roundEven(-6.5f);
		Error += F == -6.0f ? 0 : 1;
		float G = glm::roundEven(-7.5f);
		Error += G == -8.0f ? 0 : 1;
	}

	return Error;
}

int test_isnan()
{
	int Error = 0;

	float Zero_f = 0.0;
	double Zero_d = 0.0;

	{
		Error += true == glm::isnan(0.0/Zero_d) ? 0 : 1;
		Error += true == glm::any(glm::isnan(glm::dvec2(0.0 / Zero_d))) ? 0 : 1;
		Error += true == glm::any(glm::isnan(glm::dvec3(0.0 / Zero_d))) ? 0 : 1;
		Error += true == glm::any(glm::isnan(glm::dvec4(0.0 / Zero_d))) ? 0 : 1;
	}

	{
		Error += true == glm::isnan(0.0f/Zero_f) ? 0 : 1;
		Error += true == glm::any(glm::isnan(glm::vec2(0.0f/Zero_f))) ? 0 : 1;
		Error += true == glm::any(glm::isnan(glm::vec3(0.0f/Zero_f))) ? 0 : 1;
		Error += true == glm::any(glm::isnan(glm::vec4(0.0f/Zero_f))) ? 0 : 1;
	}

	return Error;
}

int test_isinf()
{
	int Error = 0;

	float Zero_f = 0.0;
	double Zero_d = 0.0;

	{
		Error += true == glm::isinf( 1.0/Zero_d) ? 0 : 1;
		Error += true == glm::isinf(-1.0/Zero_d) ? 0 : 1;
		Error += true == glm::any(glm::isinf(glm::dvec2( 1.0/Zero_d))) ? 0 : 1;
		Error += true == glm::any(glm::isinf(glm::dvec2(-1.0/Zero_d))) ? 0 : 1;
		Error += true == glm::any(glm::isinf(glm::dvec3( 1.0/Zero_d))) ? 0 : 1;
		Error += true == glm::any(glm::isinf(glm::dvec3(-1.0/Zero_d))) ? 0 : 1;
		Error += true == glm::any(glm::isinf(glm::dvec4( 1.0/Zero_d))) ? 0 : 1;
		Error += true == glm::any(glm::isinf(glm::dvec4(-1.0/Zero_d))) ? 0 : 1;
	}

	{
		Error += true == glm::isinf( 1.0f/Zero_f) ? 0 : 1;
		Error += true == glm::isinf(-1.0f/Zero_f) ? 0 : 1;
		Error += true == glm::any(glm::isinf(glm::vec2( 1.0f/Zero_f))) ? 0 : 1;
		Error += true == glm::any(glm::isinf(glm::vec2(-1.0f/Zero_f))) ? 0 : 1;
		Error += true == glm::any(glm::isinf(glm::vec3( 1.0f/Zero_f))) ? 0 : 1;
		Error += true == glm::any(glm::isinf(glm::vec3(-1.0f/Zero_f))) ? 0 : 1;
		Error += true == glm::any(glm::isinf(glm::vec4( 1.0f/Zero_f))) ? 0 : 1;
		Error += true == glm::any(glm::isinf(glm::vec4(-1.0f/Zero_f))) ? 0 : 1;
	}

	return Error;
}

int main()
{
	int Error(0);

	Error += test_modf();
	Error += test_floatBitsToInt();
	Error += test_floatBitsToUint();
	Error += test_step::run();
	Error += test_mix::run();
	Error += test_round();
	Error += test_roundEven();
	Error += test_isnan();
	//Error += test_isinf();

	return Error;
}


#ifndef __COMMON_H
#define __COMMON_H

#include <iostream>

#include <armadillo>
/*
 * Armadillo C++ library Citation:
 *
 * Conrad Sanderson and Ryan Curtin.
 * Armadillo: a template-based C++ library for linear algebra.
 * Journal of Open Source Software, Vol. 1, pp. 26, 2016.
 *
 * Conrad Sanderson and Ryan Curtin.
 * A User-Friendly Hybrid Sparse Matrix Class in C++.
 * Lecture Notes in Computer Science (LNCS), Vol. 10931, pp. 422-430, 2018.
 */

#include <math.h>
#include <unistd.h>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/thread/thread.hpp>
#include <boost/array.hpp>
#include <boost/signals2.hpp>

#include "grSim_Packet.pb.h"
#include "grSim_Commands.pb.h"
#include "grSim_Replacement.pb.h"

#include "messages_robocup_ssl_detection.pb.h"
#include "messages_robocup_ssl_geometry.pb.h"
#include "messages_robocup_ssl_refbox_log.pb.h"
#include "messages_robocup_ssl_robot_status.pb.h"
#include "messages_robocup_ssl_wrapper.pb.h"

#include "boost_logger.hpp"

using byte = unsigned char;
const int NUM_ROBOTS = 6;
enum team_color_t {
    BLUE,
    YELLOW
};

static constexpr const char* LOCAL_HOST = "127.0.0.1";

const float Pi = 3.1415926;

inline double to_radian(double deg) { return deg * Pi / 180.000;}
inline double to_degree(double rad) { return rad * (180.000 / Pi);}

extern arma::vec unit_vec_x;  // definition in common.cpp
extern arma::vec unit_vec_y;  // definition in common.cpp

arma::mat rotation_matrix_2D(double angle_degree);
arma::mat change_basis_matrix_2D(arma::vec vx, arma::vec vy) ;

using range_t = std::pair<double, double>;

double map(double value, range_t from, range_t to);
arma::vec map(arma::vec value, range_t from, range_t to);
void map2(arma::vec& value, range_t from, range_t to);

/* Synchronization for Reader/Writer problems */
typedef boost::shared_mutex reader_writer_mutex;
// get exclusive access
#define writer_lock(mutex) do { \
    boost::upgrade_lock<reader_writer_mutex> __writer_lock(mutex); \
    boost::upgrade_to_unique_lock<reader_writer_mutex> __unique_writer_lock( __writer_lock ); \
}while(0)
// get shared access
#define reader_lock(mutex) boost::shared_lock<reader_writer_mutex>  __reader_lock(mutex);

#define repr std::to_string

#endif


/**
 * Non-metric Space Library
 *
 * Authors: Bilegsaikhan Naidan (https://github.com/bileg), Leonid Boytsov (http://boytsov.info).
 * With contributions from Lawrence Cayton (http://lcayton.com/) and others.
 *
 * For the complete list of contributors and further details see:
 * https://github.com/searchivarius/NonMetricSpaceLib
 *
 * Copyright (c) 2014
 *
 * This code is released under the
 * Apache License Version 2.0 http://www.apache.org/licenses/.
 *
 */

#include <iostream>
#include <string>

#include <boost/program_options.hpp>

#include "space.h"
#include "params.h"
#include "init.h"
#include "report_intr_dim.h"
#include "spacefactory.h"

using namespace similarity;
using namespace std;

namespace po = boost::program_options;

const unsigned defaultSampleQty = 1000000;

static void Usage(const char *prog,
                  const po::options_description& desc) {
    std::cout << prog << std::endl
              << desc << std::endl;
}

template <typename dist_t>
void ComputeMuDeffect(const Space<dist_t>& space,
                    const ObjectVector& dataset,
                    double & dleft, double & dright,
                    size_t SampleQty = 1000000) {
  dleft = dright = -1;
  for (size_t n = 0; n < SampleQty; ++n) {
    size_t r1 = RandomInt() % dataset.size();
    size_t r2 = RandomInt() % dataset.size();
    size_t r3 = RandomInt() % dataset.size();
    CHECK(r1 < dataset.size());
    CHECK(r2 < dataset.size());
    CHECK(r3 < dataset.size());
    const Object* q = dataset[r1];
    const Object* a = dataset[r2];
    const Object* b = dataset[r3];
    {
      dist_t d1 = space.IndexTimeDistance(q, a);
      dist_t d2 = space.IndexTimeDistance(q, b);
      dist_t d3 = space.IndexTimeDistance(a, b);
      if (ISNAN(d1) || ISNAN(d2) || ISNAN(d3)) {
        throw runtime_error("!!! Bug: a distance returned NAN!");
      }
      if (d3 != 0) {
        dright = max(dright, (double)fabs(double(d1)-double(d2))/double(d3));
      }
    }
    {
      dist_t d1 = space.IndexTimeDistance(a, q);
      dist_t d2 = space.IndexTimeDistance(b, q);
      dist_t d3 = space.IndexTimeDistance(b, a);
      if (ISNAN(d1) || ISNAN(d2) || ISNAN(d3)) {
        throw runtime_error("!!! Bug: a distance returned NAN!");
      }
      if (d3 != 0) {
        dleft = max(dleft, (double)fabs(double(d1)-double(d2))/double(d3));
      }
    }
  }
}

template <typename dist_t>
void TestSpace(
                string spaceDesc,
                string dataFile,
                bool compMuDeffect,
                unsigned maxNumData,
                unsigned sampleQty
               ) {
  string          spaceType;
  vector<string>  vSpaceArgs;

  ParseSpaceArg(spaceDesc, spaceType, vSpaceArgs);
  AnyParams   spaceParams({AnyParams(vSpaceArgs)});

  unique_ptr<Space<dist_t>> space(SpaceFactoryRegistry<dist_t>::
                                Instance().CreateSpace(spaceType, spaceParams));

  ObjectVector    data;
  vector<string>  tmp;
  unique_ptr<DataFileInputState> inpState(space->ReadDataset(data, tmp, dataFile, maxNumData));
  space->UpdateParamsFromFile(*inpState);

  // Prints the report
  ReportIntrinsicDimensionality("********", *space, data, sampleQty);
  if (compMuDeffect) {
    double    dleft, dright;
    ComputeMuDeffect<dist_t>(
                  *space,
                  data,
                  dleft, dright,
                  sampleQty
                 );
    LOG(LIB_INFO) << "### left mu-defect. : " << dleft << " right mu-defect. :" << dright;
  }
}

int main(int argc, char* argv[]) {
  string    spaceDesc, distType;
  string    dataFile;
  unsigned  maxNumData;
  unsigned  sampleQty;
  bool      compMuDeffect;

  po::options_description ProgOptDesc("Allowed options");
  ProgOptDesc.add_options()
    ("help,h", "produce help message")
    ("spaceType,s",     po::value<string>(&spaceDesc)->required(),
                        "space type, e.g., l1, l2, lp:p=0.5")
    ("distType",        po::value<string>(&distType)->default_value(DIST_TYPE_FLOAT),
                        "distance value type: int, float, double")
    ("dataFile,i",      po::value<string>(&dataFile)->required(),
                        "input data file")
    ("maxNumData",      po::value<unsigned>(&maxNumData)->default_value(0),
                        "if non-zero, only the first maxNumData elements are used")
    ("sampleQty",       po::value<unsigned>(&sampleQty)->default_value(defaultSampleQty),
                        "a number of samples (a sample is a pair of data points)")
    ("muDeffect,m",     po::value<bool>(&compMuDeffect)->default_value(false),
                        "estimate the left and the right mu deffectiveness")

  ;

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, ProgOptDesc), vm);
    po::notify(vm);

    if (!DoesFileExist(dataFile)) {
      PREPARE_RUNTIME_ERR(err) << "data file " << dataFile << " doesn't exist";
      THROW_RUNTIME_ERR(err);
    }

    initLibrary(LIB_LOGSTDERR);

    if (DIST_TYPE_INT == distType) {
      TestSpace<int>(
                  spaceDesc,
                  dataFile,
                  compMuDeffect,
                  maxNumData,
                  sampleQty
                );
    } else if (DIST_TYPE_FLOAT == distType) {
      TestSpace<float>(
                  spaceDesc,
                  dataFile,
                  compMuDeffect,
                  maxNumData,
                  sampleQty
                 );
    } else if (DIST_TYPE_DOUBLE == distType) {
      TestSpace<double>(
                  spaceDesc,
                  dataFile,
                  compMuDeffect,
                  maxNumData,
                  sampleQty
                 );
    }

  } catch (const exception& e) {
    Usage(argv[0], ProgOptDesc);
    LOG(LIB_FATAL) << e.what();
  }

  return 0;
}


/**
 * Copyright © 2013-2014 Felix Höfling
 * Copyright © 2014      Klaus Reuter
 * All rights reserved.
 *
 * This file is part of h5xx — a C++ wrapper for the HDF5 library.
 *
 * This software may be modified and distributed under the terms of the
 * 3-clause BSD license.  See accompanying file LICENSE for details.
 */

#include <h5xx/h5xx.hpp>
#include <boost/multi_array.hpp>
#include <iostream>
#include <vector>
#include <cstdio>

typedef boost::multi_array<int, 2> array_2d_t;
typedef boost::multi_array<int, 1> array_1d_t;
typedef boost::multi_array<double, 2> array_2d_dbl_t;

const int NI=10;
const int NJ=NI;

void print_array(array_2d_t const& array)
{
    for (unsigned int j = 0; j < array.shape()[1]; j++)
    {
        for (unsigned int i = 0; i < array.shape()[0]; i++)
        {
            printf("%2d ", array[j][i]);
        }
        printf("\n");
    }
}

void print_array(array_1d_t const& array)
{
    for (unsigned int i = 0; i < array.shape()[0]; i++)
    {
        printf("%2d ", array[i]);
    }
    printf("\n");
}

void write_int_data(std::string const& filename, array_2d_t const& array)
{
    h5xx::file file(filename, h5xx::file::trunc);
    std::string name;

    {
        // --- create dataset and fill it with the default array data (positive values)
        name = "integer array";
        h5xx::create_dataset(file, name, array);
        h5xx::write_dataset(file, name, array);

        // --- create a slice object (aka hyperslab) to specify the location in the dataset to be overwritten
        std::vector<int> offset; int offset_raw[2] = {4,4}; offset.assign(offset_raw, offset_raw + 2);
        std::vector<int> count;  int count_raw[2] = {2,2}; count.assign(count_raw, count_raw + 2);
        h5xx::slice slice(offset, count);

        // --- data to be written to the slice (negative values)
        array_1d_t data(boost::extents[4]);
        int data_raw[4] = {-1,-2,-3,-4};
        data.assign(data_raw, data_raw+4);

        // --- overwrite part of the dataset as specified by slice
        h5xx::write_dataset(file, name, data, slice);
    }
}

void read_int_data(std::string const& filename)
{
    h5xx::file file(filename, h5xx::file::in);
    std::string name = "integer array";

    // read and print the full dataset
    {
        array_2d_t array;
        // --- read the complete dataset into array, the array is resized and overwritten internally
        h5xx::read_dataset(file, name, array);
        printf("original integer array read from file, negative number patch was written using a slice\n");
        print_array(array);
        printf("\n");
    }

    // read and print a subset of the dataset
    {
        array_2d_t array;

        // --- create a slice object (aka hyperslab) to specify the patch to be read from the dataset
        std::vector<int> offset; int offset_raw[2] = {3,3}; offset.assign(offset_raw, offset_raw + 2);
        std::vector<int> count;  int count_raw[2] = {4,4}; count.assign(count_raw, count_raw + 2);
        h5xx::slice slice(offset, count);

        // --- allocate memory for the slice (user's responsibility, is not done internally)
        array.resize(count);

        h5xx::read_dataset(file, name, array, slice);

        printf("2D slice of the integer array, zoom on the negative number patch\n");
        print_array(array);
        printf("\n");
    }

    // read a 2D subset of the dataset into a 1D array
    {
        array_1d_t array;

        // --- create a slice object (aka hyperslab) to specify the patch to be read from the dataset
        std::vector<int> offset; int offset_raw[2] = {3,3}; offset.assign(offset_raw, offset_raw + 2);
        std::vector<int> count;  int count_raw[2] = {4,4}; count.assign(count_raw, count_raw + 2);
        h5xx::slice slice(offset, count);

        // --- allocate memory for the slice
        std::vector<int> total_count;
        total_count.push_back(count[0]*count[1]);
        array.resize(total_count);

        h5xx::read_dataset(file, name, array, slice);

        printf("2D slice of the integer array, copied into a 1D array\n");
        print_array(array);
        printf("\n");
    }
}

int main(int argc, char** argv)
{
    std::string filename = argv[0];
    filename.append(".h5");

    // --- do a few demos/tests using integers
    {
        array_2d_t array(boost::extents[NJ][NI]);
        {
            const int nelem = NI*NJ;
            int data[nelem];
            for (int i = 0; i < nelem; i++)
                data[i] = i;
            array.assign(data, data + nelem);
        }

        write_int_data(filename, array);

        read_int_data(filename);
    }

    return 0;
}


// Copyright 2008-2010 Gordon Woodhull
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/msm/mpl_graph/depth_first_search.hpp>
#include <boost/msm/mpl_graph/adjacency_list_graph.hpp>
#include <boost/msm/mpl_graph/incidence_list_graph.hpp>

#include <iostream>

namespace mpl_graph = boost::msm::mpl_graph;
namespace mpl = boost::mpl;

// vertices
struct A{}; struct B{}; struct C{}; struct D{}; struct E{}; struct F{}; struct G{};

// edges
struct A_B{}; struct B_C{}; struct C_D{}; struct C_E{}; struct C_F{}; struct B_F{};

/*
    incidence list test graph:
    A -> B -> C -\--> D
           \     |--> E
            \    \--> F
             \-----/
    G
*/

typedef mpl::vector<mpl::vector<A_B,A,B>,
               mpl::vector<B_C,B,C>,
               mpl::vector<C_D,C,D>,
               mpl::vector<C_E,C,E>,
               mpl::vector<C_F,C,F>,
               mpl::vector<B_F,B,F> >
    some_incidence_list;
typedef mpl_graph::incidence_list_graph<some_incidence_list> some_incidence_list_graph;

/*
    adjacency list test graph:
    A -> B -> C -\--> D
           \     |--> E
            \    \--> F
             \-----/
    G
*/

typedef mpl::vector<
            mpl::pair<A, mpl::vector<mpl::pair<A_B, B> > >,
            mpl::pair<B, mpl::vector<mpl::pair<B_C, C>,
                                     mpl::pair<B_F, F> > >,
            mpl::pair<C, mpl::vector<mpl::pair<C_D, D>,
                                     mpl::pair<C_E, E>,
                                     mpl::pair<C_F, F> > >,
            mpl::pair<G, mpl::vector<> > >
    some_adjacency_list;
typedef mpl_graph::adjacency_list_graph<some_adjacency_list> some_adjacency_list_graph;

struct preordering_visitor : mpl_graph::dfs_default_visitor_operations {
    template<typename Node, typename Graph, typename State>
    struct discover_vertex :
        mpl::push_back<State, Node>
    {};
};

struct postordering_visitor : mpl_graph::dfs_default_visitor_operations {
    template<typename Node, typename Graph, typename State>
    struct finish_vertex :
        mpl::push_back<State, Node>
    {};
};

// adjacency list tests

// preordering, default start node (first)
typedef mpl::first<mpl_graph::
    depth_first_search_all<some_adjacency_list_graph,
                           preordering_visitor,
                           mpl::vector<> >::type>::type
                    preorder_adj;
BOOST_MPL_ASSERT(( mpl::equal<preorder_adj::type, mpl::vector<A,B,C,D,E,F,G> > ));

// postordering, default start node
typedef mpl::first<mpl_graph::
    depth_first_search_all<some_adjacency_list_graph,
                           postordering_visitor,
                           mpl::vector<> >::type>::type
                    postorder_adj;
BOOST_MPL_ASSERT(( mpl::equal<postorder_adj::type, mpl::vector<D,E,F,C,B,A,G> > ));

// preordering all starting at C
typedef mpl::first<mpl_graph::
    depth_first_search_all<some_adjacency_list_graph,
                           preordering_visitor,
                           mpl::vector<>,
                           C>::type>::type
                    preorder_adj_all_from_c;
BOOST_MPL_ASSERT(( mpl::equal<preorder_adj_all_from_c::type, mpl::vector<C,D,E,F,A,B,G> > ));

// preordering just those starting at C
typedef mpl::first<mpl_graph::
    depth_first_search<some_adjacency_list_graph,
                       preordering_visitor,
                       mpl::vector<>,
                       C>::type>::type
                preorder_adj_from_c;
BOOST_MPL_ASSERT(( mpl::equal<preorder_adj_from_c::type, mpl::vector<C,D,E,F> > ));

// incidence list tests

// preordering, default start node (first)
typedef mpl::first<mpl_graph::
    depth_first_search_all<some_incidence_list_graph,
                           preordering_visitor,
                           mpl::vector<> >::type>::type
                    preorder_inc;
BOOST_MPL_ASSERT(( mpl::equal<preorder_inc::type, mpl::vector<A,B,C,D,E,F> > ));

// postordering, default start node
typedef mpl::first<mpl_graph::
    depth_first_search_all<some_incidence_list_graph,
                           postordering_visitor,
                           mpl::vector<> >::type>::type
                    postorder_inc;
BOOST_MPL_ASSERT(( mpl::equal<postorder_inc::type, mpl::vector<D,E,F,C,B,A> > ));

// preordering starting at C
typedef mpl::first<mpl_graph::
    depth_first_search_all<some_incidence_list_graph,
                           preordering_visitor,
                           mpl::vector<>,
                           C>::type>::type
                    preorder_inc_all_from_c;
BOOST_MPL_ASSERT(( mpl::equal<preorder_inc_all_from_c::type, mpl::vector<C,D,E,F,A,B> > ));

// preordering starting at B
typedef mpl::first<mpl_graph::
    depth_first_search<some_incidence_list_graph,
                       preordering_visitor,
                       mpl::vector<>,
                       B>::type>::type
                preorder_inc_from_b;
BOOST_MPL_ASSERT(( mpl::equal<preorder_inc_from_b::type, mpl::vector<B,C,D,E,F> > ));

int main() {
    return 0;
}


#include <sparse.h>
#include <sparse_fill.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(SPARSE);

BOOST_AUTO_TEST_CASE(product_crm_vector_test)
{
  // scalar case

  sparse::CompressedRowMatrix<sparse::Block<1,1,float> > sA(1,1,1);
  sparse::Vector<sparse::Block<1,1,float> > su(1);
  sparse::Vector<sparse::Block<1,1,float> > sv(1);

  sA(0,0)[0] = 5;
  su(0)[0] = 2;
  sparse::prod(sA, su, sv);
  assert( sv(0)[0] == 2*5 );

  sA.clear();
  su.clear();
  sv.clear();
  sA.resize(2,4,4);
  su.resize(4);
  sv.resize(2);
  sA(0,0)[0] = 1;
  sA(0,3)[0] = 2;
  sA(1,1)[0] = 3;
  sA(1,2)[0] = 4;
  su(0) = 5;
  su(1) = 6;
  su(2) = 7;
  su(3) = 8;
  sparse::prod(sA,su,sv);
  BOOST_CHECK( sv(0)[0] == 21 );
  BOOST_CHECK( sv(1)[0] == 46 );

  // block case

  sparse::CompressedRowMatrix<sparse::Block<3,4,float> > A(1,1,1);
  sparse::Vector<sparse::Block<4,1,float> > u(1);
  sparse::Vector<sparse::Block<3,1,float> > v(1);

  sparse::fill(A(0,0));
  sparse::fill(u(0));
  sparse::prod(A,u,v);
  BOOST_CHECK( v(0)[0] == 14 );
  BOOST_CHECK( v(0)[1] == 38 );
  BOOST_CHECK( v(0)[2] == 62 );

  A.clear();
  u.clear();
  v.clear();
  A.resize(2,4,4);
  u.resize(4);
  v.resize(2);
  sparse::fill(A(0,0),1);
  sparse::fill(A(0,3),2);
  sparse::fill(A(1,1),3);
  sparse::fill(A(1,2),4);
  sparse::fill(u(0),1);
  sparse::fill(u(1),2);
  sparse::fill(u(2),3);
  sparse::fill(u(3),4);
  sparse::prod(A,u,v);
  BOOST_CHECK( v(0)[0] == 112 );
  BOOST_CHECK( v(0)[1] == 240 );
  BOOST_CHECK( v(0)[2] == 368 );
  BOOST_CHECK( v(1)[0] == 172 );
  BOOST_CHECK( v(1)[1] == 300 );
  BOOST_CHECK( v(1)[2] == 428 );

}

BOOST_AUTO_TEST_SUITE_END();


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE BoolLogicTest

#include <boost/test/unit_test.hpp>
#include <popart/boollogic.hpp>
#include <popart/logging.hpp>

namespace popart {
namespace boollogic {

BOOST_AUTO_TEST_CASE(Term_Not0) {
  Term term0 = Term::notTerm(Term::notTerm(Term::varTerm("A")));

  Term term1 = term0.pushNots();

  BOOST_CHECK_EQUAL(term1, Term::varTerm("A"));
  BOOST_CHECK_NE(term1, term0);
  BOOST_CHECK_NE(term1, Term::varTerm("B"));
}

BOOST_AUTO_TEST_CASE(Term_Not1) {
  Term term0 =
      Term::notTerm(Term::andTerm({Term::varTerm("A"),
                                   Term::varTerm("B"),
                                   Term::notTerm(Term::varTerm("C"))}));
  Term term1 = Term::notTerm(Term::orTerm({Term::varTerm("A"),
                                           Term::notTerm(Term::varTerm("B")),
                                           Term::varTerm("C")}));
  Term term2 = Term::orTerm({Term::notTerm(Term::varTerm("A")),
                             Term::notTerm(Term::varTerm("B")),
                             Term::varTerm("C")});
  Term term3 = Term::andTerm({Term::notTerm(Term::varTerm("A")),
                              Term::varTerm("B"),
                              Term::notTerm(Term::varTerm("C"))});

  Term term4 = term0.pushNots();
  Term term5 = term1.pushNots();
  Term term6 = term2.pushNots();
  Term term7 = term3.pushNots();

  BOOST_CHECK_EQUAL(term4, term2);
  BOOST_CHECK_EQUAL(term5, term3);
  BOOST_CHECK_EQUAL(term6, term2);
  BOOST_CHECK_EQUAL(term7, term3);
}

BOOST_AUTO_TEST_CASE(Term_Not2) {
  Term term0 = Term::notTerm(Term::andTerm(
      {Term::varTerm("A"),
       Term::orTerm({Term::varTerm("B"), Term::notTerm(Term::varTerm("C"))})}));
  Term term1 = Term::orTerm(
      {Term::notTerm(Term::varTerm("A")),
       Term::andTerm({Term::notTerm(Term::varTerm("B")), Term::varTerm("C")})});

  Term term2 = term0.pushNots();

  std::map<std::string, bool> vals;
  vals["A"] = false;
  vals["B"] = true;
  vals["C"] = false;

  BOOST_CHECK_EQUAL(term1, term2);
  BOOST_CHECK(term0.evaluate(vals));
  BOOST_CHECK(term1.evaluate(vals));

  vals["A"] = true;
  vals["B"] = true;
  vals["C"] = false;

  BOOST_CHECK(!term0.evaluate(vals));
  BOOST_CHECK(!term1.evaluate(vals));
}

BOOST_AUTO_TEST_CASE(Term_Flatten0) {
  Term a = Term::varTerm("A");
  Term b = Term::varTerm("B");
  Term c = Term::varTerm("C");
  Term d = Term::varTerm("D");

  Term t0 = (a || ((b && c) && d));
  Term t1 = (a && ((b || c) || d));

  Term t2 = (a || Term::andTerm({b, c, d}));
  Term t3 = (a && Term::orTerm({b, c, d}));

  Term t4 = t0.flatten();
  Term t5 = t1.flatten();

  BOOST_CHECK_EQUAL(t4, t2);
  BOOST_CHECK_EQUAL(t5, t3);
}

BOOST_AUTO_TEST_CASE(Term_Insert0) {
  Term a = Term::varTerm("A");
  Term b = Term::varTerm("B");
  Term c = Term::varTerm("C");
  Term d = Term::varTerm("D");

  Term t0 = (a || ((b && c) || d));
  Term t1 = (a && ((b || c) && d));

  Term t2 = (a || ((b && a) || b));
  Term t3 = (a && ((b || a) && b));

  std::map<std::string, Term> iterms;
  iterms.insert({"C", a});
  iterms.insert({"D", b});

  Term t4 = t0.replace(iterms);
  Term t5 = t1.replace(iterms);

  BOOST_CHECK_EQUAL(t2, t4);
  BOOST_CHECK_EQUAL(t3, t5);
}

BOOST_AUTO_TEST_CASE(Term_CNF0) {
  Term a = Term::varTerm("A");
  Term b = Term::varTerm("B");
  Term c = Term::varTerm("C");
  Term d = Term::varTerm("D");
  Term e = Term::varTerm("E");

  Term t0 = ((a && b) || (c && d)) || e;
  Term t1 = t0.getCNF();
  Term t2 = ((c || e || a) && (c || e || b) && (d || e || a) && (d || e || b))
                .flatten();

  BOOST_CHECK_EQUAL(t1, t2);
}

BOOST_AUTO_TEST_CASE(Term_DNF0) {
  Term a = Term::varTerm("A");
  Term b = Term::varTerm("B");
  Term c = Term::varTerm("C");
  Term d = Term::varTerm("D");
  Term e = Term::varTerm("E");

  Term t0 = ((a || b) && (c || d)) && e;
  Term t1 = t0.getDNF();
  Term t2 = ((c && e && a) || (c && e && b) || (d && e && a) || (d && e && b))
                .flatten();

  BOOST_CHECK_EQUAL(t1, t2);
}

} // namespace boollogic
} // namespace popart


/*
 [auto_generated]
 libs/numeric/odeint/test/euler_stepper.cpp

 [begin_description]
 This file tests explicit Euler stepper.
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

 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 BOOST_TEST_MODULE odeint_explicit_euler

#include <boost/test/unit_test.hpp>

#include <utility>
#include <iostream>
#include <vector>

#include <boost/numeric/odeint/stepper/euler.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

// test with own vector implementation

class my_vec : public std::vector< double > {

public:

    my_vec() : std::vector< double >()
        { }

    my_vec( const my_vec &x ) : std::vector< double >( x )
        { }

    my_vec( size_t dim )
        : std::vector< double >( dim )
    { }

};

namespace boost {
namespace numeric {
namespace odeint {

template<>
struct is_resizeable< my_vec >
{
    //struct type : public boost::true_type { };
    typedef boost::true_type type;
    const static bool value = type::value;
};
} } }

typedef double value_type;
//typedef std::vector< value_type > state_type;
typedef my_vec state_type;

/* use functors, because functions don't work with msvc 10, I guess this is a bug */
struct sys
{
    void operator()( const state_type &x , state_type &dxdt , const value_type t ) const
    {
        std::cout << "sys start " << dxdt.size() << std::endl;
        dxdt[0] = x[0] + 2 * x[1];
        dxdt[1] = x[1];
        std::cout << "sys done" << std::endl;
    }
};

BOOST_AUTO_TEST_SUITE( explicit_euler_test )

BOOST_AUTO_TEST_CASE( test_euler )
{
    range_algebra algebra;
    euler< state_type > stepper( algebra );
    state_type x( 2 );
    x[0] = 0.0; x[1] = 1.0;

    std::cout << "initialized" << std::endl;

    const value_type eps = 1E-12;
    const value_type dt = 0.1;

    stepper.do_step( sys() , x , 0.0 , dt );

    using std::abs;

    // compare with analytic solution of above system
    BOOST_CHECK_MESSAGE( abs( x[0] - 2.0*1.0*dt ) < eps , x[0] - 2.0*1.0*dt );
    BOOST_CHECK_MESSAGE( abs( x[1] - (1.0 + dt) ) < eps , x[1] - (1.0+dt) );

}

BOOST_AUTO_TEST_SUITE_END()


/*
 * predict.cpp
 *
 *  Created on: Feb 7, 2014
 *      Author: vikas
 */

#include "hilbert.hpp"
#include <boost/mpi.hpp>
#include "../base/context.hpp"

#define DEBUG std::cout << "error " << std::endl;

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

    int provided;
    MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);

	std::string testfile = argv[1];
	std::string modelfile = argv[2];
	boost::mpi::environment env (argc, argv);

        // get communicator
        boost::mpi::communicator comm;
        int rank = comm.rank();

        skylark::base::context_t context (12345);
	elem::Initialize (argc, argv);
	MPI_Comm mpi_world(comm);
	DistInputMatrixType X, Y;
	int m,n;
	elem::Matrix<double> W;

	if (rank == 0) {

		read_model_file(modelfile, W);
		m = W.Height();
		n = W.Width();
	}
	boost::mpi::broadcast(comm, m, 0);
	boost::mpi::broadcast(comm, n, 0);
	if (rank != 0) {
		W.Resize(m,n);
	}

	boost::mpi::broadcast(comm, W.Buffer(), m*n, 0);

	read_libsvm_dense(context, testfile, X, Y);

	elem::DistMatrix<double, elem::VC, elem::STAR>  O(X.Height(), W.Width());
	elem::MakeZeros(O);

	elem::Gemm(elem::NORMAL,elem::NORMAL,1.0, X.Matrix(), W, 0.0, O.Matrix());

	int correct = 0;
	double o, o1;
	int pred;
	for(int i=0; i<O.LocalHeight(); i++) {
		o = O.GetLocal(i,0);
		pred = 0;
		for(int j=1; j<O.Width(); j++) {
			o1 = O.GetLocal(i,j);
			if ( o1 > o) {
				o = o1;
				pred = j;
			}
		}
		if(pred== (int) Y.GetLocal(i,0))
			correct++;
	}

	comm.barrier();

	//if(rank ==0) {
	int totalcorrect;
	boost::mpi::reduce(comm, correct, totalcorrect, std::plus<double>(), 0);
	if(rank ==0)
	    std::cout << "Accuracy = " << totalcorrect*100.0/X.Height() << " %" << std::endl;
	//}
}


///////////////////////////////////////////////////////////////////////////////
// contingency_table.cpp                                                     //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#include <boost/test/test_tools.hpp>
#include <boost/test/unit_test.hpp>

#include <libs/statistics/detail/non_parametric/test/contingency_table1.cpp>
#include <libs/statistics/detail/non_parametric/test/contingency_table2.cpp>

using boost::unit_test::test_suite;

test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite* test = BOOST_TEST_SUITE(" Contingency Table Test Suite ");
    test->add ( BOOST_TEST_CASE( &test_contingency_table1 ) );
    test->add ( BOOST_TEST_CASE( &test_contingency_table2 ) );

    return test;

}


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::model::example::model::posterior_analysis.cpp       //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#include <iomanip>
#include <fstream>
#include <stdexcept>
#include <vector>
#include <limits>
#include <algorithm>
#include <iterator>
// #include <boost/archive/binary_oarchive.hpp>
// #include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/vector.hpp>

#include <boost/timer.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/iterator/range_cycle.hpp>
#include <boost/range.hpp>
#include <boost/assert.hpp>
#include <boost/foreach.hpp>

#include <boost/standard_distribution/distributions/normal.hpp>
#include <boost/standard_distribution/distributions/uniform.hpp>
#include <boost/dist_random/include.hpp>
#include <boost/statistics/empirical_cdf/algorithm/sequential_kolmogorov_smirnov_distance.hpp>
#include <boost/statistics/survival/data/include.hpp>
#include <boost/statistics/survival/model/models/exponential/include.hpp>

// Must come after survival/model/models/exponential/include
#include <boost/statistics/model/include.hpp>

#include <libs/statistics/survival/model/example/posterior_analysis.h>

void example_posterior_analysis(
    // TODO n_bath should be deduced from input (getline)
    const long& n_batch,            // = 5e4;
    const long& n_batch_mod,        // = 1e3;
    const long& n_proposal,         // = 1e4;
    const long& n_t_pars,           // = 1e4;
    const double& entry_bound,      // = const_::inf_;
    const std::string& in_path,     // = "/Users/erwann/projets/2009/Xcode/survival/build/Release/
    const std::string& out_path,    // = "/Users/erwann/projets/2009/Xcode/survival/build/Release/
    std::ostream& out
){

    out << "-> example_posterior_analysis : ";
    out.flush();

    // Steps shown in this example:
    //
    // Samples proposal draws
    // Successively loads batches of records, and for each
    // Creates events at a given time
    // Importance samples from the posterior density
    // Saves the Cook-Gelman (cg) statistics for each.
    // Checks the cg data against U[0,1] using the Kolmogorov Smirnov distance

    using namespace boost;
    using namespace statistics;
    namespace surv = survival;
    namespace e_cdf = empirical_cdf;
    namespace iw = importance_weights;

    // Types
    typedef std::string                             str_;
    typedef double                                  val_;
    typedef std::vector<val_>                       vals_;

    // [ Input ]
    typedef boost::archive::text_iarchive           ia_;
    typedef std::ifstream                           ifs_;

    const str_ prior_path       = in_path + "prior";
    const str_ xm_mngr_path     = in_path + "covariates_model_mngr";
    const str_ pr_mngrs_path    = in_path + "par_records_mngrs_path";

    // [ Model ]
    typedef val_ x_;
    typedef val_ par_;
    typedef surv::model::exponential::model<val_>               model_;
    typedef model::model_covariate_parameter_<model_,x_,par_>   mcp_;
    typedef surv::data::default_covariates_model_mngr<
        x_,
        model_
    > xm_mngr_;
    xm_mngr_ xm_mngr;
    {
        ifs_ ifs(xm_mngr_path.c_str());
        ia_ ia(ifs);
        ia >> xm_mngr;
    }
    typedef val_ par_;
    typedef surv::data::default_parameter_records_mngr<
        val_,
        par_
    > pr_mngr_;
    pr_mngr_ pr_mngr;
    ifs_ ifs_pr_mngrs( pr_mngrs_path.c_str() );
    if(!ifs_pr_mngrs.good()){
        str_ str = "error opening : ";
        str.append( pr_mngrs_path );
        throw std::runtime_error(str);
    }
    ia_ ia_pr_mngrs( ifs_pr_mngrs );

    typedef math::normal_distribution<val_> mprior_;
    mprior_ mprior;
    {
        typedef math::meta_distribution_primitives<mprior_>::type  prim_;
        ifs_ ifs(prior_path.c_str());
        ia_ ia(ifs);
        prim_ prim;
        ia >> prim;
        mprior = prim;
    }

    // [ Output ]
    typedef boost::archive::text_oarchive           oa_;
    typedef std::ofstream                           ofs_;
    const str_ t_pars_path = out_path + "t_pars";
    const str_ cg_path     = out_path + "cg";
    const str_ ks_path     = out_path + "kss";

    // Cg
    ofs_ ofs_cg( cg_path.c_str() );
    vals_ cgs; cgs.reserve(n_batch);

    // Targets
    typedef std::vector<par_> pars_;
    pars_ t_pars( n_t_pars );
    ofs_ ofs_t_pars(t_pars_path.c_str());
    oa_ oa_t_pars(ofs_t_pars);

    // Events
    typedef surv::data::responses_mngr<val_>        y_mngr_;
    y_mngr_ y_mngr;

    // Urng
    typedef boost::mt19937                          urng_;
    urng_                                           urng;

    // Proposal
    typedef mprior_                                 mproposal_;
    // This one size fits all proposal is likely to result in a small ESS. You
    // can monitor the ESS by calling out << pws. Ultimately, uniformity of
    // the Cook-Gelman will determine if either the computations are wrong
    // or if n_proposal is too small. n_proposal = 1e4 works fine for N(0,5).
    mproposal_ mproposal = mprior;
    pars_ p_pars(n_proposal);
    vals_ p_lpdfs(n_proposal);

    // Weights
    const val_ max_log = 100.0;
    typedef model::importance_sampler<val_> model_is_;
    model_is_ model_is( max_log );

    vals_ iws;
    iws.reserve( n_proposal );

    // Model
    typedef surv::model::exponential::model<val_> model_;
    typedef model::prior_model_dataset_<
        mprior_,
        xm_mngr_::model_wrapper_type::model_type,
        xm_mngr_::covariates_type,
        y_mngr_::responses_type
    > pmd_;

    {   // [ Proposal sample ]
        const long n_p_pars_kss = -1;   //n_batch;
        boost::timer t;

        generate_function_n<math::fun_wrap::log_unnormalized_pdf_>(
            boost::begin( p_pars ),
            boost::begin( p_lpdfs ),
            n_proposal,
            mproposal,
            urng
        );

        BOOST_ASSERT( size(p_pars) == size(p_lpdfs) );
    }

    // Loops over batches
    for(unsigned i = 0; i<n_batch; i++){
        ia_pr_mngrs >> pr_mngr;
        y_mngr.clear_responses();
        y_mngr.update_responses(
            boost::begin( pr_mngr.records() ),
            boost::end( pr_mngr.records() ),
            entry_bound
        );
        xm_mngr.update_covariates(
            boost::size(
                y_mngr.responses()
            )
        );

        {   // [ Posterior sample ]
            boost::timer t;
            pmd_ pmd(
                mprior,
                xm_mngr.model_wrapper().model(),
                xm_mngr.covariates(),
                y_mngr.responses()
            );
            iws.resize( boost::size( p_pars ) );

            t_pars.clear();
            t_pars.resize( n_t_pars );
            model_is.prepare_weights(
                pmd,
                boost::begin( p_pars ),
                boost::end( p_pars ),
                boost::begin( p_lpdfs )
            );
            model_is.generate_n(
                boost::begin(t_pars),
                n_t_pars,
                boost::begin( p_pars ),
                urng
            );

            oa_t_pars << t_pars;
            val_ plt = e_cdf::proportion_less_than(
                boost::begin( t_pars ),
                boost::end( t_pars ),
                pr_mngr.parameter_wrapper().parameter()
            );
            cgs.push_back( plt );
            ofs_cg << plt << ' ';
            ofs_cg.flush();
            const char* str = "i = %1%, t = %2%, pws = %3%";
            if(i%n_batch_mod == 0){
                format f(str);
                f
                % i
                % t.elapsed()
                % model_is.pws();
                out << std::endl <<  f.str();
            }
        }// Proposal sample
    }// loop over batches

    {   // Kolmorov Smirnov of the cgs
        typedef math::uniform_distribution<val_> munif_;
        long n_ks = n_batch;
        BOOST_ASSERT( n_batch % n_ks == 0 );
        vals_ kss;
        kss.reserve( n_batch );
        e_cdf::sequential_kolmogorov_smirnov_distance(
            munif_(),
            boost::begin( cgs ),
            boost::end( cgs ),
            n_ks,
            std::back_inserter(kss)
        );
        ofs_ ofs_ks(ks_path.c_str());
        std::copy(
            boost::begin( kss ),
            boost::end( kss ),
            std::ostream_iterator<val_>(ofs_ks," ")
        );
    }

    out << "<-" << std::endl;
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NETKET_ADAMAX_HPP
#define NETKET_ADAMAX_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <complex>
#include <iostream>
#include "abstract_optimizer.hpp"

namespace netket {

class AdaMax : public AbstractOptimizer {
  int npar_;

  double alpha_;
  double beta1_;
  double beta2_;

  Eigen::VectorXd ut_;
  Eigen::VectorXd mt_;

  double niter_;
  double niter_reset_;

  double epscut_;

 public:
  explicit AdaMax(double alpha = 0.001, double beta1 = 0.9,
                  double beta2 = 0.999, double epscut = 1.0e-7)
      : alpha_(alpha), beta1_(beta1), beta2_(beta2), epscut_(epscut) {
    npar_ = -1;
    niter_ = 0;
    niter_reset_ = -1;

    PrintParameters();
  }

  void PrintParameters() {
    InfoMessage() << "Adamax optimizer initialized with these parameters :"
                  << std::endl;
    InfoMessage() << "Alpha = " << alpha_ << std::endl;
    InfoMessage() << "Beta1 = " << beta1_ << std::endl;
    InfoMessage() << "Beta2 = " << beta2_ << std::endl;
    InfoMessage() << "Epscut = " << epscut_ << std::endl;
  }

  void Init(int npar) override {
    npar_ = npar;
    ut_.setZero(npar_);
    mt_.setZero(npar_);

    niter_ = 0;
  }

  void Update(const Eigen::VectorXd &grad,
              Eigen::Ref<Eigen::VectorXd> pars) override {
    assert(npar_ > 0);

    mt_ = beta1_ * mt_ + (1. - beta1_) * grad;

    for (int i = 0; i < npar_; i++) {
      ut_(i) = std::max(std::max(std::abs(grad(i)), beta2_ * ut_(i)), epscut_);
    }
    niter_ += 1.;
    if (niter_reset_ > 0) {
      if (niter_ > niter_reset_) {
        niter_ = 1;
      }
    }

    double eta = alpha_ / (1. - std::pow(beta1_, niter_));
    for (int i = 0; i < npar_; i++) {
      pars(i) -= eta * mt_(i) / ut_(i);
    }
  }

  void Reset() override {
    ut_ = Eigen::VectorXd::Zero(npar_);
    mt_ = Eigen::VectorXd::Zero(npar_);
    niter_ = 0;
  }

  void SetResetEvery(double niter_reset) { niter_reset_ = niter_reset; }
};

}  // namespace netket

#endif


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

    @copyright 2016 NumScale SAS

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

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

  /*!

    @ingroup group-arithmetic
    This function object computes two reals @c r0 and @c r1 (in an std::pair)
    such that:

    @code
    x == r0+r1
    @endcode

    and where @c r0 and @c r1 bit pattern don't overlap.

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

    @par Example:

      @snippet two_split.cpp two_split

    @par Possible output:

      @snippet two_split.txt two_split

  **/
  std::pair<Value, Value> two_split(Value const& x);
} }
#endif

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

#endif


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/bundle_adjustment/optimize_relative_position_with_known_rotation.h"

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <glog/logging.h>
#include <algorithm>
#include <vector>

#include "theia/math/util.h"
#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/triangulation/triangulation.h"

namespace theia {
namespace {

// Creates the constraint matrix such that ||A * t|| is minimized, where A is
// R_i * f_i x R_j * f_j. Given known rotations, we can solve for the
// relative translation from this constraint matrix.
void CreateConstraintMatrix(
    const std::vector<FeatureCorrespondence>& correspondences,
    const Eigen::Vector3d& rotation1,
    const Eigen::Vector3d& rotation2,
    Eigen::MatrixXd* constraint_matrix) {
  constraint_matrix->resize(3, correspondences.size());

  Eigen::Matrix3d rotation_matrix1;
  ceres::AngleAxisToRotationMatrix(
      rotation1.data(), ceres::ColumnMajorAdapter3x3(rotation_matrix1.data()));
  Eigen::Matrix3d rotation_matrix2;
  ceres::AngleAxisToRotationMatrix(
      rotation2.data(), ceres::ColumnMajorAdapter3x3(rotation_matrix2.data()));

  for (int i = 0; i < correspondences.size(); i++) {
    const Eigen::Vector3d rotated_feature1 =
        rotation_matrix1.transpose() *
        correspondences[i].feature1.point_.homogeneous();
    const Eigen::Vector3d rotated_feature2 =
        rotation_matrix2.transpose() *
        correspondences[i].feature2.point_.homogeneous();

    constraint_matrix->col(i) =
        rotated_feature2.cross(rotated_feature1).transpose() *
        rotation_matrix1.transpose();
  }
}

// Determines if the majority of the points are in front of the cameras. This is
// useful for determining the sign of the relative position. Returns true if
// more than 50% of correspondences are in front of both cameras and false
// otherwise.
bool MajorityOfPointsInFrontOfCameras(
    const std::vector<FeatureCorrespondence>& correspondences,
    const Eigen::Vector3d& rotation1,
    const Eigen::Vector3d& rotation2,
    const Eigen::Vector3d& relative_position) {
  // Compose the relative rotation.
  Eigen::Matrix3d rotation_matrix1, rotation_matrix2;
  ceres::AngleAxisToRotationMatrix(
      rotation1.data(), ceres::ColumnMajorAdapter3x3(rotation_matrix1.data()));
  ceres::AngleAxisToRotationMatrix(
      rotation2.data(), ceres::ColumnMajorAdapter3x3(rotation_matrix2.data()));
  const Eigen::Matrix3d relative_rotation_matrix =
      rotation_matrix2 * rotation_matrix1.transpose();

  // Tests all points for cheirality.
  int num_points_in_front_of_cameras = 0;
  for (const FeatureCorrespondence& match : correspondences) {
    if (IsTriangulatedPointInFrontOfCameras(match,
                                            relative_rotation_matrix,
                                            relative_position)) {
      ++num_points_in_front_of_cameras;
    }
  }

  return num_points_in_front_of_cameras > (correspondences.size() / 2);
}

}  // namespace

// Given known camera rotations and feature correspondences, this method solves
// for the relative translation that optimizes the epipolar error
// f_i * E * f_j^t = 0.
bool OptimizeRelativePositionWithKnownRotation(
    const std::vector<FeatureCorrespondence>& correspondences,
    const Eigen::Vector3d& rotation1,
    const Eigen::Vector3d& rotation2,
    Eigen::Vector3d* relative_position) {
  CHECK_NOTNULL(relative_position);

  // Set the initial relative position to random. This helps avoid a bad local
  // minima that is achieved from poor initialization.
  relative_position->setRandom();

  // Constants used for the IRLS solving.
  const double eps = 1e-5;
  const int kMaxIterations = 100;
  const int kMaxInnerIterations = 10;
  const double kMinWeight = 1e-7;

  // Create the constraint matrix from the known correspondences and rotations.
  Eigen::MatrixXd constraint_matrix;
  CreateConstraintMatrix(correspondences,
                         rotation1,
                         rotation2,
                         &constraint_matrix);

  // Initialize the weighting terms for each correspondence.
  Eigen::VectorXd weights(correspondences.size());
  weights.setConstant(1.0);

  // Solve for the relative positions using a robust IRLS.
  double cost = 0;
  int num_inner_iterations = 0;
  for (int i = 0;
       i < kMaxIterations && num_inner_iterations < kMaxInnerIterations;
       i++) {
    // Limit the minimum weight at kMinWeight.
    weights = (weights.array() < kMinWeight).select(kMinWeight, weights);

    // Apply the weights to the constraint matrix.
    const Eigen::Matrix3d lhs = constraint_matrix *
                                weights.asDiagonal().inverse() *
                                constraint_matrix.transpose();

    // Solve for the relative position which is the null vector of the weighted
    // constraints.
    const Eigen::Vector3d new_relative_position =
        lhs.jacobiSvd(Eigen::ComputeFullU).matrixU().rightCols<1>();

    // Update the weights based on the current errors.
    weights =
        (new_relative_position.transpose() * constraint_matrix).array().abs();

    // Compute the new cost.
    const double new_cost = weights.sum();

    // Check for convergence.
    const double delta = std::max(std::abs(cost - new_cost),
                                  1 - new_relative_position.squaredNorm());

    // If we have good convergence, attempt an inner iteration.
    if (delta <= eps) {
      ++num_inner_iterations;
    } else {
      num_inner_iterations = 0;
    }

    cost = new_cost;
    *relative_position = new_relative_position;
  }

  // The position solver above does not consider the sign of the relative
  // position. We can determine the sign by choosing the sign that puts the most
  // points in front of the camera.
  if (!MajorityOfPointsInFrontOfCameras(correspondences,
                                        rotation1,
                                        rotation2,
                                        *relative_position)) {
    *relative_position *= -1.0;
  }

  return true;
}

}  // namespace theia


//
// Created by philipp on 13.01.20.
//

#ifndef FUNNELS_CPP_EX1_HEU_HH
#define FUNNELS_CPP_EX1_HEU_HH

#include <Eigen/Core>
#include <cmath>
#include <sstream>
#include <algorithm>

#include <funnels/utils.hh>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795d
#endif

// Attention, in example 1, velocity coincides with the control
// signal
// Attention, assumes constant velocity/input

const double max_r_step = 0.1; // different funnel sizes in a family
const double min_r = 0.001; // Minimal radius
const double max_r = 0.5; // Minimal radius
const double max_alpha_step = max_r_step*max_r_step; // Minimal step for alpha
const double min_alpha = min_r*min_r; // Minimal alpha for ellipsoids
const double max_alpha = max_r*max_r; // Minimal alpha for ellipsoids
const double max_vel = 1.; // Norm of velocity
const double min_vel_diff = 0.1; // Norm difference in velocity
const double min_ang_diff = 20.*M_PI/180.;
const double gamma_conv = 3.; // exponential convergence rate
const double max_traj_length = 0.2; // Length of the new segment
const double dt_step = 0.05; //time step between two verification points
const double max_time = 10.;
const double min_englobe_fac = 1.1;

Eigen::Matrix2d getR(double alpha);

template<class DERIVED>
double get_smallest_englobing(double r_src,
    const Eigen::MatrixBase<DERIVED> &delta_vel){
  return min_englobe_fac*(r_src + delta_vel.norm());
}

double get_smallest_englobing(double r_src, double d_v_norm);

template <class FUN_PTR_T>
FUN_PTR_T get_this_funnel(const FUN_PTR_T &src, double radius,
    const Eigen::Vector4d &x0, const Eigen::Vector2d u){

  using fun_t = typename FUN_PTR_T::element_type;

  // Get maximal time before the maximal length is attained
  // or the playground is left
  double n_u = u.norm()+1e-200;
  double t_max = std::min(max_time, max_traj_length/n_u);
  // distance to x/y-border
  for (size_t i=0; i<2; i++){
    if(x0(i)>=0. && u(i)>=0.){
      t_max = std::min(t_max, (1.-x0(i))/(u(i)+1e-200));
    }else if (x0(i)<=0. && u(i)<=0.){
      t_max = std::min(t_max, (1.+x0(i))/(u(i)+1e-200));
    }
  }
  if (t_max < 3.*dt_step){
    // trajectory is too short
    // initial is on the boundary and we are pointing outwards
    return nullptr;
  }

  // Convert to number of points
  size_t this_n_verif = t_max/dt_step + 3;

  // todo check if this funnel already exists in the
  // funnel system
  // get the new name
  std::stringstream sstream;
  sstream.setf(std::ios::fixed);
  sstream.precision(2);

  sstream << "fun_";
  for(size_t i=0; i<4; i++){
    sstream << (double) x0(i) << "_";
  }
  sstream << radius;
  std::string name = sstream.str();
  std::replace( name.begin(), name.end(), '.', 'p');
  std::replace( name.begin(), name.end(), '-', 'm');

  // If exactly this funnel exists, the name will exist
  if (utils_ext::loc_map.find(name)){
    return nullptr;
  }

  FUN_PTR_T new_fun = src->make_copy(name, this_n_verif);

  // set the new radius
  new_fun->set_P(src->get_P()*(src->get_alpha()/(radius*radius)));
  // and convergence
  new_fun->set_gamma(gamma_conv);

  new_fun->compute(x0, 0., t_max, u);

  // He starts a family
  new_fun->start_family(new_fun);

  return new_fun;
}

template <class FUN_PTR_T>
std::vector<FUN_PTR_T> get_children(FUN_PTR_T &parent){

  using fun_t = typename FUN_PTR_T::element_type;
  using matrix_t = typename fun_t::matrix_t;

  FUN_PTR_T this_child_fun;
  FUN_PTR_T this_parent_fun = parent;

  std::stringstream sstream;
  std::string name;

  // Original unscaled matrix
  matrix_t P_p = parent->get_P()*parent->get_alpha();

  sstream.setf(std::ios::fixed);
  sstream.precision(2);

  std::vector<FUN_PTR_T> fun_vec_child;

  const double alpha_step = max_alpha_step;
  const double r_step = max_r_step;

  // Radius of parent
  double alpha = parent->get_alpha();

  if(std::sqrt(alpha) < min_r + r_step){
    return fun_vec_child;
  }

  alpha = std::max(min_alpha, (std::sqrt(alpha)-r_step)*(std::sqrt(alpha)-r_step));

  while(alpha>=min_alpha){
    // Get the name
    sstream.str(std::string());
    sstream << "fun_";
    for(size_t i=0; i<4; i++){
      sstream << (double) parent->x0()(i) << "_";
    }
    sstream << std::sqrt(alpha);
    name = sstream.str();
    std::replace( name.begin(), name.end(), '.', 'p');
    std::replace( name.begin(), name.end(), '-', 'm');
    if (utils_ext::loc_map.find(name)){
      // Already exists
      return fun_vec_child;
    }

    this_child_fun = this_parent_fun->make_copy(name, this_parent_fun->size());
    // set the new radius
    this_child_fun->set_P(P_p/alpha);
    this_child_fun->set_cyclic(this_parent_fun->get_cyclic());
    // And convergence
    this_child_fun->set_gamma(gamma_conv);

    fun_t::set_parent(this_parent_fun, this_child_fun);
    fun_vec_child.emplace_back(this_child_fun);
    this_parent_fun = this_child_fun;
    this_child_fun = nullptr;
    // Update alpha
    alpha = (std::sqrt(alpha)-r_step); // New alpha as r
    if (alpha <= min_r){
      break; // Necessary as negative radii become positive in next step
    }
    alpha *= alpha; // Convert to alpha
  }
  return fun_vec_child;
}

template<class FUN_PTR_T, class FUN_SYS>
void add_new_funnels(const FUN_PTR_T& src,
    double t_src, FUN_SYS &fun_sys){

  using fun_t = typename FUN_PTR_T::element_type;
  using vector_t = typename fun_t::vector_t;
  using matrix_t = typename fun_t::matrix_t;

  // Return val
  FUN_PTR_T fun_parent;
  std::vector<FUN_PTR_T> fun_vec_child;

  // Radius of source
  double r_src = std::sqrt(src->get_alpha());
  if(r_src>=0.99*max_r){
    return; // Quick exit
  }

  Eigen::Vector2d v_src = src->x().block(2,0,2,1);
  double n_v_src = v_src.norm();
  Eigen::Vector2d v_src_n;
  if (n_v_src<1e-10){
    // The funnel is static -> any direction will do
    n_v_src = 0.;
    v_src_n(0) = 1.;
    v_src_n(1) = 0.;
  }else{
    v_src_n = v_src/(n_v_src+1e-200); // Normalized
  }

  //Initial position
  size_t idx_t = (src->t().array()-t_src).array().abs().minCoeff();
  // Branching point of new funnels
  Eigen::Vector2d pos_t = src->x().block(0,idx_t,2,1);

  // Create all possible successors with min_ang_diff
  Eigen::Vector2d new_vel_n, new_vel;
  Eigen::Vector4d x0;
  double n_new_vel, tgt_radius;
  double d_alpha = -M_PI;
  while (d_alpha <= M_PI-min_ang_diff/2){

    // Desired new velocity direction
    new_vel_n = getR(d_alpha)*v_src_n;

    // Loop over all allowed velocity norms
    for (double delta_new_vel = -2.*max_vel; delta_new_vel<=2.*max_vel;
        delta_new_vel+=min_vel_diff){
      n_new_vel = n_v_src + delta_new_vel;
      if(n_new_vel<-max_vel || n_new_vel > max_vel){
        // New velocity norm is forbidden
        continue;
      }
      if ((std::abs(n_new_vel)<1e-10) && (d_alpha != -M_PI/2) ){
        // No need to add many static funnels with "different"
        // directions
        continue;
      }
      tgt_radius = get_smallest_englobing(r_src, std::abs(delta_new_vel));
      if (tgt_radius>max_r){
        // Minimal funnel size needed to englobe is larger then
        // the maximal size
        continue;
      }
      // Now we have a new parent funnel candidate
      x0.block(0,0,2,1) = pos_t;
      // todo refactor norm vs normalized
      x0.block(2,0,2,1) = n_new_vel*new_vel_n;
      // Parent
      fun_parent = get_this_funnel(src, tgt_radius, x0, n_new_vel*new_vel_n);
      if (fun_parent == nullptr){
        continue;
      }else{
        // add
        fun_sys.add_funnel(fun_parent, true);
        // all_children
        fun_vec_child = get_children(fun_parent);
        for (auto a_f : fun_vec_child){
          fun_sys.add_funnel(a_f);
        }
      }
    } // Done velocity
    d_alpha += min_ang_diff; // Update angles
  }// Done angles
  // All calculated
  return;
}

#endif //FUNNELS_CPP_EX1_HEU_HH


#include <NTL/vec_ZZ.h>

#include <NTL/new.h>

NTL_START_IMPL

NTL_vector_impl(ZZ,vec_ZZ)

NTL_eq_vector_impl(ZZ,vec_ZZ)

NTL_io_vector_impl(ZZ,vec_ZZ)

void InnerProduct(ZZ& xx, const vec_ZZ& a, const vec_ZZ& b)
{
   ZZ t1, x;

   long n = min(a.length(), b.length());
   long i;

   clear(x);
   for (i = 1; i <= n; i++) {
      mul(t1, a(i), b(i));
      add(x, x, t1);
   }

   xx = x;
}

void mul(vec_ZZ& x, const vec_ZZ& a, const ZZ& b_in)
{
   ZZ b = b_in;
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      mul(x[i], a[i], b);
}

void mul(vec_ZZ& x, const vec_ZZ& a, long b)
{
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      mul(x[i], a[i], b);
}

void add(vec_ZZ& x, const vec_ZZ& a, const vec_ZZ& b)
{
   long n = a.length();
   if (b.length() != n) Error("vector add: dimension mismatch");

   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      add(x[i], a[i], b[i]);
}

void sub(vec_ZZ& x, const vec_ZZ& a, const vec_ZZ& b)
{
   long n = a.length();
   if (b.length() != n) Error("vector sub: dimension mismatch");
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      sub(x[i], a[i], b[i]);
}

void clear(vec_ZZ& x)
{
   long n = x.length();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

void negate(vec_ZZ& x, const vec_ZZ& a)
{
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      negate(x[i], a[i]);
}

long IsZero(const vec_ZZ& a)
{
   long n = a.length();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

vec_ZZ operator+(const vec_ZZ& a, const vec_ZZ& b)
{
   vec_ZZ res;
   add(res, a, b);
   NTL_OPT_RETURN(vec_ZZ, res);
}

vec_ZZ operator-(const vec_ZZ& a, const vec_ZZ& b)
{
   vec_ZZ res;
   sub(res, a, b);
   NTL_OPT_RETURN(vec_ZZ, res);
}

vec_ZZ operator-(const vec_ZZ& a)
{
   vec_ZZ res;
   negate(res, a);
   NTL_OPT_RETURN(vec_ZZ, res);
}

ZZ operator*(const vec_ZZ& a, const vec_ZZ& b)
{
   ZZ res;
   InnerProduct(res, a, b);
   NTL_OPT_RETURN(ZZ, res);
}

void VectorCopy(vec_ZZ& x, const vec_ZZ& a, long n)
{
   if (n < 0) Error("VectorCopy: negative length");
   if (NTL_OVERFLOW(n, 1, 0)) Error("overflow in VectorCopy");

   long m = min(n, a.length());

   x.SetLength(n);

   long i;

   for (i = 0; i < m; i++)
      x[i] = a[i];

   for (i = m; i < n; i++)
      clear(x[i]);
}

NTL_END_IMPL


//Copyright (c) 2013 Singapore-MIT Alliance for Research and Technology
//Licensed under the terms of the MIT License, as described in the file:
//   license.txt   (http://opensource.org/licenses/MIT)

/*
 * ReactionTimeDistributions.hpp
 *
 * \author Li Zhemin
 * \author Seth N. Hetu
 */

#pragma once

#include <boost/random.hpp>
#include <boost/math/distributions.hpp>
#include <boost/version.hpp>

//Boost::Random is slightly messy re: uniform_int_distribution >1.47.0
//For now we just workaround via a macro and another define (which is undone after this file).
#if BOOST_VERSION >= 104700
#include <boost/random/uniform_int_distribution.hpp>
#define  boost_uniform_int  boost::random::uniform_int_distribution
#else
#include <boost/random/uniform_int.hpp>
#define  boost_uniform_int  boost::uniform_int
#endif

namespace sim_mob {

///Simple base class for reaction time distributions
class ReactionTimeDist {
public:
    virtual ~ReactionTimeDist() {}
    virtual double getReactionTime() = 0;
};

///Pre-specialized sub-classes for reaction time distributions
class UniformReactionTimeDist : public ReactionTimeDist {
private:
    boost::mt19937 gen;
    boost_uniform_int<> dist;

public:
    UniformReactionTimeDist(double min, double max)
     : gen(), dist(min, max)
    {}
    virtual ~UniformReactionTimeDist() {}

    virtual double getReactionTime() { return dist(gen); }
};

///Pre-specialized sub-classes for reaction time distributions
class NormalReactionTimeDist : public ReactionTimeDist {
private:
    boost::mt19937 gen;
    boost::normal_distribution<double> dist;
    boost::variate_generator<boost::mt19937, boost::normal_distribution<double> > varGen;

public:
    NormalReactionTimeDist(double mean, double stdev)
     : gen(), dist(mean, stdev), varGen(gen, dist)
    {}
    virtual ~NormalReactionTimeDist(){}

    virtual double getReactionTime() { return varGen(); }
};

///Pre-specialized sub-classes for reaction time distributions
class LognormalReactionTimeDist : public ReactionTimeDist {
private:
    boost::mt19937 gen;
    boost::lognormal_distribution<double> dist;
    boost::variate_generator<boost::mt19937, boost::lognormal_distribution<double> > varGen;

public:
    LognormalReactionTimeDist(double mean, double stdev)
     : gen(), dist(mean, stdev), varGen(gen, dist)
    {}
    virtual ~LognormalReactionTimeDist(){}

    virtual double getReactionTime() { return varGen(); }
};

}

//Remove our macro; we don't need it after this.
#undef  boost_uniform_int