/*
(**************************************************************************)
(*                                                                        *)
(*                                Schifra                                 *)
(*                Reed-Solomon Error Correcting Code Library              *)
(*                                                                        *)
(* Release Version 0.0.1                                                  *)
(* http://www.schifra.com                                                 *)
(* Copyright (c) 2000-2018 Arash Partow, All Rights Reserved.             *)
(*                                                                        *)
(* The Schifra Reed-Solomon error correcting code library and all its     *)
(* components are supplied under the terms of the General Schifra License *)
(* agreement. The contents of the Schifra Reed-Solomon error correcting   *)
(* code library and all its components may not be copied or disclosed     *)
(* except in accordance with the terms of that agreement.                 *)
(*                                                                        *)
(* URL: http://www.schifra.com/license.html                               *)
(*                                                                        *)
(**************************************************************************)
*/

/*
   Description: This example will demonstrate the use of the Reed-Solomon
                encoder and decoder capabilities in a threaded context. One
                must note that the number of threads should not exceed the
                architecture's ability to efficiently and productively run the
                threads. A simple limiting strategy would be not to have more
                threads than the number of available cores on the processor.
*/

#include <cstddef>
#include <iostream>
#include <string>
#include <limits>

#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/thread/thread.hpp>

#include "schifra_galois_field.hpp"
#include "schifra_galois_field_polynomial.hpp"
#include "schifra_sequential_root_generator_polynomial_creator.hpp"
#include "schifra_reed_solomon_encoder.hpp"
#include "schifra_reed_solomon_decoder.hpp"
#include "schifra_reed_solomon_block.hpp"
#include "schifra_error_processes.hpp"
#include "schifra_ecc_traits.hpp"
#include "schifra_utilities.hpp"

const std::size_t round_count = 1000;

template <typename Encoder, typename Decoder>
class process
{
public:

   process(const unsigned int& process_id,
           const Encoder& encoder,
           const Decoder& decoder,
           const std::vector<std::string>& message_list)
   : process_id_(process_id),
     total_time_(0.0),
     encoder_(encoder),
     decoder_(decoder),
     message_list_(message_list)
   {}

   process& operator=(const process& proc)
   {
      process_id_ = proc.process_id_;
      total_time_ = proc.total_time_;
      return *this;
   }

   double time() { return total_time_; }

   inline void execute()
   {
      schifra::traits::equivalent_encoder_decoder<Encoder,Decoder>();
      typedef schifra::reed_solomon::block<Encoder::trait::code_length,Encoder::trait::fec_length> block_type;

      std::vector<block_type> block_list(message_list_.size());

      for (std::size_t i = 0; i < message_list_.size(); ++i)
      {
         if (!encoder_.encode(message_list_[i],block_list[i]))
         {
            std::cout << "[" << process_id_ << "] (0)Error - Critical encoding failure!" << std::endl;
            return;
         }
         schifra::corrupt_message_all_errors00(block_list[i],0,3);
      }

      schifra::utils::timer timer;
      timer.start();

      for (std::size_t k = 0; k < round_count; ++k)
      {
         for (std::size_t i = 0; i < message_list_.size(); ++i)
         {
            if (!decoder_.decode(block_list[i]))
            {
               std::cout << "[" << process_id_ << "] (1)Error - Critical decoding failure!" << std::endl;
               return;
            }
            else if (!schifra::is_block_equivelent(block_list[i],message_list_[i]))
            {
               std::cout << "[" << process_id_ << "] (2)Error - Error correction failed!" << std::endl;
               return;
            }
         }
      }

      timer.stop();
      total_time_ = timer.time();
   }

private:

   unsigned int process_id_;
   double total_time_;
   const Encoder& encoder_;
   const Decoder& decoder_;
   const std::vector<std::string>& message_list_;
};

void generate_messages(const std::size_t data_length, std::vector<std::string>& message_list)
{
   for (unsigned int c = 0; c < 256; ++c)
   {
      message_list.push_back(std::string(data_length,static_cast<unsigned char>(c)));
   }
}

int main()
{
   /* Reed Solomon Code Parameters */
   const std::size_t code_length = 255;
   const std::size_t fec_length  =  32;
   const std::size_t data_length = code_length - fec_length;

   /* Finite Field Parameters */
   const std::size_t field_descriptor                =   8;
   const std::size_t generator_polynomial_index      = 120;
   const std::size_t generator_polynomial_root_count = fec_length;

   /* Instantiate Finite Field and Generator Polynomials */
   schifra::galois::field field(field_descriptor,
                                schifra::galois::primitive_polynomial_size06,
                                schifra::galois::primitive_polynomial06);

   schifra::galois::field_polynomial generator_polynomial(field);

   if (
        !schifra::make_sequential_root_generator_polynomial(field,
                                                            generator_polynomial_index,
                                                            generator_polynomial_root_count,
                                                            generator_polynomial)
      )
   {
      std::cout << "Error - Failed to create sequential root generator!" << std::endl;
      return 1;
   }

   typedef schifra::reed_solomon::encoder<code_length,fec_length> encoder_type;
   typedef schifra::reed_solomon::decoder<code_length,fec_length> decoder_type;
   typedef process<encoder_type,decoder_type>                     process_type;
   typedef boost::shared_ptr<process_type>                        process_ptr_type;

   /* Instantiate Encoder and Decoder (Codec) */
   encoder_type encoder(field,generator_polynomial);
   decoder_type decoder(field,generator_polynomial_index);

   std::vector<std::string> message_list;

   generate_messages(data_length,message_list);

   const unsigned int max_thread_count = 4; // number of functional cores.
   std::vector<process_ptr_type> process_list;

   boost::thread_group threads;

   for (unsigned int i = 0; i < max_thread_count; ++i)
   {
      process_list.push_back(process_ptr_type(new process_type(i,encoder,decoder,message_list)));
      threads.create_thread(boost::bind(&process_type::execute,process_list[i]));
   }

   threads.join_all();

   double time = -1.0;

   /* Determine the process with the longest running time. */
   for (std::size_t i = 0; i < process_list.size(); ++i)
   {
      time = ((time < process_list[i]->time()) ? process_list[i]->time() : time);
   }

   double mbps = (max_thread_count * round_count * message_list.size() * data_length * 8.0) / (1048576.0 * time);

   std::cout << "Blocks decoded: " << max_thread_count * round_count * message_list.size() << "\tTime: " << time <<"sec\tRate: " << mbps << "Mbps" << std::endl;

   return 0;
}


#pragma once
#include <Eigen/Dense>
#include <vector>
#include <autd3.hpp>

std::vector<Eigen::Vector3f> cornersWorkspaceAll(const Eigen::Vector3f& corner0, const Eigen::Vector3f& corner1);

std::vector<float> range2Points(const Eigen::Vector3f & pos_self, const Eigen::Quaternionf &quo_self, std::vector<Eigen::Vector3f> const &points);

namespace dynaman {

	bool isInsideWorkspace(const Eigen::Vector3f& pos, const Eigen::Vector3f& lowerbound, const Eigen::Vector3f& upperbound);

	Eigen::Matrix3f RotForDeviceId(int deviceId, autd::GeometryPtr geo);

	std::vector<Eigen::Matrix3f> RotsAutd(autd::GeometryPtr geo);

	std::vector<Eigen::Matrix3f> RotsAutd(std::shared_ptr<autd::Controller> pAupa);

	Eigen::Vector3f CenterForDeviceId(int deviceId, autd::GeometryPtr geo);

	Eigen::Matrix3Xf CentersAutd(autd::GeometryPtr geo);

	Eigen::Matrix3Xf DirectionsAutd(autd::GeometryPtr geo);
}


#ifndef UTIL_VIEWPORT_HPP
#define UTIL_VIEWPORT_HPP

#include "util/coordinate.hpp"
#include "util/web_mercator.hpp"

#include <boost/assert.hpp>

#include <cmath>
#include <tuple>

// Port of https://github.com/mapbox/geo-viewport

namespace osrm
{
namespace util
{
namespace viewport
{

namespace detail
{
static constexpr unsigned MAX_ZOOM = 18;
static constexpr unsigned MIN_ZOOM = 1;
// this is an upper bound to current display sizes
static constexpr double VIEWPORT_WIDTH = 8 * web_mercator::TILE_SIZE;
static constexpr double VIEWPORT_HEIGHT = 5 * web_mercator::TILE_SIZE;
static double INV_LOG_2 = 1. / std::log(2);
}

inline unsigned getFittedZoom(util::Coordinate south_west, util::Coordinate north_east)
{
    const auto min_x = web_mercator::degreeToPixel(toFloating(south_west.lon), detail::MAX_ZOOM);
    const auto max_y = web_mercator::degreeToPixel(toFloating(south_west.lat), detail::MAX_ZOOM);
    const auto max_x = web_mercator::degreeToPixel(toFloating(north_east.lon), detail::MAX_ZOOM);
    const auto min_y = web_mercator::degreeToPixel(toFloating(north_east.lat), detail::MAX_ZOOM);
    const double width_ratio = (max_x - min_x) / detail::VIEWPORT_WIDTH;
    const double height_ratio = (max_y - min_y) / detail::VIEWPORT_HEIGHT;
    const auto zoom = detail::MAX_ZOOM -
                      std::max(std::log(width_ratio), std::log(height_ratio)) * detail::INV_LOG_2;

    if (std::isfinite(zoom))
        return std::max<unsigned>(detail::MIN_ZOOM, zoom);
    else
        return detail::MIN_ZOOM;
}
}
}
}

#endif


// Copyright 2005-2009 Daniel James.
// 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)

// A general purpose hash function for non-zero floating point values.

#if !defined(BOOST_FUNCTIONAL_HASH_DETAIL_HASH_FLOAT_GENERIC_HEADER)
#define BOOST_FUNCTIONAL_HASH_DETAIL_HASH_FLOAT_GENERIC_HEADER

#include <boost/functional/hash/detail/float_functions.hpp>
#include <boost/integer/static_log2.hpp>
#include <boost/functional/hash/detail/limits.hpp>

#if defined(_MSC_VER) && (_MSC_VER >= 1020)
# pragma once
#endif

#if defined(BOOST_MSVC)
#pragma warning(push)
#if BOOST_MSVC >= 1400
#pragma warning(disable:6294) // Ill-defined for-loop: initial condition does
                              // not satisfy test. Loop body not executed
#endif
#endif

namespace boost
{
    namespace hash_detail
    {
        inline void hash_float_combine(std::size_t& seed, std::size_t value)
        {
            seed ^= value + (seed<<6) + (seed>>2);
        }

        template <class T>
        inline std::size_t float_hash_impl2(T v)
        {
            boost::hash_detail::call_frexp<T> frexp;
            boost::hash_detail::call_ldexp<T> ldexp;

            int exp = 0;

            v = frexp(v, &exp);

            // A postive value is easier to hash, so combine the
            // sign with the exponent and use the absolute value.
            if(v < 0) {
                v = -v;
                exp += limits<T>::max_exponent -
                    limits<T>::min_exponent;
            }

            v = ldexp(v, limits<std::size_t>::digits);
            std::size_t seed = static_cast<std::size_t>(v);
            v -= seed;

            // ceiling(digits(T) * log2(radix(T))/ digits(size_t)) - 1;
            std::size_t const length
                = (limits<T>::digits *
                        boost::static_log2<limits<T>::radix>::value
                        + limits<std::size_t>::digits - 1)
                / limits<std::size_t>::digits;

            for(std::size_t i = 0; i != length; ++i)
            {
                v = ldexp(v, limits<std::size_t>::digits);
                std::size_t part = static_cast<std::size_t>(v);
                v -= part;
                hash_float_combine(seed, part);
            }

            hash_float_combine(seed, exp);

            return seed;
        }

        template <class T>
        inline std::size_t float_hash_impl(T v)
        {
            typedef BOOST_DEDUCED_TYPENAME select_hash_type<T>::type type;
            return float_hash_impl2(static_cast<type>(v));
        }
    }
}

#if defined(BOOST_MSVC)
#pragma warning(pop)
#endif

#endif


#include <possumwood_sdk/node_implementation.h>

#define EIGEN_STACK_ALLOCATION_LIMIT 0

#include <actions/traits.h>
#include <tbb/task_group.h>

#include <Eigen/Sparse>
#include <mutex>
#include <opencv2/opencv.hpp>

#include "frame.h"

namespace {

class Triplets;

class Row {
  public:
	Row() = default;

	void addValue(int64_t row, int64_t col, double value) {
		if(value != 0.0)
			m_values[(row << 32) + col] += value;
	}

  private:
	Row(const Row&) = delete;
	Row& operator=(const Row&) = delete;

	std::map<int64_t, double> m_values;

	friend class Triplets;
};

class Triplets {
  public:
	Triplets(int rows, int cols) : m_rowCount(0), m_rows(rows), m_cols(cols) {
	}

	void addRow(const Row& r) {
		for(auto& v : r.m_values) {
			int32_t row = v.first >> 32;
			int32_t col = v.first & 0xffffffff;

			assert(row < m_rows);
			assert(col < m_cols);

			m_triplets.push_back(Eigen::Triplet<double>(m_rowCount, row * m_cols + col, v.second));
		}

		++m_rowCount;
	}

	std::size_t rows() const {
		return m_rowCount;
	}

	const std::vector<Eigen::Triplet<double>>& triplets() const {
		return m_triplets;
	}

  private:
	std::vector<Eigen::Triplet<double>> m_triplets;

	int m_rowCount, m_rows, m_cols;

	friend class Row;
};

static const cv::Mat kernel = (cv::Mat_<double>(3, 3) << 0.0, -1.0, 0.0, -1.0, 4.0, -1.0, 0.0, -1.0, 0.0);

// static const cv::Mat kernel = (cv::Mat_<double>(3,3) <<
// 	-1.0, -1.0, -1.0,
// 	-1.0,  8.0, -1.0,
// 	-1.0, -1.0, -1.0
// );

// static const cv::Mat kernel = (cv::Mat_<double>(3,3) <<
// 	-1.0, -2.0, -1.0,
// 	-2.0, 12.0, -2.0,
// 	-1.0, -2.0, -1.0
// );

// static const cv::Mat kernel = (cv::Mat_<double>(5,5) <<
// 	 0.0,  0.0,  1.0,  0.0,  0.0,
// 	 0.0,  2.0, -8.0,  2.0,  0.0,
// 	 1.0, -8.0, 20.0, -8.0,  1.0,
// 	 0.0,  2.0, -8.0,  2.0,  0.0,
// 	 0.0,  0.0,  1.0,  0.0,  0.0
// );

float buildMatrices(const cv::Mat& image, const cv::Mat& mask, Eigen::SparseMatrix<double>& A, Eigen::VectorXd& b,
                    const cv::Rect2i& roi) {
	Triplets triplets(roi.height, roi.width);
	std::vector<double> values;

	std::size_t validCtr = 0, interpolatedCtr = 0;

	for(int y = roi.y; y < roi.y + roi.height; ++y)
		for(int x = roi.x; x < roi.x + roi.width; ++x) {
			Row row;

			// masked and/or edge
			if(mask.at<unsigned char>(y, x) > 128) {
				values.push_back(0.0f);

				// convolution
				for(int yi = 0; yi < kernel.rows; ++yi)
					for(int xi = 0; xi < kernel.cols; ++xi) {
						int ypos = y + yi - kernel.rows / 2;
						int xpos = x + xi - kernel.cols / 2;

						// handling of edges - "clip" (or "mirror", commented out for now)
						if(ypos < roi.y)
							// ypos = -ypos;
							ypos = roi.y;
						if(ypos >= roi.y + roi.height)
							// ypos = (image.rows-1) - (ypos-image.rows);
							ypos = roi.y + roi.height - 1;

						if(xpos < roi.x)
							// xpos = -xpos;
							xpos = roi.x;
						if(xpos >= roi.x + roi.width)
							// xpos = (image.cols-1) - (xpos-image.cols);
							xpos = roi.x + roi.width - 1;

						row.addValue(ypos - roi.y, xpos - roi.x, kernel.at<double>(yi, xi));
					}

				++interpolatedCtr;
			}

			// non-masked
			if(mask.at<unsigned char>(y, x) <= 128) {
				values.push_back(image.at<float>(y, x));
				row.addValue(y - roi.y, x - roi.x, 1);

				++validCtr;
			}

			triplets.addRow(row);
		}

	// initialise the sparse matrix
	A = Eigen::SparseMatrix<double>(triplets.rows(), roi.height * roi.width);
	A.setFromTriplets(triplets.triplets().begin(), triplets.triplets().end());

	// and the "b" vector
	assert(values.size() == triplets.rows());
	b = Eigen::VectorXd(values.size());
	for(std::size_t i = 0; i < values.size(); ++i)
		b[i] = values[i];

	return (float)validCtr / ((float)validCtr + (float)interpolatedCtr);
}

dependency_graph::InAttr<possumwood::opencv::Frame> a_inFrame, a_inMask;
dependency_graph::InAttr<unsigned> a_mosaic;
dependency_graph::OutAttr<possumwood::opencv::Frame> a_outFrame;

dependency_graph::State compute(dependency_graph::Values& data) {
	dependency_graph::State state;

	const cv::Mat& input = *data.get(a_inFrame);
	const cv::Mat& mask = *data.get(a_inMask);
	const unsigned mosaic = data.get(a_mosaic);

	if(input.depth() != CV_32F)
		throw std::runtime_error("Laplacian inpainting - input image type has to be CV_32F.");
	if(mask.type() != CV_8UC1 && mask.type() != CV_8UC3)
		throw std::runtime_error("Laplacian inpainting - mask image type has to be CV_8UC1 or CV_8UC3.");
	if(input.empty() || mask.empty())
		throw std::runtime_error("Laplacian inpainting - empty input image and/or mask.");
	if(input.size != mask.size)
		throw std::runtime_error("Laplacian inpainting - input and mask image size have to match.");
	if(input.cols % mosaic != 0 || input.rows % mosaic != 0)
		throw std::runtime_error(
		    "Laplacian inpainting - image size is not divisible by mosaic count - invalid mosaic?.");

	std::vector<std::vector<float>> x(input.channels(), std::vector<float>(input.rows * input.cols, 0.0f));

	tbb::task_group tasks;
	std::mutex solve_mutex;

	// split the inputs and masks per channel
	std::vector<cv::Mat> inputs, masks;
	cv::split(input, inputs);
	cv::split(mask, masks);

	assert((int)inputs.size() == input.channels());
	assert((int)masks.size() == mask.channels());

	const unsigned mosaic_rows = input.rows / mosaic;
	const unsigned mosaic_cols = input.cols / mosaic;

	for(unsigned yi = 0; yi < mosaic; ++yi) {
		for(unsigned xi = 0; xi < mosaic; ++xi) {
			cv::Rect2i roi;
			roi.y = yi * mosaic_rows;
			roi.x = xi * mosaic_cols;
			roi.height = mosaic_rows;
			roi.width = mosaic_cols;

			for(int channel = 0; channel < input.channels(); ++channel) {
				tasks.run([channel, &inputs, &masks, &x, &state, &solve_mutex, roi]() {
					cv::Mat inTile = inputs[channel];

					cv::Mat inMask;
					if(masks.size() == 1)
						inMask = masks[0];
					else
						inMask = masks[channel];

					Eigen::SparseMatrix<double> A;
					Eigen::VectorXd b, tmp;

					const float ratio = buildMatrices(inTile, inMask, A, b, roi);

					if(ratio > 0.003) {
						const char* stage = "solver construction";

						Eigen::SparseLU<Eigen::SparseMatrix<double> /*, Eigen::NaturalOrdering<int>*/> chol(A);

						if(chol.info() == Eigen::Success) {
							stage = "analyze pattern";

							chol.analyzePattern(A);

							if(chol.info() == Eigen::Success) {
								stage = "factorize";

								chol.factorize(A);

								if(chol.info() == Eigen::Success) {
									stage = "solve";

									tmp = chol.solve(b);

									assert(tmp.size() == roi.height * roi.width);
									for(int i = 0; i < tmp.size(); ++i) {
										const int row = i / roi.width;
										const int col = i % roi.width;
										const int index = (row + roi.y) * masks[0].cols + col + roi.x;

										assert((std::size_t)index < x[channel].size());

										x[channel][index] = tmp[i];
									}
								}
							}
						}

						std::lock_guard<std::mutex> guard(solve_mutex);

						if(chol.info() == Eigen::NumericalIssue)
							state.addWarning("Decomposition failed - Eigen::NumericalIssue at stage " +
							                 std::string(stage));
						else if(chol.info() == Eigen::NoConvergence)
							state.addWarning("Decomposition failed - Eigen::NoConvergence at stage " +
							                 std::string(stage));
						else if(chol.info() == Eigen::InvalidInput)
							state.addWarning("Decomposition failed - Eigen::InvalidInput at stage " +
							                 std::string(stage));
						else if(chol.info() != Eigen::Success)
							state.addWarning("Decomposition failed - unknown error at stage " + std::string(stage));
					}
				});
			}
		}
	}

	tasks.wait();

	cv::Mat result = input.clone();
	for(int yi = 0; yi < result.rows; ++yi)
		for(int xi = 0; xi < result.cols; ++xi)
			for(int c = 0; c < input.channels(); ++c)
				result.ptr<float>(yi, xi)[c] = x[c][yi * result.cols + xi];

	data.set(a_outFrame, possumwood::opencv::Frame(result));

	return state;
}

void init(possumwood::Metadata& meta) {
	meta.addAttribute(a_inFrame, "frame", possumwood::opencv::Frame(), possumwood::AttrFlags::kVertical);
	meta.addAttribute(a_inMask, "mask", possumwood::opencv::Frame(), possumwood::AttrFlags::kVertical);
	meta.addAttribute(a_mosaic, "mosaic", 1u);
	meta.addAttribute(a_outFrame, "out_frame", possumwood::opencv::Frame(), possumwood::AttrFlags::kVertical);

	meta.addInfluence(a_inFrame, a_outFrame);
	meta.addInfluence(a_inMask, a_outFrame);
	meta.addInfluence(a_mosaic, a_outFrame);

	meta.setCompute(compute);
}

possumwood::NodeImplementation s_impl("opencv/inpaint_laplacian", init);

}  // namespace


#include <librealsense2/rs.hpp>
#include <Eigen/Core>
#include <iostream>
#include "D435iCalibWriter.h"

int main(int argc, char *argv[]) try
{
    //ensure an output directory is supplied
    if (argc != 2){
        std::cout<<"usage: "<< argv[0] <<" <camera name>\n"<<
        "Intrisics file will be saved as <camera name>_intr.yaml\n";
        return 0;
    }

    std::string cam_name(argv[1]);

    ark::Recorder rec(cam_name);

    //Set up realsense pipeline
    rs2::config cfg;
    cfg.enable_stream(RS2_STREAM_DEPTH,-1,640,480,RS2_FORMAT_Z16,30);
    cfg.enable_stream(RS2_STREAM_INFRARED, 1, 640, 480, RS2_FORMAT_Y8, 30);
    cfg.enable_stream(RS2_STREAM_INFRARED, 2, 640, 480, RS2_FORMAT_Y8, 30);
    //rs2::config cfg2;
    cfg.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F,250);
    cfg.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F,200);

    cfg.enable_stream(RS2_STREAM_COLOR,-1,640,480,RS2_FORMAT_BGR8,30);

    rs2::pipeline pipe;
    //rs2::pipeline motion_pipe;

    // Start streaming with default recommended configuration
    rs2::pipeline_profile selection = pipe.start(cfg);

    // Get image streams
    auto depth_stream = selection.get_stream(RS2_STREAM_DEPTH)
                             .as<rs2::video_stream_profile>();

    auto ir1_stream = selection.get_stream(RS2_STREAM_INFRARED,1)
                             .as<rs2::video_stream_profile>();

    auto ir2_stream = selection.get_stream(RS2_STREAM_INFRARED,2)
                             .as<rs2::video_stream_profile>();
    auto color_stream = selection.get_stream(RS2_STREAM_COLOR)
                             .as<rs2::video_stream_profile>();
    // Get motion streams
    auto gyro_stream =  selection.get_stream(RS2_STREAM_GYRO)
                             .as<rs2::motion_stream_profile>();
    auto accel_stream =  selection.get_stream(RS2_STREAM_ACCEL)
                             .as<rs2::motion_stream_profile>();

    //get extrinsics between cameras and imu
    rs2_extrinsics extrinsics = ir1_stream.get_extrinsics_to(accel_stream);
    Eigen::Vector3f T_12(extrinsics.translation[0], extrinsics.translation[1], extrinsics.translation[2]);
    Eigen::Map<Eigen::Matrix3f>R_12(extrinsics.rotation);

    Eigen::Matrix4f tf_ir1 = Eigen::Matrix4f::Identity();
    tf_ir1.block<3,3>(0,0)=R_12;
    tf_ir1.block<3,1>(0,3)=T_12;

    rs2_extrinsics extrinsics2 = ir2_stream.get_extrinsics_to(accel_stream);
    Eigen::Vector3f T2_12(extrinsics2.translation[0], extrinsics2.translation[1], extrinsics2.translation[2]);
    Eigen::Map<Eigen::Matrix3f>R2_12(extrinsics2.rotation);
    Eigen::Matrix4f tf_ir2 = Eigen::Matrix4f::Identity();
    tf_ir2.block<3,3>(0,0)=R2_12;
    tf_ir2.block<3,1>(0,3)=T2_12;

    rs2_extrinsics extrinsics3 = depth_stream.get_extrinsics_to(accel_stream);
    Eigen::Vector3f T3_12(extrinsics3.translation[0], extrinsics3.translation[1], extrinsics3.translation[2]);
    Eigen::Map<Eigen::Matrix3f>R3_12(extrinsics3.rotation);
    Eigen::Matrix4f tf_depth = Eigen::Matrix4f::Identity();
    tf_depth.block<3,3>(0,0)=R3_12;
    tf_depth.block<3,1>(0,3)=T3_12;

    rs2_extrinsics extrinsics4 = color_stream.get_extrinsics_to(accel_stream);
    Eigen::Vector3f T4_12(extrinsics4.translation[0], extrinsics4.translation[1], extrinsics4.translation[2]);
    Eigen::Map<Eigen::Matrix3f>R4_12(extrinsics4.rotation);
    Eigen::Matrix4f tf_color = Eigen::Matrix4f::Identity();
    tf_color.block<3,3>(0,0)=R4_12;
    tf_color.block<3,1>(0,3)=T4_12;

    //get camera intrinsics
    rs2_intrinsics rs_intr_ir1 = ir1_stream.get_intrinsics();
    rs2_intrinsics rs_intr_ir2 = ir2_stream.get_intrinsics();
    rs2_intrinsics rs_intr_depth = depth_stream.get_intrinsics();
    rs2_intrinsics rs_intr_color = color_stream.get_intrinsics();

    //compose calibration for slam cameras
    ark::CameraCalibration ir1_calib(tf_ir1.transpose(),rs_intr_ir1);
    ark::CameraCalibration ir2_calib(tf_ir2.transpose(),rs_intr_ir2);

    std::vector<ark::CameraCalibration> camera_calibs;
    camera_calibs.push_back(ir1_calib);
    camera_calibs.push_back(ir2_calib);

    //Write intrinsics to file
    rec.write_camera_intrinsics_and_extrinsics(camera_calibs);

    //compose calibration for additional cameras
    ark::CameraCalibration depth_calib(tf_depth.transpose(),rs_intr_depth);
    ark::CameraCalibration color_calib(tf_color.transpose(),rs_intr_color);

    std::vector<ark::CameraCalibration> add_camera_calibs;
    add_camera_calibs.push_back(depth_calib);
    add_camera_calibs.push_back(color_calib);
    rec.write_additional_intrinsics_and_extrinsics(add_camera_calibs);

    rec.write_camera_params();
    rec.write_imu_intrinsics();
    rec.write_additional_parameters();

    Eigen::Matrix3f cam_mat_ir1;
    cam_mat_ir1(0,0) = rs_intr_ir1.fx;
    cam_mat_ir1(0,2) = rs_intr_ir1.ppx;
    cam_mat_ir1(1,1) = rs_intr_ir1.fy;
    cam_mat_ir1(1,2) = rs_intr_ir1.ppy;
    cam_mat_ir1(2,2) = 1;

    Eigen::Matrix3f cam_mat_ir2;
    cam_mat_ir2(0,0) = rs_intr_ir2.fx;
    cam_mat_ir2(0,2) = rs_intr_ir2.ppx;
    cam_mat_ir2(1,1) = rs_intr_ir2.fy;
    cam_mat_ir2(1,2) = rs_intr_ir2.ppy;
    cam_mat_ir2(2,2) = 1;

    std::cout << "IR1: " << cam_mat_ir1.inverse() << std::endl;
    std::cout << "IR2: " << cam_mat_ir2.inverse() <<  std::endl;

    std::cout << "----------------Camera 1----------------\n";
    std::cout << "Principal Point         : " << rs_intr_ir1.ppx << ", " << rs_intr_ir1.ppy << std::endl;
    std::cout << "Focal Length            : " << rs_intr_ir1.fx << ", " << rs_intr_ir1.fy << std::endl;
    std::cout << "Distortion Model        : " << rs_intr_ir1.model << std::endl;
    std::cout << "Distortion Coefficients : [" << rs_intr_ir1.coeffs[0] << "," << rs_intr_ir1.coeffs[1] << "," <<
        rs_intr_ir1.coeffs[2] << "," << rs_intr_ir1.coeffs[3] << "," << rs_intr_ir1.coeffs[4] << "]" << std::endl;

    std::cout << "----------------Camera 2----------------\n";
    std::cout << "Principal Point         : " << rs_intr_ir2.ppx << ", " << rs_intr_ir2.ppy << std::endl;
    std::cout << "Focal Length            : " << rs_intr_ir2.fx << ", " << rs_intr_ir2.fy << std::endl;
    std::cout << "Distortion Model        : " << rs_intr_ir2.model << std::endl;
    std::cout << "Distortion Coefficients : [" << rs_intr_ir2.coeffs[0] << "," << rs_intr_ir2.coeffs[1] << "," <<
        rs_intr_ir2.coeffs[2] << "," << rs_intr_ir2.coeffs[3] << "," << rs_intr_ir2.coeffs[4] << "]" << std::endl;
    std::cout << "----------------Depth----------------\n";
    std::cout << "Principal Point         : " << rs_intr_depth.ppx << ", " << rs_intr_depth.ppy << std::endl;
    std::cout << "Focal Length            : " << rs_intr_depth.fx << ", " << rs_intr_depth.fy << std::endl;
    std::cout << "Distortion Model        : " << rs_intr_depth.model << std::endl;
    std::cout << "Distortion Coefficients : [" << rs_intr_depth.coeffs[0] << "," << rs_intr_depth.coeffs[1] << "," <<
        rs_intr_depth.coeffs[2] << "," << rs_intr_depth.coeffs[3] << "," << rs_intr_depth.coeffs[4] << "]" << std::endl;
    std::cout << "----------------Color----------------\n";
    std::cout << "Principal Point         : " << rs_intr_color.ppx << ", " << rs_intr_color.ppy << std::endl;
    std::cout << "Focal Length            : " << rs_intr_color.fx << ", " << rs_intr_color.fy << std::endl;
    std::cout << "Distortion Model        : " << rs_intr_color.model << std::endl;
    std::cout << "Distortion Coefficients : [" << rs_intr_color.coeffs[0] << "," << rs_intr_color.coeffs[1] << "," <<
        rs_intr_color.coeffs[2] << "," << rs_intr_color.coeffs[3] << "," << rs_intr_color.coeffs[4] << "]" << std::endl;

    std::cout << "---------------Extrinsics Cam1---------------\n";
    std::cout << "Translation Vector : [" << extrinsics.translation[0] << "," << extrinsics.translation[1] << "," << extrinsics.translation[2] << "]\n";
    std::cout << "Rotation Matrix    : [" << extrinsics.rotation[0] << "," << extrinsics.rotation[3] << "," << extrinsics.rotation[6] << "]\n";
    std::cout << "                   : [" << extrinsics.rotation[1] << "," << extrinsics.rotation[4] << "," << extrinsics.rotation[7] << "]\n";
    std::cout << "                   : [" << extrinsics.rotation[2] << "," << extrinsics.rotation[5] << "," << extrinsics.rotation[8] << "]" << std::endl;

    std::cout << "---------------Extrinsics Cam2---------------\n";
    std::cout << "Translation Vector : [" << extrinsics2.translation[0] << "," << extrinsics2.translation[1] << "," << extrinsics2.translation[2] << "]\n";
    std::cout << "Rotation Matrix    : [" << extrinsics2.rotation[0] << "," << extrinsics2.rotation[3] << "," << extrinsics2.rotation[6] << "]\n";
    std::cout << "                   : [" << extrinsics2.rotation[1] << "," << extrinsics2.rotation[4] << "," << extrinsics2.rotation[7] << "]\n";
    std::cout << "                   : [" << extrinsics2.rotation[2] << "," << extrinsics2.rotation[5] << "," << extrinsics2.rotation[8] << "]" << std::endl;
    std::cout << "---------------Extrinsics Depth---------------\n";
    std::cout << "Translation Vector : [" << extrinsics3.translation[0] << "," << extrinsics3.translation[1] << "," << extrinsics3.translation[2] << "]\n";
    std::cout << "Rotation Matrix    : [" << extrinsics3.rotation[0] << "," << extrinsics3.rotation[3] << "," << extrinsics3.rotation[6] << "]\n";
    std::cout << "                   : [" << extrinsics3.rotation[1] << "," << extrinsics3.rotation[4] << "," << extrinsics3.rotation[7] << "]\n";
    std::cout << "                   : [" << extrinsics3.rotation[2] << "," << extrinsics3.rotation[5] << "," << extrinsics3.rotation[8] << "]" << std::endl;
    std::cout << "---------------Extrinsics Color---------------\n";
    std::cout << "Translation Vector : [" << extrinsics4.translation[0] << "," << extrinsics4.translation[1] << "," << extrinsics4.translation[2] << "]\n";
    std::cout << "Rotation Matrix    : [" << extrinsics4.rotation[0] << "," << extrinsics4.rotation[3] << "," << extrinsics4.rotation[6] << "]\n";
    std::cout << "                   : [" << extrinsics4.rotation[1] << "," << extrinsics4.rotation[4] << "," << extrinsics4.rotation[7] << "]\n";
    std::cout << "                   : [" << extrinsics4.rotation[2] << "," << extrinsics4.rotation[5] << "," << extrinsics4.rotation[8] << "]" << std::endl;

    std::cout << "Closing Files..." << std::flush << std::endl;
    rec.close();
    std::cout << "Terminated." << std::flush << std::endl;
	return 0;
}catch(...)
{
    printf("Unhandled excepton occured'n");
    return EXIT_FAILURE;
}


#ifndef MATHEVAL_IMPLEMENTATION
#error "Do not include ast.hpp directly!"
#endif

#pragma once

#include <boost/variant.hpp>

#include <list>
#include <string>

namespace matheval {

namespace ast {

struct nil {};
struct unary_op;
struct binary_op;
struct ternary_op;
struct expression;

// clang-format off
typedef boost::variant<
        nil // can't happen!
        , double
        , std::string
        , boost::recursive_wrapper<unary_op>
        , boost::recursive_wrapper<binary_op>
        , boost::recursive_wrapper<ternary_op>
        , boost::recursive_wrapper<expression>
        >
operand;
// clang-format on

struct unary_op {
    double (*op)(double);
    operand rhs;
    unary_op() {}
    unary_op(double (*op)(double), operand const &rhs) : op(op), rhs(rhs) {}
};

struct binary_op {
    double (*op)(double, double);
    operand lhs;
    operand rhs;
    binary_op() {}
    binary_op(double (*op)(double, double), operand const &lhs,
              operand const &rhs)
        : op(op), lhs(lhs), rhs(rhs) {}
};

struct ternary_op {
    double (*op)(double, double, double);
    operand p1, p2, p3;
    ternary_op() {}
    ternary_op(double (*op_)(double, double, double),
	       operand const &p1_,
               operand const &p2_,
	       operand const &p3_)
      : op(op_), p1(p1_), p2(p2_), p3(p3_) {}
};

struct operation {
    double (*op)(double, double);
    operand rhs;
    operation() {}
    operation(double (*op)(double, double), operand const &rhs)
        : op(op), rhs(rhs) {}
};

struct expression {
    operand lhs;
    std::list<operation> rhs;
    expression() {}
    expression(operand const &lhs, std::list<operation> const &rhs)
        : lhs(lhs), rhs(rhs) {}
};

} // namespace ast

} // namespace matheval


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 * @file    testDecisionTree.cpp
 * @brief    Develop DecisionTree
 * @author  Frank Dellaert
 * @date  Mar 6, 2011
 */

#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DiscreteKey.h> // make sure we have traits
// headers first to make sure no missing headers
//#define DT_NO_PRUNING
#include <gtsam/discrete/AlgebraicDecisionTree.h>
#include <gtsam/discrete/DecisionTree-inl.h> // for convert only
#define DISABLE_TIMING

#include <boost/timer.hpp>
#include <boost/tokenizer.hpp>
#include <boost/assign/std/map.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;

#include <CppUnitLite/TestHarness.h>
#include <gtsam/discrete/Signature.h>

using namespace std;
using namespace gtsam;

/* ******************************************************************************** */
typedef AlgebraicDecisionTree<Key> ADT;

// traits
namespace gtsam {
template<> struct traits<ADT> : public Testable<ADT> {};
}

#define DISABLE_DOT

template<typename T>
void dot(const T&f, const string& filename) {
#ifndef DISABLE_DOT
  f.dot(filename);
#endif
}

/** I can't get this to work !
 class Mul: boost::function<double(const double&, const double&)> {
 inline double operator()(const double& a, const double& b) {
 return a * b;
 }
 };

 // If second argument of binary op is Leaf
 template<typename L>
 typename DecisionTree<L, double>::Node::Ptr DecisionTree<L, double>::Choice::apply_fC_op_gL(
 Cache& cache, const Leaf& gL, Mul op) const {
 Ptr h(new Choice(label(), cardinality()));
 for(const NodePtr& branch: branches_)
 h->push_back(branch->apply_f_op_g(cache, gL, op));
 return Unique(cache, h);
 }
 */

/* ******************************************************************************** */
// instrumented operators
/* ******************************************************************************** */
size_t muls = 0, adds = 0;
boost::timer timer;
void resetCounts() {
  muls = 0;
  adds = 0;
  timer.restart();
}
void printCounts(const string& s) {
#ifndef DISABLE_TIMING
  cout << boost::format("%s: %3d muls, %3d adds, %g ms.") % s % muls % adds
  % (1000 * timer.elapsed()) << endl;
#endif
  resetCounts();
}
double mul(const double& a, const double& b) {
  muls++;
  return a * b;
}
double add_(const double& a, const double& b) {
  adds++;
  return a + b;
}

/* ******************************************************************************** */
// test ADT
TEST(ADT, example3)
{
  // Create labels
  DiscreteKey A(0,2), B(1,2), C(2,2), D(3,2), E(4,2);

  // Literals
  ADT a(A, 0.5, 0.5);
  ADT notb(B, 1, 0);
  ADT c(C, 0.1, 0.9);
  ADT d(D, 0.1, 0.9);
  ADT note(E, 0.9, 0.1);

  ADT cnotb = c * notb;
  dot(cnotb, "ADT-cnotb");

//  a.print("a: ");
//  cnotb.print("cnotb: ");
  ADT acnotb = a * cnotb;
//  acnotb.print("acnotb: ");
//  acnotb.printCache("acnotb Cache:");

  dot(acnotb, "ADT-acnotb");

  ADT big = apply(apply(d, note, &mul), acnotb, &add_);
  dot(big, "ADT-big");
}

/* ******************************************************************************** */
// Asia Bayes Network
/* ******************************************************************************** */

/** Convert Signature into CPT */
ADT create(const Signature& signature) {
  ADT p(signature.discreteKeysParentsFirst(), signature.cpt());
  static size_t count = 0;
  const DiscreteKey& key = signature.key();
  string dotfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str();
  dot(p, dotfile);
  return p;
}

/* ************************************************************************* */
// test Asia Joint
TEST(ADT, joint)
{
  DiscreteKey A(0, 2), S(1, 2), T(2, 2), L(3, 2), B(4, 2), E(5, 2), X(6, 2), D(7, 2);

  resetCounts();
  ADT pA = create(A % "99/1");
  ADT pS = create(S % "50/50");
  ADT pT = create(T | A = "99/1 95/5");
  ADT pL = create(L | S = "99/1 90/10");
  ADT pB = create(B | S = "70/30 40/60");
  ADT pE = create((E | T, L) = "F T T T");
  ADT pX = create(X | E = "95/5 2/98");
  ADT pD = create((D | E, B) = "9/1 2/8 3/7 1/9");
  printCounts("Asia CPTs");

  // Create joint
  resetCounts();
  ADT joint = pA;
  dot(joint, "Asia-A");
  joint = apply(joint, pS, &mul);
  dot(joint, "Asia-AS");
  joint = apply(joint, pT, &mul);
  dot(joint, "Asia-AST");
  joint = apply(joint, pL, &mul);
  dot(joint, "Asia-ASTL");
  joint = apply(joint, pB, &mul);
  dot(joint, "Asia-ASTLB");
  joint = apply(joint, pE, &mul);
  dot(joint, "Asia-ASTLBE");
  joint = apply(joint, pX, &mul);
  dot(joint, "Asia-ASTLBEX");
  joint = apply(joint, pD, &mul);
  dot(joint, "Asia-ASTLBEXD");
  EXPECT_LONGS_EQUAL(346, (long)muls);
  printCounts("Asia joint");

  ADT pASTL = pA;
  pASTL = apply(pASTL, pS, &mul);
  pASTL = apply(pASTL, pT, &mul);
  pASTL = apply(pASTL, pL, &mul);

  // test combine
  ADT fAa = pASTL.combine(L, &add_).combine(T, &add_).combine(S, &add_);
  EXPECT(assert_equal(pA, fAa));
  ADT fAb = pASTL.combine(S, &add_).combine(T, &add_).combine(L, &add_);
  EXPECT(assert_equal(pA, fAb));
}

/* ************************************************************************* */
// test Inference with joint
TEST(ADT, inference)
{
  DiscreteKey A(0,2), D(1,2),//
      B(2,2), L(3,2), E(4,2), S(5,2), T(6,2), X(7,2);

  resetCounts();
  ADT pA = create(A % "99/1");
  ADT pS = create(S % "50/50");
  ADT pT = create(T | A = "99/1 95/5");
  ADT pL = create(L | S = "99/1 90/10");
  ADT pB = create(B | S = "70/30 40/60");
  ADT pE = create((E | T, L) = "F T T T");
  ADT pX = create(X | E = "95/5 2/98");
  ADT pD = create((D | E, B) = "9/1 2/8 3/7 1/9");
  //  printCounts("Inference CPTs");

  // Create joint
  resetCounts();
  ADT joint = pA;
  dot(joint, "Joint-Product-A");
  joint = apply(joint, pS, &mul);
  dot(joint, "Joint-Product-AS");
  joint = apply(joint, pT, &mul);
  dot(joint, "Joint-Product-AST");
  joint = apply(joint, pL, &mul);
  dot(joint, "Joint-Product-ASTL");
  joint = apply(joint, pB, &mul);
  dot(joint, "Joint-Product-ASTLB");
  joint = apply(joint, pE, &mul);
  dot(joint, "Joint-Product-ASTLBE");
  joint = apply(joint, pX, &mul);
  dot(joint, "Joint-Product-ASTLBEX");
  joint = apply(joint, pD, &mul);
  dot(joint, "Joint-Product-ASTLBEXD");
  EXPECT_LONGS_EQUAL(370, (long)muls); // different ordering
  printCounts("Asia product");

  ADT marginal = joint;
  marginal = marginal.combine(X, &add_);
  dot(marginal, "Joint-Sum-ADBLEST");
  marginal = marginal.combine(T, &add_);
  dot(marginal, "Joint-Sum-ADBLES");
  marginal = marginal.combine(S, &add_);
  dot(marginal, "Joint-Sum-ADBLE");
  marginal = marginal.combine(E, &add_);
  dot(marginal, "Joint-Sum-ADBL");
  EXPECT_LONGS_EQUAL(161, (long)adds);
  printCounts("Asia sum");
}

/* ************************************************************************* */
TEST(ADT, factor_graph)
{
  DiscreteKey B(0,2), L(1,2), E(2,2), S(3,2), T(4,2), X(5,2);

  resetCounts();
  ADT pS = create(S % "50/50");
  ADT pT = create(T % "95/5");
  ADT pL = create(L | S = "99/1 90/10");
  ADT pE = create((E | T, L) = "F T T T");
  ADT pX = create(X | E = "95/5 2/98");
  ADT pD = create(B | E = "1/8 7/9");
  ADT pB = create(B | S = "70/30 40/60");
  //  printCounts("Create CPTs");

  // Create joint
  resetCounts();
  ADT fg = pS;
  fg = apply(fg, pT, &mul);
  fg = apply(fg, pL, &mul);
  fg = apply(fg, pB, &mul);
  fg = apply(fg, pE, &mul);
  fg = apply(fg, pX, &mul);
  fg = apply(fg, pD, &mul);
  dot(fg, "FactorGraph");
  EXPECT_LONGS_EQUAL(158, (long)muls);
  printCounts("Asia FG");

  fg = fg.combine(X, &add_);
  dot(fg, "Marginalized-6X");
  fg = fg.combine(T, &add_);
  dot(fg, "Marginalized-5T");
  fg = fg.combine(S, &add_);
  dot(fg, "Marginalized-4S");
  fg = fg.combine(E, &add_);
  dot(fg, "Marginalized-3E");
  fg = fg.combine(L, &add_);
  dot(fg, "Marginalized-2L");
  EXPECT(adds = 54);
  printCounts("marginalize");

  // BLESTX

  // Eliminate X
  ADT fE = pX;
  dot(fE, "Eliminate-01-fEX");
  fE = fE.combine(X, &add_);
  dot(fE, "Eliminate-02-fE");
  printCounts("Eliminate X");

  // Eliminate T
  ADT fLE = pT;
  fLE = apply(fLE, pE, &mul);
  dot(fLE, "Eliminate-03-fLET");
  fLE = fLE.combine(T, &add_);
  dot(fLE, "Eliminate-04-fLE");
  printCounts("Eliminate T");

  // Eliminate S
  ADT fBL = pS;
  fBL = apply(fBL, pL, &mul);
  fBL = apply(fBL, pB, &mul);
  dot(fBL, "Eliminate-05-fBLS");
  fBL = fBL.combine(S, &add_);
  dot(fBL, "Eliminate-06-fBL");
  printCounts("Eliminate S");

  // Eliminate E
  ADT fBL2 = fE;
  fBL2 = apply(fBL2, fLE, &mul);
  fBL2 = apply(fBL2, pD, &mul);
  dot(fBL2, "Eliminate-07-fBLE");
  fBL2 = fBL2.combine(E, &add_);
  dot(fBL2, "Eliminate-08-fBL2");
  printCounts("Eliminate E");

  // Eliminate L
  ADT fB = fBL;
  fB = apply(fB, fBL2, &mul);
  dot(fB, "Eliminate-09-fBL");
  fB = fB.combine(L, &add_);
  dot(fB, "Eliminate-10-fB");
  printCounts("Eliminate L");
}

/* ************************************************************************* */
// test equality
TEST(ADT, equality_noparser)
{
  DiscreteKey A(0,2), B(1,2);
  Signature::Table tableA, tableB;
  Signature::Row rA, rB;
  rA += 80, 20; rB += 60, 40;
  tableA += rA; tableB += rB;

  // Check straight equality
  ADT pA1 = create(A % tableA);
  ADT pA2 = create(A % tableA);
  EXPECT(pA1 == pA2); // should be equal

  // Check equality after apply
  ADT pB = create(B % tableB);
  ADT pAB1 = apply(pA1, pB, &mul);
  ADT pAB2 = apply(pB, pA1, &mul);
  EXPECT(pAB2 == pAB1);
}

/* ************************************************************************* */
// test equality
TEST(ADT, equality_parser)
{
  DiscreteKey A(0,2), B(1,2);
  // Check straight equality
  ADT pA1 = create(A % "80/20");
  ADT pA2 = create(A % "80/20");
  EXPECT(pA1 == pA2); // should be equal

  // Check equality after apply
  ADT pB = create(B % "60/40");
  ADT pAB1 = apply(pA1, pB, &mul);
  ADT pAB2 = apply(pB, pA1, &mul);
  EXPECT(pAB2 == pAB1);
}

/* ******************************************************************************** */
// Factor graph construction
// test constructor from strings
TEST(ADT, constructor)
{
  DiscreteKey v0(0,2), v1(1,3);
  Assignment<Key> x00, x01, x02, x10, x11, x12;
  x00[0] = 0, x00[1] = 0;
  x01[0] = 0, x01[1] = 1;
  x02[0] = 0, x02[1] = 2;
  x10[0] = 1, x10[1] = 0;
  x11[0] = 1, x11[1] = 1;
  x12[0] = 1, x12[1] = 2;

  ADT f1(v0 & v1, "0 1 2 3 4 5");
  EXPECT_DOUBLES_EQUAL(0, f1(x00), 1e-9);
  EXPECT_DOUBLES_EQUAL(1, f1(x01), 1e-9);
  EXPECT_DOUBLES_EQUAL(2, f1(x02), 1e-9);
  EXPECT_DOUBLES_EQUAL(3, f1(x10), 1e-9);
  EXPECT_DOUBLES_EQUAL(4, f1(x11), 1e-9);
  EXPECT_DOUBLES_EQUAL(5, f1(x12), 1e-9);

  ADT f2(v1 & v0, "0 1 2 3 4 5");
  EXPECT_DOUBLES_EQUAL(0, f2(x00), 1e-9);
  EXPECT_DOUBLES_EQUAL(2, f2(x01), 1e-9);
  EXPECT_DOUBLES_EQUAL(4, f2(x02), 1e-9);
  EXPECT_DOUBLES_EQUAL(1, f2(x10), 1e-9);
  EXPECT_DOUBLES_EQUAL(3, f2(x11), 1e-9);
  EXPECT_DOUBLES_EQUAL(5, f2(x12), 1e-9);

  DiscreteKey z0(0,5), z1(1,4), z2(2,3), z3(3,2);
  vector<double> table(5 * 4 * 3 * 2);
  double x = 0;
  for(double& t: table)
  t = x++;
  ADT f3(z0 & z1 & z2 & z3, table);
  Assignment<Key> assignment;
  assignment[0] = 0;
  assignment[1] = 0;
  assignment[2] = 0;
  assignment[3] = 1;
  EXPECT_DOUBLES_EQUAL(1, f3(assignment), 1e-9);
}

/* ************************************************************************* */
// test conversion to integer indices
// Only works if DiscreteKeys are binary, as size_t has binary cardinality!
TEST(ADT, conversion)
{
  DiscreteKey X(0,2), Y(1,2);
  ADT fDiscreteKey(X & Y, "0.2 0.5 0.3 0.6");
  dot(fDiscreteKey, "conversion-f1");

  std::map<Key, Key> keyMap;
  keyMap[0] = 5;
  keyMap[1] = 2;

  AlgebraicDecisionTree<Key> fIndexKey(fDiscreteKey, keyMap);
  //  f1.print("f1");
  //  f2.print("f2");
  dot(fIndexKey, "conversion-f2");

  Assignment<Key> x00, x01, x02, x10, x11, x12;
  x00[5] = 0, x00[2] = 0;
  x01[5] = 0, x01[2] = 1;
  x10[5] = 1, x10[2] = 0;
  x11[5] = 1, x11[2] = 1;
  EXPECT_DOUBLES_EQUAL(0.2, fIndexKey(x00), 1e-9);
  EXPECT_DOUBLES_EQUAL(0.5, fIndexKey(x01), 1e-9);
  EXPECT_DOUBLES_EQUAL(0.3, fIndexKey(x10), 1e-9);
  EXPECT_DOUBLES_EQUAL(0.6, fIndexKey(x11), 1e-9);
}

/* ******************************************************************************** */
// test operations in elimination
TEST(ADT, elimination)
{
  DiscreteKey A(0,2), B(1,3), C(2,2);
  ADT f1(A & B & C, "1 2  3 4  5 6    1 8  3 3  5 5");
  dot(f1, "elimination-f1");

  {
    // sum out lower key
    ADT actualSum = f1.sum(C);
    ADT expectedSum(A & B, "3  7  11    9  6  10");
    CHECK(assert_equal(expectedSum,actualSum));

    // normalize
    ADT actual = f1 / actualSum;
    vector<double> cpt;
    cpt += 1.0 / 3, 2.0 / 3, 3.0 / 7, 4.0 / 7, 5.0 / 11, 6.0 / 11, //
    1.0 / 9, 8.0 / 9, 3.0 / 6, 3.0 / 6, 5.0 / 10, 5.0 / 10;
    ADT expected(A & B & C, cpt);
    CHECK(assert_equal(expected,actual));
  }

  {
    // sum out lower 2 keys
    ADT actualSum = f1.sum(C).sum(B);
    ADT expectedSum(A, 21, 25);
    CHECK(assert_equal(expectedSum,actualSum));

    // normalize
    ADT actual = f1 / actualSum;
    vector<double> cpt;
    cpt += 1.0 / 21, 2.0 / 21, 3.0 / 21, 4.0 / 21, 5.0 / 21, 6.0 / 21, //
    1.0 / 25, 8.0 / 25, 3.0 / 25, 3.0 / 25, 5.0 / 25, 5.0 / 25;
    ADT expected(A & B & C, cpt);
    CHECK(assert_equal(expected,actual));
  }
}

/* ******************************************************************************** */
// Test non-commutative op
TEST(ADT, div)
{
  DiscreteKey A(0,2), B(1,2);

  // Literals
  ADT a(A, 8, 16);
  ADT b(B, 2, 4);
  ADT expected_a_div_b(A & B, "4 2 8 4"); // 8/2 8/4 16/2 16/4
  ADT expected_b_div_a(A & B, "0.25 0.5 0.125 0.25"); // 2/8 4/8 2/16 4/16
  EXPECT(assert_equal(expected_a_div_b, a / b));
  EXPECT(assert_equal(expected_b_div_a, b / a));
}

/* ******************************************************************************** */
// test zero shortcut
TEST(ADT, zero)
{
  DiscreteKey A(0,2), B(1,2);

  // Literals
  ADT a(A, 0, 1);
  ADT notb(B, 1, 0);
  ADT anotb = a * notb;
  //  GTSAM_PRINT(anotb);
  Assignment<Key> x00, x01, x10, x11;
  x00[0] = 0, x00[1] = 0;
  x01[0] = 0, x01[1] = 1;
  x10[0] = 1, x10[1] = 0;
  x11[0] = 1, x11[1] = 1;
  EXPECT_DOUBLES_EQUAL(0, anotb(x00), 1e-9);
  EXPECT_DOUBLES_EQUAL(0, anotb(x01), 1e-9);
  EXPECT_DOUBLES_EQUAL(1, anotb(x10), 1e-9);
  EXPECT_DOUBLES_EQUAL(0, anotb(x11), 1e-9);
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


/*=============================================================================

  PHAS0100ASSIGNMENT2: PHAS0100 Assignment 2 Gravitational N-body Simulation

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "catch.hpp"
#include "nbsimCatchMain.h"
#include "nbsimMyFunctions.h"
#include "nbsimParticle.h"
#include "nbsimMassiveParticle.h"
#include <iostream>
#include <vector>
#include <Eigen/Dense>

TEST_CASE( "My first test", "[some group identifier]" ) {
  int a = 5;
  REQUIRE( a < 6 );
}

TEST_CASE( "My second test", "[some group identifier]" ) {
  std::vector<int> a;
  REQUIRE( a.size() == 0 );
}

TEST_CASE( "Simple add", "[MyFirstAddFunction]") {
  REQUIRE( nbsim::MyFirstAddFunction(1, 2) == 3);
}

// ------ //
// Testing for Task 1:
TEST_CASE( "Test a: No acceleration", "[Particle]") {

  Eigen::Vector3d a(0,0,0);

  Eigen::Vector3d p_test(0,0,0);
  Eigen::Vector3d v_test(1,1,1);

  Eigen::Vector3d p_expect(0.01,0.01,0.01);
  Eigen::Vector3d v_expect(1,1,1);

  nbsim::Particle particle_a(p_test,v_test);
  double dt = 0.01;

  REQUIRE(particle_a.getPosition().isApprox(p_test));
  REQUIRE(particle_a.getVelocity().isApprox(v_test));

  particle_a.integrateTimestep(a,dt);
  REQUIRE(particle_a.getPosition().isApprox(p_expect));
  REQUIRE(particle_a.getVelocity().isApprox(v_expect));
}

TEST_CASE( "Test b: Constant acceleration", "[Particle]") {

  Eigen::Vector3d a(1,1,1);

  Eigen::Vector3d p_test(0,0,0);
  Eigen::Vector3d v_test(1,1,1);

  Eigen::Vector3d p_expect1(0.01,0.01,0.01);
  Eigen::Vector3d v_expect1(1.01,1.01,1.01);

  nbsim::Particle particle_b(p_test,v_test);
  double dt = 0.01;

  particle_b.integrateTimestep(a,dt);
  REQUIRE(particle_b.getPosition().isApprox(p_expect1));
  REQUIRE(particle_b.getVelocity().isApprox(v_expect1));

  Eigen::Vector3d p_expect2(0.0201,0.0201,0.0201);
  Eigen::Vector3d v_expect2(1.02,1.02,1.02);

  particle_b.integrateTimestep(a,dt);
  REQUIRE(particle_b.getPosition().isApprox(p_expect2));
  REQUIRE(particle_b.getVelocity().isApprox(v_expect2));
}

TEST_CASE( "Test c: Fictitious centripetal acceleration", "[Particle]") {

  Eigen::Vector3d p_test(0,0,0);
  Eigen::Vector3d v_test(1,1,1);

  nbsim::Particle particle_c(p_test,v_test);
  double dt = 0.1;
  double time = 0.3;

  for(double i=0;i<time;i+=dt){
    Eigen::Vector3d a = -particle_c.getPosition();
    particle_c.integrateTimestep(a,dt);
  }

  Eigen::Vector3d p_expect(0.299,0.299,0.299);
  Eigen::Vector3d v_expect(0.97,0.97,0.97);

  REQUIRE(particle_c.getPosition().isApprox(p_expect));
  REQUIRE(particle_c.getVelocity().isApprox(v_expect));

}

TEST_CASE( "Test a: Still models linear motion correctly with no attractors", "[MassiveParticle]") {

  Eigen::Vector3d p_test(0,0,0);
  Eigen::Vector3d v_test(1,1,1);

  Eigen::Vector3d p_expect(0.1,0.1,0.1);
  Eigen::Vector3d v_expect(1,1,1);

  double mass = 1;
  double dt = 0.1;

  nbsim::MassiveParticle MassiveParticle_a(p_test,v_test,mass);
  MassiveParticle_a.calculateAcceleration();
  MassiveParticle_a.integrateTimestep(dt);

  REQUIRE(MassiveParticle_a.getPosition().isApprox(p_expect));
  REQUIRE(MassiveParticle_a.getVelocity().isApprox(v_expect));

}

TEST_CASE( "Test b: With gravitationally attractors", "[MassiveParticle]") {

  Eigen::Vector3d p_1(1,0,0);
  Eigen::Vector3d v_1(0,0.5,0);

  Eigen::Vector3d p_2(-1,0,0);
  Eigen::Vector3d v_2(0,-0.5,0);

  double mu = 1;

  std::shared_ptr<nbsim::MassiveParticle> attractor_ptr1(new nbsim::MassiveParticle(p_1,v_1,mu));
  std::shared_ptr<nbsim::MassiveParticle> attractor_ptr2(new nbsim::MassiveParticle(p_2,v_2,mu));

  attractor_ptr1 -> addAttractor(attractor_ptr2);
  attractor_ptr2 -> addAttractor(attractor_ptr1);

  double dt = 0.001;
  double t = 1;

  for (double i=0;i<=t;i+=dt){
    attractor_ptr1->calculateAcceleration();
    attractor_ptr1->integrateTimestep(dt);

    attractor_ptr2->calculateAcceleration();
    attractor_ptr2->integrateTimestep(dt);
  }

  double distance = (attractor_ptr1->getPosition() - attractor_ptr2->getPosition()).norm();
  double error_expect = std::abs(distance-2);
  REQUIRE(error_expect <= 0.01);
}


// (C) 2014 Arek Olek

#include <iostream>

#include <boost/graph/adjacency_list.hpp>

#include "dfs.hpp"
#include "range.hpp"

using namespace std;

boost::property<boost::edge_color_t,
  boost::default_color_type>            typedef color;
boost::adjacency_list<
  boost::hash_setS, boost::vecS, boost::undirectedS,
  boost::no_property, color>            typedef graph;

int main() {
  ios_base::sync_with_stdio(0);

  int z, n, m, s, t;
  int internal = 0;

  cin >> z;

  for(int Z = 0; Z < z; ++Z) {
    cin >> n >> m;
    graph g(n);
    for(int i = 0; i < m; ++i) {
      cin >> s >> t;
      add_edge(s, t, g);
    }
    auto T = dfs_tree(g);
    for(auto v : range(vertices(T)))
      internal += degree(v, T) > 1;
  }
  cout << internal/(double)z << endl;

  return 0;
}


#ifndef HMM_HPP
#define HMM_HPP

#include <Eigen/Dense>

using Matrix = Eigen::MatrixXd;
using Vector = Eigen::VectorXd;
using MatrixType = Eigen::Ref<const Eigen::MatrixXd>;
using VectorType = Eigen::Ref<const Eigen::VectorXd>;

namespace hmm {

  class HMM {

  public:

    HMM() = default;

    /**
     * @brief Construct an HMM object
     * given the sizes of hidden and observed states.
     *
     * Matrices of probabilities are initialized with the inverse of the
     * matrix size so that all states and emissions have equal probability.
     *
     * @param num_hidden Number of hidden states.
     * @param num_observed Number of observed states.
     */
    HMM(std::size_t num_hidden, std::size_t num_observed) noexcept;

    /**
     * @brief Construct an HMM object with given probability matrices.
     *
     * Invokes copy constructors for the Eigen data types.
     *
     * @param transition_matrix Matrix of transition probabilities of
     * dimensions [num_hidden, num_hidden].
     *
     * @param emission_matrix Matrix of emission probabilities of
     * dimensions [num_hidden, num_observed].
     *
     * @param initial_probs Vector of initial probabilities.
     */
    HMM(const MatrixType& transition_probs,
        const MatrixType& emission_probs,
        const VectorType& initial_probs);

    HMM(const MatrixType& transition_probs,
        const MatrixType& emission_probs);

    /**
     * @brief Calculate forward probabilities for a sequence of observed states.
     *
     * @param observations Vector of observed states as `int` values.
     * @returns Matrix of forward probabilities.
     */
    Matrix forward(const VectorType& observations) noexcept;

    Matrix backward(const VectorType&) noexcept;

    /**
     * Calculate the Viterbi path for a sequence of observed states.
     *
     * @param Vector of observed states as `int` values.
     *
     * @return Vector of the most likely sequence of hidden states
     * that gave rise to the sequence of observed states provided as argument.
     */
    Vector viterbi(const VectorType&) noexcept;

    // getters
    Matrix transition_matrix() const noexcept;
    Matrix emission_matrix() const noexcept;

    // setters
    void transition_matrix(const MatrixType&);
    void emission_matrix(const MatrixType&);

  private:
    std::size_t m_num_hidden = 1;
    std::size_t m_num_observed = 1;

    Matrix m_transition_probs =
      Matrix::Constant(m_num_hidden,
                       m_num_hidden,
                       1.0 / m_num_hidden);

    Matrix m_emission_probs =
      Matrix::Constant(m_num_hidden,
                       m_num_observed,
                       1.0 / m_num_observed);

    Vector m_initial_probs =
      Vector::Constant(m_num_hidden,
                       1.0 / m_num_observed);
  };

} /* class HMM */

#endif /* HMM_HPP */


#include <cmath>
#include <string>
#include <utility>
#include <numeric>
#include <algorithm>

#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

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

#include <tf/tf.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/image_encodings.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>

#include "LaserSubscriber.hpp"
#include "ImageSubscriber.hpp"

// topic names
constexpr const char* kCmdVelocityTopic = "/mobile_base/commands/velocity";

constexpr const char* kLaserScanTopic   = "/scan";
constexpr const char* kImageRawTopic    = "/camera/rgb/image_raw";

// OpenCV window names
constexpr const char* kCaptureWindow = "Raw Video";
constexpr const char* kHsvWindow     = "HSV Video";

// HSV filter parameters
constexpr std::pair<int, int> kHue        = {35, 80};
constexpr std::pair<int, int> kValue      = {5, 55};
constexpr std::pair<int, int> kSaturation = {50, 125};

using RPY = boost::tuple<double, double, double>;

RPY rpy_from(const geometry_msgs::Quaternion& q) {
  tf::Quaternion q_tf(q.x, q.y, q.z, q.w);
  double r, p, y;

  tf::Matrix3x3 m(q_tf);
  m.getRPY(r, p ,y);

  return {r, p, y};
}

cv::Point compute_centroid(const cv::Mat& binary_frame) {
  auto moments = cv::moments(binary_frame, /* binary */ true);
  // returns negative values in case if there are no white pixels
  return cv::Point(int(moments.m10 / moments.m00), int(moments.m01 / moments.m00));
}

float compute_ratio(const cv::Mat& binary_frame) {
  int max_horizontal = 0;
  int max_vertical = 0;

  for (int j = 0; j < binary_frame.cols; ++j) {
    max_vertical = std::max(cv::countNonZero(binary_frame.col(j)), max_vertical);
  }

  for (int i = 0; i < binary_frame.rows; ++i) {
    max_horizontal = std::max(cv::countNonZero(binary_frame.row(i)), max_horizontal);
  }

  return max_horizontal / static_cast<float>(max_vertical);
}

void image_callback(const sensor_msgs::ImageConstPtr& image, const float& scan_distance, cv::Mat& binary_frame, const geometry_msgs::Pose& pose) {
  if (scan_distance < 0.0f) return;  // no scan message arrived

  cv::Mat hsv_frame;

  auto image_ptr = cv_bridge::toCvShare(image, sensor_msgs::image_encodings::BGR8);

  cv::GaussianBlur(image_ptr->image, image_ptr->image, cv::Size(5, 5), 0, 0);

  cv::cvtColor(image_ptr->image, hsv_frame, cv::COLOR_BGR2HSV);
  cv::inRange(hsv_frame, cv::Scalar(kHue.first, kSaturation.first, kValue.first), cv::Scalar(kHue.second, kSaturation.second, kValue.second), binary_frame);

  auto white_pixels_num = cv::countNonZero(binary_frame);
  auto centroid = compute_centroid(binary_frame);
  auto ratio = compute_ratio(binary_frame);

  double roll, pitch, yaw;
  float x = pose.position.x;
  float y = pose.position.y;
  boost::tie(roll, pitch, yaw) = rpy_from(pose.orientation);

  cv::putText(image_ptr->image, std::string{"Ratio: "} + std::to_string(ratio), cv::Point(5, 15),
            cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, cv::Scalar(255, 255, 255), 1);

  cv::putText(image_ptr->image, std::string{"Distance (mean): "} + std::to_string(scan_distance), cv::Point(5, 35),
          cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, cv::Scalar(255, 255, 255), 1);

  cv::putText(image_ptr->image, std::string{"White pixels: "} + std::to_string(white_pixels_num), cv::Point(5, 55),
          cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, cv::Scalar(255, 255, 255), 1);

  cv::putText(image_ptr->image, std::string{"Pixels/Distance: "} + std::to_string(white_pixels_num / scan_distance), cv::Point(5, 75),
          cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, cv::Scalar(255, 255, 255), 1);

  cv::putText(image_ptr->image, std::string{"Pose (x, y, theta): ("} + std::to_string(x) + ", " + std::to_string(y) + ", " + std::to_string(yaw) + ")",
   cv::Point(5, 95), cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, cv::Scalar(255, 255, 255), 1);

  if (centroid.x >= 0 && centroid.y >= 0) {
    cv::circle(image_ptr->image, centroid, 5, cv::Scalar(0, 0, 255), -1);
  }

  cv::imshow(kCaptureWindow, image_ptr->image);
  cv::imshow(kHsvWindow, binary_frame);

  cv::waitKey(33);
}

void laser_callback(const sensor_msgs::LaserScanConstPtr& scan, float& scan_distance) {
  int max_idx = std::floor((scan->angle_max - scan->angle_min) / scan->angle_increment);

  std::vector<float> filtered_ranges;

  std::copy_if(scan->ranges.begin(), scan->ranges.end(),
   std::back_inserter(filtered_ranges), [&scan](float range) {
      return range > scan->range_min && range < scan->range_max;
   });

  float mean = 0.0f;

  if (filtered_ranges.size() < 10) {
    mean = std::accumulate(filtered_ranges.begin(), filtered_ranges.end(), 0.0) / filtered_ranges.size();
  } else {
    // distance around the middle
    auto middle_it = filtered_ranges.begin() + filtered_ranges.size() / 2;
    mean = std::accumulate(middle_it - 5, middle_it + 4, 0.0) / 10.0;
  }

  scan_distance = mean;  // update scan distance
}

void odom_callback(const nav_msgs::OdometryConstPtr& odom, geometry_msgs::Pose& pose) {
  pose = odom->pose.pose;
}

void publish_cmd_vel(float x, float z, ros::Publisher& pub) {
  geometry_msgs::Twist cmd;
  cmd.linear.x = x;
  cmd.angular.z = z;
  pub.publish(cmd);
}

void turn_and_move_forward(ros::Publisher& pub, const geometry_msgs::Pose& pose, float& scan_distance) {
  double roll, pitch, yaw;
  boost::tie(roll, pitch, yaw) = rpy_from(pose.orientation);

  double desired_yaw = 0.0f;

  if (yaw >= 0.0 && yaw <= M_PI_2) {  // 1st quarter
    desired_yaw = yaw + M_PI_4;

  } else if (yaw >= M_PI_2 && yaw <= M_PI) {  // 2nd quarter
    desired_yaw = yaw - M_PI_4;

  } else if (yaw >= -M_PI && yaw <= -M_PI_2) {  // 3rd quarter
    desired_yaw = yaw + M_PI_4;

  } else { // 4th quarter
    desired_yaw = yaw - M_PI_4;
  }

  ROS_INFO("Reference yaw: %.6f, target: %.6f", yaw, desired_yaw);

  ros::Rate rate(30);

  while(std::abs(boost::get<2>(rpy_from(pose.orientation)) - desired_yaw) > 0.2f) {
    publish_cmd_vel(0.0f, 0.15f, pub);

    ros::spinOnce();
    rate.sleep();
  }

  double ref_x = pose.position.x;
  double ref_y = pose.position.y;

  double desired_distance = 10.0f;  // meters^2

  ROS_INFO("Reference (x, y) = (%.6f, %.6f)", ref_x, ref_y);

  while ((std::isnan(scan_distance) || scan_distance >= 0.2f)
    && std::abs(std::pow(pose.position.x - ref_x, 2) + std::pow(pose.position.y - ref_y, 2) - desired_distance) > 1.0f)
  {
    publish_cmd_vel(0.12f, 0.0f, pub);

    ros::spinOnce();
    rate.sleep();
  }
}

int main(int argc, char *argv[]) {
  ros::init(argc, argv, "algorithm_node");

  ros::NodeHandle nh;

  float scan_distance = -1.0f;
  geometry_msgs::Pose pose;
  cv::Mat binary_frame;

  ImageSubscriber img_subscriber(nh, kImageRawTopic, boost::bind(&image_callback, _1, boost::cref(scan_distance), boost::ref(binary_frame), boost::cref(pose)));
  LaserSubscriber laser_subscriber(nh, kLaserScanTopic, boost::bind(&laser_callback, _1, boost::ref(scan_distance)));

  auto odom_subscriber = nh.subscribe<nav_msgs::Odometry>("/odom", 3, boost::bind(&odom_callback, _1, boost::ref(pose)));
  auto cmd_vel_publisher = nh.advertise<geometry_msgs::Twist>(kCmdVelocityTopic, 1);

  cv::namedWindow(kCaptureWindow);
  cv::namedWindow(kHsvWindow);

  ros::Rate rate(60);

  while(nh.ok()) {

    if (!binary_frame.empty() && (scan_distance > 0.0f || std::isnan(scan_distance))) {
      auto centroid = compute_centroid(binary_frame);
      auto white_pixels_num = cv::countNonZero(binary_frame);

      float err = centroid.x - binary_frame.cols / 2;

      if (std::abs(err) > 50 || white_pixels_num < 400) {
        ROS_INFO("Searching for the container ...");

        if (white_pixels_num < 400) {
          publish_cmd_vel(0.0f, -0.15f, cmd_vel_publisher);
        } else {
          publish_cmd_vel(0.0f, err < 0.0f? 0.15f : -0.15f, cmd_vel_publisher);
        }

      } else {
        if (std::isinf(scan_distance)) ROS_WARN("INFINITY distance is detected");

        // approach the container (to 2-3 meters from it)
        if (scan_distance > 2.0f) {
          publish_cmd_vel(0.15f, 0.0f, cmd_vel_publisher);
        } else {
          auto ratio = compute_ratio(binary_frame);
          auto pixel_distance_ratio = white_pixels_num / scan_distance;
          bool is_large_side = false;

          if (ratio > 1.4f) {
            is_large_side = true;

          } else if (ratio > 1.18f && pixel_distance_ratio > 20 * 1E3f) {
            is_large_side = true;

          } else {
            is_large_side = false;
          }

          if (is_large_side && scan_distance > 1.03f) {
            ROS_INFO("Approaching container for 1 meter ...");
            publish_cmd_vel(0.1f, 0.0f, cmd_vel_publisher);
          }

          if (!is_large_side) {
            // turn and move forward
            turn_and_move_forward(cmd_vel_publisher, pose, scan_distance);
          }
        }
      }

    }

    ros::spinOnce();
    rate.sleep();
  }

  return 0;
}


#ifndef __GTRANSFORM_H__
#define __GTRANSFORM_H__
#include <iostream>
#include <boost/signals2.hpp>

#include "glm/vec3.hpp"
#include "glm/mat4x4.hpp"
#include "glm/gtx/quaternion.hpp"

#include "GCom.hpp"

NS_G4Y_BEGIN

class G4Y_DLL GTransform : public GCom
{
    G_COM
public:
	typedef boost::signals2::signal<void (float, float, float)> position_changed_sig_t;
	typedef boost::signals2::signal<void (float, float, float, float)> rotation_changed_sig_t;
public:
    GTransform();
    virtual ~GTransform();

    virtual void Start() override;

    glm::vec3 Position();
    glm::vec3 EulerAngles();
    glm::quat Rotation();

    void SetPosition(glm::vec3);
    void SetRotation(glm::vec3 eulers);
    void SetRotation(glm::quat q);
    void RotateAround(glm::vec3 target, glm::vec3 axis, float euler);

    glm::vec3 LocalPosition();
    glm::vec3 LocalEulerAngles();
    glm::quat LocalRotation();

    void SetLocalPosition(glm::vec3);
    void SetLocalRotation(glm::vec3);
    void SetLocalRotation(glm::quat q);

    void LookAt(glm::vec3 target, glm::vec3 wld_up = glm::vec3(0, 1, 0));

    glm::vec3 Forward();
    glm::vec3 Right();
    glm::vec3 Up();

    void Translate(glm::vec3 translation);
    void Translate(float x, float y, float z);

    glm::mat4 ToMat4();

    glm::vec3 Scale();
    void SetScale(glm::vec3);

	boost::signals2::connection connect(const position_changed_sig_t::slot_type &subscriber);
	boost::signals2::connection connect(const rotation_changed_sig_t::slot_type &subscriber);

	virtual std::string Name() { return m_name; }
	virtual void SetName(std::string name) { m_name = name; }
private:
    void UpdateTransform(std::shared_ptr<GObj> obj, bool update_local);
    void UpdateGlobalTransform(std::shared_ptr<GObj> obj);

    struct G4Y_DLL Transform{
        glm::quat rot;
        glm::vec3 pos;
        glm::vec3 scale;

        Transform();

        Transform(const glm::vec3& _pos, const glm::quat& _rot, glm::vec3 _scale);

        Transform(const Transform &trans);

        Transform& operator=(const Transform &trans);

        Transform Inverted() const;

        Transform operator*(const Transform& rhs) const;

        glm::mat4 ToMat4() const;
    };

    Transform local_trans;
    Transform wld_trans;
	std::string m_name;
    std::weak_ptr<GTransform> m_parent_trans;
	position_changed_sig_t m_position_changed_sig;
	rotation_changed_sig_t m_rotation_changed_sig;
};

class G4Y_DLL GTransformWarp : public GComWarp
{
public:
	GTransformWarp();
	GTransformWarp(std::shared_ptr<GTransform> t);
	GTransformWarp(const GTransformWarp& o);

	~GTransformWarp();

	GTransformWarp& operator=(const GTransformWarp& o);

	static boost::python::object getMethodList();

	static std::string getMethodInfo(const std::string&);

	void setPosition(boost::python::object o);
	boost::python::object getPosition();

	void setEulerAngles(boost::python::object o);

	boost::python::object getEulerAngles();

	void setScale(boost::python::object o);
	boost::python::object getScale();

	boost::python::object getForward();
	boost::python::object getRight();
	boost::python::object getUp();

	void translate(boost::python::object o);
};

NS_G4Y_END

#endif


//
// Created by xetql on 12.03.18.
//

#ifndef LIBLJ_FITTEDMLP_HPP
#define LIBLJ_FITTEDMLP_HPP

#include <mlpack/core.hpp>

#include <mlpack/core/optimizers/rmsprop/rmsprop.hpp>
#include <mlpack/core/optimizers/sgd/update_policies/vanilla_update.hpp>
#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/ffn.hpp>

#include <armadillo>

#include <type_traits>
#include <vector>

#include "FunctionApproximator.hpp"
#include "MLP.hpp"

namespace librl {
namespace approximator {
namespace action_value {
/**
 * NeuralNet approximator
 * @tparam OptimizerType Optimizer Type
 * @tparam ParametersType Parameters type of the neural net. See mlpack.
 */
template<typename OptimizerType, class... ParametersType>
class FittedMLP : public MLP<OptimizerType, ParametersType...> {
    const std::vector<int> actions;
public:
    FittedMLP(const std::vector<int>& action_space, OptimizerType opt, ParametersType... params) : actions(action_space), MLP<OptimizerType, ParametersType...>(opt, params...) {}
    FittedMLP(const std::vector<int>& action_space, OptimizerType opt) : actions(action_space), MLP<OptimizerType, ParametersType...>(opt)  {}

    virtual int argmax(const arma::mat &state, const std::vector<int> &available_actions) const {
        arma::mat response;
        int argmax = -1;
        double max=std::numeric_limits<double>::lowest();
        for(const int& action : actions){
            arma::mat in(state);
            in.insert_rows(in.n_rows, 1);
            in(in.n_rows - 1) = action;
            this->model->Predict(in, response);
            double tmp = response.at(0);
            if(tmp > max){
                max = tmp;
                argmax = action;
            }
        }
        assert(argmax >= 0);
        return argmax;
    }

    virtual void Q(const arma::mat &state, const int &action, double value) {
        arma::mat out = {0};
        out(0) = value;
        arma::mat in(state);
        in.insert_rows(in.n_rows, 1);
        in(in.n_rows - 1) = action;
        this->model->Train(in, out);
    }

    virtual double max(const arma::mat &state) const {
        arma::mat response;
        double max=std::numeric_limits<double>::lowest();
        for(const int& action : actions){
            arma::mat in(state);
            in.insert_rows(in.n_rows, 1);
            in(in.n_rows - 1) = action;
            this->model->Predict(in, response);
            double tmp = response.at(0);
            if(tmp > max){
                max = tmp;
            }
        }
        return max;
    }

    virtual double Q(const arma::mat &state, const int &action) const {
        arma::mat response;
        arma::mat in(state);
        in.insert_rows(in.n_rows, 1);
        in(in.n_rows - 1) = action;
        this->model->Predict(in, response);
        return response.at(0);
    }
};

}}}

#endif //LIBLJ_FITTEDMLP_HPP


#ifndef STAN_MATH_PRIM_SCAL_PROB_SKEW_NORMAL_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_SKEW_NORMAL_RNG_HPP

#include <boost/random/variate_generator.hpp>
#include <boost/math/distributions.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/fun/owens_t.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/prob/uniform_rng.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>

namespace stan {
  namespace math {

    template <class RNG>
    inline double
    skew_normal_rng(double mu,
                    double sigma,
                    double alpha,
                    RNG& rng) {
      boost::math::skew_normal_distribution<> dist(mu, sigma, alpha);

      static const char* function("skew_normal_rng");

      check_finite(function, "Location parameter", mu);
      check_finite(function, "Shape parameter", alpha);
      check_positive(function, "Scale parameter", sigma);

      return quantile(dist, uniform_rng(0.0, 1.0, rng));
    }

  }
}
#endif


/**********************************************************************
*  Copyright (c) 2008-2013, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#include <utilities/geometry/Geometry.hpp>
#include <utilities/geometry/Transformation.hpp>

#include <utilities/core/Assert.hpp>

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

namespace openstudio{
  /// convert degrees to radians
  double degToRad(double degrees)
  {
    return degrees*boost::math::constants::pi<double>()/180.0;
  }

  /// convert radians to degrees
  double radToDeg(double radians)
  {
    return radians*180.0/boost::math::constants::pi<double>();
  }

  /// compute area from surface as Point3dVector
  boost::optional<double> getArea(const Point3dVector& points)
  {
    boost::optional<double> result;
    OptionalVector3d newall = getNewallVector(points);
    if (newall){
      result = newall->length() / 2.0;
    }
    return result;
  }

  // compute Newall vector from Point3dVector, direction is same as outward normal
  // magnitude is twice the area
  OptionalVector3d getNewallVector(const Point3dVector& points)
  {
    OptionalVector3d result;
    unsigned N = points.size();
    if (N >= 3){
      Vector3d vec;
      for (unsigned i = 1; i < N-1; ++i){
        Vector3d v1 = points[i] - points[0];
        Vector3d v2 = points[i+1] - points[0];
        vec += v1.cross(v2);
      }
     result = vec;
   }
   return result;
  }

  // compute outward normal from Point3dVector
  OptionalVector3d getOutwardNormal(const Point3dVector& points)
  {
    OptionalVector3d result = getNewallVector(points);
    if (result){
      if (!result->normalize()){
        result.reset();
      }
    }
    return result;
  }

  /// compute centroid from surface as Point3dVector
  OptionalPoint3d getCentroid(const Point3dVector& points)
  {
    OptionalPoint3d result;

    if (points.size() >= 3){
      // convert to face coordinates
      Transformation alignFace = Transformation::alignFace(points);
      Point3dVector surfacePoints = alignFace.inverse()*points;

      unsigned N = surfacePoints.size();
      double A = 0;
      double cx = 0;
      double cy = 0;
      for (unsigned i = 0; i < N; ++i){
        double x1, x2, y1, y2;
        if (i == N-1){
          x1 = surfacePoints[i].x();
          x2 = surfacePoints[0].x();
          y1 = surfacePoints[i].y();
          y2 = surfacePoints[0].y();
        }else{
          x1 = surfacePoints[i].x();
          x2 = surfacePoints[i+1].x();
          y1 = surfacePoints[i].y();
          y2 = surfacePoints[i+1].y();
        }

        double dA = (x1*y2-x2*y1);
        A += 0.5*dA;
        cx += (x1+x2)*dA;
        cy += (y1+y2)*dA;
      }

      if (A > 0){
        // centroid in face coordinates
        Point3d surfaceCentroid(cx/(6.0*A), cy/(6.0*A), 0.0);

        // centroid
        result = alignFace*surfaceCentroid;
      }
    }
    return result;
  }

  /// reorder points to upper-left-corner convention
  Point3dVector reorderULC(const Point3dVector& points)
  {
    unsigned N = points.size();
    if (N < 3){
      return Point3dVector();
    }

    // transformation to align face
    Transformation t = Transformation::alignFace(points);
    Point3dVector facePoints = t.inverse()*points;

    // find ulc index in face coordinates
    double maxY = std::numeric_limits<double>::min();
    double minX = std::numeric_limits<double>::max();
    unsigned ulcIndex = 0;
    for(unsigned i = 0; i < N; ++i){
      OS_ASSERT(std::abs(facePoints[i].z()) < 0.001);
      if ((maxY < facePoints[i].y()) || ((maxY < facePoints[i].y() + 0.00001) && (minX > facePoints[i].x()))){
        ulcIndex = i;
        maxY = facePoints[i].y();
        minX = facePoints[i].x();
      }
    }

    // no-op
    if (ulcIndex == 0){
      return points;
    }

    // create result
    Point3dVector result;
    std::copy (points.begin() + ulcIndex, points.end(), std::back_inserter(result));
    std::copy (points.begin(), points.begin() + ulcIndex, std::back_inserter(result));
    OS_ASSERT(result.size() == N);
    return result;
  }

  std::vector<Point3d> removeColinear(const Point3dVector& points, double tol)
  {
    unsigned N = points.size();
    if (N < 3){
      return points;
    }

    std::vector<Point3d> result;
    Point3d lastPoint = points[0];
    result.push_back(lastPoint);

    for (unsigned i = 1; i < N; ++i){
      Point3d currentPoint = points[i];
      Point3d nextPoint = points[0];
      if (i < N-1){
        nextPoint = points[i+1];
      }

      Vector3d a = (currentPoint - lastPoint);
      Vector3d b = (nextPoint - currentPoint);

      // if these fail to normalize we have zero length vectors (e.g. adjacent points)
      if (a.normalize()){
        if (b.normalize()){

          Vector3d c = a.cross(b);
          if (c.length() >= tol){
            // cross product is significant
            result.push_back(currentPoint);
            lastPoint = currentPoint;
          }else{
            // see if dot product is near -1
            double d = a.dot(b);
            if (d <= -1.0 + tol){
              // this is a line reversal
              result.push_back(currentPoint);
              lastPoint = currentPoint;
            }
          }
        }
      }
    }

    return result;
  }

  double getDistance(const Point3d& point1, const Point3d& point2) {
    double dx = point1.x() - point2.x();
    double dy = point1.y() - point2.y();
    double dz = point1.z() - point2.z();
    double result = std::sqrt(dx*dx + dy*dy + dz*dz);
    return result;
  }

  double getAngle(const Vector3d& vector1, const Vector3d& vector2) {
    Vector3d working1(vector1);
    working1.normalize();
    Vector3d working2(vector2);
    working2.normalize();
    return acos(working1.dot(working2));
  }

  /// compute distance in meters between two points on the Earth's surface
  /// lat and lon are specified in degrees
  double getDistanceLatLon(double lat1, double lon1, double lat2, double lon2)
  {

    // for more accuracy would want to use WGS-84 ellipsoid params and Vincenty formula

    // Haversine formula
    double R = 6371000; // Earth radius meters
    double deltaLat = degToRad(lat2-lat1);
    double deltaLon = degToRad(lon2-lon1);
    double a = sin(deltaLat/2) * sin(deltaLat/2) +
               cos(degToRad(lat1)) * cos(degToRad(lat2)) *
               sin(deltaLon/2) * sin(deltaLon/2);
    double c = 2 * atan2(sqrt(a), sqrt(1-a));
    double d = R * c;

    return d;
  }

  bool circularEqual(const Point3dVector& points1, const Point3dVector& points2, double tol)
  {
    unsigned N = points1.size();
    if (N != points2.size()){
      return false;
    }

    if (N == 0){
      return true;
    }

    bool result = false;

    // look for a common starting point
    for (unsigned i = 0; i < N; ++i){
      if (getDistance(points1[0], points2[i]) <= tol){

        result = true;

        // check all other points
        for (unsigned j = 0; j < N; ++j){
          if (getDistance(points1[j], points2[(i + j) % N]) > tol){
            result = false;
            break;
          }
        }
      }

      if (result){
        return result;
      }
    }

    return result;
  }

} // openstudio


#define BOOST_TEST_MODULE factorial static test
#include <boost/test/unit_test.hpp>
#include "../factorial.cpp"

BOOST_AUTO_TEST_CASE(Factorials_for_zero) {
    BOOST_TEST( Factorial(0) == 1 );
}

BOOST_AUTO_TEST_CASE(Factorials_for_positive_numbers) {
    BOOST_TEST( Factorial(1) == 1 );
    BOOST_TEST( Factorial(2) == 2 );
    BOOST_TEST( Factorial(3) == 6 );
    BOOST_TEST( Factorial(10) == 3628800 );
}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file staticallycorrectedyieldtermstructure.hpp
    \brief Statically corrected yield term structure
    \ingroup termstructures
*/

#ifndef quantext_statically_corrected_yts_hpp
#define quantext_statically_corrected_yts_hpp

#include <qle/termstructures/dynamicstype.hpp>

#include <ql/termstructures/yieldtermstructure.hpp>

#include <boost/unordered_map.hpp>

namespace QuantExt {
using namespace QuantLib;

//! Statically Corrected Yield Term Structure
/*! This termstructure takes a floating reference date term structure
    and two fixed reference date term structures, applying a static
    correction to the floating ts implied by the two fixed ones.
    Usually the floating term structure will coincide with
    the first fixed at construction time. Also, the two fixed
    termstructures should have the same reference date and all three
    termstructures should have the same day counter.

    \ingroup termstructures
 */
class StaticallyCorrectedYieldTermStructure : public YieldTermStructure {
public:
    StaticallyCorrectedYieldTermStructure(const Handle<YieldTermStructure>& floatingTermStructure,
                                          const Handle<YieldTermStructure>& fixedSourceTermStructure,
                                          const Handle<YieldTermStructure>& fixedTargetTermStructure,
                                          const YieldCurveRollDown& rollDown = ForwardForward)
        : YieldTermStructure(floatingTermStructure->dayCounter()), x_(floatingTermStructure),
          source_(fixedSourceTermStructure), target_(fixedTargetTermStructure), rollDown_(rollDown) {
        registerWith(floatingTermStructure);
        registerWith(fixedSourceTermStructure);
        registerWith(fixedTargetTermStructure);
    }

    Date maxDate() const { return x_->maxDate(); }
    void update() {}
    const Date& referenceDate() const { return x_->referenceDate(); }

    Calendar calendar() const { return x_->calendar(); }
    Natural settlementDays() const { return x_->settlementDays(); }

    void flushCache() { cache_c_.clear(); }

protected:
    Real discountImpl(Time t) const;

private:
    // FIXME: remove cache
    // cache for source and target forwards
    struct cache_key {
        double t0, t;
        bool operator==(const cache_key& o) const { return (t0 == o.t0) && (t == o.t); }
    };
    struct cache_hasher : std::unary_function<cache_key, std::size_t> {
        std::size_t operator()(cache_key const& x) const {
            std::size_t seed = 0;
            boost::hash_combine(seed, x.t0);
            boost::hash_combine(seed, x.t);
            return seed;
        }
    };
    mutable boost::unordered_map<cache_key, Real, cache_hasher> cache_c_;
    // end cache
    const Handle<YieldTermStructure> x_, source_, target_;
    const YieldCurveRollDown rollDown_;
};

// inline

inline Real StaticallyCorrectedYieldTermStructure::discountImpl(Time t) const {
    Real c = 1.0;
    if (rollDown_ == ForwardForward) {
        Real t0 = source_->timeFromReference(referenceDate());
        // roll down = ForwardForward
        // cache lookup
        cache_key k = { t0, t };
        boost::unordered_map<cache_key, Real>::const_iterator i = cache_c_.find(k);
        if (i == cache_c_.end()) {
            c = source_->discount(t0) / source_->discount(t0 + t) * target_->discount(t0 + t) / target_->discount(t0);
            cache_c_.insert(std::make_pair(k, c));
        } else {
            c = i->second;
        }
    } else {
        // roll down = ConstantDiscount
        // cache lookup
        cache_key k = { 0.0, t };
        boost::unordered_map<cache_key, Real>::const_iterator i = cache_c_.find(k);
        if (i == cache_c_.end()) {
            c = target_->discount(t) / source_->discount(t);
            cache_c_.insert(std::make_pair(k, c));
        } else {
            c = i->second;
        }
    }
    return x_->discount(t) * c;
}

} // namespace QuantExt

#endif


#include <boost/static_assert.hpp>
#include <iostream>

template <unsigned long N>
struct binary
{
  BOOST_STATIC_ASSERT((N % 10 < 2));

  static unsigned const value = binary<N / 10>::value * 2 + N % 10;
};

template <>
struct binary<0>
{
  static unsigned const value = 0;
};

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

#define PRINT_AS_BINARY_AND_VALUE(theValue)                                    \
  {                                                                            \
    std::cout << theValue << " is the binary representation of "               \
              << binary<theValue>::value << std::endl;                         \
  }

  PRINT_AS_BINARY_AND_VALUE(111);
  PRINT_AS_BINARY_AND_VALUE(1011);
  PRINT_AS_BINARY_AND_VALUE(1101);
  PRINT_AS_BINARY_AND_VALUE(101010);

#undef PRINT_AS_BINARY_AND_VALUE
}


//=============================================================================
//
//  CLASS ArpackSolver
//
//=============================================================================

#ifndef COMISO_ARPACKSOLVER_HH
#define COMISO_ARPACKSOLVER_HH

//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>
#if (COMISO_ARPACK_AVAILABLE && COMISO_SUITESPARSE_AVAILABLE && COMISO_EIGEN3_AVAILABLE)

//== INCLUDES =================================================================
#include <CoMISo/Config/CoMISoDefines.hh>

#include <Eigen/Eigen>
#include "EigenArpackMatrixT.hh"

#include <arpack++/arssym.h>

//== FORWARDDECLARATIONS ======================================================

//== NAMESPACES ===============================================================

namespace COMISO {

//== CLASS DEFINITION =========================================================

/** \class ArpackSolver ArpackSolver.hh <COMISO/.../ArpackSolver.hh>

    Brief Description.

    A more elaborate description follows.
*/

class COMISODLLEXPORT ArpackSolver
{
public:

  // sparse matrix type
  typedef EigenArpackMatrixT<double,Eigen::SparseMatrix<double,Eigen::ColMajor> > Matrix;

  /// Constructor
  ArpackSolver() {}

  /// Destructor
  ~ArpackSolver() {}

  // solve eigenproblem
  // number of desired eigenvalues -> _n_eigenvalues
  // which eigenvalues -> one of {LA (largest algebraic), SA (smalles algebraic), LM (largest magnitude), SM(smallest magnitued), BE(both ends)}
  template<class MatrixT,class MatrixT2>
  void solve(const MatrixT&       _A,
             std::vector<double>& _eigenvalues,
             MatrixT2&            _eigenvectors,
             const int            _n_eigvalues = 1,
             const char*          _which_eigs = "SM");

  // solve eigenproblem
  // number of desired eigenvalues -> _n_eigenvalues
  // which eigenvalues -> one of {LA (largest algebraic), SA (smalles algebraic), LM (largest magnitude), SM(smallest magnitued), BE(both ends)}
  template<class MatrixT,class MatrixT2>
  void solve_inverse(const MatrixT&       _A,
                     std::vector<double>& _eigenvalues,
                     MatrixT2&            _eigenvectors,
                     const int            _n_eigvalues = 1,
                     const char*          _which_eigs = "LM");

  // check resulting eigenvalues/eigenvectors
  template<class MatrixT,class MatrixT2>
  void check_result(const MatrixT& _A, std::vector<double>& _eigenvalues, MatrixT2& _eigenvectors);

private:

};

//=============================================================================
} // namespace ACG
//=============================================================================
#if defined(INCLUDE_TEMPLATES) && !defined(COMISO_ARPACKSOLVER_C)
#define COMISO_ARPACKSOLVER_TEMPLATES
#include "ArpackSolver.cc"
#endif
//=============================================================================
#endif // COMISO_SUITESPARSE_AVAILABLE
//=============================================================================
#endif // ACG_ARPACKSOLVER_HH defined
//=============================================================================


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include "Element.hpp"

#include <cmath>
#include <vector>

#include "Config.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>

#include <boost/math/special_functions/sign.hpp>

#include "utils/Sphere.hpp"

void Element::spherical_polygon(Eigen::Vector3d & t_, Eigen::Vector3d & b_,
    std::vector<double> & theta, std::vector<double> & phi,
    std::vector<double> & phinumb, std::vector<int> & numb) const
{
  Eigen::Vector3d sph_center = sphere_.center;
  double radius = sphere_.radius;
  // Calculate the azimuthal and polar angles for the tessera vertices:
  // we use the normal, tangent and bitangent as a local reference frame
  for (int i = 0; i < nVertices_; ++i) {
    Eigen::Vector3d vertex_normal = vertices_.col(i) - sph_center;
    // The cosine of the polar angle is given as the dot product of the normal at the vertex and the
    // normal at the tessera center: R\cos\theta
    double cos_theta = vertex_normal.dot(normal_) / radius;
    if (cos_theta >=  1.0) cos_theta = 1.0;
    if (cos_theta <= -1.0) cos_theta = -1.0;
    theta[i] = std::acos(cos_theta);
    // The cosine of the azimuthal angle is given as the dot product of the normal at the vertex and the
    // tangent at the tessera center divided by the sine of the polar angle: R\sin\theta\cos\phi
    double cos_phi = vertex_normal.dot(t_) / (radius * std::sin(theta[i]));
    if (cos_phi >=  1.0) cos_phi = 1.0;
    if (cos_phi <= -1.0) cos_phi = -1.0;
    phi[i] = std::acos(cos_phi);
    // The sine of the azimuthal angle is given as the dot product of the normal at the vertex and the
    // bitangent at the tessera center divided by the sine of the polar angle: R\sin\theta\sin\phi
    double sin_phi = vertex_normal.dot(b_) / (radius * std::sin(theta[i]));
    if (sin_phi <= 0.0) phi[i] = 2 * M_PI - phi[i];
  }
  for (int i = 1; i < nVertices_; ++i) {
    phi[i] = phi[i] - phi[0];
    if (phi[i] < 0.0) phi[i] = 2 * M_PI + phi[i];
  }
  // Rewrite tangent as linear combination of original tangent and bitangent
  // then recalculate bitangent so that it's orthogonal to the tangent
  t_ = t_ * std::cos(phi[0]) + b_ * std::sin(phi[0]);
  b_ = normal_.cross(t_);
  // Populate numb and phinumb arrays
  phi[0] = 0.0;
  numb[0] = 0; numb[1] = 1;
  phinumb[0] = phi[0]; phinumb[1] = phi[1];
  for (int i = 2; i < nVertices_; ++i) {// This loop is 2-based
    for (int j = 1; j < i; ++j) {// This loop is 1-based
      if (phi[i] < phinumb[j]) {
        for (int k = 0; k < (i - j); ++k) {
          numb[i - k] = numb[i - k -1];
          phinumb[i - k] = phinumb[i - k -1];
        }
        numb[j] = i;
        phinumb[j] = phi[i];
        goto jump;
      }
    }
    numb[i] = i;
    phinumb[i] = phi[i];
jump:
    ; // Do nothing...
  }
  numb[nVertices_] = numb[0];
  phinumb[nVertices_] = 2 * M_PI;
}

void tangent_and_bitangent(const Eigen::Vector3d & n_,
    Eigen::Vector3d & t_, Eigen::Vector3d & b_)
{
  double rmin = 0.99;
  double n0 = n_(0), n1 = n_(1), n2 = n_(2);
  if (std::abs(n0) <= rmin) {
    rmin = std::abs(n0);
    t_(0) = 0.0;
    t_(1) = - n2 / std::sqrt(1.0 - std::pow(n0, 2));
    t_(2) =   n1 / std::sqrt(1.0 - std::pow(n0, 2));
  }
  if (std::abs(n1) <= rmin) {
    rmin = std::abs(n1);
    t_(0) =   n2 / std::sqrt(1.0 - std::pow(n1, 2));
    t_(1) =   0.0;
    t_(2) = - n0 / std::sqrt(1.0 - std::pow(n1, 2));
  }
  if (std::abs(n2) <= rmin) {
    rmin = std::abs(n2);
    t_(0) =  n1 / std::sqrt(1.0 - std::pow(n2, 2));
    t_(1) = -n0 / std::sqrt(1.0 - std::pow(n2, 2));
    t_(2) =  0.0;
  }
  b_ = n_.cross(t_);
  // Check that the calculated Frenet-Serret frame is left-handed (levogiro)
  // by checking that the determinant of the matrix whose columns are the normal,
  // tangent and bitangent vectors has determinant 1 (the system is orthonormal!)
  Eigen::Matrix3d M;
  M.col(0) = n_;
  M.col(1) = t_;
  M.col(2) = b_;
  if (boost::math::sign(M.determinant()) != 1) {
    PCMSOLVER_ERROR("Frenet-Serret local frame is not left-handed!", BOOST_CURRENT_FUNCTION);
  }
}


//
// Created by a.kiryanenko on 3/26/20.
//

#include "../SpuUltraGraphAdapter.h"
#include "../SpuUltraGraphProperty.h"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include "GraphPerformanceTest.h"

using namespace SPU_GRAPH;
using namespace boost;

typedef boost::adjacency_list <
        boost::vecS, // как хранить вершины - в векторе
        boost::vecS, // как хранить ребра из каждой вершины - в векторе
        boost::directedS,
        no_property,
        property < edge_weight_t, int >
> AdjacencyListGraph;

template <class G>
pair<typename graph_traits<G>::edge_descriptor, bool>
add_weight_edge(typename graph_traits<G>::vertex_descriptor u, typename graph_traits<G>::vertex_descriptor v, G& g) {
    auto weight = rand() % 16;
    return add_edge(u, v, weight, g);
}

template <>
pair<typename graph_traits<SpuUltraGraph>::edge_descriptor, bool>
add_weight_edge(typename graph_traits<SpuUltraGraph>::vertex_descriptor u, typename graph_traits<SpuUltraGraph>::vertex_descriptor v, SpuUltraGraph& g) {
    auto weight = rand() % 16;
    return {g.add_edge(g.get_free_edge_descriptor(weight), u, v), true};
}

template <class G>
void kruskal_test(G &g) {
    typedef typename graph_traits<G>::vertex_descriptor vertex_t;
    typedef typename graph_traits<G>::edge_descriptor edge_t;

    std::vector < edge_t > spanning_tree;
    // Выполняю алгоритм Краскала для построения минимального остовного дерева
    kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));
}

int main()
{
    cout << "SpuUltraGraph performance test" << endl;
    cout << "==========================================" << endl;
    GraphPerformanceTest<SpuUltraGraph> spu_graph_test(kruskal_test, "kruskal_test_SpuUltraGraph.csv");
    spu_graph_test.is_mutable_test = false;
    spu_graph_test.add_edge_func = add_weight_edge;
    spu_graph_test.start();

    cout << "adjacency_list performance test" << endl;
    cout << "==========================================" << endl;
    GraphPerformanceTest<AdjacencyListGraph> adjacency_list_test(kruskal_test, "kruskal_test_adjacency_list.csv");
    adjacency_list_test.is_mutable_test = false;
    adjacency_list_test.add_edge_func = add_weight_edge;
    adjacency_list_test.start();

    cout << "adjacency_matrix performance test" << endl;
    cout << "==========================================" << endl;
    GraphPerformanceTest<AdjacencyMatrixGraph> adjacency_matrix_test(kruskal_test, "kruskal_test_adjacency_matrix.csv");
    adjacency_matrix_test.is_mutable_test = false;
    adjacency_matrix_test.add_edge_func = add_weight_edge;
    adjacency_matrix_test.end_vertices_cnt = 20000;
    adjacency_matrix_test.start();
    return 0;
}