#include <jni.h>
#include <android/log.h>

#include <cpu-features.h>

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include <Eigen/Core>

using namespace std;

#define  LOG_TAG	"MatrixOp"
#define  LOGI(...)	__android_log_print(ANDROID_LOG_INFO, LOG_TAG, __VA_ARGS__)
#define  LOGE(...)	__android_log_print(ANDROID_LOG_ERROR, LOG_TAG, __VA_ARGS__)

template <typename T1, typename T2> void c_mulVector(T1 matrix[], T2 vector[], int rows, int cols, T1 result[]);
//void arm_neon_mulVector3F(float matrix[], float vector[], float result[]);
template <typename T1, typename T2> void c_mulMatrix(T1 matrix1[], T2 matrix2[], int rows1, int cols1, int cols2, T1 result[]);
//void arm_neon_mulMatrix3F(float matrix1[], float matrix2[], float result[]);

//bool neon = (android_getCpuFamily() == ANDROID_CPU_FAMILY_ARM) && ((android_getCpuFeatures() & ANDROID_CPU_ARM_FEATURE_NEON) != 0);

extern "C"
{
	JNIEXPORT jfloatArray JNICALL Java_it_unibo_slam_datatypes_eigen_typefloat_EigenMatrixF_matrixMulVectorF(JNIEnv *env, jobject obj,
			jfloatArray m, jfloatArray v, jint numRowsMatrix, jint numColsMatrix);
	JNIEXPORT jfloatArray JNICALL Java_it_unibo_slam_datatypes_eigen_typefloat_EigenMatrixF_matrixMulMatrixF(JNIEnv *env, jobject obj,
			jfloatArray m1, jfloatArray m2, jint numRowsMatrix1, jint numColsMatrix1, jint numColsMatrix2);
	JNIEXPORT jfloatArray JNICALL Java_it_unibo_slam_datatypes_eigen_typefloat_EigenMatrixF_matrixMulVectorFD(JNIEnv *env, jobject obj,
			jfloatArray m, jdoubleArray v, jint numRowsMatrix, jint numColsMatrix);
	JNIEXPORT jfloatArray JNICALL Java_it_unibo_slam_datatypes_eigen_typefloat_EigenMatrixF_matrixMulMatrixFD(JNIEnv *env, jobject obj,
			jfloatArray m1, jdoubleArray m2, jint numRowsMatrix1, jint numColsMatrix1, jint numColsMatrix2);
	JNIEXPORT jdoubleArray JNICALL Java_it_unibo_slam_datatypes_eigen_typedouble_EigenMatrixD_matrixMulVectorD(JNIEnv *env, jobject obj,
			jdoubleArray m, jdoubleArray v, jint numRowsMatrix, jint numColsMatrix);
	JNIEXPORT jdoubleArray JNICALL Java_it_unibo_slam_datatypes_eigen_typedouble_EigenMatrixD_matrixMulMatrixD(JNIEnv *env, jobject obj,
			jdoubleArray m1, jdoubleArray m2, jint numRowsMatrix1, jint numColsMatrix1, jint numColsMatrix2);
	JNIEXPORT jdoubleArray JNICALL Java_it_unibo_slam_datatypes_eigen_typedouble_EigenMatrixD_matrixMulVectorDF(JNIEnv *env, jobject obj,
			jdoubleArray m, jfloatArray v, jint numRowsMatrix, jint numColsMatrix);
	JNIEXPORT jdoubleArray JNICALL Java_it_unibo_slam_datatypes_eigen_typedouble_EigenMatrixD_matrixMulMatrixDF(JNIEnv *env, jobject obj,
			jdoubleArray m1, jfloatArray m2, jint numRowsMatrix1, jint numColsMatrix1, jint numColsMatrix2);
}

JNIEXPORT jfloatArray JNICALL Java_it_unibo_slam_datatypes_eigen_typefloat_EigenMatrixF_matrixMulVectorF(JNIEnv *env, jobject obj,
		jfloatArray m, jfloatArray v, jint numRowsMatrix, jint numColsMatrix)
{
	jfloatArray result;
	int rows = numRowsMatrix;
	int cols = numColsMatrix;
	int sizeMatrix = rows * cols;
	result = env->NewFloatArray(rows);

	float *tempResult = new float[rows];
	float *tempM = new float[sizeMatrix];
	float *tempV = new float[cols];

	env->GetFloatArrayRegion(m, 0, sizeMatrix, tempM);
	env->GetFloatArrayRegion(v, 0, cols, tempV);

	c_mulVector<float, float>(tempM, tempV, rows, cols, tempResult);

	env->SetFloatArrayRegion(result, 0, rows, tempResult);

	delete [] tempResult;
	delete [] tempM;
	delete [] tempV;

	return result;
}

JNIEXPORT jfloatArray JNICALL Java_it_unibo_slam_datatypes_eigen_typefloat_EigenMatrixF_matrixMulMatrixF(JNIEnv *env, jobject obj,
		jfloatArray m1, jfloatArray m2, jint numRowsMatrix1, jint numColsMatrix1, jint numColsMatrix2)
{
	jfloatArray result;
	int rows1 = numRowsMatrix1;
	int cols1 = numColsMatrix1;
	int cols2 = numColsMatrix2;
	int sizeMatrix1 = rows1 * cols1;
	int sizeMatrix2 = cols1 * cols2;
	int sizeResult = rows1 * cols2;
	result = env->NewFloatArray(sizeResult);

	float *tempResult = new float[sizeResult];
	float *tempM1 = new float[sizeMatrix1];
	float *tempM2 = new float[sizeMatrix2];

	env->GetFloatArrayRegion(m1, 0, sizeMatrix1, tempM1);
	env->GetFloatArrayRegion(m2, 0, sizeMatrix2, tempM2);

	c_mulMatrix<float, float>(tempM1, tempM2, rows1, cols1, cols2, tempResult);

	env->SetFloatArrayRegion(result, 0, sizeResult, tempResult);

	delete [] tempResult;
	delete [] tempM1;
	delete [] tempM2;

	return result;
}

JNIEXPORT jfloatArray JNICALL Java_it_unibo_slam_datatypes_eigen_typefloat_EigenMatrixF_matrixMulVectorFD(JNIEnv *env, jobject obj,
		jfloatArray m, jdoubleArray v, jint numRowsMatrix, jint numColsMatrix)
{
	jfloatArray result;
	int rows = numRowsMatrix;
	int cols = numColsMatrix;
	int sizeMatrix = rows * cols;
	result = env->NewFloatArray(rows);

	float *tempResult = new float[rows];
	float *tempM = new float[sizeMatrix];
	double *tempV = new double[cols];

	env->GetFloatArrayRegion(m, 0, sizeMatrix, tempM);
	env->GetDoubleArrayRegion(v, 0, cols, tempV);

	c_mulVector<float, double>(tempM, tempV, rows, cols, tempResult);

	env->SetFloatArrayRegion(result, 0, rows, tempResult);

	delete [] tempResult;
	delete [] tempM;
	delete [] tempV;

	return result;
}

JNIEXPORT jfloatArray JNICALL Java_it_unibo_slam_datatypes_eigen_typefloat_EigenMatrixF_matrixMulMatrixFD(JNIEnv *env, jobject obj,
		jfloatArray m1, jdoubleArray m2, jint numRowsMatrix1, jint numColsMatrix1, jint numColsMatrix2)
{
	jfloatArray result;
	int rows1 = numRowsMatrix1;
	int cols1 = numColsMatrix1;
	int cols2 = numColsMatrix2;
	int sizeMatrix1 = rows1 * cols1;
	int sizeMatrix2 = cols1 * cols2;
	int sizeResult = rows1 * cols2;
	result = env->NewFloatArray(sizeResult);

	float *tempResult = new float[sizeResult];
	float *tempM1 = new float[sizeMatrix1];
	double *tempM2 = new double[sizeMatrix2];

	env->GetFloatArrayRegion(m1, 0, sizeMatrix1, tempM1);
	env->GetDoubleArrayRegion(m2, 0, sizeMatrix2, tempM2);

	c_mulMatrix<float, double>(tempM1, tempM2, rows1, cols1, cols2, tempResult);

	env->SetFloatArrayRegion(result, 0, sizeResult, tempResult);

	delete [] tempResult;
	delete [] tempM1;
	delete [] tempM2;

	return result;
}

JNIEXPORT jdoubleArray JNICALL Java_it_unibo_slam_datatypes_eigen_typedouble_EigenMatrixD_matrixMulVectorD(JNIEnv *env, jobject obj,
		jdoubleArray m, jdoubleArray v, jint numRowsMatrix, jint numColsMatrix)
{
	jdoubleArray result;
	int rows = numRowsMatrix;
	int cols = numColsMatrix;
	int sizeMatrix = rows * cols;
	result = env->NewDoubleArray(rows);

	double *tempResult = new double[rows];
	double *tempM = new double[sizeMatrix];
	double *tempV = new double[cols];

	env->GetDoubleArrayRegion(m, 0, sizeMatrix, tempM);
	env->GetDoubleArrayRegion(v, 0, cols, tempV);

	c_mulVector<double, double>(tempM, tempV, rows, cols, tempResult);

	env->SetDoubleArrayRegion(result, 0, rows, tempResult);

	delete [] tempResult;
	delete [] tempM;
	delete [] tempV;

	return result;
}

JNIEXPORT jdoubleArray JNICALL Java_it_unibo_slam_datatypes_eigen_typedouble_EigenMatrixD_matrixMulMatrixD(JNIEnv *env, jobject obj,
		jdoubleArray m1, jdoubleArray m2, jint numRowsMatrix1, jint numColsMatrix1, jint numColsMatrix2)
{
	jdoubleArray result;
	int rows1 = numRowsMatrix1;
	int cols1 = numColsMatrix1;
	int cols2 = numColsMatrix2;
	int sizeMatrix1 = rows1 * cols1;
	int sizeMatrix2 = cols1 * cols2;
	int sizeResult = rows1 * cols2;
	result = env->NewDoubleArray(sizeResult);

	double *tempResult = new double[sizeResult];
	double *tempM1 = new double[sizeMatrix1];
	double *tempM2 = new double[sizeMatrix2];

	env->GetDoubleArrayRegion(m1, 0, sizeMatrix1, tempM1);
	env->GetDoubleArrayRegion(m2, 0, sizeMatrix2, tempM2);

	c_mulMatrix<double, double>(tempM1, tempM2, rows1, cols1, cols2, tempResult);

	env->SetDoubleArrayRegion(result, 0, sizeResult, tempResult);

	delete [] tempResult;
	delete [] tempM1;
	delete [] tempM2;

	return result;
}

JNIEXPORT jdoubleArray JNICALL Java_it_unibo_slam_datatypes_eigen_typedouble_EigenMatrixD_matrixMulVectorDF(JNIEnv *env, jobject obj,
		jdoubleArray m, jfloatArray v, jint numRowsMatrix, jint numColsMatrix)
{
	jdoubleArray result;
	int rows = numRowsMatrix;
	int cols = numColsMatrix;
	int sizeMatrix = rows * cols;
	result = env->NewDoubleArray(rows);

	double *tempResult = new double[rows];
	double *tempM = new double[sizeMatrix];
	float *tempV = new float[cols];

	env->GetDoubleArrayRegion(m, 0, sizeMatrix, tempM);
	env->GetFloatArrayRegion(v, 0, cols, tempV);

	c_mulVector<double, float>(tempM, tempV, rows, cols, tempResult);

	env->SetDoubleArrayRegion(result, 0, rows, tempResult);

	delete [] tempResult;
	delete [] tempM;
	delete [] tempV;

	return result;
}

JNIEXPORT jdoubleArray JNICALL Java_it_unibo_slam_datatypes_eigen_typedouble_EigenMatrixD_matrixMulMatrixDF(JNIEnv *env, jobject obj,
		jdoubleArray m1, jfloatArray m2, jint numRowsMatrix1, jint numColsMatrix1, jint numColsMatrix2)
{
	jdoubleArray result;
	int rows1 = numRowsMatrix1;
	int cols1 = numColsMatrix1;
	int cols2 = numColsMatrix2;
	int sizeMatrix1 = rows1 * cols1;
	int sizeMatrix2 = cols1 * cols2;
	int sizeResult = rows1 * cols2;
	result = env->NewDoubleArray(sizeResult);

	double *tempResult = new double[sizeResult];
	double *tempM1 = new double[sizeMatrix1];
	float *tempM2 = new float[sizeMatrix2];

	env->GetDoubleArrayRegion(m1, 0, sizeMatrix1, tempM1);
	env->GetFloatArrayRegion(m2, 0, sizeMatrix2, tempM2);

	c_mulMatrix<double, float>(tempM1, tempM2, rows1, cols1, cols2, tempResult);

	env->SetDoubleArrayRegion(result, 0, sizeResult, tempResult);

	delete [] tempResult;
	delete [] tempM1;
	delete [] tempM2;

	return result;
}

template <typename T1, typename T2>
void c_mulMatrix(T1 matrix1[], T2 matrix2[], int rows1, int cols1, int cols2, T1 result[])
{
	T1 tempVal;

	for (int x = 0, y = 0; x < cols2; x++, y += cols1)
	{
		for (int i = 0; i < rows1; i++, result++)
		{
			tempVal = 0;
			for (int j = 0, k = 0; j < cols1; j++, k += rows1)
				tempVal += matrix1[i + k] * matrix2[j + y];
			*result = tempVal;
		}
	}
}

template <typename T1, typename T2>
void c_mulVector(T1 matrix[], T2 vector[], int rows, int cols, T1 result[])
{
	T1 tempVal;

	for (int i = 0; i < rows; i++)
	{
		tempVal = 0;
		for (int j = 0, k = 0; j < cols; j++, k += rows)
			tempVal += matrix[i + k] * (T1)vector[j];
		result[i] = tempVal;
	}
}

/*void arm_neon_mulVector3F(float matrix[], float vector[], float result[])
{
	int temp;
	asm volatile
	(
		"mov                    %3, #12						\n\t"   //r3 = 12
		"vld1.32                {d0, d1}, [%1]             	\n\t"   //Q0 = v
		"vld1.32                {d2, d3}, [%0], %3     		\n\t"   //Q1 = m
		"vld1.32                {d4, d5}, [%0], %3        	\n\t"   //Q2 = m+12
		"vld1.32                {d6, d7}, [%0], %3         	\n\t"   //Q3 = m+24

		"vmul.f32               q9, q1, d0[0]             	\n\t"   //Q9 = Q1*Q0[0]
		"vmla.f32               q9, q2, d0[1]              	\n\t"   //Q9 += Q2*Q0[1]
		"vmla.f32               q9, q3, d1[0]              	\n\t"   //Q9 += Q3*Q0[2]
		"vmov.f32               q0, q9           			\n\t"   //Q0 = q9

		"vst1.32                d0, [%2]!                	\n\t"   //r2 = D24
		"fsts                   s2, [%2]                 	\n\t"   //r2 = D25[0]

		:
		: "r" (matrix), "r" (vector), "r" (result), "r" (temp)
		: "q0", "q9", "q10", "q11", "q12", "q13", "memory"
	);
}

void arm_neon_mulMatrix3F(float matrix1[], float matrix2[], float result[])
{
	asm volatile
	(
		"vld1.32                {d0, d1}, [%1]!			\n\t"   //q0 = m1
		"vld1.32                {d2, d3}, [%1]!			\n\t"   //q1 = m1+4
		"flds                   s8, [%1]				\n\t"   //q2 = m1+8

		"vld1.32                {d6, d7}, [%0]			\n\t"   //q3[0] = m0
		"add                    %0, %0, #12				\n\t"   //q3[0] = m0
		"vld1.32                {d8, d9}, [%0]			\n\t"   //q4[0] = m0+12
		"add                    %0, %0, #12				\n\t"   //q3[0] = m0
		"vld1.32                {d10}, [%0]				\n\t"   //q5[0] = m0+24
		"add                    %0, %0, #8				\n\t"   //q3[0] = m0
		"flds                   s22, [%0]				\n\t"   //q2 = m1+8

		"vmul.f32               q6, q3, d0[0]			\n\t"   //q12 = q3 * d0[0]
		"vmul.f32               q7, q3, d1[1]			\n\t"   //q13 = q3 * d2[0]
		"vmul.f32               q8, q3, d3[0]			\n\t"   //q14 = q3 * d4[0]
		"vmla.f32               q6, q4, d0[1] 			\n\t"   //q12 = q9 * d0[1]
		"vmla.f32               q7, q4, d2[0]    		\n\t"   //q13 = q9 * d2[1]
		"vmla.f32               q8, q4, d3[1] 			\n\t"   //q14 = q9 * d4[1]
		"vmla.f32               q6, q5, d1[0]			\n\t"   //q12 = q10 * d0[0]
		"vmla.f32               q7, q5, d2[1]			\n\t"   //q13 = q10 * d2[0]
		"vmla.f32               q8, q5, d4[0]			\n\t"   //q14 = q10 * d4[0]

		"vmov.f32               q0, q8					\n\t"   //q14 = q10 * d4[0]
		"vst1.32                {d12, d13}, [%2]		\n\t"   //d = q12
		"add                    %2, %2, #12				\n\t"   //q3[0] = m0
		"vst1.32                {d14, d15}, [%2]		\n\t"   //d+4 = q13
		"add                    %2, %2, #12				\n\t"   //q3[0] = m0
		"vst1.32                {d0}, [%2]				\n\t"   //d+8 = q14
		"add                    %2, %2, #8				\n\t"   //q3[0] = m0
		"fsts                   s2, [%2]				\n\t"   //d = q12

		:
		: "r"(matrix1), "r"(matrix2), "r"(result)
		: "d8", "d9", "d10", "d11", "d12", "d13", "d14", "d15", "memory"
	);
}*/


#ifndef INCLUDE_SWIFT_VIO_SIMPLE_IMU_PROPAGATION_JACOBIAN_HPP_
#define INCLUDE_SWIFT_VIO_SIMPLE_IMU_PROPAGATION_JACOBIAN_HPP_

#include <Eigen/Core>
#include <okvis/Time.hpp>
#include <okvis/kinematics/Transformation.hpp>

namespace swift_vio {
/**
 * @brief SimpleImuPropagationJacobian computes Jacobians of propagated states
 * relative to initial states.
 * Error states are defined by okvis::kinematics::oplus and minus.
 * @warning: this function assumes that endEpoch and startEpoch differ small, e.g.,
 * less than 0.04 sec.
 */
class SimpleImuPropagationJacobian
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SimpleImuPropagationJacobian() {}

  void initialize(const okvis::Time startEpoch,
                  const okvis::Time endEpoch,
                  const okvis::kinematics::Transformation& endT_WB,
                  const Eigen::Matrix<double, 3, 1>& endV_W,
                  const Eigen::Matrix<double, 3, 1>& endOmega_B);

  SimpleImuPropagationJacobian(const okvis::Time startEpoch,
                               const okvis::Time endEpoch,
                               const okvis::kinematics::Transformation& endT_WB,
                               const Eigen::Matrix<double, 3, 1>& endV_W,
                               const Eigen::Matrix<double, 3, 1>& endOmega_B);

  inline void dp_dt_WB(Eigen::Matrix3d* j) {
    j->setIdentity();
  }

  inline void dp_dtheta_WB(Eigen::Matrix3d* j) {
    j->setZero();
  }

  inline void dp_dv_WB(Eigen::Matrix3d* j) {
    j->setIdentity();
    double stride = (endEpoch_ - startEpoch_).toSec();
    (*j)(0, 0) = stride;
    (*j)(1, 1) = stride;
    (*j)(2, 2) = stride;
  }

  inline void dtheta_dt_WB(Eigen::Matrix3d* j) {
    j->setZero();
  }

  inline void dtheta_dtheta_WB(Eigen::Matrix3d* j) {
    j->setIdentity();
  }

  inline void dtheta_dv_WB(Eigen::Matrix3d* j) {
    j->setZero();
  }

  inline void dp_dt(Eigen::Vector3d* j) {
    (*j) = endV_WB_W_;
  }

  inline void dtheta_dt(Eigen::Vector3d* j) {
    (*j) = endq_WB_ * endOmega_WB_B_;
  }

  static Eigen::Vector3d dp_dt(const Eigen::Vector3d& endV_WB_W) {
    return endV_WB_W;
  }

  static Eigen::Vector3d dtheta_dt(const Eigen::Vector3d& endOmega_WB_B,
                                   const Eigen::Quaterniond& endq_WB) {
    return endq_WB * endOmega_WB_B;
  }

  static Eigen::Matrix3d dp_dv_WB(double deltaTime);

  static Eigen::Matrix<double, 3, 2>
  dp_dunitgW(double deltaTime, const Eigen::Vector3d &gravityDirection,
             double gravityMagnitude);

  static Eigen::Matrix<double, 3, 2>
  dtheta_dunitgW(double deltaTime, const Eigen::Vector3d &gravityDirection,
                 double gravityMagnitude);

private:
  okvis::Time startEpoch_;
  okvis::Time endEpoch_;
  Eigen::Matrix<double, 3, 1> endp_WB_B_;
  Eigen::Quaterniond endq_WB_;
  Eigen::Matrix<double, 3, 1> endV_WB_W_;
  Eigen::Matrix<double, 3, 1> endOmega_WB_B_;
};
} // namespace swift_vio

#endif // INCLUDE_SWIFT_VIO_SIMPLE_IMU_PROPAGATION_JACOBIAN_HPP_


/*
 * Copyright 2015 David A. Boyuka II
 *
 * 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.
 */

/*
 * cblq-setops-fast.cpp
 *
 *  Created on: Jun 11, 2014
 *      Author: David A. Boyuka II
 */

#include <type_traits>
#include <vector>

#include <boost/smart_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/optional.hpp>

#include "pique/region/cblq/cblq.hpp"
#include "pique/setops/cblq/cblq-setops.hpp"

// Special values needed for optimization below
// -2 -> 0 | 2, -1 -> 1 | 2, 0 -> 2 | 2, 1 -> 2 | 1, 2 -> 2 | 0
// -2 -> COPY_R, -1 -> DELETE_R, 0 -> UNION, 1 -> DELETE_L, 2 -> COPY_L
enum struct cblq_union_action_t { COPY_R = -2, DELETE_R = -1, UNION = 0, DELETE_L = 1, COPY_L = 2, };

struct cblq_action_block_t {
	cblq_action_block_t() : action(cblq_union_action_t::UNION), count(0) {}
	cblq_action_block_t(cblq_union_action_t action, size_t count) :
		action(action), count(count)
	{}
	cblq_union_action_t action;
	size_t count;
};

template<int ndim>
inline static int count_two_codes(typename CBLQRegionEncoding<ndim>::cblq_word_t word) {
	int count = 0;
	for (int i = 0; i < CBLQRegionEncoding<ndim>::CODES_PER_WORD; i++) {
		count += (word & 0b10);
		word >>= CBLQRegionEncoding<ndim>::BITS_PER_CODE;
	}
	return count >> 1; // Divide by 2, since each 2-code added 2
}

template<int ndim, cblq_union_action_t inaction>
inline static void apply_unary_action_helper(
		size_t count,
		typename std::vector< typename CBLQRegionEncoding<ndim>::cblq_word_t >::const_iterator &in_it,
		typename std::vector< typename CBLQRegionEncoding<ndim>::cblq_word_t >::iterator &out_it,
		typename std::vector< cblq_action_block_t >::iterator &actionqueue_out_it)
{
	typedef typename CBLQRegionEncoding<ndim>::cblq_word_t cblq_word_t;
	size_t children = 0;
	do {
		const cblq_word_t in_word = *in_it;
		++in_it;
		if (inaction == cblq_union_action_t::COPY_L || inaction == cblq_union_action_t::COPY_R) {
			*out_it = in_word;
			++out_it;
		}
		children += count_two_codes<ndim>(in_word);
	} while (--count > 0);

	if (children) {
		*actionqueue_out_it = cblq_action_block_t(inaction, children);
		++actionqueue_out_it;
	}
}

template<int ndim>
inline static void apply_union_helper(
		typename std::vector< typename CBLQRegionEncoding<ndim>::cblq_word_t >::const_iterator &left_it,
		typename std::vector< typename CBLQRegionEncoding<ndim>::cblq_word_t >::const_iterator &right_it,
		typename std::vector< typename CBLQRegionEncoding<ndim>::cblq_word_t >::iterator &out_it,
		typename std::vector< cblq_action_block_t >::iterator &actionqueue_out_it)
{
	typedef typename CBLQRegionEncoding<ndim>::cblq_word_t cblq_word_t;

	cblq_word_t left_word = *left_it;
	cblq_word_t right_word = *right_it;
	++left_it;
	++right_it;

	cblq_word_t or_word = left_word | right_word; // 0/1 U 0/1 -> 0/1, 0 U 2 -> 2, 2 U 2 -> 2, 1 U 2 -> 3 (fix this)
	const cblq_word_t out_word = or_word & ~((or_word & CBLQRegionEncoding<ndim>::ONE_CODES_WORD) << 1); // Masks out 2's with a mask created from the 1's
	*out_it = out_word;
	++out_it;

	// Search for 2-codes in both words in parallel, and queue actions if found
	for (int codepos = 0; codepos < CBLQRegionEncoding<ndim>::CODES_PER_WORD; codepos++) {
		if (or_word & 0b10) {
			// -2 -> 0 | 2, -1 -> 1 | 2, 0 -> 2 | 2, 1 -> 2 | 1, 2 -> 2 | 0
			// -2 -> COPY_R, -1 -> DELETE_R, 0 -> UNION, 1 -> DELETE_L, 2 -> DELETE_R
			const cblq_union_action_t codediff_action = (cblq_union_action_t)((int64_t)(left_word & 0b11) - (int64_t)(right_word & 0b11));
			*actionqueue_out_it = cblq_action_block_t(codediff_action, 1);
			++actionqueue_out_it;
		}
		if (codepos < CBLQRegionEncoding<ndim>::CODES_PER_WORD - 1) {
			left_word >>= CBLQRegionEncoding<ndim>::BITS_PER_CODE;
			right_word >>= CBLQRegionEncoding<ndim>::BITS_PER_CODE;
			or_word >>= CBLQRegionEncoding<ndim>::BITS_PER_CODE;
		}
	}
}

template<typename block_t>
inline static void copy_bits(
		const size_t count,
		typename std::vector< block_t >::const_iterator &in_bits, block_t &in_headblock, int &in_shift_remaining,
		typename std::vector< block_t >::iterator &out_bits, block_t &out_headblock, int &out_shift_remaining)
{
	const int BITS_PER_BLOCK = (sizeof(block_t) << 3);
	ptrdiff_t scount = (ptrdiff_t)count;
	do {
		out_headblock |= (in_headblock << (BITS_PER_BLOCK - out_shift_remaining));

		ptrdiff_t bits_used = scount;
		if (out_shift_remaining < bits_used)
			bits_used = out_shift_remaining;
		if (in_shift_remaining < bits_used)
			bits_used = in_shift_remaining;

		if (bits_used == out_shift_remaining) {
			*out_bits = out_headblock;
			++out_bits;
			out_shift_remaining = BITS_PER_BLOCK;
			out_headblock = 0;
		} else {
			out_shift_remaining -= bits_used;
			out_headblock &= (~(block_t)0) >> out_shift_remaining; // Mask out excess bits
		}

		if (bits_used == in_shift_remaining) {
			++in_bits;
			in_headblock = *in_bits;
			in_shift_remaining = BITS_PER_BLOCK;
		} else {
			in_shift_remaining -= bits_used;
			in_headblock >>= bits_used;
		}

		scount -= bits_used;
	} while (scount > 0);
}

template<typename block_t>
inline static void skip_bits(
		const size_t count,
		typename std::vector< block_t >::const_iterator &in_bits, block_t &in_headblock, int &in_shift_remaining)
{
	const int BITS_PER_BLOCK = (sizeof(block_t) << 3);
	ptrdiff_t scount = (ptrdiff_t)count;
	do {
		if (in_shift_remaining <= scount) { // in_shift_remaining is the limiting factor
			scount -= in_shift_remaining;
			in_shift_remaining = BITS_PER_BLOCK;
			++in_bits;
			in_headblock = *in_bits;
		} else { // scount is the limiting factor
			in_shift_remaining -= scount;
			in_headblock >>= scount;
			break;
		}
	} while (scount > 0);
}

template<int ndim, typename semiword_block_t>
inline static void binary_union_set_op(
		bool has_dense_suffix, int non_dense_levels,
		typename std::vector< typename CBLQRegionEncoding<ndim>::cblq_word_t >::const_iterator &left_it,
		typename std::vector< typename CBLQRegionEncoding<ndim>::cblq_word_t >::const_iterator &right_it,
		typename std::vector< typename CBLQRegionEncoding<ndim>::cblq_word_t >::iterator &out_it,
		typename std::vector< semiword_block_t>::const_iterator &left_denseit,
		typename std::vector< semiword_block_t>::const_iterator &right_denseit,
		typename std::vector< semiword_block_t>::iterator &out_denseit,
		int &out_dense_remaining_bits,
		std::vector<size_t> &out_level_lens)
{
	typedef typename CBLQRegionEncoding<ndim>::cblq_word_t cblq_word_t;
	std::vector< cblq_action_block_t > this_actionqueue;
	std::vector< cblq_action_block_t > next_actionqueue = { cblq_action_block_t(cblq_union_action_t::UNION, 1) };

    for (int level = 0; level < non_dense_levels; level++) {
    	this_actionqueue = std::move(next_actionqueue);
    	next_actionqueue.clear();
    	next_actionqueue.resize(this_actionqueue.size() * CBLQRegionEncoding<ndim>::CODES_PER_WORD + 1); // All cblq_action_block_t's default to is_copy=false and left/right_count = 0
    	auto next_actionqueue_out_it = next_actionqueue.begin();

    	//const size_t level_len = this_actionqueue.size(); // not needed
    	const typename std::vector<cblq_word_t>::iterator out_it_before_level = out_it;

    	for (auto it = this_actionqueue.cbegin(), end_it = this_actionqueue.cend(); it != end_it; ++it) {
    		const cblq_action_block_t &curaction = *it;
    		const cblq_union_action_t action = curaction.action;
    		const size_t count = curaction.count;

    		switch (action) {
    		case cblq_union_action_t::UNION:
    			apply_union_helper<ndim>(left_it, right_it, out_it, next_actionqueue_out_it);
    			break;
    		case cblq_union_action_t::DELETE_L:
    			apply_unary_action_helper<ndim, cblq_union_action_t::DELETE_L>(count, left_it, out_it, next_actionqueue_out_it);
    			break;
    		case cblq_union_action_t::DELETE_R:
    			apply_unary_action_helper<ndim, cblq_union_action_t::DELETE_R>(count, right_it, out_it, next_actionqueue_out_it);
    			break;
    		case cblq_union_action_t::COPY_L:
    			apply_unary_action_helper<ndim, cblq_union_action_t::COPY_L>(count, left_it, out_it, next_actionqueue_out_it);
    			break;
    		case cblq_union_action_t::COPY_R:
    			apply_unary_action_helper<ndim, cblq_union_action_t::COPY_R>(count, right_it, out_it, next_actionqueue_out_it);
    			break;
    		}
    	}

    	const size_t num_output_words = out_it - out_it_before_level;
    	const size_t num_actions = next_actionqueue_out_it - next_actionqueue.begin();

    	out_level_lens[level] = num_output_words;
    	next_actionqueue.resize(num_actions);
    }

    if (has_dense_suffix) {
    	const int BITS_PER_BLOCK = (sizeof(semiword_block_t) << 3);
    	// Invariants: left/right/out_denseit always points to where the corresponding headblock came from/is going to
    	semiword_block_t left_headblock = *left_denseit;
    	semiword_block_t right_headblock = *right_denseit;
    	semiword_block_t out_headblock = *out_denseit;
    	int left_shift_remaining = BITS_PER_BLOCK;
    	int right_shift_remaining = BITS_PER_BLOCK;
    	int out_shift_remaining = BITS_PER_BLOCK;

    	//typename std::remove_reference<decltype(out_denseit)>::type			out_denseit_save;
    	//typename std::remove_reference<decltype(out_shift_remaining)>::type	out_shift_remaining_save;

    	for (auto it = next_actionqueue.cbegin(), end_it = next_actionqueue.cend(); it != end_it; ++it) {
    		const cblq_action_block_t &curaction = *it;
    		const cblq_union_action_t action = curaction.action;
    		const size_t count_in_bits = curaction.count * CBLQRegionEncoding<ndim>::BITS_PER_SEMIWORD;

    		switch (action) {
    		case cblq_union_action_t::UNION:
    			// For union, just copy both left and right, since they OR into the same buffer
    			/* no break */
    		case cblq_union_action_t::COPY_L:
    			// copy (OR) the bits from the left buffer into the out buffer
    			copy_bits(count_in_bits, left_denseit, left_headblock, left_shift_remaining, out_denseit, out_headblock, out_shift_remaining);
    			if (action == cblq_union_action_t::COPY_L)
    				break;

    			// If we get here, ut was really UNION, so reset the out pointer and fall through to COPY_R
    			if (out_shift_remaining == BITS_PER_BLOCK) { // We just wrapped around to a new block (since UNION always has count exactly 1)
    				--out_denseit;
    				out_headblock = *out_denseit;
    				out_shift_remaining = CBLQRegionEncoding<ndim>::BITS_PER_SEMIWORD;
    			} else {
    				out_shift_remaining += CBLQRegionEncoding<ndim>::BITS_PER_SEMIWORD;
    			}
    			/* no break */
    		case cblq_union_action_t::COPY_R:
    			// copy (OR) the bits from the right buffer into the out buffer
    			copy_bits(count_in_bits, right_denseit, right_headblock, right_shift_remaining, out_denseit, out_headblock, out_shift_remaining);
    			break;
    		case cblq_union_action_t::DELETE_L:
    			// skip the appropriate number of bits in the left buffer
    			skip_bits(count_in_bits, left_denseit, left_headblock, left_shift_remaining);
    			break;
    		case cblq_union_action_t::DELETE_R:
    			// skip the appropriate number of bits in the right buffer
    			skip_bits(count_in_bits, right_denseit, right_headblock, right_shift_remaining);
    			break;
    		}
    	}

    	*out_denseit = out_headblock; // Copy any remaining bits; don't worry about overflow garbage bits
    	++out_denseit; // Advance past this last (partial) block; "out_dense_remaining_bits" tells how many bits to disregard in this block
    	out_dense_remaining_bits = out_shift_remaining;
    }
}

template<int ndim>
boost::shared_ptr< CBLQRegionEncoding<ndim> >
CBLQSetOperationsFast<ndim>::unary_set_op_impl(boost::shared_ptr< const CBLQRegionEncoding<ndim> > region, UnarySetOperation op) const {
	using cblq_word_t = typename CBLQRegionEncoding<ndim>::cblq_word_t;
	using semiword_block_t = typename CBLQSemiwords<ndim>::block_t;

	const bool has_dense_suffix = region->has_dense_suffix;

	boost::shared_ptr< CBLQRegionEncoding<ndim> > output = boost::make_shared< CBLQRegionEncoding<ndim> >(region->get_domain_size());
	output->level_lens = region->level_lens;
	output->has_dense_suffix = region->has_dense_suffix;

	auto &out_cblq = output->words;
	auto &out_dense = *output->dense_suffix;
	auto &in_cblq = region->words;
	auto &in_dense = *region->dense_suffix;

	out_cblq.resize(in_cblq.size());
	if (has_dense_suffix) {
		out_dense.num_semiwords = in_dense.num_semiwords;
		out_dense.semiwords.resize(in_dense.semiwords.size());
	}

	switch (op) {
	case UnarySetOperation::COMPLEMENT:
	{
		auto in_it = in_cblq.cbegin();
		auto out_it = out_cblq.begin();
		for (; in_it != in_cblq.cend(); ++in_it, ++out_it) {
			const cblq_word_t word = *in_it;
			*out_it = word ^ ((word & CBLQRegionEncoding<ndim>::TWO_CODES_WORD) >> 1) ^ CBLQRegionEncoding<ndim>::ONE_CODES_WORD; // 0->1, 1->0, 2->2
		}

		if (has_dense_suffix) {
			for (auto in_it = in_dense.semiwords.begin(), out_it = out_dense.semiwords.begin(); in_it != in_dense.semiwords.end(); ++in_it, ++out_it) {
				*out_it = ~*in_it;
			}
		}

		break;
	}
	default:
		abort();
	}

	return output;
}

template<int ndim>
boost::shared_ptr< CBLQRegionEncoding<ndim> >
CBLQSetOperationsFast<ndim>::binary_set_op_impl(boost::shared_ptr< const CBLQRegionEncoding<ndim> > left, boost::shared_ptr< const CBLQRegionEncoding<ndim> > right, NArySetOperation op) const {
	if (op != NArySetOperation::UNION)
		return this->template CBLQSetOperations<ndim>::binary_set_op_impl(left, right, op); // Delegate upward if this isn't a union

	assert(left->get_domain_size() == right->get_domain_size());
	assert(left->get_num_levels() == right->get_num_levels());

	const boost::optional< bool > common_has_dense_suffix = CBLQRegionEncoding<ndim>::deduce_common_suffix_density(*left, *right);
	assert(common_has_dense_suffix); // Only supports both dense, both non-dense, or when at least one operand has an empty suffix

	const int levels = left->get_num_levels();
	const bool has_dense_suffix = *common_has_dense_suffix;
	const int non_dense_levels = has_dense_suffix ? levels - 1 : levels;

	// Allocate an output CBLQRegionEncoding
	boost::shared_ptr< CBLQRegionEncoding<ndim> > output = boost::make_shared< CBLQRegionEncoding<ndim> >(left->get_domain_size());
	output->has_dense_suffix = has_dense_suffix;
	output->level_lens.resize(levels);

	// Capture the input CBLQ word deques, and extract iterators
	const std::vector<cblq_word_t> &left_cblq = left->words;
	const std::vector<cblq_word_t> &right_cblq = right->words;
	std::vector<cblq_word_t> &output_cblq = output->words;
	output_cblq.resize(left_cblq.size() + right_cblq.size()); // Worst case, it is the sum of both

	CBLQSemiwords<ndim> &left_dense = *left->dense_suffix;
	CBLQSemiwords<ndim> &right_dense = *right->dense_suffix;
	CBLQSemiwords<ndim> &out_dense = *output->dense_suffix;
	if (has_dense_suffix) {
		// Pad the left and right semiword buffer, since we may access one block beyond the end
		// Note: appending a new block has no effect on the content of a CBLQSemiwords, as actual length is tracked separately
		left_dense.ensure_padded();
		right_dense.ensure_padded();
		// Resize and clear to 0 the output semiword buffer
		out_dense.semiwords.clear();
		out_dense.semiwords.resize(left_dense.semiwords.size() + right_dense.semiwords.size(), 0);
	}

	auto left_it = left_cblq.cbegin();
	auto right_it = right_cblq.cbegin();
	auto out_it = output_cblq.begin();
	auto left_denseit = left_dense.semiwords.cbegin();
	auto right_denseit = right_dense.semiwords.cbegin();
	auto out_denseit = out_dense.semiwords.begin();
	int out_dense_remaining_bits;

	switch (op) {
	case NArySetOperation::UNION:
		binary_union_set_op< ndim, typename CBLQSemiwords<ndim>::block_t >(has_dense_suffix, non_dense_levels, left_it, right_it, out_it, left_denseit, right_denseit, out_denseit, out_dense_remaining_bits, output->level_lens);
		break;
	default:
		abort();
	}

	assert(left_it == left_cblq.cend());
	assert(right_it == right_cblq.cend());
	output_cblq.resize(out_it - output_cblq.begin()); // Trim to length

	if (has_dense_suffix) {
		out_dense.num_semiwords = (out_denseit - out_dense.semiwords.begin()) * CBLQSemiwords<ndim>::SEMIWORDS_PER_BLOCK -
				(out_dense_remaining_bits / CBLQRegionEncoding<ndim>::BITS_PER_SEMIWORD); // All full blocks, minus any remaining bit space in the last block
		out_dense.trim(out_dense.num_semiwords);
	}

    if (this->conf.compact_after_setop)
    	output->compact();

    return output;
}

// Explicit instantiation of 2D-4D CBLQ set ops
template class CBLQSetOperationsFast<2>;
template class CBLQSetOperationsFast<3>;
template class CBLQSetOperationsFast<4>;


// #include <boost/filesystem.hpp>

#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <visualization_msgs/Marker.h>

// Include opencv2
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

// #include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/PointCloud.h>

#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/publisher.h>
#include <pcl/ModelCoefficients.h>
#include <pcl_msgs/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/common/eigen.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/range_image/range_image_planar.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/segmentation/extract_clusters.h>

#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl/common/pca.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/normal_3d.h>
#include <geometry_msgs/PointStamped.h>

#include <pcl/filters/crop_hull.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/surface/convex_hull.h>

#include <iostream>
#include <experimental/filesystem>
#include <iterator>
#include <vector>
#include <algorithm>
#include <math.h>
#include <regex>

using namespace cv;
using namespace std;
using namespace pcl_msgs;
// namespace fs = boost::filesystem;
namespace fs = std::experimental::filesystem;

class Planes2Depth
{
public:
    // typedef pcl::PointXYZRGB Point;
    typedef pcl::PointXYZI Point;
    typedef pcl::PointCloud<Point> PointCloud;

    /*
   * constructor
   * setup which topics are passed out (advertise) and to which topics are listend (subscribe)
   */
    Planes2Depth() : it_(nh_)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

        // "~" means, that the node hand is opened within the private namespace (to get the "own" paraemters)
        ros::NodeHandle private_nh("~");
        // ########################################
        // This should be used without a subscriber
        // ########################################

        string saveDirOrig = "/home/marian/calibration_ws/monodepth-FPN/MonoDepth-FPN-PyTorch/dataset/training_data/training_data/curvature_grad/verify_network/orig_planes.PCD.train/"; // BEWARE of that last / !!! You don't want to forget that, trust me!
        string saveDirPred = "/home/marian/calibration_ws/monodepth-FPN/MonoDepth-FPN-PyTorch/dataset/training_data/training_data/curvature_grad/verify_network/pred_planes.PCD.train/"; // BEWARE of that last / !!! You don't want to forget that, trust me!
        long int lenOrig = saveDirOrig.length();
        char saveDirOrigArr[lenOrig + 1];
        long int lenPred = saveDirPred.length();
        char saveDirPredArr[lenPred + 1];
        strcpy(saveDirOrigArr, saveDirOrig.c_str());
        strcpy(saveDirPredArr, saveDirPred.c_str());
        if (private_nh.getParam("pcd_folder", _param))
        {
            ROS_INFO("Got param: %s", _param.c_str());
            int i = 0;
            fs::path p(_param.c_str());
            typedef vector<fs::path> vec; // store paths,
            vec v;                        // so we can sort them later
            copy(fs::directory_iterator(p), fs::directory_iterator(), back_inserter(v));
            sort(v.begin(), v.end());
            for (vec::const_iterator it(v.begin()), it_end(v.end()); it != it_end; ++it)
            {

                cout << "pcd_file:" << *it << '\n';
                if (pcl::io::loadPCDFile<pcl::PointXYZ>(*it, *cloud) == -1) //* load the file
                {
                    PCL_ERROR("Couldn't read file %s \n", it);
                }
                // cout << entry.path() << '\n';
                std::cout << "Loaded "
                          << cloud->width * cloud->height
                          << " data points"
                          << std::endl;
                string saveDirOrig(saveDirOrigArr);
                char numberDirOrigArr[6];
                sprintf(numberDirOrigArr, "%05d", i);
                string origFilename = saveDirOrig + numberDirOrigArr;

                string saveDirPred(saveDirPredArr);
                char numberDirPredArr[6];
                sprintf(numberDirPredArr, "%05d", i);
                string predFilename = saveDirPred + numberDirPredArr;

                pcl::PointCloud<pcl::PointXYZ>::Ptr resulted_plane;
                resulted_plane = getGTPlanes(cloud, origFilename, predFilename);

                // ############################################
                // for loading the prediction cloud as well
                string pcd_file = *it;
                string temp_pred_pcd_file = regex_replace(pcd_file,regex("orig.PCD.train"),"pred.PCD.train");
                string pred_pcd_file = regex_replace(temp_pred_pcd_file,regex("_orig.pcd"),"_pred.pcd");

                cout << "pcd_file:" << pred_pcd_file << '\n';
                if (pcl::io::loadPCDFile<pcl::PointXYZ>(pred_pcd_file, *cloud) == -1) //* load the file
                {
                    PCL_ERROR("Couldn't read file %s \n", it);
                }

                pcl::PointCloud<pcl::PointXYZ>::Ptr pred_resulted_plane;
                pred_resulted_plane = getGTPlanes(cloud, predFilename, origFilename);

                if(!resulted_plane->empty() && !pred_resulted_plane->empty()){
                    // save the orig plane to a lonely file
                    char number[6];
                    sprintf(number, "%01d", i);
                    string pcd_version = "_orig";
                    pcl::io::savePCDFileASCII(origFilename + pcd_version +".pcd", *resulted_plane);
                    std::cerr << "Saved " << resulted_plane->size() << " data points to" << origFilename << std::endl;

                    char pred_number[6];
                    sprintf(pred_number, "%01d", i);
                    pcd_version = "_pred";
                    pcl::io::savePCDFileASCII(predFilename + pcd_version +".pcd", *pred_resulted_plane);
                    std::cerr << "Saved " << pred_resulted_plane->size() << " data points to" << predFilename << std::endl;
                    i++;
                }

            }
        }
        else
        {
            ROS_ERROR("Failed to get param 'pcd_folder' ");
        }
    }

    ~Planes2Depth() {}

    pcl::PointCloud<pcl::PointXYZ>::Ptr getGTPlanes(pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud, string pcd_file_name, string no_filter_file_name)
    {
        // string only_points_path = "my_full_custom_pcdmultiP1.pcd";

        // Set variables for the current data set,
        // because these are different depending on the camera range
        // ################################## //
        //       multiP_pico_2m_50.bag
        // ################################## //
        _distanceThreshold = 0.0000175;
        // _okDistanceThreshold = 0.005;
        _clusterTolerance = 0.000006;
        _minClusterSize = 12000;
        _maxClusterSize = 250000;
        _maxIterations = 100;
        _remained_pointcloud = 0.2;
        // _alpha = 0.01;

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::copyPointCloud(*inputCloud, *cloud1);

        std::cerr << "#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#-#" << std::endl;
        std::cerr << "Point cloud sizeeeeeeeeeeeeeeeee: " << cloud1->size() << std::endl;

        // cleaning the point cloud of NaNs in order to further apply the Euclidean cluster extraction
        boost::shared_ptr<std::vector<int>> indices(new std::vector<int>);
        pcl::removeNaNFromPointCloud(*cloud1, *indices);
        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud1);
        extract.setIndices(indices);
        extract.setNegative(false);
        extract.filter(*cloud1);

        // ///////////////////////////////////////////////////////////////////////////////////////////

        // Preparing the plane extraction
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
        // Create the segmentation object
        pcl::SACSegmentation<pcl::PointXYZ> seg;
        // Optional
        seg.setOptimizeCoefficients(true);
        // Mandatory
        seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
        seg.setMethodType(pcl::SAC_RANSAC);
        seg.setMaxIterations(_maxIterations);
        seg.setDistanceThreshold(_distanceThreshold);

        //because we want a specific plane (X-Z Plane) (In camera coordinates the ground plane is perpendicular to the y axis)
        Eigen::Vector3f axis = Eigen::Vector3f(0.0, 1.0, 0.0); //y axis
        seg.setAxis(axis);
        int angle = 23;
        seg.setEpsAngle(angle * (M_PI / 180.0f)); // plane can be within 10.0 degrees of X-Z plane

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_rest(new pcl::PointCloud<pcl::PointXYZ>());

        int i = 0, nr_points = (int)cloud1->size(); ////////////////// ?????????????????????????
        std::cerr << "Point cloud size: " << nr_points << std::endl;

        // While nr_points% of the original cloud is still there
        while (cloud1->size() > _remained_pointcloud * nr_points)
        {
            // Extract inliers
            pcl::ExtractIndices<pcl::PointXYZ> plane_extracter;

            pcl::PointCloud<pcl::PointXYZ>::Ptr plane_only(new pcl::PointCloud<pcl::PointXYZ>());
            // Extracting the plane using RANSAC
            seg.setInputCloud(cloud1);
            seg.segment(*inliers, *coefficients);

            std::vector<int> ransac_plane_indices;
            ransac_plane_indices = inliers->indices;

            if (inliers->indices.size() == 0)
            {
                PCL_ERROR("Could not estimate a planar model for the given dataset.\n");
            }

            plane_extracter.setInputCloud(cloud1);
            plane_extracter.setIndices(inliers);
            plane_extracter.setNegative(false);
            // Get the points associated with the planar surface
            plane_extracter.filter(*plane_only);
            std::cerr << "PointCloud representing the planar component: " << plane_only->width * plane_only->height << " data points." << std::endl;

            // ////////////////////////////////////////////////////////////////////////

            // Extracting indices of detected clusters for identifying the only plane
            pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
            tree->setInputCloud(plane_only);

            std::vector<pcl::PointIndices> cluster_indices;
            pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
            ec.setClusterTolerance(_clusterTolerance); // in meters
            ec.setMinClusterSize(_minClusterSize);
            ec.setMaxClusterSize(_maxClusterSize);
            ec.setSearchMethod(tree);
            ec.setInputCloud(plane_only);

            std::vector<int> POIIndices;

            ec.extract(cluster_indices);

            // here should not be more than a single cluster extracted per plane
            int j = 0;
            for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
            {
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
                for (const auto &idx : it->indices)
                {
                    cloud_cluster->push_back((*plane_only)[idx]); //*
                    POIIndices.push_back(ransac_plane_indices[idx]);
                }
                cloud_cluster->width = cloud_cluster->size();
                cloud_cluster->height = 1;
                cloud_cluster->is_dense = true;

                std::cout << "PointCloud representing the Cluster: " << cloud_cluster->size() << " data points." << std::endl;

                j++;
            }
            i++;

            // POI and scraps
            pcl::PointCloud<pcl::PointXYZ>::Ptr scraps_cloud(new pcl::PointCloud<pcl::PointXYZ>);

            // now we need to extract the prior detected plane
            pcl::PointIndices::Ptr clusterInliersIndices(new pcl::PointIndices);
            clusterInliersIndices->indices = POIIndices;

            // Extract Inliers from the input cloud
            pcl::ExtractIndices<pcl::PointXYZ> cluster_extracter;
            cluster_extracter.setInputCloud(cloud1);
            cluster_extracter.setIndices(clusterInliersIndices);
            cluster_extracter.setNegative(false); // Keep only the inliers (i.e. the plane)
            cluster_extracter.filter(*plane_only);

            cluster_extracter.setNegative(true); // Remove the inliers (i.e. remove only the plane and keep the rest)
            cluster_extracter.filter(*scraps_cloud);
            // //////////////////////////////////////////////////////////////////////////
            // Statistical outlier remover for a plane

            pcl::PointCloud<pcl::PointXYZ>::Ptr plane_filtered (new pcl::PointCloud<pcl::PointXYZ>);
            pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cloud (new pcl::PointCloud<pcl::PointXYZ>);
            std::cerr << "Cloud before filtering: " << std::endl;
            std::cerr << *plane_only << std::endl;

            if (plane_only->size() > 0){
                // pcl::io::savePCDFileASCII(pcd_file_name+ "_just_plane.pcd", *plane_only);
                // std::cerr << "Saved " << plane_filtered->size() << " data points to just_plane" << std::endl;

                // Create the filtering object
                pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
                sor.setInputCloud (plane_only);
                sor.setMeanK (50);
                sor.setStddevMulThresh (2);
                sor.filter (*plane_filtered);

                std::cerr << "Cloud after filtering: " << std::endl;
                std::cerr << *plane_filtered << std::endl;

                return plane_filtered;
                // sor.setNegative (true);
                // sor.filter (*plane_filtered);
                // pcl::io::savePCDFileASCII(pcd_file_name+ number + pcd_version + "_nofilter.pcd", *plane_filtered);
                // std::cerr << "Saved " << plane_filtered->size() << " data points to" << pcd_file_name << std::endl;

                // //////////////////////////////////////////////////////

                // adding the scraps to  the original point cloud
                cloud1->points = scraps_cloud->points;
            }
            return empty_cloud;
            //  one iteration is more than enough
            break;
        }

    }

private:
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    ros::Subscriber sub_;

    // Pico parameters OK!!!!!!!!!!!!!!
    double K[9] = {460.585, 0.0, 334.081,
                   0.0, 460.268, 169.808,
                   0.0, 0.0, 1.0};

    double centerX_ = K[2];
    double centerY_ = K[5];
    double focalX_ = K[0];
    double focalY_ = K[4];
    // int height_ = 480;
    int height_ = 360; // only for pico (I guess it has its name for a reason! XD)
    int width_ = 640;
    int pixel_pos_x, pixel_pos_y;
    float z, u, v;
    Mat cv_image;
    std::vector<Point2d> imagePoints;
    float _distanceThreshold;
    float _okDistanceThreshold;
    float _clusterTolerance;
    float _planesDelay;
    int _minClusterSize;
    int _maxClusterSize;
    float _remained_pointcloud;
    int _maxIterations;
    float _alpha;
    string _param;
};

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

    Planes2Depth c2d;

    return 0;
}


// This file is part of LatNet Builder.
//
// Copyright (C) 2012-2021  The LatNet Builder author's, supervised by Pierre L'Ecuyer, Universite de Montreal.
//
// 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.

#include "netbuilder/NetConstructionTraits.h"
#include "netbuilder/Helpers/JoeKuo.h"

#include <sstream>
#include <boost/algorithm/string/erase.hpp>

namespace NetBuilder {

    const std::string NetConstructionTraits<NetConstruction::LMS>::name = "LMS";

    typedef typename NetConstructionTraits<NetConstruction::LMS>::GenValue GenValue;

    typedef std::vector<uInteger> DirectionNumber;
    typedef std::pair<unsigned int, unsigned int> PrimitivePolynomial;
    typedef std::vector<std::vector<bool>> BinaryMatrix;

    typedef NetConstructionTraits<NetConstruction::LMS>::SizeParameter SizeParameter;

    bool NetConstructionTraits<NetConstruction::LMS>::checkGenValue(const GenValue& genValue, const SizeParameter& sizeParameter)
    {
        return genValue.nRows() == nRows(sizeParameter) && genValue.nCols() == nCols(sizeParameter);
    }

    unsigned int NetConstructionTraits<NetConstruction::LMS>::nRows(const SizeParameter& sizeParameter) {return (unsigned int) sizeParameter.first.first; }

    unsigned int NetConstructionTraits<NetConstruction::LMS>::nCols(const SizeParameter& sizeParameter) {return (unsigned int) sizeParameter.first.second; }

    GeneratingMatrix*  NetConstructionTraits<NetConstruction::LMS>::createGeneratingMatrix(const GenValue& genValue, const SizeParameter& sizeParameter, const Dimension& dimension_j, const unsigned int nRows)
    {
        GeneratingMatrix* result = new GeneratingMatrix(genValue * *sizeParameter.second[dimension_j]);
        unsigned int finalnRows = (nRows == 0)? NetConstructionTraits<NetConstruction::LMS>::nRows(sizeParameter) : nRows;
        result->resize(finalnRows, nCols(sizeParameter));
        return result;
    }

    std::vector<GenValue> NetConstructionTraits<NetConstruction::LMS>::genValueSpaceCoord(Dimension coord, const SizeParameter& sizeParameter)
    {
        throw std::logic_error("The space of all matrices is far too big to be exhautively explored.");
        return std::vector<GenValue>{};
    }

    std::vector<std::vector<GenValue>> NetConstructionTraits<NetConstruction::LMS>::genValueSpace(Dimension dimension, const SizeParameter& sizeParameter)
    {
        throw std::logic_error("The space of all matrices is far too big to be exhautively explored.");
        return std::vector<std::vector<GenValue>>{};
    }

    std::string NetConstructionTraits<NetConstruction::LMS>::format(const std::vector<std::shared_ptr<GeneratingMatrix>>& genMatrices, const std::vector<std::shared_ptr<GenValue>>& genVals, const SizeParameter& sizeParameter, OutputStyle outputStyle, unsigned int interlacingFactor)
    {
        std::ostringstream stream;

        if (outputStyle == OutputStyle::TERMINAL){
            stream << "Left-Matrix-Scrambled Digital Net - Matrix size = " << sizeParameter.first.first << "x" << sizeParameter.first.second << std::endl;
            stream << "Printing only the scrambled generating matrices - see output.txt file for original matrices and scrambling matrices.\n";
            for (Dimension dim = 0; dim < genMatrices.size(); ++dim)
            {
                stream << "Coordinate " << dim << std::endl;
                stream << *genMatrices[dim] << std::endl;
            }
        }

        else if (outputStyle == OutputStyle::RANDOMIZED_NET){
            unsigned long dimension = genMatrices.size();
            unsigned int nb_col = sizeParameter.first.second;
            stream << "# Parameters for a digital net in base 2\n";
            stream << dimension / interlacingFactor << "    # s = " << dimension / interlacingFactor << " dimensions\n";
            if (interlacingFactor > 1){
                stream << interlacingFactor << "    # Interlacing factor" << "\n";
                stream << dimension << "    # Number of components = interlacing factor x dimension" << "\n";
            }
            stream << nb_col << "    # k = " << nb_col << ",  n = 2^" << nb_col << " = ";
            stream << (int)pow(2,nb_col) << " points"<< std::endl;
            stream << "31    # r = 31 binary output digits\n";

            if (interlacingFactor == 1){
                stream << "# Columns of original gen. matrices C_1,...,C_s, one matrix per line\n";
            }
            else {
                stream << "# Columns of original gen. matrices C_1,...,C_{ds}, one matrix per line\n";
            }
            for(unsigned int coord = 0; coord < dimension; coord++)
            {
                stream << sizeParameter.second[coord]->formatToColumnsReverse();
                stream << "\n";
            }

            if (interlacingFactor == 1){
                stream << "# Columns of scrambling matrices C_1,...,C_s, one matrix per line\n";
            }
            else {
                stream << "# Columns of scrambling matrices C_1,...,C_{ds}, one matrix per line\n";
            }
            for(unsigned int coord = 0; coord < dimension; coord++)
            {
                stream << genVals[coord]->formatToColumnsReverse();
                stream << "\n";
            }

            if (interlacingFactor == 1){
                stream << "# Columns of scrambled gen. matrices C_1,...,C_s, one matrix per line\n";
            }
            else {
                stream << "# Columns of scrambled gen. matrices C_1,...,C_{ds}, one matrix per line\n";
            }
            for(unsigned int coord = 0; coord < dimension; coord++)
            {
                stream << genMatrices[coord]->formatToColumnsReverse();
                if (coord < dimension - 1){
                    stream << "\n";
                }
            }
        }
        return stream.str();
    }
}


#include "rinne.hpp"

#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <unistd.h>

#ifdef __APPLE__
  #include <GLUT/glut.h>
#else
  #include <GL/glut.h>
#endif

#include <iostream>
#include <fstream>
#include <string>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/bind.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
                              boost::property<boost::vertex_name_t,
                                              std::string> > Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
typedef boost::graph_traits<Graph>::vertex_iterator vertex_iter;
typedef boost::graph_traits<Graph>::edge_iterator edge_iter;

#define TIMERSEC 16
#define SQRTPI 0.5641898
#define CAMERA_Y -3.0
#define NODE_R_MAX 0.05
#define NODE_R_MIN 0.003
#define NODE_R_DIFF (NODE_R_MAX - NODE_R_MIN)
#define NODE_MAX_G 0.7
#define NODE_MIN_G 0.5
#define NODE_MAX_B 0.6
#define NODE_MIN_B 0.2
#define EDGE_MAX_G 0.5
#define EDGE_MIN_G 0.06
#define EDGE_MAX_B 0.4
#define EDGE_MIN_B 0.0
#define EDGE_MAX_ALPHA 1.0
#define EDGE_MIN_ALPHA 0.15
#define EDGE_LINES 6
#define LABEL_MAX_G 1.0
#define LABEL_MIN_G 0.4
#define LABEL_MAX_B 0.8
#define LABEL_MIN_B 0.2
#define MENU_ROTATE 1
#define MENU_BLINK  2

#define DISTANCE2(D, A) do {                    \
        double x2, y2, z2;                      \
        x2 = (A).x * (A).x;                     \
        y2 = (A).y * (A).y;                     \
        z2 = (A).z * (A).z;                     \
        (D) = x2 + y2 + z2;                     \
    } while (0)

#define CROSS_PRODUCT(D, A, B) do {             \
        (D).x = (A).y * (B).z - (A).z * (B).y;  \
        (D).y = (A).z * (B).x - (A).x * (B).z;  \
        (D).z = (A).x * (B).y - (A).y * (B).x;  \
    } while (0)

#define TO_SPHERICAL(D, A) do {                         \
        double tmp = (A).x * (A).x + (A).y * (A).y;     \
        double r   = sqrt(tmp + (A).z * (A).z);         \
        double rxy = sqrt(tmp);                         \
        if (r > 0.0) {                                  \
            (D).theta = acos((A).z / r);                \
            (D).phi = acos((A).x / rxy);                \
            if ((A).y < 0.0) {                          \
                (D).phi = 2 * M_PI - (D).phi;           \
            }                                           \
        } else {                                        \
            (D).theta = 0.0;                            \
            (D).phi   = 0.0;                            \
        }                                               \
    } while (0)

#define TO_RECTANGULAR(D, A, R) do {            \
        double sin_theta = sin((A).theta);      \
        (D).x = (R) * sin_theta * cos((A).phi); \
        (D).y = (R) * sin_theta * sin((A).phi); \
        (D).z = (R) * cos((A).theta);           \
    } while (0)

#define GET_UV(U, V, A) do {                   \
        double cos_theta = cos((A).theta);     \
        double cos_phi   = cos((A).phi);       \
        double sin_phi   = sin((A).phi);       \
        (U).x = - cos_theta * cos_phi;         \
        (U).y = - cos_theta * sin_phi;         \
        (U).z = sin(A.theta);                  \
        (V).x = sin_phi;                       \
        (V).y = - cos_phi;                     \
        (V).z = 0.0;                           \
    } while (0)

#define QUATERNION_MUL(D, A, B) do {                                    \
        (D).w = (A).w * (B).w - (A).i * (B).i - (A).j * (B).j - (A).k * (B).k; \
        (D).i = (A).i * (B).w + (A).w * (B).i - (A).k * (B).j + (A).j * (B).k; \
        (D).j = (A).j * (B).w + (A).k * (B).i + (A).w * (B).j - (A).i * (B).k; \
        (D).k = (A).k * (B).w - (A).j * (B).i + (A).i * (B).j + (A).w * (B).k; \
    } while (0)

#define ROTATE(A, V, RAD) do {                  \
        rn_quaternion p, q, r;                  \
        double r2 = (RAD) * -0.5;               \
        double sin_rad2 = sin(r2);              \
        double cos_rad2 = cos(r2);              \
                                                \
        p.w = 0.0;                              \
        p.i = (A).x;                            \
        p.j = (A).y;                            \
        p.k = (A).z;                            \
                                                \
        q.w = cos_rad2;                         \
        q.i = (V).x * sin_rad2;                 \
        q.j = (V).y * sin_rad2;                 \
        q.k = (V).z * sin_rad2;                 \
                                                \
        r.w = cos_rad2;                         \
        r.i = - q.i;                            \
        r.j = - q.j;                            \
        r.k = - q.k;                            \
                                                \
        rn_quaternion tmp, result;              \
                                                \
        QUATERNION_MUL(tmp, r, p);              \
        QUATERNION_MUL(result, tmp, q);         \
                                                \
        (A).x = result.i;                       \
        (A).y = result.j;                       \
        (A).z = result.k;                       \
    } while (0)

#define NORMALIZE(V) do {                                               \
        double d = sqrt((V).x * (V).x + (V).y * (V).y + (V).z * (V).z); \
        if (d > 0.0001) {                                               \
            d = 1.0 / d;                                                \
            (V).x *= d;                                                 \
            (V).y *= d;                                                 \
            (V).z *= d;                                                 \
        } else {                                                        \
            (V).x = (V).y = (V).z = 0.0;                                \
        }                                                               \
    } while (0)

rn_node *node_pool;
rn_edge *edge_pool;

rinne rinne_inst;

void
render_string(double x, double y, double z, std::string const& str)
{
    int len;

    glRasterPos3f(x, y, z);

    len = str.size();

    for (int i = 0; i < len; i++) {
        glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, str.at(i));
    }
}

void
render_string2d(int x0, int y0, std::string const &str)
{
    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    gluOrtho2D(0, rinne_inst.get_window_w(), rinne_inst.get_window_h(), 0);
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glLoadIdentity();

    glRasterPos2f(x0, y0);
    int size = (int)str.size();
    for (int i = 0; i < size; ++i) {
        char ic = str[i];
        glutBitmapCharacter(GLUT_BITMAP_9_BY_15, ic);
    }

    glPopMatrix();
    glMatrixMode(GL_PROJECTION);
    glPopMatrix();
    glMatrixMode(GL_MODELVIEW);
}

void
run(int id)
{
    for (int i = 0; ; i++) {
        timeval t0, t1;
        double sec, score;

        if (id == 0) {
            gettimeofday(&t0, NULL);
        }

        rinne_inst.force_directed(id);

        if (id == 0) {
            gettimeofday(&t1, NULL);

            sec = (double)t1.tv_sec + (double)t1.tv_usec * 0.000001;
            sec -= (double)t0.tv_sec + (double)t0.tv_usec * 0.000001;

            score = (double)(rinne_inst.get_num_node() * rinne_inst.get_num_node() + rinne_inst.get_num_edge() * 2) / sec;
            rinne_inst.set_score(score);
            rinne_inst.inc_loop();

            if (i == 25) {
                rinne_inst.reduce_step();
            } else if (i == 50) {
                rinne_inst.reduce_step();
            } else if (i == 100) {
                rinne_inst.reduce_step();
            } else if (i == 1000) {
                rinne_inst.reduce_step();
            }
        }
        //usleep(100000);
    }
}

void
rinne::update_time()
{
    double  t, diff, r;
    double  cycle = m_cycle * 0.5;
    timeval tv;

    gettimeofday(&tv, NULL);

    t = (double)tv.tv_sec + (double)tv.tv_usec * 0.000001;

    m_prev_sec = m_current_sec;
    m_current_sec = t;

    diff = t - m_init_sec;

    r = sin(M_PI * (diff - M_PI * 0.5) / cycle) * 0.5 + 0.5;

    if (diff > cycle && r < 0.004) {
        m_init_sec = t;
        m_top_idx++;
        if (m_top_idx > m_top_n)
            m_top_idx = 1;
    }
}

void
rinne::rotate_view()
{
    if (m_is_auto_rotate && m_top_idx  > 0) {
        double theta = m_node_top[m_top_idx - 1]->pos.theta + M_PI_2;
        double diff, tmp;

        theta += m_rotate_x * 2 * M_PI;

        if (theta > 2 * M_PI) {
            theta -= 2 * M_PI;
        }

        diff = M_PI - theta;

        if (fabs(diff) > 0.05) {
            if (diff > 0.0) {
                m_rotate_x += (m_current_sec - m_prev_sec) / (m_cycle * 4.0);
            } else {
                m_rotate_x -= (m_current_sec - m_prev_sec) / (m_cycle * 4.0);
            }

            m_rotate_x = modf(m_rotate_x, &tmp);
            if (m_rotate_x < 0.0) {
                m_rotate_x += 1.0;
            }
        }

        double phi = m_node_top[m_top_idx - 1]->pos.phi;

        phi += m_rotate_z * 2 * M_PI;

        if (phi > 2 * M_PI) {
            phi -= 2 * M_PI;
        }

        diff = 3 * M_PI_2 - phi;
        if (fabs(diff) > 0.05) {
            if (0 < diff && diff < M_PI) {
                m_rotate_z += (m_current_sec - m_prev_sec) / (m_cycle * 4.0);
            } else {
                m_rotate_z -= (m_current_sec - m_prev_sec) / (m_cycle * 4.0);
            }

            m_rotate_z = modf(m_rotate_z, &tmp);
            if (m_rotate_z < 0.0) {
                m_rotate_z += 1.0;
            }
        }
    }
}

void
rinne::draw_status()
{
    std::string str;

    glColor3f(1.0f, 1.0f, 1.0f);

    str = "score:";
    render_string2d(10, 15, str);

    str = "#opration/s = ";
    str += boost::lexical_cast<std::string>((int)m_score);
    render_string2d(25, 30, str);

    str = "#loop = ";
    str += boost::lexical_cast<std::string>(m_score_loop);
    render_string2d(25, 45, str);

    str = "graph status:";
    glColor3f(1.0f, 1.0f, 1.0f);
    render_string2d(10, 60, str);

    str = "#node = ";
    str += boost::lexical_cast<std::string>(m_num_node);
    render_string2d(25, 75, str);

    str = "#edge = ";
    str += boost::lexical_cast<std::string>(m_num_edge);
    render_string2d(25, 90, str);

    if (m_is_blink && m_top_idx > 0) {
        rn_node *p = m_node_top[m_top_idx - 1];

        str = m_label[p - m_node];
        str += ":";
        render_string2d(10, 105, str);

        str = "indegree  = ";
        str += boost::lexical_cast<std::string>(p->num_bp_edge);
        render_string2d(25, 120, str);

        str = "outdegree = ";
        str += boost::lexical_cast<std::string>(p->num_edge);
        render_string2d(25, 135, str);
    }
}

void
rinne::display()
{
    update_time();
    rotate_view();

    glClearColor(0.0, 0.0, 0.0, 1.0);
    glClear(GL_COLOR_BUFFER_BIT);
    glFlush();

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(40.0, (double)m_window_w / (double)m_window_h, 0.1, 8);
    gluLookAt(0.0, CAMERA_Y, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
    glMatrixMode(GL_MODELVIEW);

    glPushMatrix();

    glRotated(360 * m_rotate_x, 1.0, 0.0, 0.0);
    glRotated(360 * m_rotate_z, 0.0, 0.0, 1.0);

    draw_node();

    glPopMatrix();

    draw_status();

    glutSwapBuffers();
}

void
display()
{
    rinne_inst.display();
}

void
on_keyboard(unsigned char key, int x, int y)
{
    rinne_inst.on_keyboard(key, x, y);
}

void
rinne::on_keyboard(unsigned char key, int x, int y)
{
    switch (key) {
    case 'f':
    case 'F':
        if (m_is_fullscreen) {
            glutPositionWindow(0,0);
            glutReshapeWindow(1200, 900);
        } else {
            glutFullScreen();
        }
        m_is_fullscreen = ! m_is_fullscreen;
        break;
    case 'q':
    case 'Q':
    case '\033': // ESC
        exit(0);
    default:
        break;
    }
}

void
on_resize(int w, int h)
{
    rinne_inst.on_resize(w, h);
}

void
on_mouse(int button, int state, int x, int y)
{
    if (state == GLUT_DOWN) {
        rinne_inst.on_mouse_down(button, x, y);
    } else if (state == GLUT_UP) {
        rinne_inst.on_mouse_up(button, x, y);
    }
}

void
on_mouse_move(int x, int y)
{
    rinne_inst.on_mouse_move(x, y);
}

void
glut_timer(int val)
{
    glutPostRedisplay();
    glutTimerFunc(TIMERSEC, glut_timer, 0);
}

void
rinne::on_menu(int id)
{
    glutPostRedisplay();

    switch (id) {
    case MENU_ROTATE:
        m_is_auto_rotate = m_is_auto_rotate ? 0 : 1;
        break;
    case MENU_BLINK:
        m_is_blink = m_is_blink ? 0 : 1;
        break;
    default:
        ;
    }
}

void
on_menu(int id)
{
    rinne_inst.on_menu(id);
}

void
init_menu()
{
    glutCreateMenu(on_menu);
    glutAddMenuEntry("toggle automatic rotation", 1);
    glutAddMenuEntry("toggle blinking", 2);
    glutAttachMenu(GLUT_RIGHT_BUTTON);
}

void
init_glut(int argc, char *argv[])
{
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
    glutInitWindowSize(1024, 768);
    glutCreateWindow(argv[0]);
    glutDisplayFunc(display);
    glutKeyboardFunc(on_keyboard);
    glutReshapeFunc(on_resize);
    glutMouseFunc(on_mouse);
    glutMotionFunc(on_mouse_move);
    glutTimerFunc(TIMERSEC, glut_timer, 0);

    glEnable(GL_BLEND);
    glBlendFunc(GL_ONE, GL_ONE_MINUS_SRC_ALPHA);
    glBlendEquation(GL_FUNC_ADD);

    init_menu();

    glutMainLoop();
}

void
rinne::on_resize(int w, int h)
{
    glViewport(0, 0, w, h);

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(40.0, (double)w / (double)h, 0.1, 8);
    gluLookAt(0.0, CAMERA_Y, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
    glMatrixMode(GL_MODELVIEW);

    m_window_w = w;
    m_window_h = h;
}

void
rinne::on_mouse_down(int button, int x, int y)
{
    m_is_mouse_down = true;
    m_mouse_x = x;
    m_mouse_y = y;
}

void
rinne::on_mouse_up(int button, int x, int y)
{
    m_is_mouse_down = false;
}

void
rinne::on_mouse_move(int x, int y)
{
    if (m_is_mouse_down && ! m_is_auto_rotate) {
        int dx = x - m_mouse_x;
        int dy = y - m_mouse_y;

        m_rotate_z += dx * 0.001;
        m_rotate_x += dy * 0.001;

        double tmp;
        m_rotate_z = modf(m_rotate_z, &tmp);
        if (m_rotate_z < 0.0) {
            m_rotate_z += 1.0;
        }

        m_rotate_x = modf(m_rotate_x, &tmp);
        if (m_rotate_x < 0.0) {
            m_rotate_x += 1.0;
        }

        m_mouse_x = x;
        m_mouse_y = y;

        glutPostRedisplay();
    }
}

void
rinne::draw_label()
{
    double g, b, alpha;
    rn_vec v;

    for (int i = 0; i < m_top_n; i++) {
        if (m_is_blink && i + 1 == m_top_idx)
            continue;

        TO_RECTANGULAR(v, m_node_top[i]->pos, 1.0);

        glColor3f(0.0, 0.0, 0.0);
        render_string(v.x - 0.005, v.y, v.z - 0.005,
                      m_label[(m_node_top[i] - m_node)]);

        get_color(g, b, alpha, LABEL_MIN_G, LABEL_MAX_G,
                  LABEL_MIN_B, LABEL_MAX_B, 0.0, 0.0);

        if (m_is_blink) {
            if (m_top_idx == 0) {
                glColor3f(0.0, g, b);
            } else{
                glColor3f(0.0, LABEL_MIN_G, LABEL_MIN_B);
            }
        } else {
            glColor3f(0.0, LABEL_MAX_G, LABEL_MAX_B);
        }

        render_string(v.x, v.y, v.z, m_label[(m_node_top[i] - m_node)]);
    }

    if (m_is_blink && m_top_idx > 0) {
        TO_RECTANGULAR(v, m_node_top[m_top_idx - 1]->pos, 1.0);

        glColor3f(0.0, 0.0, 0.0);
        render_string(v.x - 0.005, v.y, v.z - 0.005,
                      m_label[(m_node_top[m_top_idx - 1] - m_node)]);

        get_color(g, b, alpha, LABEL_MIN_G, LABEL_MAX_G,
                  LABEL_MIN_B, LABEL_MAX_B, 0.0, 0.0);
        glColor3f(0.0, g, b);
        render_string(v.x, v.y, v.z,
                      m_label[(m_node_top[m_top_idx - 1] - m_node)]);
    }
}

void
rinne::draw_tau()
{
    if (m_num_node == 0)
        return;

    rn_vec a;
    TO_RECTANGULAR(a, m_node[0].pos, 1.0);

    double cos_theta_a = cos(m_node[0].pos.theta);
    double sin_theta_a = sin(m_node[0].pos.theta);

    rn_vec au, av;

    GET_UV(au, av, m_node[0].pos);

    for (rn_node *p = m_node + 1; p < &m_node[m_num_node]; p++) {
        rn_vec b;
        double  t, x, y, z;

        TO_RECTANGULAR(b, p->pos, 1.0);

        t = 1.0 - a.x * b.x - a.y * b.y - a.z * b.z;

        x = b.x + a.x * t;
        y = b.y + a.y * t;
        z = b.z + a.z * t;

        glPushMatrix();
        glColor3f(1.0, 1.0, 0.0);
        glTranslated(x, y, z);
        glutWireSphere(0.1, 16, 16);
        glPopMatrix();

        glBegin(GL_LINES);
        glVertex3f(b.x, b.y, b.z);
        glVertex3f(x, y, z);
        glEnd();

        double psi = acos(cos_theta_a * cos(p->pos.theta) +
                          sin_theta_a * sin(p->pos.theta) *
                          cos(m_node[0].pos.phi - p->pos.phi));
        rn_vec ba;
        ba.x = x - a.x;
        ba.y = y - a.y;
        ba.z = z - a.z;

        double dist;
        DISTANCE2(dist, ba);

        ba.x /= sqrt(dist);
        ba.y /= sqrt(dist);
        ba.z /= sqrt(dist);

        ba.x *= psi;
        ba.y *= psi;
        ba.z *= psi;

        glPushMatrix();
        glColor3f(1.0, 0.0, 1.0);
        glTranslated(a.x, a.y, a.z);

        glBegin(GL_LINES);
        glVertex3f(0.0, 0.0, 0.0);
        glVertex3f(ba.x, ba.y, ba.z);
        glEnd();

        glPopMatrix();

        glPushMatrix();
        glColor3f(1.0, 0.0, 1.0);
        glTranslated(ba.x + a.x, ba.y + a.y, ba.z + a.z);
        glutWireSphere(0.1, 16, 16);
        glPopMatrix();

        rn_uv buv;
        double bb;

        buv.u = au.x * x + au.y * y + au.z * z;
        buv.v = av.x * x + av.y * y + av.z * z;
        bb    = a.x * x + a.y * y + a.z * z;

        //buv.u *= 0.5;
        //buv.v *= 0.5;

        rn_vec b2;

        b2.x = au.x * buv.u + av.x * buv.v + a.x * bb;
        b2.y = au.y * buv.u + av.y * buv.v + a.y * bb;
        b2.z = au.z * buv.u + av.z * buv.v + a.z * bb;

/*
        glPushMatrix();
        glColor3d(1.0, 0.0, 1.0);
        glTranslated(b2.x, b2.y, b2.z);
        glutWireSphere(0.1, 16, 16);
        glPopMatrix();

        std::cout << "(x, y, z) = (" << x << ", " << y << ", " << z << ")"
                  << std::endl;

        rn_uv buv;
        double bb;

        buv.u = au.x * x + av.x * y + a.x * z;
        buv.v = au.y * x + av.y * y + a.y * z;
        bb    = au.z * x + av.z * y + a.z * z;

        std::cout << "u = " << buv.u << std::endl;
        std::cout << "v = " << buv.v << std::endl;
        std::cout << "a = " << bb << std::endl;

        rn_vec b2;

        b2.x = au.x * buv.u + au.y * buv.v + au.z;
        b2.y = av.x * buv.u + av.y * buv.v + av.z;
        b2.z = a.x  * buv.u + a.y  * buv.v + a.z;

        std::cout << "(x', y', z') = ("
                  << b2.x << ", " << b2.y << ", " << b2.z << ")\n"
                  << std::endl;
*/
    }
}

void
rinne::draw_edge(double g, double b, double alpha)
{
    rn_node *dst = m_node_top[m_top_idx - 1];

    for (rn_node *p = m_node; p != &m_node[m_num_node]; p++) {
        rn_vec ev, ev1, ev2;
        double cos_theta_a = cos(p->pos.theta);
        double sin_theta_a = sin(p->pos.theta);

        TO_RECTANGULAR(ev, p->pos, 1.0);

        for (rn_edge *p_edge = p->edge; p_edge != NULL; p_edge = p_edge->next) {
            rn_pos delta;

            delta.theta = p->pos.theta - p_edge->dst->pos.theta;
            delta.phi   = p->pos.phi - p_edge->dst->pos.phi;

            if (delta.phi > M_PI) {
                delta.phi = -(2 * M_PI - delta.phi);
            } else if (delta.phi < -M_PI) {
                delta.phi = 2 * M_PI + delta.phi;
            }

            if (!m_is_blink || m_top_idx == 0 || dst == p_edge->dst) {
                glColor4f(0.0, g, b, alpha);
            } else {
                glColor4f(0.0, EDGE_MIN_G, EDGE_MIN_B, EDGE_MIN_ALPHA);
            }

            if (m_is_blink && m_top_idx != 0 && dst == p_edge->dst) {
                glLineWidth(2.0f);
            } else {
                glLineWidth(1.0f);
            }

            TO_RECTANGULAR(ev2, p_edge->dst->pos, 1.0);

            double psi = acos(cos_theta_a * cos(p_edge->dst->pos.theta) +
                              sin_theta_a * sin(p_edge->dst->pos.theta) *
                              cos(p->pos.phi - p_edge->dst->pos.phi));

            rn_vec cross;

            ev1 = ev;
            psi /= EDGE_LINES;

            CROSS_PRODUCT(cross, ev, ev2);
            NORMALIZE(cross);
            glBegin(GL_LINE_STRIP);

            glVertex3f(ev.x, ev.y , ev.z);

            for (int i = 0; i < EDGE_LINES - 1; i++) {
                ROTATE(ev1, cross, psi);
                glVertex3f(ev1.x, ev1.y, ev1.z);
            }

            glVertex3f(ev2.x, ev2.y , ev2.z);

            glEnd();
            glLineWidth(1.0f);
        }
    }
}

void
rinne::draw_node()
{
    double r_denom = 1.0 / m_max_in_degree;
    double max_alpha = 1.0;
    double min_alpha = 0.0;
    double max_g;
    double min_g;
    double max_b;
    double min_b;
    double b, g, alpha;

    // draw far side nodes
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(40.0, (double)m_window_w / (double)m_window_h, -CAMERA_Y, 8);
    gluLookAt(0.0, CAMERA_Y, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
    glMatrixMode(GL_MODELVIEW);

    glColor3d(0.0, NODE_MIN_G * 0.125, NODE_MIN_B * 0.125);
    for (rn_node *p = m_node; p != &m_node[m_num_node]; p++) {
        rn_vec a;
        double r = p->num_bp_edge * r_denom;

        r = r * NODE_R_DIFF + NODE_R_MIN;

        TO_RECTANGULAR(a, p->pos, 1.0);

        glPushMatrix();

        glTranslatef(a.x, a.y, a.z);
        glutSolidSphere(r, 8, 8);

        glPopMatrix();
    }

    glColor3f(0.4, 0.4, 0.4);
    glutWireSphere(1.0, 16, 16);

    // draw edges
    if (m_is_blink) {
        max_alpha = EDGE_MAX_ALPHA;
        min_alpha = EDGE_MIN_ALPHA;
        max_g = EDGE_MAX_G;
        min_g = EDGE_MIN_G;
        max_b = EDGE_MAX_B;
        min_b = EDGE_MIN_B;

        get_color(g, b, alpha, min_g, max_g, min_b, max_b, min_alpha, max_alpha);
    } else {
        g = EDGE_MAX_G;
        b = EDGE_MAX_B;
        alpha = EDGE_MAX_ALPHA;
    }

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(40.0, (double)m_window_w / (double)m_window_h,
                   0.1, -CAMERA_Y);
    gluLookAt(0.0, CAMERA_Y, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
    glMatrixMode(GL_MODELVIEW);

    draw_edge(g, b, alpha);
    glColor3f(0.4, 0.4, 0.4);

    glutWireSphere(1.0, 16, 16);

    // draw near side nodes
    if (m_is_blink) {
        max_g = NODE_MAX_G;
        min_g = NODE_MIN_G;
        max_b = NODE_MAX_B;
        min_b = NODE_MIN_B;

        get_color(g, b, alpha, min_g, max_g, min_b, max_b, min_alpha, max_alpha);
    } else {
        g = NODE_MAX_G;
        b = NODE_MIN_G;
    }

    rn_node *dst = m_node_top[m_top_idx - 1];

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(40.0, (double)m_window_w / (double)m_window_h,
                   0.1, -CAMERA_Y);
    gluLookAt(0.0, CAMERA_Y, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
    glMatrixMode(GL_MODELVIEW);

    for (rn_node *p = m_node; p != &m_node[m_num_node]; p++) {
        rn_vec a;
        double r = p->num_bp_edge * r_denom;

        r = r * NODE_R_DIFF + NODE_R_MIN;

        TO_RECTANGULAR(a, p->pos, 1.0);

        if (!m_is_blink || m_top_idx == 0 || p == dst) {
            glColor3f(0.0, g, b);
        } else {
            glColor3f(0.0, NODE_MIN_G, NODE_MIN_B);
        }

        glPushMatrix();

        glTranslatef(a.x, a.y, a.z);
        glutSolidSphere(r, 8, 8);

        glPopMatrix();
    }

    draw_label();
}

void
rinne::get_spring_vec(rn_vec &uv, double psi)
{
    double power;
    double p = psi / M_PI + 1.0;

    p *= p;
    p *= p;
    p *= p;
    p *= p;
    p *= p;

    power = m_factor_step * m_factor_spring * p;

    uv.x *= power;
    uv.y *= power;
    uv.z *= power;
}

void
rinne::get_repulse_vec(rn_vec &uv, double psi)
{
    double power;
    double p = psi + M_PI;

    power = - m_factor_step * m_factor_repulse / (p * p);

    uv.x *= power;
    uv.y *= power;
    uv.z *= power;
}

void
rinne::get_uv_vec(rn_vec &v, const rn_pos &a, const rn_pos &b)
{
    rn_vec va, vb;
    double t;

    TO_RECTANGULAR(va, a, 1.0);
    TO_RECTANGULAR(vb, b, 1.0);

    t = 1.0 - va.x * vb.x - va.y * vb.y - va.z * vb.z;

    v.x = vb.x + va.x * t;
    v.y = vb.y + va.y * t;
    v.z = vb.z + va.z * t;

    v.x -= va.x;
    v.y -= va.y;
    v.z -= va.z;

    NORMALIZE(v);
}

int
cmp_node(const void *lhs, const void *rhs)
{
    const rn_node *p1 = *(rn_node**)lhs;
    const rn_node *p2 = *(rn_node**)rhs;

    if (p1->num_bp_edge > p2->num_bp_edge)
        return -1;

    if (p1->num_bp_edge < p2->num_bp_edge)
        return 1;

    return 0;
}

void
rinne::get_top_n()
{
    rn_node **p;

    if (m_num_node < m_top_n)
        m_top_n = m_num_node;

    p = new rn_node*[m_num_node];
    m_node_top = new rn_node*[m_top_n];

    for (int i = 0; i < m_num_node; i++) {
        p[i] = &m_node[i];
    }

    qsort(p, m_num_node, sizeof(p), cmp_node);

    for (int j = 0; j < m_top_n; j++) {
        m_node_top[j] = p[j];
    }

    delete[] p;
}

void
rinne::get_uv_vec_rand(rn_vec &v, const rn_pos &a)
{
    static int i = 0, j = 0;
    static double theta[7] = {0.0,
                              M_PI / 7.0,
                              2 * M_PI / 7.0,
                              3 * M_PI / 7.0,
                              4 * M_PI / 7.0,
                              5 * M_PI / 7.0,
                              6 * M_PI / 7.0};
    static double phi[11] = {0.0,
                             M_PI / 11.0,
                             2 * M_PI / 11.0,
                             3 * M_PI / 11.0,
                             4 * M_PI / 11.0,
                             5 * M_PI / 11.0,
                             6 * M_PI / 11.0,
                             7 * M_PI / 11.0,
                             8 * M_PI / 11.0,
                             9 * M_PI / 11.0,
                             10 * M_PI / 11.0};

    for (;;) {
        rn_pos b;
        b.theta = theta[i++];
        b.phi   = phi[j++];

        if (i >= 7)
            i = i % 7;
        if (j >= 11)
            j = j % 11;

        double psi;
        psi = acos(cos(a.theta) * cos(b.theta) +
                   sin(a.theta) * sin(b.theta) *
                   cos(a.phi - b.phi));

        if (isnan(psi))
            continue;

        get_uv_vec(v, a, b);
        get_repulse_vec(v, 0.0001);

        break;
    }
}

void
rinne::force_directed(int id)
{
    if (m_num_node < 2)
        return;

    //rn_pos *p_pos = new rn_pos[m_num_node];
    //rn_pos *pos_idx = p_pos;
    if (id == 0) {
        m_pos_tmp = new rn_pos[m_num_node];
    }

    m_barrier->wait();

    rn_pos *pos_idx = &m_pos_tmp[id];

    for (rn_node *p1 = &m_node[id]; p1 < &m_node[m_num_node];
         p1 += m_num_thread) {
        rn_node *p2;
        rn_vec v1 = {0.0, 0.0, 0.0};
        rn_vec v2;
        double cos_theta_a, sin_theta_a;
        double psi;

        cos_theta_a = cos(p1->pos.theta);
        sin_theta_a = sin(p1->pos.theta);

        for (p2 = m_node; p2 != &m_node[m_num_node]; p2++) {
            if (p1 == p2)
                continue;

            psi = acos(cos_theta_a * cos(p2->pos.theta) +
                       sin_theta_a * sin(p2->pos.theta) *
                       cos(p1->pos.phi - p2->pos.phi));

            if (isnan(psi)) {
                get_uv_vec_rand(v2, p1->pos);
            } else {
                get_uv_vec(v2, p1->pos, p2->pos);
                get_repulse_vec(v2, psi);
            }

            v1.x += v2.x;
            v1.y += v2.y;
            v1.z += v2.z;
        }

        rn_edge *p_edge;
        for (p_edge = p1->edge; p_edge != NULL; p_edge = p_edge->next) {
            rn_vec v3;

            psi = acos(cos_theta_a * cos(p_edge->dst->pos.theta) +
                       sin_theta_a * sin(p_edge->dst->pos.theta) *
                       cos(p1->pos.phi - p_edge->dst->pos.phi));

            if (isnan(psi))
                continue;

            get_uv_vec(v3, p1->pos, p_edge->dst->pos);
            get_spring_vec(v3, psi);

            v1.x += v3.x;
            v1.y += v3.y;
            v1.z += v3.z;
        }

        for (p_edge = p1->bp_edge; p_edge != NULL; p_edge = p_edge->bp_next) {
            rn_vec v3;

            psi = acos(cos_theta_a * cos(p_edge->src->pos.theta) +
                       sin_theta_a * sin(p_edge->src->pos.theta) *
                       cos(p1->pos.phi - p_edge->src->pos.phi));

            if (isnan(psi))
                continue;

            get_uv_vec(v3, p1->pos, p_edge->src->pos);
            get_spring_vec(v3, psi);

            v1.x += v3.x;
            v1.y += v3.y;
            v1.z += v3.z;
        }

        rn_vec pvec, cross;
        double rad, norm;

        TO_RECTANGULAR(pvec, p1->pos, 1.0);
        DISTANCE2(rad, v1);

        rad = sqrt(rad);
        if (rad > M_PI_4)
            rad = M_PI_4;

        CROSS_PRODUCT(cross, pvec, v1);
        DISTANCE2(norm, cross);

        norm = 1.0 / sqrt(norm);

        cross.x *= norm;
        cross.y *= norm;
        cross.z *= norm;

        ROTATE(pvec, cross, rad);

        *pos_idx = p1->pos;
        TO_SPHERICAL(*pos_idx, pvec);
        pos_idx += m_num_thread;
    }

    m_barrier->wait();

    for (int i = 0; i < m_num_node; i++) {
        m_node[i].pos = m_pos_tmp[i];
    }

    m_barrier->wait();

    if (id == 0)
        delete m_pos_tmp;
}

void
rinne::init_pos()
{
    srand(time(NULL));
    for (rn_node *p = m_node; p != &m_node[m_num_node]; p++) {
        p->pos.theta = M_PI_2 * ((double)rand() / RAND_MAX) + M_PI_4;
        p->pos.phi   = 2 * M_PI * ((double)rand() / RAND_MAX);
    }
}

void
rinne::read_dot(char *path)
{
    std::ifstream file(path);

    Graph g;
    boost::dynamic_properties dp(boost::ignore_other_properties);
    dp.property("vertex_name", get(boost::vertex_name, g));

    boost::read_graphviz(file, g, dp, "vertex_name");

    m_num_node = num_vertices(g);
    m_num_edge = num_edges(g);

    m_node  = new rn_node[m_num_node];
    m_edge  = new rn_edge[m_num_edge];
    m_label = new std::string[m_num_node];

    memset(m_node, 0, sizeof(*m_node) * m_num_node);
    memset(m_edge, 0, sizeof(*m_edge) * m_num_edge);

    std::pair<vertex_iter, vertex_iter> vp;
    int i = 0;
    for (vp = vertices(g); vp.first != vp.second; ++vp.first) {
        m_label[i++] = get(boost::vertex_name, g, *vp.first);
    }

    std::pair<edge_iter, edge_iter> ep;
    rn_edge *p_edge = m_edge;
    for (ep = edges(g); ep.first != ep.second; ++ep.first) {
        int s = source(*ep.first, g);
        int t = target(*ep.first, g);

        p_edge->src = &m_node[s];
        p_edge->dst = &m_node[t];

        p_edge->next = p_edge->src->edge;
        p_edge->src->edge = p_edge;
        p_edge->bp_next = p_edge->dst->bp_edge;
        p_edge->dst->bp_edge = p_edge;
        p_edge->src->num_edge++;
        p_edge->dst->num_bp_edge++;

        if (m_max_out_degree < p_edge->src->num_edge)
            m_max_out_degree = p_edge->src->num_edge;

        if (m_max_in_degree < p_edge->dst->num_bp_edge)
            m_max_in_degree = p_edge->dst->num_bp_edge;

        p_edge++;
    }

    init_pos();

    std::cout << "#node = " << m_num_node << std::endl;
    std::cout << "#edge = " << m_num_edge << std::endl;

    m_factor_step /= m_num_node * 100;

    get_top_n();

    int cores = boost::thread::hardware_concurrency();

    std::cout << cores << " CPU cores found" << std::endl;

    cores /= 2;
    cores--;
    if (cores < 0)
        cores = 1;

    m_num_thread = cores;
    m_thread = new boost::thread[cores];
    m_barrier = new boost::barrier(cores);

    for (int i = 0; i < cores; i++) {
        boost::thread th(boost::bind(&run, i));
        m_thread[i] = move(th);
    }
}

void
rinne::get_color(double &g, double &b, double &alpha,
                 double min_g, double max_g,
                 double min_b, double max_b,
                 double min_alpha, double max_alpha)
{
    double diff_g = max_g - min_g;
    double diff_b = max_b - min_b;
    double diff_alpha = max_alpha - min_alpha;
    double diff, r, cycle;

    cycle = m_cycle * 0.5;

    diff = m_current_sec - m_init_sec;

    r = sin(M_PI * (diff - M_PI * 0.5) / cycle) * 0.5 + 0.5;

    //std::cout << r << std::endl;

    g = r * diff_g + min_g;
    b = r * diff_b + min_b;
    alpha = r * diff_alpha + min_alpha;
}

int
main(int argc, char *argv[])
{
    if (argc < 2) {
        std::cerr << "usage: " << argv[0] << " graph.dot" << std::endl;
        return 1;
    }

    std::cout << "loading " << argv[1] << " ..." << std::endl;
    rinne_inst.read_dot(argv[1]);

    init_glut(argc, argv);

    return 0;
}