//=======================================================================
// Copyright 2002 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <string>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/undirected_dfs.hpp>
#include <boost/cstdlib.hpp>
#include <iostream>

/*
  Example graph from Tarjei Knapstad.

                   H15
                   |
          H8       C2
            \     /  \
          H9-C0-C1    C3-O7-H14
            /   |     |
          H10   C6    C4
               /  \  /  \
              H11  C5    H13
                   |
                   H12
*/

std::string name[] = { "C0", "C1", "C2", "C3", "C4", "C5", "C6", "O7",
                       "H8", "H9", "H10", "H11", "H12", "H13", "H14", "H15"};

struct detect_loops : public boost::dfs_visitor<>
{
  template <class Edge, class Graph>
  void back_edge(Edge e, const Graph& g) {
    std::cout << name[source(e, g)]
              << " -- "
              << name[target(e, g)] << "\n";
  }
};

int main(int, char*[])
{
  using namespace boost;
  typedef adjacency_list< vecS, vecS, undirectedS,
    no_property,
    property<edge_color_t, default_color_type> > graph_t;
  typedef graph_traits<graph_t>::vertex_descriptor vertex_t;

  const std::size_t N = sizeof(name)/sizeof(std::string);
  graph_t g(N);

  add_edge(0, 1, g);
  add_edge(0, 8, g);
  add_edge(0, 9, g);
  add_edge(0, 10, g);
  add_edge(1, 2, g);
  add_edge(1, 6, g);
  add_edge(2, 15, g);
  add_edge(2, 3, g);
  add_edge(3, 7, g);
  add_edge(3, 4, g);
  add_edge(4, 13, g);
  add_edge(4, 5, g);
  add_edge(5, 12, g);
  add_edge(5, 6, g);
  add_edge(6, 11, g);
  add_edge(7, 14, g);

  std::cout << "back edges:\n";
  detect_loops vis;
  undirected_dfs(g, root_vertex(vertex_t(0)).visitor(vis)
                 .edge_color_map(get(edge_color, g)));
  std::cout << std::endl;

  return boost::exit_success;
}


#include <kd_tree.h>
#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include <cstdlib>
#include <ctime>

using namespace std;
using namespace Eigen;

int main(int argc, char * argv[]) {
    int n = 100;
    int range = 1;
    if (argc > 1) {
        n = atoi(argv[1]);
    }

    vector<Vector3d> points;
    points.clear();

    double random(double,double);
    srand(unsigned(time(0)));
    for(int icnt = 0; icnt != n; ++icnt) {
        points.push_back(Vector3d(random(0, range), random(0, range), random(0,range)));
    }

    Vector3d nn;
    Vector3d target;

    KDTree kdtree(points);

    vector<double> ddist(n);
    // int ans2[KNN_NUM];
    // double best = INFINITY;
    // double dist;
    // int best_index;

    for(int k = 0; k < n; ++k) {
        target = Vector3d(random(0, range), random(0, range), random(0,range));

        vector<int> ans = kdtree.GetKNN(target, 5, 0, -1);

        // for(int i = 0; i < n; ++i) {
        //     ddist[i] = (points[i]-target).norm();
        // }
        // for(int i = 0; i < KNN_NUM; ++i) {
        //     best = INFINITY;
        //     best_index = -1;
        //     for(int j = 0; j < n; ++j) {
        //         if (ddist[j] < best && (i == 0 || ddist[j] > ddist[ans2[i-1]])) {
        //             best = ddist[j];
        //             best_index = j;
        //         }
        //     }

        //     ans2[i] = best_index;
        // }

        // for(int j = 0; j < KNN_NUM; ++j) {
        //     if (ans[j] != ans2[KNN_NUM - 1 - j]) {
        //         cout << "wrong!" << endl;
        //         cout << "target: " << target.transpose() << endl;

        //         for(int m = 0; m < KNN_NUM; ++m){
        //             cout << ans[m] << ',' << ans2[KNN_NUM - 1 - m] << " : " << ddist[ans[m]] << ',' << ddist[ans2[KNN_NUM - 1 - m]] << endl;
        //         }
        //         cout << ddist[0] << endl;

        //         break;
        //     }
        // }

    }

    return 1;
}

double random(double start, double end)
{
    return start+(end-start)*rand()/(RAND_MAX + 1.0);
}


//#define BACKWARD_HAS_DW 1
//#include "backward.hpp"
// namespace backward
//{
// backward::SignalHandling sh;
//}
#include <mutex>
#include <queue>
#include <iostream>

#include <opencv2/opencv.hpp>

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Vector3Stamped.h>

#include <rosbag/bag.h>
#include <rosbag/view.h>

#include <code_utils/ros_utils.h>

#include "acc_lib/allan_acc.h"
#include "acc_lib/fitallan_acc.h"
#include "gyr_lib/allan_gyr.h"
#include "gyr_lib/fitallan_gyr.h"

std::mutex m_buf;

std::queue<sensor_msgs::ImuConstPtr> imu_buf;
imu::AllanGyr* gyr_x;
imu::AllanGyr* gyr_y;
imu::AllanGyr* gyr_z;
imu::AllanAcc* acc_x;
imu::AllanAcc* acc_y;
imu::AllanAcc* acc_z;
double start_t;
bool start = true;
bool end = false;
int max_time_min = 10;
std::string data_save_path;

void imu_callback(const sensor_msgs::ImuConstPtr& imu_msg) {
    //    m_buf.lock( );
    //    imu_buf.push( imu_msg );
    //    m_buf.unlock( );
    double time = imu_msg->header.stamp.toSec();
    gyr_x->pushRadPerSec(imu_msg->angular_velocity.x, time);
    gyr_y->pushRadPerSec(imu_msg->angular_velocity.y, time);
    gyr_z->pushRadPerSec(imu_msg->angular_velocity.z, time);
    acc_x->pushMPerSec2(imu_msg->linear_acceleration.x, time);
    acc_y->pushMPerSec2(imu_msg->linear_acceleration.y, time);
    acc_z->pushMPerSec2(imu_msg->linear_acceleration.z, time);

    if (start) {
        start_t = time;
        start = false;
    } else {
        double time_min = (time - start_t) / 60;
        if (time_min > max_time_min)
            end = true;
    }
}

void writeData1(
        const std::string sensor_name, //
        const std::vector<double>& gyro_ts_x,
        const std::vector<double>& gyro_d) {
    std::ofstream out_t;
    std::ofstream out_x;
    out_t.open(data_save_path + "data_" + sensor_name + "_t.txt",
            std::ios::trunc);
    out_x.open(data_save_path + "data_" + sensor_name + "_x.txt",
            std::ios::trunc);
    out_t << std::setprecision(10);
    out_x << std::setprecision(10);
    for (unsigned int index = 0; index < gyro_ts_x.size(); ++index) {
        out_t << gyro_ts_x[index] << '\n';
        out_x << gyro_d[index] << '\n';
    }
    out_t.close();
    out_x.close();
}

void writeData3(const std::string sensor_name,
        const std::vector<double>& gyro_ts_x,
        const std::vector<double>& gyro_d_x,
        const std::vector<double>& gyro_d_y,
        const std::vector<double>& gyro_d_z) {
    std::ofstream out_t;
    std::ofstream out_x;
    std::ofstream out_y;
    std::ofstream out_z;
    out_t.open(data_save_path + "data_" + sensor_name + "_t.txt",
            std::ios::trunc);
    out_x.open(data_save_path + "data_" + sensor_name + "_x.txt",
            std::ios::trunc);
    out_y.open(data_save_path + "data_" + sensor_name + "_y.txt",
            std::ios::trunc);
    out_z.open(data_save_path + "data_" + sensor_name + "_z.txt",
            std::ios::trunc);
    out_t << std::setprecision(10);
    out_x << std::setprecision(10);
    out_y << std::setprecision(10);
    out_z << std::setprecision(10);

    for (int index = 0; index < gyro_ts_x.size(); ++index) {
        out_t << gyro_ts_x[index] << '\n';
        out_x << gyro_d_x[index] << '\n';
        out_y << gyro_d_y[index] << '\n';
        out_z << gyro_d_z[index] << '\n';
    }

    out_t.close();
    out_x.close();
    out_y.close();
    out_z.close();
}

void writeYAML(const std::string data_path, const std::string sensor_name,
        const imu::FitAllanGyr& gyr_x, const imu::FitAllanGyr& gyr_y,
        const imu::FitAllanGyr& gyr_z, const imu::FitAllanAcc& acc_x,
        const imu::FitAllanAcc& acc_y, const imu::FitAllanAcc& acc_z) {
    cv::FileStorage fs(data_path + sensor_name + "_imu_param.yaml",
            cv::FileStorage::WRITE);

    fs << "type" << "IMU";

    fs << "name" << sensor_name;

    fs << "Gyr";
    fs << "{";
    fs << "unit" << " rad/s";

    fs << "avg-axis";
    fs << "{";
    fs << std::string("gyr_n")
            << (gyr_x.getWhiteNoise() + gyr_y.getWhiteNoise()
                    + gyr_z.getWhiteNoise()) / 3;
    fs << std::string("gyr_w")
            << (gyr_x.getBiasInstability() + gyr_y.getBiasInstability()
                    + gyr_z.getBiasInstability()) / 3;

    fs << "}";

    fs << "x-axis";
    fs << "{";
    fs << std::string("gyr_n") << gyr_x.getWhiteNoise();
    fs << std::string("gyr_w") << gyr_x.getBiasInstability();
    fs << "}";

    fs << "y-axis";
    fs << "{";
    fs << std::string("gyr_n") << gyr_y.getWhiteNoise();
    fs << std::string("gyr_w") << gyr_y.getBiasInstability();
    fs << "}";

    fs << "z-axis";
    fs << "{";
    fs << std::string("gyr_n") << gyr_z.getWhiteNoise();
    fs << std::string("gyr_w") << gyr_z.getBiasInstability();
    fs << "}";

    fs << "}";

    fs << "Acc";
    fs << "{";
    fs << "unit" << " m/s^2";

    fs << "avg-axis";
    fs << "{";
    fs << std::string("acc_n")
            << (acc_x.getWhiteNoise() + acc_y.getWhiteNoise()
                    + acc_z.getWhiteNoise()) / 3;
    fs << std::string("acc_w")
            << (acc_x.getBiasInstability() + acc_y.getBiasInstability()
                    + acc_z.getBiasInstability()) / 3;
    fs << "}";

    fs << "x-axis";
    fs << "{";
    fs << std::string("acc_n") << acc_x.getWhiteNoise();
    fs << std::string("acc_w") << acc_x.getBiasInstability();
    fs << "}";

    fs << "y-axis";
    fs << "{";
    fs << std::string("acc_n") << acc_y.getWhiteNoise();
    fs << std::string("acc_w") << acc_y.getBiasInstability();
    fs << "}";

    fs << "z-axis";
    fs << "{";
    fs << std::string("acc_n") << acc_z.getWhiteNoise();
    fs << std::string("acc_w") << acc_z.getBiasInstability();
    fs << "}";

    fs << "}";

    fs.release();
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "gyro_test");
    ros::NodeHandle nh("~");
    ros::console::set_logger_level( ROSCONSOLE_DEFAULT_NAME,
            ros::console::levels::Debug);

    std::string IMU_TOPIC;
    std::string IMU_NAME;
    int max_cluster;

    IMU_TOPIC = ros_utils::readParam<std::string>(nh, "imu_topic");
    IMU_NAME = ros_utils::readParam<std::string>(nh, "imu_name");
    data_save_path = ros_utils::readParam<std::string>(nh, "data_save_path");
    max_time_min = ros_utils::readParam<int>(nh, "max_time_min");
    max_cluster = ros_utils::readParam<int>(nh, "max_cluster");

    ros::Subscriber sub_imu = nh.subscribe(IMU_TOPIC, //
            20000000, imu_callback, ros::TransportHints().tcpNoDelay());
    //    ros::Publisher pub = n.advertise< geometry_msgs::Vector3Stamped >( ALLAN_TOPIC,
    //    2000 );

    /* use bag file */
    std::cout << "Loading bag file..." << std::flush;
    std::string bag_path = ros_utils::readParam<std::string>(nh, "bag_path");
    double bag_skip_second = ros_utils::readParam<double>(nh,
            "bag_skip_second");
    rosbag::Bag bag;
    bag.open(bag_path, rosbag::bagmode::Read);
    std::vector<std::string> topics;
    topics.push_back(IMU_TOPIC);
    rosbag::View* view = NULL;
    view = new rosbag::View(bag, rosbag::TopicQuery(topics));
    if (bag_skip_second > 0) {
        ros::Time start_time = view->getBeginTime()
                + ros::Duration(bag_skip_second);
        delete view;
        view = new rosbag::View(bag, rosbag::TopicQuery(topics), start_time);
    }
    std::cout << "OK" << std::endl;

    gyr_x = new imu::AllanGyr("gyr x", max_cluster);
    gyr_y = new imu::AllanGyr("gyr y", max_cluster);
    gyr_z = new imu::AllanGyr("gyr z", max_cluster);
    acc_x = new imu::AllanAcc("acc x", max_cluster);
    acc_y = new imu::AllanAcc("acc y", max_cluster);
    acc_z = new imu::AllanAcc("acc z", max_cluster);
    std::cout << "wait for imu data." << std::endl;

    auto last_print = std::chrono::steady_clock::now();
    uint64_t count_imu = 0;
    std::cout << "Message count: " << count_imu << std::flush;

    foreach(rosbag::MessageInstance const msg, *view) {
        sensor_msgs::Imu::ConstPtr msg_imu =
                msg.instantiate<sensor_msgs::Imu>();

        if (msg_imu != NULL) {
            imu_callback(msg_imu);
            count_imu++;
        }

        if (std::chrono::steady_clock::now() - last_print
                >= std::chrono::seconds(1)) {
            last_print = std::chrono::steady_clock::now();
            std::cout << '\r' << "Message count: " << count_imu << std::flush;
        }

        if (end) {
            break;
        }
    }
    std::cout << std::endl;

    std::cout << "Finished " << count_imu << std::endl;

    //    ros::spin( );
//    while (!end && ros::ok()) {
//        ros::spinOnce();
//    }
//    std::cout << "ok " << acc_x->numData << std::endl;

    ///
    gyr_x->calc();
    std::vector<double> gyro_v_x = gyr_x->getVariance();
    std::vector<double> gyro_d_x = gyr_x->getDeviation();
    std::vector<double> gyro_ts_x = gyr_x->getTimes();

    gyr_y->calc();
    std::vector<double> gyro_v_y = gyr_y->getVariance();
    std::vector<double> gyro_d_y = gyr_y->getDeviation();
    std::vector<double> gyro_ts_y = gyr_y->getTimes();

    gyr_z->calc();
    std::vector<double> gyro_v_z = gyr_z->getVariance();
    std::vector<double> gyro_d_z = gyr_z->getDeviation();
    std::vector<double> gyro_ts_z = gyr_z->getTimes();

    std::cout << "Gyro X " << std::endl;
    imu::FitAllanGyr fit_gyr_x(gyro_v_x, gyro_ts_x, gyr_x->getFreq());
    std::cout << "  bias " << gyr_x->getAvgValue() / 3600 << " degree/s"
            << std::endl;
    std::cout << "-------------------" << std::endl;

    std::cout << "Gyro y " << std::endl;
    imu::FitAllanGyr fit_gyr_y(gyro_v_y, gyro_ts_y, gyr_y->getFreq());
    std::cout << "  bias " << gyr_y->getAvgValue() / 3600 << " degree/s"
            << std::endl;
    std::cout << "-------------------" << std::endl;

    std::cout << "Gyro z " << std::endl;
    imu::FitAllanGyr fit_gyr_z(gyro_v_z, gyro_ts_z, gyr_z->getFreq());
    std::cout << "  bias " << gyr_z->getAvgValue() / 3600 << " degree/s"
            << std::endl;
    std::cout << "-------------------" << std::endl;

    std::vector<double> gyro_sim_d_x = fit_gyr_x.calcSimDeviation(gyro_ts_x);
    std::vector<double> gyro_sim_d_y = fit_gyr_y.calcSimDeviation(gyro_ts_y);
    std::vector<double> gyro_sim_d_z = fit_gyr_z.calcSimDeviation(gyro_ts_z);

    writeData3(IMU_NAME + "_sim_gyr", gyro_ts_x, gyro_sim_d_x, gyro_sim_d_y,
            gyro_sim_d_z);
    writeData3(IMU_NAME + "_gyr", gyro_ts_x, gyro_d_x, gyro_d_y, gyro_d_z);

    std::cout << "==============================================" << std::endl;
    std::cout << "==============================================" << std::endl;

    acc_x->calc();
    std::vector<double> acc_v_x = acc_x->getVariance();
    std::vector<double> acc_d_x = acc_x->getDeviation();
    std::vector<double> acc_ts_x = acc_x->getTimes();

    acc_y->calc();
    std::vector<double> acc_v_y = acc_y->getVariance();
    std::vector<double> acc_d_y = acc_y->getDeviation();
    std::vector<double> acc_ts_y = acc_y->getTimes();

    acc_z->calc();
    std::vector<double> acc_v_z = acc_z->getVariance();
    std::vector<double> acc_d_z = acc_z->getDeviation();
    std::vector<double> acc_ts_z = acc_z->getTimes();

    std::cout << "acc X " << std::endl;
    imu::FitAllanAcc fit_acc_x(acc_v_x, acc_ts_x, acc_x->getFreq());
    std::cout << "-------------------" << std::endl;

    std::cout << "acc y " << std::endl;
    imu::FitAllanAcc fit_acc_y(acc_v_y, acc_ts_y, acc_y->getFreq());
    std::cout << "-------------------" << std::endl;

    std::cout << "acc z " << std::endl;
    imu::FitAllanAcc fit_acc_z(acc_v_z, acc_ts_z, acc_z->getFreq());
    std::cout << "-------------------" << std::endl;

    std::vector<double> acc_sim_d_x = fit_acc_x.calcSimDeviation(acc_ts_x);
    std::vector<double> acc_sim_d_y = fit_acc_y.calcSimDeviation(acc_ts_x);
    std::vector<double> acc_sim_d_z = fit_acc_z.calcSimDeviation(acc_ts_x);

    writeData3(IMU_NAME + "_sim_acc", acc_ts_x, acc_sim_d_x, acc_sim_d_y,
            acc_sim_d_z);
    writeData3(IMU_NAME + "_acc", acc_ts_x, acc_d_x, acc_d_y, acc_d_z);

    writeYAML(data_save_path, IMU_NAME, fit_gyr_x, fit_gyr_y, fit_gyr_z,
            fit_acc_x, fit_acc_y, fit_acc_z);

    return 0;
}


#pragma once

#include <deal.II/base/tensor.h>
#include <type_traits>

namespace dealii {
// ----------------------------------------------------------------------
template <int dim, typename NUMBER>
inline Tensor<1, dim, NUMBER>
outer_product(NUMBER alpha, const Tensor<1, dim, NUMBER>& t)
{
  return alpha * t;
}

// ----------------------------------------------------------------------
template <int dim, typename NUMBER>
Tensor<1, dim, NUMBER>
outer_product(const Tensor<1, dim, NUMBER>& t, NUMBER alpha)
{
  return alpha * t;
}

// ----------------------------------------------------------------------
template <int dim, typename NUMBER>
inline Tensor<1, dim, NUMBER>
outer_product(const Tensor<1, dim, NUMBER>& t, const Tensor<0, dim, NUMBER>& alpha)
{
  return t * alpha;
}

// ----------------------------------------------------------------------
template <int dim, typename NUMBER>
Tensor<1, dim, NUMBER> inline outer_product(const Tensor<0, dim, NUMBER>& alpha,
                                            const Tensor<1, dim, NUMBER>& t)
{
  return t * alpha;
}

// ----------------------------------------------------------------------
template <int dim, typename NUMBER>
inline void outer_product(Tensor<1, dim, NUMBER>& dst,
                          const Tensor<0, dim, NUMBER> alpha,
                          const Tensor<1, dim, NUMBER>& t)
{
  // hack to extract alpha as a number...

  dst = NUMBER(alpha) * t;
}

template <int dim, typename NUMBER>
inline void outer_product(Tensor<0, dim, NUMBER>& dst,
                          const Tensor<0, dim, NUMBER> alpha,
                          const Tensor<0, dim, NUMBER>& t)
{
  dst = alpha * t;
}

#if (DEAL_II_VERSION_MAJOR >= 8 && DEAL_II_VERSION_MINOR <= 3)
// ----------------------------------------------------------------------
template <int dim, typename NUMBER>
inline void outer_product(Tensor<1, dim, NUMBER>& dst,
                          const Tensor<1, dim, NUMBER>& t,
                          const Tensor<0, dim, NUMBER>& alpha)
{
  dst = alpha[0] * t;
}

// ----------------------------------------------------------------------
template <int dim, class NUMBER>
inline NUMBER
scalar_product(const Tensor<1, dim, NUMBER>& t1, const Tensor<1, dim, NUMBER>& t2)
{
  NUMBER sum = 0;
  for (int i = 0; i < dim; ++i) sum += t1[i] * t2[i];

  return sum;
}

// ----------------------------------------------------------------------
template <int dim, class NUMBER>
inline NUMBER
scalar_product(const Tensor<0, dim, NUMBER>& t1, const Tensor<0, dim, NUMBER>& t2)
{
  return t1 * t2;
}
#endif  // DEAL_II_VERSION_MAJOR >= 8 && DEAL_II_VERSION_MINOR <= 3

// ----------------------------------------------------------------------
template <class NUMBER>
inline typename std::enable_if<std::is_scalar<NUMBER>::value, NUMBER>::type
scalar_product(const NUMBER& t1, const NUMBER& t2)
{
  return t1 * t2;
}

}  // end namespace dealii


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "test_tensor"

#include <boost/test/unit_test.hpp>
#include "nanocv/tensor.h"

namespace
{
        using namespace ncv;

        void check_tensor(tensor_t& tensor, size_t dims, size_t rows, size_t cols, scalar_t constant)
        {
                tensor.resize(dims, rows, cols);

                BOOST_CHECK_EQUAL(tensor.dims(), dims);
                BOOST_CHECK_EQUAL(tensor.rows(), rows);
                BOOST_CHECK_EQUAL(tensor.cols(), cols);
                BOOST_CHECK_EQUAL(tensor.size(), dims * rows * cols);
                BOOST_CHECK_EQUAL(tensor.planeSize(), rows * cols);

                BOOST_CHECK_EQUAL(tensor.vector().size(), tensor.size());
                BOOST_CHECK_EQUAL(tensor.vector(dims / 2).size(), tensor.planeSize());

                BOOST_CHECK_EQUAL(tensor.matrix(dims - 1).rows(), tensor.rows());
                BOOST_CHECK_EQUAL(tensor.matrix(dims - 1).cols(), tensor.cols());

                tensor.setConstant(constant);

                BOOST_CHECK_EQUAL(tensor.vector().minCoeff(), constant);
                BOOST_CHECK_EQUAL(tensor.vector().maxCoeff(), constant);
        }
}

BOOST_AUTO_TEST_CASE(test_tensor)
{
        using namespace ncv;

        const size_t dims = 4;
        const size_t rows = 7;
        const size_t cols = 3;

        tensor_t tensor;

        check_tensor(tensor, dims, rows, cols, 0);
        check_tensor(tensor, dims, rows, cols, 1);

        check_tensor(tensor, 4 * dims, rows, cols, 3);
        check_tensor(tensor, dims, 3 * rows, 7 * cols, -2.3);
}


//  Copyright John Maddock 2010.
//  Copyright Paul A. Bristow 2010.

//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_STATS_INVERSE_GAUSSIAN_HPP
#define BOOST_STATS_INVERSE_GAUSSIAN_HPP

#ifdef _MSC_VER
#pragma warning(disable: 4512) // assignment operator could not be generated
#endif

// http://en.wikipedia.org/wiki/Normal-inverse_Gaussian_distribution
// http://mathworld.wolfram.com/InverseGaussianDistribution.html

// The normal-inverse Gaussian distribution
// also called the Wald distribution (some sources limit this to when mean = 1).

// It is the continuous probability distribution
// that is defined as the normal variance-mean mixture where the mixing density is the
// inverse Gaussian distribution. The tails of the distribution decrease more slowly
// than the normal distribution. It is therefore suitable to model phenomena
// where numerically large values are more probable than is the case for the normal distribution.

// The Inverse Gaussian distribution was first studied in relationship to Brownian motion.
// In 1956 M.C.K. Tweedie used the name 'Inverse Gaussian' because there is an inverse
// relationship between the time to cover a unit distance and distance covered in unit time.

// Examples are returns from financial assets and turbulent wind speeds.
// The normal-inverse Gaussian distributions form
// a subclass of the generalised hyperbolic distributions.

// See also

// http://en.wikipedia.org/wiki/Normal_distribution
// http://www.itl.nist.gov/div898/handbook/eda/section3/eda3661.htm
// Also:
// Weisstein, Eric W. "Normal Distribution."
// From MathWorld--A Wolfram Web Resource.
// http://mathworld.wolfram.com/NormalDistribution.html

// http://www.jstatsoft.org/v26/i04/paper General class of inverse Gaussian distributions.
// ig package - withdrawn but at http://cran.r-project.org/src/contrib/Archive/ig/

// http://www.stat.ucl.ac.be/ISdidactique/Rhelp/library/SuppDists/html/inverse_gaussian.html
// R package for dinverse_gaussian, ...

// http://www.statsci.org/s/inverse_gaussian.s  and http://www.statsci.org/s/inverse_gaussian.html

//#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/erf.hpp> // for erf/erfc.
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/gamma.hpp> // for gamma function
// using boost::math::gamma_p;

#include <boost/math/tools/tuple.hpp>
//using std::tr1::tuple;
//using std::tr1::make_tuple;
#include <boost/math/tools/roots.hpp>
//using boost::math::tools::newton_raphson_iterate;

#include <utility>

namespace boost{ namespace math{

template <class RealType = double, class Policy = policies::policy<> >
class inverse_gaussian_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   inverse_gaussian_distribution(RealType mean = 1, RealType scale = 1)
      : m_mean(mean), m_scale(scale)
   { // Default is a 1,1 inverse_gaussian distribution.
     static const char* function = "boost::math::inverse_gaussian_distribution<%1%>::inverse_gaussian_distribution";

     RealType result;
     detail::check_scale(function, scale, &result, Policy());
     detail::check_location(function, mean, &result, Policy());
   }

   RealType mean()const
   { // alias for location.
      return m_mean; // aka mu
   }

   // Synonyms, provided to allow generic use of find_location and find_scale.
   RealType location()const
   { // location, aka mu.
      return m_mean;
   }
   RealType scale()const
   { // scale, aka lambda.
      return m_scale;
   }

   RealType shape()const
   { // shape, aka phi = lambda/mu.
      return m_scale / m_mean;
   }

private:
   //
   // Data members:
   //
   RealType m_mean;  // distribution mean or location, aka mu.
   RealType m_scale;    // distribution standard deviation or scale, aka lambda.
}; // class normal_distribution

typedef inverse_gaussian_distribution<double> inverse_gaussian;

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const inverse_gaussian_distribution<RealType, Policy>& /*dist*/)
{ // Range of permissible values for random variable x, zero to max.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(static_cast<RealType>(0.), max_value<RealType>()); // - to + max value.
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const inverse_gaussian_distribution<RealType, Policy>& /*dist*/)
{ // Range of supported values for random variable x, zero to max.
  // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(static_cast<RealType>(0.),  max_value<RealType>()); // - to + max value.
}

template <class RealType, class Policy>
inline RealType pdf(const inverse_gaussian_distribution<RealType, Policy>& dist, const RealType& x)
{ // Probability Density Function
   BOOST_MATH_STD_USING  // for ADL of std functions

   RealType scale = dist.scale();
   RealType mean = dist.mean();
   RealType result = 0;
   static const char* function = "boost::math::pdf(const inverse_gaussian_distribution<%1%>&, %1%)";
   if(false == detail::check_scale(function, scale, &result, Policy()))
   {
      return result;
   }
   if(false == detail::check_location(function, mean, &result, Policy()))
   {
      return result;
   }
   if(false == detail::check_positive_x(function, x, &result, Policy()))
   {
      return result;
   }

   if (x == 0)
   {
     return 0; // Convenient, even if not defined mathematically.
   }

   result =
     sqrt(scale / (constants::two_pi<RealType>() * x * x * x))
    * exp(-scale * (x - mean) * (x - mean) / (2 * x * mean * mean));
   return result;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const inverse_gaussian_distribution<RealType, Policy>& dist, const RealType& x)
{ // Cumulative Density Function.
   BOOST_MATH_STD_USING  // for ADL of std functions.

   RealType scale = dist.scale();
   RealType mean = dist.mean();
   static const char* function = "boost::math::cdf(const inverse_gaussian_distribution<%1%>&, %1%)";
   RealType result = 0;
   if(false == detail::check_scale(function, scale, &result, Policy()))
   {
      return result;
   }
   if(false == detail::check_location(function, mean, &result, Policy()))
   {
      return result;
   }
   if(false == detail::check_positive_x(function, x, &result, Policy()))
   {
     return result;
   }
   if (x == 0)
   {
     return 0; // Convenient, even if not defined mathematically.
   }
   // Problem with this formula for large scale > 1000 or small x,
   //result = 0.5 * (erf(sqrt(scale / x) * ((x / mean) - 1) / constants::root_two<RealType>(), Policy()) + 1)
   //  + exp(2 * scale / mean) / 2
   //  * (1 - erf(sqrt(scale / x) * (x / mean + 1) / constants::root_two<RealType>(), Policy()));
   // so use normal distribution version:
   // Wikipedia CDF equation http://en.wikipedia.org/wiki/Inverse_Gaussian_distribution.

   normal_distribution<RealType> n01;

   RealType n0 = sqrt(scale / x);
   n0 *= ((x / mean) -1);
   RealType n1 = cdf(n01, n0);
   RealType expfactor = exp(2 * scale / mean);
   RealType n3 = - sqrt(scale / x);
   n3 *= (x / mean) + 1;
   RealType n4 = cdf(n01, n3);
   result = n1 + expfactor * n4;
   return result;
} // cdf

template <class RealType, class Policy>
struct inverse_gaussian_quantile_functor
{

  inverse_gaussian_quantile_functor(const boost::math::inverse_gaussian_distribution<RealType, Policy> dist, RealType const& p)
    : distribution(dist), prob(p)
  {
  }
  boost::math::tuple<RealType, RealType> operator()(RealType const& x)
  {
    RealType c = cdf(distribution, x);
    RealType fx = c - prob;  // Difference cdf - value - to minimize.
    RealType dx = pdf(distribution, x); // pdf is 1st derivative.
    // return both function evaluation difference f(x) and 1st derivative f'(x).
    return boost::math::make_tuple(fx, dx);
  }
  private:
  const boost::math::inverse_gaussian_distribution<RealType, Policy> distribution;
  RealType prob;
};

template <class RealType, class Policy>
struct inverse_gaussian_quantile_complement_functor
{
    inverse_gaussian_quantile_complement_functor(const boost::math::inverse_gaussian_distribution<RealType, Policy> dist, RealType const& p)
    : distribution(dist), prob(p)
  {
  }
  boost::math::tuple<RealType, RealType> operator()(RealType const& x)
  {
    RealType c = cdf(complement(distribution, x));
    RealType fx = c - prob;  // Difference cdf - value - to minimize.
    RealType dx = -pdf(distribution, x); // pdf is 1st derivative.
    // return both function evaluation difference f(x) and 1st derivative f'(x).
    //return std::tr1::make_tuple(fx, dx); if available.
    return boost::math::make_tuple(fx, dx);
  }
  private:
  const boost::math::inverse_gaussian_distribution<RealType, Policy> distribution;
  RealType prob;
};

namespace detail
{
  template <class RealType>
  inline RealType guess_ig(RealType p, RealType mu = 1, RealType lambda = 1)
  { // guess at random variate value x for inverse gaussian quantile.
      BOOST_MATH_STD_USING
      using boost::math::policies::policy;
      // Error type.
      using boost::math::policies::overflow_error;
      // Action.
      using boost::math::policies::ignore_error;

      typedef policy<
        overflow_error<ignore_error> // Ignore overflow (return infinity)
      > no_overthrow_policy;

    RealType x; // result is guess at random variate value x.
    RealType phi = lambda / mu;
    if (phi > 2.)
    { // Big phi, so starting to look like normal Gaussian distribution.
      //    x=(qnorm(p,0,1,true,false) - 0.5 * sqrt(mu/lambda)) / sqrt(lambda/mu);
      // Whitmore, G.A. and Yalovsky, M.
      // A normalising logarithmic transformation for inverse Gaussian random variables,
      // Technometrics 20-2, 207-208 (1978), but using expression from
      // V Seshadri, Inverse Gaussian distribution (1998) ISBN 0387 98618 9, page 6.

      normal_distribution<RealType, no_overthrow_policy> n01;
      x = mu * exp(quantile(n01, p) / sqrt(phi) - 1/(2 * phi));
     }
    else
    { // phi < 2 so much less symmetrical with long tail,
      // so use gamma distribution as an approximation.
      using boost::math::gamma_distribution;

      // Define the distribution, using gamma_nooverflow:
      typedef gamma_distribution<RealType, no_overthrow_policy> gamma_nooverflow;

      gamma_distribution<RealType, no_overthrow_policy> g(static_cast<RealType>(0.5), static_cast<RealType>(1.));

      // gamma_nooverflow g(static_cast<RealType>(0.5), static_cast<RealType>(1.));
      // R qgamma(0.2, 0.5, 1)  0.0320923
      RealType qg = quantile(complement(g, p));
      //RealType qg1 = qgamma(1.- p, 0.5, 1.0, true, false);
      x = lambda / (qg * 2);
      //
      if (x > mu/2) // x > mu /2?
      { // x too large for the gamma approximation to work well.
        //x = qgamma(p, 0.5, 1.0); // qgamma(0.270614, 0.5, 1) = 0.05983807
        RealType q = quantile(g, p);
       // x = mu * exp(q * static_cast<RealType>(0.1));  // Said to improve at high p
       // x = mu * x;  // Improves at high p?
        x = mu * exp(q / sqrt(phi) - 1/(2 * phi));
      }
    }
    return x;
  }  // guess_ig
} // namespace detail

template <class RealType, class Policy>
inline RealType quantile(const inverse_gaussian_distribution<RealType, Policy>& dist, const RealType& p)
{
   BOOST_MATH_STD_USING  // for ADL of std functions.
   // No closed form exists so guess and use Newton Raphson iteration.

   RealType mean = dist.mean();
   RealType scale = dist.scale();
   static const char* function = "boost::math::quantile(const inverse_gaussian_distribution<%1%>&, %1%)";

   RealType result = 0;
   if(false == detail::check_scale(function, scale, &result, Policy()))
      return result;
   if(false == detail::check_location(function, mean, &result, Policy()))
      return result;
   if(false == detail::check_probability(function, p, &result, Policy()))
      return result;
   if (p == 0)
   {
     return 0; // Convenient, even if not defined mathematically?
   }
   if (p == 1)
   { // overflow
      result = policies::raise_overflow_error<RealType>(function,
        "probability parameter is 1, but must be < 1!", Policy());
      return result; // std::numeric_limits<RealType>::infinity();
   }

  RealType guess = detail::guess_ig(p, dist.mean(), dist.scale());
  using boost::math::tools::max_value;

  RealType min = 0.; // Minimum possible value is bottom of range of distribution.
  RealType max = max_value<RealType>();// Maximum possible value is top of range.
  // int digits = std::numeric_limits<RealType>::digits; // Maximum possible binary digits accuracy for type T.
  // digits used to control how accurate to try to make the result.
  // To allow user to control accuracy versus speed,
  int get_digits = policies::digits<RealType, Policy>();// get digits from policy,
  boost::uintmax_t m = policies::get_max_root_iterations<Policy>(); // and max iterations.
  using boost::math::tools::newton_raphson_iterate;
  result =
    newton_raphson_iterate(inverse_gaussian_quantile_functor<RealType, Policy>(dist, p), guess, min, max, get_digits, m);
   return result;
} // quantile

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<inverse_gaussian_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING  // for ADL of std functions.

   RealType scale = c.dist.scale();
   RealType mean = c.dist.mean();
   RealType x = c.param;
   static const char* function = "boost::math::cdf(const complement(inverse_gaussian_distribution<%1%>&), %1%)";
   // infinite arguments not supported.
   //if((boost::math::isinf)(x))
   //{
   //  if(x < 0) return 1; // cdf complement -infinity is unity.
   //  return 0; // cdf complement +infinity is zero
   //}
   // These produce MSVC 4127 warnings, so the above used instead.
   //if(std::numeric_limits<RealType>::has_infinity && x == std::numeric_limits<RealType>::infinity())
   //{ // cdf complement +infinity is zero.
   //  return 0;
   //}
   //if(std::numeric_limits<RealType>::has_infinity && x == -std::numeric_limits<RealType>::infinity())
   //{ // cdf complement -infinity is unity.
   //  return 1;
   //}
   RealType result = 0;
   if(false == detail::check_scale(function, scale, &result, Policy()))
      return result;
   if(false == detail::check_location(function, mean, &result, Policy()))
      return result;
   if(false == detail::check_positive_x(function, x, &result, Policy()))
      return result;

   normal_distribution<RealType> n01;
   RealType n0 = sqrt(scale / x);
   n0 *= ((x / mean) -1);
   RealType cdf_1 = cdf(complement(n01, n0));

   RealType expfactor = exp(2 * scale / mean);
   RealType n3 = - sqrt(scale / x);
   n3 *= (x / mean) + 1;

   //RealType n5 = +sqrt(scale/x) * ((x /mean) + 1); // note now positive sign.
   RealType n6 = cdf(complement(n01, +sqrt(scale/x) * ((x /mean) + 1)));
   // RealType n4 = cdf(n01, n3); // =
   result = cdf_1 - expfactor * n6;
   return result;
} // cdf complement

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<inverse_gaussian_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   RealType scale = c.dist.scale();
   RealType mean = c.dist.mean();
   static const char* function = "boost::math::quantile(const complement(inverse_gaussian_distribution<%1%>&), %1%)";
   RealType result = 0;
   if(false == detail::check_scale(function, scale, &result, Policy()))
      return result;
   if(false == detail::check_location(function, mean, &result, Policy()))
      return result;
   RealType q = c.param;
   if(false == detail::check_probability(function, q, &result, Policy()))
      return result;

   RealType guess = detail::guess_ig(q, mean, scale);
   // Complement.
   using boost::math::tools::max_value;

  RealType min = 0.; // Minimum possible value is bottom of range of distribution.
  RealType max = max_value<RealType>();// Maximum possible value is top of range.
  // int digits = std::numeric_limits<RealType>::digits; // Maximum possible binary digits accuracy for type T.
  // digits used to control how accurate to try to make the result.
  int get_digits = policies::digits<RealType, Policy>();
  boost::uintmax_t m = policies::get_max_root_iterations<Policy>();
  using boost::math::tools::newton_raphson_iterate;
  result =
    newton_raphson_iterate(inverse_gaussian_quantile_complement_functor<RealType, Policy>(c.dist, q), guess, min, max, get_digits, m);
   return result;
} // quantile

template <class RealType, class Policy>
inline RealType mean(const inverse_gaussian_distribution<RealType, Policy>& dist)
{ // aka mu
   return dist.mean();
}

template <class RealType, class Policy>
inline RealType scale(const inverse_gaussian_distribution<RealType, Policy>& dist)
{ // aka lambda
   return dist.scale();
}

template <class RealType, class Policy>
inline RealType shape(const inverse_gaussian_distribution<RealType, Policy>& dist)
{ // aka phi
   return dist.shape();
}

template <class RealType, class Policy>
inline RealType standard_deviation(const inverse_gaussian_distribution<RealType, Policy>& dist)
{
  BOOST_MATH_STD_USING
  RealType scale = dist.scale();
  RealType mean = dist.mean();
  RealType result = sqrt(mean * mean * mean / scale);
  return result;
}

template <class RealType, class Policy>
inline RealType mode(const inverse_gaussian_distribution<RealType, Policy>& dist)
{
  BOOST_MATH_STD_USING
  RealType scale = dist.scale();
  RealType  mean = dist.mean();
  RealType result = mean * (sqrt(1 + (9 * mean * mean)/(4 * scale * scale))
      - 3 * mean / (2 * scale));
  return result;
}

template <class RealType, class Policy>
inline RealType skewness(const inverse_gaussian_distribution<RealType, Policy>& dist)
{
  BOOST_MATH_STD_USING
  RealType scale = dist.scale();
  RealType  mean = dist.mean();
  RealType result = 3 * sqrt(mean/scale);
  return result;
}

template <class RealType, class Policy>
inline RealType kurtosis(const inverse_gaussian_distribution<RealType, Policy>& dist)
{
  RealType scale = dist.scale();
  RealType  mean = dist.mean();
  RealType result = 15 * mean / scale -3;
  return result;
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const inverse_gaussian_distribution<RealType, Policy>& dist)
{
  RealType scale = dist.scale();
  RealType  mean = dist.mean();
  RealType result = 15 * mean / scale;
  return result;
}

} // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_INVERSE_GAUSSIAN_HPP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 polynom toolbox - polyvalm/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of polynom components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 06/03/2011
///
#include <nt2/include/functions/polyvalm.hpp>
#include <nt2/include/functions/eye.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/functions/isequal.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/table.hpp>

NT2_TEST_CASE_TPL ( polyvalm_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::polyvalm;
  using nt2::tag::polyvalm_;
  nt2::table<T> p =  nt2::_(T(1), T(4));
  nt2::table<T> a =  nt2::eye(3, nt2::meta::as_<T>());
  NT2_DISPLAY(polyvalm(p, a));
  NT2_TEST(nt2::isequal(polyvalm(p, a), T(10)*a));
  nt2::table<T> b = T(15)*nt2::eye(2, nt2::meta::as_<T>());
  b(2) = b(3) = T(11);
  nt2::table<T> c =  nt2::ones(2, nt2::meta::as_<T>());
  NT2_TEST(nt2::isequal(polyvalm(p, c), b));
  NT2_DISPLAY(polyvalm(p, c));
  NT2_DISPLAY(b);
} // end of test for floating_


//
//  MatrixFactory.hpp
//  Eigen_test
//
//  Created by Emil Iliev on 17.10.19.
//  Copyright © 2019 Emil Iliev. All rights reserved.
//

#ifndef MatrixFactory_hpp
#define MatrixFactory_hpp

#include <stdio.h>
#include <Eigen/Dense>
#define PI 3.14159265
#define CONVERT_TO_RAD(x) x*(M_PI/180)

class MatrixFactory {
    MatrixFactory() {}

    static MatrixFactory* _instance;
public:

/***********************************************************************
    Rotation matrix
***********************************************************************/

    Eigen::Matrix4f createRotationMatrixAroundZ(float alpha );
    Eigen::Matrix4f createRotationMatrixAroundY(float beta );
    Eigen::Matrix4f createRotationMatrixAroundX(float gamma );

/************************************************************************/
/* Translation matrix                                                   */
/************************************************************************/

    Eigen::Matrix4f createTranslateMatrixX(float dx );
    Eigen::Matrix4f createTranslateMatrixY(float dy );
    Eigen::Matrix4f createTranslateMatrixZ(float dz );

/************************************************************************/
/* Transformation matrix from frame i to i-1                            */
/************************************************************************/

    Eigen::Matrix4f calculateHTranslationMatrix(float alpha, float a, float d, float theta);

    /************************************************************************/
/* Help functions                                                       */
/************************************************************************/
    Eigen::Matrix3f extractRotationMatrix(Eigen::Matrix4f & hm );
    Eigen::Vector3f extractTranslationVector (Eigen::Matrix4f & hm );
    Eigen::Vector3f multiplyVectors(Eigen::Vector3f& vec_one ,Eigen::Vector3f& vec_two);
    float getLengthOfVector(Eigen::VectorXf& invec);
/************************************************************************/
/* Full calculation algo                                                */
/************************************************************************/

    static MatrixFactory* getInstance();
};

#endif /* MatrixFactory_hpp */


// File: geometry.cpp
// Project: lib
// Created Date: 08/06/2016
// Author: Seki Inoue
// -----
// Last Modified: 27/12/2020
// Modified By: Shun Suzuki (suzuki@hapis.k.u-tokyo.ac.jp)
// -----
// Copyright (c) 2016-2020 Hapis Lab. All rights reserved.
//

#if WIN32
#include <codeanalysis/warnings.h>  // NOLINT
#pragma warning(push)
#pragma warning(disable : ALL_CODE_ANALYSIS_WARNINGS)
#endif
#include <Eigen/Geometry>
#if WIN32
#pragma warning(pop)
#endif

#include <map>

#include "autd3.hpp"
#include "autd_logic.hpp"
#include "geometry.hpp"

using autd::IsMissingTransducer;
using autd::NUM_TRANS_IN_UNIT;
using autd::NUM_TRANS_X;
using autd::NUM_TRANS_Y;
using autd::TRANS_SIZE_MM;

namespace autd {
struct Device {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  static Device Create(const Vector3& position, const Quaternion& quaternion) {
    const Eigen::Transform<Float, 3, Eigen::Affine> transform_matrix = Eigen::Translation<Float, 3>(position) * quaternion;
    const auto x_direction = quaternion * Vector3(1, 0, 0);
    const auto y_direction = quaternion * Vector3(0, 1, 0);
    const auto z_direction = quaternion * Vector3(0, 0, 1);

    Eigen::Matrix<Float, 3, NUM_TRANS_IN_UNIT> local_trans_positions;

    auto index = 0;
    for (size_t y = 0; y < NUM_TRANS_Y; y++)
      for (size_t x = 0; x < NUM_TRANS_X; x++)
        if (!IsMissingTransducer(x, y))
          local_trans_positions.col(index++) = Vector3(static_cast<Float>(x) * TRANS_SIZE_MM, static_cast<Float>(y) * TRANS_SIZE_MM, 0);

    const auto global_trans_positions = transform_matrix * local_trans_positions;

    return Device{x_direction, y_direction, z_direction, global_trans_positions};
  }

  static Device Create(const Vector3& position, const Vector3& euler_angles) {
    const auto quaternion = Eigen::AngleAxis<Float>(euler_angles.x(), Vector3::UnitZ()) *
                            Eigen::AngleAxis<Float>(euler_angles.y(), Vector3::UnitY()) * Eigen::AngleAxis<Float>(euler_angles.z(), Vector3::UnitZ());

    return Create(position, quaternion);
  }

  Vector3 x_direction;
  Vector3 y_direction;
  Vector3 z_direction;
  Eigen::Matrix<Float, 3, NUM_TRANS_IN_UNIT> global_trans_positions;
};

class AUTDGeometry final : public Geometry {
 public:
  AUTDGeometry() : _wavelength(8.5) {}
  ~AUTDGeometry() override = default;
  AUTDGeometry(const AUTDGeometry& v) noexcept = default;
  AUTDGeometry& operator=(const AUTDGeometry& obj) = default;
  AUTDGeometry(AUTDGeometry&& obj) = default;
  AUTDGeometry& operator=(AUTDGeometry&& obj) = default;

  size_t AddDevice(Vector3 position, Vector3 euler_angles, size_t group = 0) override;
  size_t AddDeviceQuaternion(Vector3 position, Quaternion quaternion, size_t group = 0) override;

  Float wavelength() noexcept override;
  void set_wavelength(Float wavelength) noexcept override;

  size_t num_devices() noexcept override;
  size_t num_transducers() noexcept override;
  size_t group_id_for_device_idx(size_t device_idx) override;
  Vector3 position(size_t global_transducer_idx) override;
  Vector3 position(size_t device, size_t local_transducer_idx) override;
  Vector3 local_position(size_t device_idx, Vector3 global_position) override;

  Vector3 direction(size_t device_idx) override;
  Vector3 x_direction(size_t device_idx) override;
  Vector3 y_direction(size_t device_idx) override;
  Vector3 z_direction(size_t device_idx) override;
  size_t device_idx_for_trans_idx(size_t transducer_idx) override;

 private:
  std::vector<Device> _devices;
  std::map<size_t, size_t> _group_map;
  Float _wavelength;
};

GeometryPtr Geometry::Create() { return std::make_shared<AUTDGeometry>(); }

size_t AUTDGeometry::AddDevice(const Vector3 position, const Vector3 euler_angles, const size_t group) {
  const auto device_id = this->_devices.size();
  this->_devices.emplace_back(Device::Create(position, euler_angles));
  this->_group_map[device_id] = group;
  return device_id;
}

size_t AUTDGeometry::AddDeviceQuaternion(const Vector3 position, const Quaternion quaternion, const size_t group) {
  const auto device_id = this->_devices.size();
  this->_devices.emplace_back(Device::Create(position, quaternion));
  this->_group_map[device_id] = group;
  return device_id;
}

Float AUTDGeometry::wavelength() noexcept { return this->_wavelength; }
void AUTDGeometry::set_wavelength(const Float wavelength) noexcept { this->_wavelength = wavelength; }

size_t AUTDGeometry::num_devices() noexcept { return this->_devices.size(); }

size_t AUTDGeometry::num_transducers() noexcept { return this->num_devices() * NUM_TRANS_IN_UNIT; }

size_t AUTDGeometry::group_id_for_device_idx(const size_t device_idx) { return this->_group_map[device_idx]; }

Vector3 AUTDGeometry::position(const size_t global_transducer_idx) {
  const auto local_trans_id = global_transducer_idx % NUM_TRANS_IN_UNIT;
  return position(this->device_idx_for_trans_idx(global_transducer_idx), local_trans_id);
}

Vector3 AUTDGeometry::position(const size_t device, const size_t local_transducer_idx) {
  const auto& dev = this->_devices[device];
  return dev.global_trans_positions.col(local_transducer_idx);
}

Vector3 AUTDGeometry::local_position(const size_t device_idx, const Vector3 global_position) {
  const auto& device = this->_devices[device_idx];
  const auto& local_origin = device.global_trans_positions.col(0);
  const auto& x_dir = device.x_direction;
  const auto& y_dir = device.y_direction;
  const auto& z_dir = device.z_direction;
  const auto rv = global_position - local_origin;
  return Vector3(rv.dot(x_dir), rv.dot(y_dir), rv.dot(z_dir));
}

Vector3 AUTDGeometry::direction(const size_t device_idx) { return z_direction(device_idx); }

Vector3 AUTDGeometry::x_direction(const size_t device_idx) {
  const auto& dir = this->_devices[device_idx].x_direction;
  return dir;
}

Vector3 AUTDGeometry::y_direction(const size_t device_idx) {
  const auto& dir = this->_devices[device_idx].x_direction;
  return dir;
}

Vector3 AUTDGeometry::z_direction(const size_t device_idx) {
  const auto& dir = this->_devices[device_idx].x_direction;
  return dir;
}

size_t AUTDGeometry::device_idx_for_trans_idx(const size_t transducer_idx) { return transducer_idx / NUM_TRANS_IN_UNIT; }
}  // namespace autd


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#define BOOST_SYSTEM_NO_DEPRECATED
#define BOOST_FILESYSTEM_NO_DEPRECATED
#include "boost/filesystem.hpp"
#include "boost/program_options.hpp"

#include "Molassembler/Shapes/Data.h"
#include "Molassembler/Shapes/ContinuousMeasures.h"
#include "Molassembler/Shapes/InertialMoments.h"
#include "Molassembler/Shapes/Diophantine.h"

#include "Molassembler/Temple/Adaptors/Iota.h"
#include "Molassembler/Temple/Functional.h"
#include "Molassembler/Temple/Stringify.h"
#include "Molassembler/Temple/constexpr/Jsf.h"
#include "Molassembler/Temple/constexpr/Numeric.h"

#include <Eigen/Core>
#include <random>
#include <iostream>
#include <iomanip>

using namespace Scine;
using namespace Molassembler;
using namespace Shapes;

template<typename PRNG>
Eigen::Vector3d randomVectorOnSphere(const double radius, PRNG& prng) {
  std::normal_distribution<double> normal {};
  Eigen::Vector3d v = Eigen::Vector3d::Zero();

  while(v.norm() < 0.01) {
    v << normal(prng),
         normal(prng),
         normal(prng);
  }

  return radius * v / v.norm();
}

template<typename PRNG>
Eigen::Vector3d randomVectorInSphere(const double radius, PRNG& prng) {
  std::uniform_real_distribution<double> uniform {};
  std::normal_distribution<double> normal {};
  const double u = std::cbrt(uniform(prng));
  Eigen::Vector3d v = Eigen::Vector3d::Zero();

  while(v.norm() < 0.01) {
    v << normal(prng),
         normal(prng),
         normal(prng);
  }

  return radius * u * v / v.norm();
}

template<typename PRNG>
Eigen::Vector3d normallyDistributedVectorInSphere(const double radius, PRNG& prng) {
  std::normal_distribution<double> radiusDistribution {1.0, 0.2};
  std::normal_distribution<double> normal {};
  Eigen::Vector3d v = Eigen::Vector3d::Zero();

  while(v.norm() < 0.01) {
    v << normal(prng),
         normal(prng),
         normal(prng);
  }

  return radius * (1 + radiusDistribution(prng)) * v / v.norm();
}

template<typename PRNG>
Eigen::Matrix<double, 3, Eigen::Dynamic> generateCoordinates(unsigned P, PRNG& prng) {
  Eigen::Matrix<double, 3, Eigen::Dynamic> positions(3, P + 1);
  positions.col(0) = Eigen::Vector3d::Zero();
  for(unsigned i = 1; i <= P; ++i) {
    positions.col(i) = normallyDistributedVectorInSphere(1.0, prng);
  }
  return positions;
}

constexpr unsigned nExperiments = 1000;

template<typename PRNG, typename F>
std::vector<double> averageRandomCsm(const unsigned N, PRNG& prng, F&& f) {
  assert(N >= 2);
  return Temple::map(
    Temple::Adaptors::range(nExperiments),
    [&](unsigned /* i */) -> double {
      auto normalized = Continuous::normalize(generateCoordinates(N, prng));
      Top top = standardizeTop(normalized);
      if(top == Top::Asymmetric) {
        reorientAsymmetricTop(normalized);
      }
      return f(normalized);
    }
  );
}

std::ostream& operator << (std::ostream& os, const std::vector<double>& values) {
  const auto end = std::end(values);
  for(auto it = std::begin(values); it != end; ++it) {
    os << *it;
    if(it != end - 1) {
      os << ", ";
    }
  }

  return os;
}

/* Does it make more sense to just maximize the CSM using two parameters at
 * each point and use LBFGS with numerical derivatives? More principled than
 * many random points, no guarantee maximum CSM is actually found.
 *
 * Procedure for finding upper bounds on the CSM for which clear distinctions
 * can be made between unrelated point groups (unrelated meaning neither is a
 * subset of the other).
 * - Generate points P for which CSM(P, D3h) = 0, and points Q for which CSM(Q, Oh) = 0.
 * - Add uniform direction fixed magnitude (fuzz) distortions to P and Q, making P' and Q'
 * - Find the maximum CSM(P', D3h) and maximum CSM(Q', Oh)
 * - Find the minimum CSM(Q', D3h) and minimum CSM(P', Oh)
 * - If the maximum CSM(P', D3h) < minimum CSM(Q', D3h)
 *   and the maximum CSM(Q', Oh) < minimum CSM(P', Oh),
 *   increase the fuzz magnitude
 *
 * To find the maximum and minimum CSM for a particular fuzziness of a set of
 * group symmetric points by numerical optimization:
 * - Generate A-symmetric points: Go through each solution of the diophantine
 *   for the usable group sizes and the required number of points (exclude
 *   groups whose probe point is the origin):
 *   - If only a single group size has multiplier > 0, generate group-symmetric
 *     points by applying a symmetry element from each group to the normalized
 *     probe point.
 *   - If there are multiple group sizes with multiplier > 0, the norms of each
 *     probe point are meta-parameters that need to be optimized too.
 */

struct RScriptWriter {
  std::ofstream file;

  RScriptWriter(const std::string& str) : file(str) {
    writeHeader();
  }

  void writeHeader() {
    file << "symmetryNames <- c(\"" << Temple::condense(
      Temple::map(Shapes::allShapes, [](auto name) { return Shapes::name(name); }),
      "\",\""
    ) << "\")\n";
    file << "symmetrySizes <- c(" << Temple::condense(
      Temple::map(Shapes::allShapes, [](auto name) { return Shapes::size(name); })
    ) << ")\n";
    file << "results <- array(numeric(), c(" << Shapes::allShapes.size() << ", " << nExperiments << "))\n";
  }

  void writeSeed(int seed) {
    file << "seed <- " << seed << "\n";
  }

  void addResults(const Shapes::Shape name, const std::vector<double>& results) {
    const unsigned symmetryIndex = nameIndex(name) + 1;
    file << "results[" << symmetryIndex << ",] <- c(" << results << ")\n";
  }

  template<typename F, typename PRNG>
  void addElementArray(const std::string& nameBase, F&& f, PRNG&& prng) {
    file << std::scientific;
    file << nameBase << "Array <- array(numeric(), c(7, " << nExperiments << "))\n";

    std::array<int, 7> seeds;
    for(unsigned i = 0; i < 7; ++i) {
      seeds[i] = prng();
    }

#pragma omp parallel for
    for(unsigned N = 2; N <= 8; ++N) {
      Temple::JSF64 localPrng {seeds.at(N - 2)};
      const auto values = averageRandomCsm(N, localPrng, std::forward<F>(f));

#pragma omp critical
      {
        std::cout << "CSM(" << nameBase << ", " << N << ") = " << Temple::average(values) << " +- " << Temple::stddev(values) << "\n";
        file << nameBase << "Array[" << (N - 1) << ",] <- c(" << values << ")\n";
      }
    }

    file << "elementArrays[[\"" << nameBase << "\"]] <- " << nameBase << "Array)\n";
  }
};

int main(int argc, char* argv[]) {
  bool showElements = false;
  /* Set up program options */
  boost::program_options::options_description options_description("Recognized options");
  options_description.add_options()
    ("help,h", "Produce help message")
    (
      "seed,s",
      boost::program_options::value<int>(),
      "Seed to initialize PRNG with."
    )
    (
      "elements,e",
      boost::program_options::bool_switch(&showElements),
      "Show element CSM statistics instead of point groups"
    )
  ;

  /* Parse */
  boost::program_options::variables_map options_variables_map;
  boost::program_options::store(
    boost::program_options::command_line_parser(argc, argv).
    options(options_description).
    style(
      boost::program_options::command_line_style::unix_style
      | boost::program_options::command_line_style::allow_long_disguise
    ).run(),
    options_variables_map
  );
  boost::program_options::notify(options_variables_map);

  if(options_variables_map.count("help") > 0) {
    std::cout << options_description << "\n";
    return 0;
  }

  RScriptWriter writer {
    showElements ? "elements.R" : "point_groups_data.R"
  };
  Temple::JSF64 prng;
  if(options_variables_map.count("seed") > 0) {
    const int seed = options_variables_map["seed"].as<int>();
    prng.seed(seed);
    writer.writeSeed(seed);
    std::cout << "PRNG seeded from parameters: " << seed << ".\n";
  } else {
    std::random_device randomDevice;
    const int seed = std::random_device {}();
    std::cout << "PRNG seeded from random_device: " << seed << ".\n";
    prng.seed(seed);
    writer.writeSeed(seed);
  }

  writer.file << std::scientific;

  if(showElements) {
    writer.file << "elementArrays <- list()\n";

    /* Inversion */
    writer.addElementArray(
      "inversion",
      [](const Continuous::PositionCollection& positions) -> double {
        return Continuous::element(positions, Elements::Inversion {});
      },
      prng
    );

    /* Cinf */
    writer.addElementArray(
      "Cinf",
      [](const Continuous::PositionCollection& positions) -> double {
        return Continuous::Cinf(positions);
      },
      prng
    );

    /* Sigma */
    writer.addElementArray(
      "sigma",
      [](const Continuous::PositionCollection& positions) -> double {
        return Continuous::element(positions, Elements::Reflection {Eigen::Vector3d::UnitZ()}).first;
      },
      prng
    );

    /* Cn axes */
    for(unsigned order = 2; order <= 8; ++order) {
      writer.addElementArray(
        "C" + std::to_string(order),
        [order](const Continuous::PositionCollection& positions) -> double {
          return Continuous::element(positions,
            Elements::Rotation::Cn(Eigen::Vector3d::UnitZ(), order)
          ).first;
        },
        prng
      );
    }

    /* Sn axes */
    for(unsigned order = 4; order <= 8; order += 2) {
      writer.addElementArray(
        "S" + std::to_string(order),
        [order](const Continuous::PositionCollection& positions) -> double {
          return Continuous::element(positions,
            Elements::Rotation::Sn(Eigen::Vector3d::UnitZ(), order)
          ).first;
        },
        prng
      );
    }
  }

  if(!showElements) {
    std::cout << "Average CSM for uniform coordinates in sphere:\n";
    for(const Shape shape : allShapes) {
      const PointGroup group = pointGroup(shape);
      for(unsigned N = 2; N < 8; ++N) {
        /* Generate 100 random coordinates within a uniform sphere for each
         * symmetry and evaluate the CSM
         */
        const auto values = Temple::map(
          Temple::Adaptors::range(nExperiments),
          [&](unsigned /* i */) -> double {
            auto normalized = Continuous::normalize(generateCoordinates(N, prng));
            Top top = standardizeTop(normalized);
            if(top == Top::Asymmetric) {
              reorientAsymmetricTop(normalized);
            }
            return Continuous::pointGroup(normalized, group);
          }
        );
        const double csmAverage = Temple::average(values);
        const double csmStddev = Temple::stddev(values);

        writer.addResults(shape, values);
        std::cout << name(shape) << " - " << N << ": " << csmAverage << " +- " << csmStddev << "\n";
      }
    }
  }

  return 0;
}


/// HEADER
#include "path_follower/utils/extended_kalman_filter.h"

/// SYSTEM
#include <Eigen/Dense>
#include <ros/console.h>

#include<tf/tf.h>
#include<ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>

namespace {

double angleDiff(const double a1, const double a2)
{
    return std::atan2(std::sin(a1 - a2), std::cos(a1 - a2)) ;
}

}

EKF::EKF()
{
    x_ = Eigen::Matrix<double, 6, 1>::Zero();

    x_(0,0) = 0;
    x_(1,0) = 0;
    x_(2,0) = 0;
    x_(3,0) = -0.45;
    x_(4,0) = 0.45;
    x_(5,0) = -0.004;

    F = Eigen::Matrix<double, 6, 6>::Zero();
    P = Eigen::Matrix<double, 6, 6>::Identity();
    R = Eigen::Matrix<double, 3, 3>::Zero();
    Q = Eigen::Matrix<double, 6, 6>::Zero();

    R(0,0) = pow(0.01, 2);
    R(1,1) = pow(0.01, 2);
    R(2,2) = pow(M_PI/180, 2);

    Q(0,0) = pow(0.1, 2);
    Q(1,1) = pow(0.1, 2);
    Q(2,2) = pow(5*M_PI/180, 2);
    Q(3,3) = pow(0.001, 2);
    Q(4,4) = pow(0.001, 2);
    Q(5,5) = pow(0.001, 2);

}

void EKF::reset()
{
    x_ = Eigen::Matrix<double, 6, 1>::Zero();
    F = Eigen::Matrix<double, 6, 6>::Zero();
    P = Eigen::Matrix<double, 6, 6>::Identity();
}

void EKF::predict(const std_msgs::Float64MultiArray::ConstPtr& array, double dt)
{

    double flw = array->data[0];
    double frw = array->data[1];
    double brw = array->data[2];
    double blw = array->data[3];

    double Vl = (flw + blw)/2.0;
    double Vr = (frw + brw)/2.0;

    double theta = x_(2,0);
    double y_ICRr = x_(3,0);
    double y_ICRl = x_(4,0);
    double x_ICR = x_(5,0);

    double vx = (Vr*y_ICRl - Vl*y_ICRr)/(y_ICRl - y_ICRr);
    double vy = x_ICR*(Vl-Vr)/std::abs(y_ICRl - y_ICRr);
    double omega = -(Vl - Vr)/std::abs(y_ICRl - y_ICRr);

    x_(0,0) = x_(0,0) + dt*(vx*std::cos(theta) - vy*std::sin(theta));
    x_(1,0) = x_(1,0) + dt*(vy*std::cos(theta) + vx*std::sin(theta));
    x_(2,0) = theta + dt*omega;
    x_(3,0) = y_ICRr;
    x_(4,0) = y_ICRl;
    x_(5,0) = x_ICR;

    Eigen::Matrix<double, 6, 6> L;
    L = dt*Eigen::Matrix<double, 6, 6>::Identity();

    F.block(0,0,3,3) = Eigen::Matrix<double, 3, 3>::Identity();
    F(0,2) = dt*(-vx*std::sin(theta) - vy*std::cos(theta));
    F(1,2) = dt*(vx*std::cos(theta) - vy*std::sin(theta));
    F.block(3,0,3,3) = Eigen::Matrix<double, 3, 3>::Zero();
    F.block(3,3,3,3) = Eigen::Matrix<double, 3, 3>::Identity();
    F(0,3) = dt*((y_ICRl*(Vr-Vl)/(pow((y_ICRl-y_ICRr),2)))*std::cos(theta)
                 - (x_ICR*(Vl-Vr)/(pow(y_ICRl-y_ICRr,2)))*std::sin(theta));
    F(0,4) = dt*((y_ICRr*(Vl-Vr)/(pow((y_ICRl-y_ICRr),2)))*std::cos(theta)
                 + (x_ICR*(Vl-Vr)/(pow((y_ICRl-y_ICRr),2)))*std::sin(theta));
    F(0,5) = -dt*((Vl-Vr)/(y_ICRl-y_ICRr))*std::sin(theta);
    F(1,3) = dt*((y_ICRl*(Vr-Vl)/(pow((y_ICRl-y_ICRr),2)))*std::sin(theta)
                 + (x_ICR*(Vl-Vr)/(pow((y_ICRl-y_ICRr),2)))*std::cos(theta));
    F(1,4) = dt*((y_ICRr*(Vl-Vr)/(pow((y_ICRl-y_ICRr),2)))*std::sin(theta)
                 - (x_ICR*(Vl-Vr)/(pow((y_ICRl-y_ICRr),2)))*std::cos(theta));
    F(1,5) = dt*((Vl-Vr)/(y_ICRl-y_ICRr))*std::cos(theta);
    F(2,3) = -dt*(Vl-Vr)/(pow((y_ICRl-y_ICRr),2));
    F(2,4) = dt*(Vl-Vr)/(pow((y_ICRl-y_ICRr),2));
    F(2,5) = 0;

    P = F*P*F.transpose() + L*Q*L.transpose();
}

void EKF::correct(const Eigen::Vector3d& delta)
{

    Eigen::MatrixXd M = Eigen::Matrix<double, 3,3>::Identity();

    Eigen::Matrix<double, 3, 6> H;
    H.block(0,0,3,3) = M;
    H.block(0,3,3,3) = Eigen::Matrix<double, 3, 3>::Zero();

    Eigen::Matrix<double, 6, 3> K;
    Eigen::Matrix<double, 3, 3> T;

    T = H*P*H.transpose() + M*R*M.transpose();

    K = P*H.transpose()*T.inverse();

    Eigen::Vector3d innovation = delta - x_.block(0,0,3,1);

    innovation(2) = angleDiff(delta(2), x_(2));

    x_ += K*innovation;

    Eigen::Matrix<double, 6, 6> I = Eigen::Matrix<double, 6, 6>::Identity();

    P = (I - K*H)*P;
}


#include <cassert>
#include <cstdlib>
#include <iostream>

#include <boost/smart_ptr/scoped_ptr.hpp>

#include <BnSimulator/core/BooleanNetwork.hpp>
#include <BnSimulator/core/Trajectory.hpp>
#include <BnSimulator/core/Attractor.hpp>
#include <BnSimulator/experiment/TrajectoryRunner.hpp>

int main(int argc, char **argv) {
	using namespace bn;
	// network and parameter initialization
	const uint maxSteps = std::atoi(argv[4]);
	BooleanNetwork net = BooleanNetwork::makeNetwork(std::atoi(argv[1]),
			argv[2], argv[3]);
	std::srand(std::atoi(argv[5]));
	// experiment initialization
	TrajectoryRunner runner(maxSteps);
	// run experiment
	boost::scoped_ptr<const Trajectory> t(runner.findAttractor(net));
	// post-process results
	std::cout << *t << std::endl; // print the whole trajectory
	std::cout << "Cycle:\n" << t->printCycle(); // print only the cycle
	if (t->cycleLength() > 0) {
		const Attractor a(*t);
		std::cout << "Attractor representant:\n" << a << std::endl;
		std::cout << "Attractor length: " << a.getLength() << std::endl;
		boost::scoped_ptr<ExplicitAttractor> ax(new ExplicitAttractor(*t));
		std::cout << "Attractor states:\n" << *ax;
		t.reset(runner.findAttractor(net, a.getRepresentant()));
		assert(t->getTransient().empty());
		assert(t->getCycle().size() == a.getLength());
		std::cout << "Cycle:\n" << t->printCycle();
	}
	return EXIT_SUCCESS;
}


#pragma once

#include <cmath>
#include <ros/ros.h>
#include <ct/optcon/optcon.h>
#include <lqr_controller/declarations_euler.hpp>
#include <lqr_controller/quadModelParameters.hpp>
#include <nav_msgs/Odometry.h>
#include <Eigen/Geometry>
#include <mavros/frame_tf.h>
#include <mav_msgs/eigen_mav_msgs.h>
#include <lqr_controller/trajectory.hpp>
#include <mav_trajectory_generation/polynomial_optimization_linear.h>
#include <mav_trajectory_generation/polynomial_optimization_nonlinear.h>
#include <mav_trajectory_generation/trajectory.h>
#include <mav_trajectory_generation/trajectory_sampling.h>
#include <mav_trajectory_generation_ros/ros_visualization.h>
#include <visualization_msgs/MarkerArray.h>
#include <mavros_msgs/RCIn.h>
#include <ros/package.h>

namespace lqr {
class LQR_Euler {
  public:
    /*!
     * Constructor.
     * @param nodeHandle the ROS node handle.
     */
    LQR_Euler(ros::NodeHandle& nodeHandle);

    /*!
     * Destructor.
     */
    virtual ~LQR_Euler();

    control_vector_t getTrajectoryControl();
    state_vector_t getError();
    ct::core::FeedbackMatrix<nStates, nControls> getGain();
    void setOutput(control_vector_t output);
    control_vector_t getOutput();
    state_vector_t getRefStates();
    double getMotorCmd();

   private:

    double k1_; // quadratic first coefficient
    double k2_; // quadratic second coefficient
    double k3_; // quadratic third coefficient

    /*!
     * ROS topic callback method.
     * @param message the received message.
     */
    void setRCIn(const mavros_msgs::RCIn::ConstPtr& msg);
    void topicCallback(const nav_msgs::Odometry::ConstPtr& msg);
    void setStates(const nav_msgs::Odometry::ConstPtr& msg, state_vector_t& x);
    void setError(const state_vector_t& xref, const state_vector_t& x, state_vector_t& xerror);
    bool setTrajectoryReference(state_vector_t& xref,control_vector_t& uref, int trajectory_type);
    bool setStaticReference(state_vector_t& xref,control_vector_t& uref,Eigen::Vector4d& flat_states);
    //Eigen::Vector3d quaternion_to_rpy_wrap(const Eigen::Quaterniond &q);
    void generateTrajectory(mav_msgs::EigenTrajectoryPoint::Vector& states, int trajectory_type);
    bool readParameters();
    //! ROS node handle.
    ros::NodeHandle& nodeHandle_;

    //! ROS topic subscriber.
    ros::Subscriber odom_sub_;
    ros::Subscriber RCIn_sub_;

    //Marker publisher
    ros::Publisher marker_pub_;

    //! State and control matrix dimensions
    const size_t state_dim = nStates;
    const size_t control_dim = nControls;

    //Trajectory
    double sampling_interval = 0.1;
    const double v_max = 6;
    const double a_max = 3;
    const int dimension = 3;
    int traj_index;
    bool ref_reached_ = false;
    bool initiated;
    std::vector<uint16_t> channels_;
    mav_msgs::EigenTrajectoryPoint::Vector states_;
    visualization_msgs::MarkerArray markers;
    bool closed_traj_;
    enum trajectory_type {POLYNOMIAL, CIRCLE};

    Eigen::Vector3d position_enu_;
    Eigen::Vector3d velocity_enu_;
    Eigen::Quaterniond q_enu_;
    Eigen::Quaterniond q_ned_;
    state_matrix_t A_;
    control_gain_matrix_t B_;
    ct::core::FeedbackMatrix<nStates, nControls> Kold_;
    ct::core::FeedbackMatrix<nStates, nControls> Knew_;
    ros::Time callBack_;
    double init_time_;
    state_vector_t x_;
    control_vector_t u_;
    state_vector_t xref_;
    control_vector_t uref_;
    state_vector_t xerror_;
    control_vector_t output_;

    ct::optcon::TermQuadratic<nStates, nControls> quadraticCost_;
    ct::optcon::TermQuadratic<nStates, nControls>::state_matrix_t Q_;
    ct::optcon::TermQuadratic<nStates, nControls>::control_matrix_t R_;
    ct::optcon::LQR<nStates, nControls> lqrSolver_;

    state_matrix_t A_quadrotor(const state_vector_t& x, const control_vector_t& u);
    control_gain_matrix_t B_quadrotor(const state_vector_t& x, const control_vector_t& u);
  };

} /* namespace */


#include <boost/test/unit_test.hpp>
#include <pose_estimation/GeographicProjection.hpp>
#include <Eigen/Core>
#include <ogr_spatialref.h>

using namespace pose_estimation;

BOOST_AUTO_TEST_CASE(test_coordinate_projection)
{
    // DFKI Bremen
    double latitude = 0.92698121;
    double longitude = 0.154595663;
    GeographicProjection projection(latitude, longitude);

    // test identity
    Eigen::Vector2d pos;
    BOOST_CHECK(projection.worldToNav(latitude, longitude, pos.x(), pos.y()));

    BOOST_CHECK(pos.x() == 0);
    BOOST_CHECK(pos.y() == 0);

    double latitude2, longitude2;
    BOOST_CHECK(projection.navToWorld(pos.x(), pos.y(), latitude2, longitude2));

    BOOST_CHECK(latitude2 == latitude);
    BOOST_CHECK(longitude2 == longitude);

    // test with pos offset
    GeographicProjection projection2(latitude, longitude, -500., 1234.);

    BOOST_CHECK(projection2.worldToNav(latitude, longitude, pos.x(), pos.y()));

    BOOST_CHECK(pos.x() == -500.);
    BOOST_CHECK(pos.y() == 1234.);

    // inverse
    BOOST_CHECK(projection2.navToWorld(pos.x(), pos.y(), latitude2, longitude2));

    BOOST_CHECK(latitude2 == latitude);
    BOOST_CHECK(longitude2 == longitude);

    // add 0.1 degree in gps frame
    Eigen::Vector2d pos2;
    BOOST_CHECK(projection2.worldToNav(latitude + 0.1, longitude + 0.1, pos2.x(), pos2.y()));

    BOOST_CHECK(pos2.x() > pos.x());
    BOOST_CHECK(pos2.y() < pos.y());

    // substract 10000m in nav frame
    BOOST_CHECK(projection2.navToWorld(pos.x() - 10000, pos.y() - 10000, latitude2, longitude2));

    BOOST_CHECK(latitude2 < latitude);
    BOOST_CHECK(longitude2 > longitude);
}


/*-------------determinant.cpp------------------------------------------------//
*
* Purpose: Simple multiplication to help visualize eigenvectors
*
*   Notes: compile with:
*            g++ -I /usr/include/eigen3/ eigentest.cpp -Wno-ignored-attributes -Wno-deprecated-declarations
*
*-----------------------------------------------------------------------------*/

#include <iostream>
#include <Eigen/Core>
#include <vector>
#include <fstream>

using namespace Eigen;

int main(){

    // Opening file to writing
    std::ofstream output;
    output.open("out.dat");

    int size = 2, count=0;

    // setting up positions of all the desired points to transform
    double x[size*size*size], y[size*size*size], z[size*size*size],
           x2[size*size*size], y2[size*size*size], z2[size*size*size],
           xval = -1, yval = -1, zval = -1;
    MatrixXd Arr(3, 3);
    MatrixXd pos(3, 1);

    // Putting in "random" values for matrix
    Arr << 1, 2, 0,
           2, 1, 0,
           0, 0, -3;
/*
    Arr << 1, 0, 0,
           0, 1, 0,
           0, 0, 1;
*/
    std::cout << Arr << '\n';

    // Creating initial x, y, and z locations
    for (int i = 0; i < size; ++i){
        for (int j = 0; j < size; ++j){
            for (int k = 0; k < size; ++k){
                xval = -1 + 2 * ((double)i / ((double)size - 1));
                yval = -1 + 2 * ((double)j / ((double)size - 1));
                zval = -1 + 2 * ((double)k / ((double)size - 1));
                x[count] = xval;
                y[count] = yval;
                z[count] = zval;

                // Performing multiplication / setting up vector
                pos(0) = xval;
                pos(1) = yval;
                pos(2) = zval;
                pos = Arr * pos;

                // Storing values in x2, y2, and z2
                x2[count] = pos(0);
                y2[count] = pos(1);
                z2[count] = pos(2);
                count += 1;
            }
        }
    }
    count = 8;
    for (int i = 0; i < count; ++i){
        std::cout << x[i] << '\t' << y[i] << '\t' << z[i] << '\n';
        pos(0) = x[i];
        pos(1) = y[i];
        pos(2) = z[i];

        pos = Arr * pos;

        // Storing values in x2, y2, and z2
        x2[i] = pos(0);
        y2[i] = pos(1);
        z2[i] = pos(2);

    }
/*

    // Creating initial x, y, and z locations
    for (int i = 0; i < size; ++i){
        for (int j = 0; j < size; ++j){
            for (int k = 0; k < size; ++k){
                xval = -1 + 2 * ((double)i / (double)size);
                yval = -1 + 2 * ((double)j / (double)size);
                zval = -1 + 2 * ((double)k / (double)size);
                x[count] = xval;
                y[count] = yval;
                z[count] = zval;

                // Performing multiplication / setting up vector
                pos(0) = xval;
                pos(1) = yval;
                pos(2) = zval;
                pos = Arr * pos;

                // Storing values in x2, y2, and z2
                x2[count] = pos(0);
                y2[count] = pos(1);
                z2[count] = pos(2);
                count += 1;
            }
        }
    }

    // Writing to file in correct format
    count = 0;
    for (int i = 0; i < size; ++i){
        for (int j = 0; j < size; ++j){
            for (int k = 0; k < size; ++k){
                output << x[count] << '\t' << y[count]
                       << '\t' << z[count] << '\t' << count << '\n';
                count++;
            }
        }
    }

    output << '\n' << '\n';

    count = 0;
    for (int i = 0; i < size; ++i){
        for (int j = 0; j < size; ++j){
            for (int k = 0; k < size; ++k){
                output << x2[count] << '\t' << y2[count]
                       << '\t' << z2[count] << '\t' << count << '\n';
                count++;
            }
        }
    }
*/

    int frames = 60;
    count = 0;
    double xvel[size*size*size], yvel[size*size*size], zvel[size*size*size];
    double vid_time = 1.0;

    count = 8;

    for (int i = 0; i < count; ++i){
        xvel[i] = (x[i] - x2[i]) / vid_time;
        yvel[i] = (y[i] - y2[i]) / vid_time;
        zvel[i] = (z[i] - z2[i]) / vid_time;
    }

    for (int f = 0; f < frames; ++f){
        for (int i = 0; i < count; ++i){
            xval = x[i] + ((x2[i] - x[i])
                   * ((double)f / (double)frames));
            yval = y[i] + ((y2[i] - y[i])
                   * ((double)f / (double)frames));
            zval = z[i] + ((z2[i] - z[i])
                   * ((double)f / (double)frames));

            output << xval << '\t' << yval
                   << '\t' << zval << '\t'
                   << xvel[i] << '\t' << yvel[i] << '\t'
                   << zvel[i] << '\t' << 1 << '\t' << i << '\n';

        }
        output << '\n' << '\n';
    }

    /*
    for (int i = 0; i < size; ++i){
        for (int j = 0; j < size; ++j){
            for (int k = 0; k < size; ++k){
                xvel[count] = (x[count] - x2[count]) / vid_time;
                yvel[count] = (y[count] - y2[count]) / vid_time;
                zvel[count] = (z[count] - z2[count]) / vid_time;
                count = count + 1;
            }
        }
    }

    for (int f = 0; f < frames; ++f){
        count = 0;
        for (int i = 0; i < size; ++i){
            for (int j = 0; j < size; ++j){
                for (int k = 0; k < size; ++k){
                    xval = x[count] + ((x2[count] - x[count])
                           * ((double)f / (double)frames));
                    yval = y[count] + ((y2[count] - y[count])
                           * ((double)f / (double)frames));
                    zval = z[count] + ((z2[count] - z[count])
                           * ((double)f / (double)frames));

                    output << xval << '\t' << yval
                           << '\t' << zval << '\t'
                           << xvel[count] << '\t' << yvel[count] << '\t'
                           << zvel[count] << '\t' << 1 << '\t' << count << '\n';
                    count++;
                }
            }
        }

        output << '\n' << '\n';
    }
    */

    output.close();

}


#include <geometry.h>
#include <tiny.h>

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

#include <vector>

typedef tiny::MathTypes<float> MT;
typedef MT::vector3_type       V;
typedef MT::quaternion_type    Q;
typedef MT::real_type          T;
typedef MT::value_traits       VT;

class ContactInfo
{
public:

  V m_point;
  V m_normal;
  T m_distance;

};

class MyCallback
  : public geometry::ContactsCallback<V>
{
public:

  std::vector<ContactInfo> m_contacts;

public:

  void operator()(
                  V const & point
                  , V const & normal
                  , typename V::real_type const & distance
                  )
  {
    ContactInfo info;

    info.m_point = point;
    info.m_normal = normal;
    info.m_distance = distance;

    m_contacts.push_back(info);
  }

};

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(contacts_obb_cylinder_test)
{
  {
    V const center    = V::make(0.0, 0.0, 0.0);
    V const half_ext  = V::make(1.0, 1.0, 1.0);
    Q const q         = Q::identity();

    V const center2   = V::zero();
    V const axis      = V::i();
    T const height    = 1.0;
    T const radius    = 1.0;

    geometry::OBB<MT>     const & obb = geometry::make_obb<MT>(center, q, half_ext);
    geometry::Cylinder<V> const & cyl = geometry::make_cylinder(radius, height, axis, center2);

    MyCallback mycallback;

    bool const test = geometry::contacts_obb_cylinder(obb, cyl, 0.0, mycallback, false );

    BOOST_CHECK(!test);
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include "headers/Reader.hpp"
#include "headers/Hittable.hpp"
#include "headers/Ambient.hpp"
#include "headers/Sphere.hpp"
#include "headers/Triangle.hpp"
#include <armadillo>
#include <vector>

using namespace arma;
using namespace std;

Reader::Reader(const char *txt, vector<Object *> &objetos, vector<Light> &luz, Ambient &ambiente, mat &keye, int &cont_tri){

  double cont, k, j, z[3], cont_obj=0;
  vec aux1, aux2;
  mat vertice, face;
  char tipo_objeto[2], tmp[32];
  float valor, valor1, valor2, valor3;
  char barra1[16], barra2[16], barra3[16];
  vector<Sphere *> bola;

  ifstream arq (txt, ios_base::in);

  arq >> cont;

  for (k=0; k<cont; k++){

      arq >> tipo_objeto;

      if (strcmp(tipo_objeto, "t")==0){

	  for (j=0;j<28;j++){

	    arq >> valor;
	    aux1 << valor;
	    aux2 = join_cols(aux2,aux1);
	  }

	  objetos.push_back(new Triangle(aux2));
	  aux2.reset(); cont_tri++;
	}

      else if (strcmp(tipo_objeto, "s")==0){

	  for (j=0;j<23;j++){

	    arq >> valor;
	    aux1 << valor;
	    aux2 = join_cols(aux2,aux1);
	  }

	  bola.push_back(new Sphere(aux2));
	  aux2.reset();
	}
    }

    arq >> cont;

    for (k=0; k<cont; k++){

	arq >> tipo_objeto;

	if (strcmp(tipo_objeto, "o")==0){

	    arq >> tmp;

	    for (j=0;j<19;j++){
		arq >> valor;
		aux1 << valor;
		aux2 = join_cols(aux2,aux1);
	    }

	    ifstream obj (tmp, ios_base::in);

	    while (obj){
		obj >> tipo_objeto;

		if (strcmp(tipo_objeto,"v")==0){
		    obj >> valor1 >> valor2 >> valor3;

		    aux1 << valor1 << valor2 << valor3;
		    vertice = join_cols(vertice,aux1.t());
		}

		else if (strcmp(tipo_objeto,"f")==0){
		    obj >> barra1 >> barra2 >> barra3;

		    z[0]=(atoi(barra1));  z[1]=(atoi(barra2));  z[2]=(atoi(barra3));
		    if (z[0]<0) { z[0] = z[0] * -1; }
		    if (z[1]<0) { z[1] = z[1] * -1; }
		    if (z[2]<0) { z[2] = z[2] * -1; }

		    aux1 << z[0] << z[1] << z[2];
		    face = join_cols(face,aux1.t()); cont_obj++;
		}
	      }
	    obj.close();

	    for (j=0;j<cont_obj;j++){
		aux1 << vertice(face(j,0)-1,0) << vertice(face(j,0)-1,1) << vertice(face(j,0)-1,2)
		     << vertice(face(j,1)-1,0) << vertice(face(j,1)-1,1) << vertice(face(j,1)-1,2)
		     << vertice(face(j,2)-1,0) << vertice(face(j,2)-1,1) << vertice(face(j,2)-1,2);
		aux1 = join_cols(aux1,aux2);

		objetos.push_back(new Triangle(aux1));
		cont_tri++;
	    }
	      vertice.reset(); face.reset(); cont_obj=0;
	}
	aux2.reset();
     }

  for(unsigned int a=0;a<bola.size();a++){
    objetos.push_back(bola[a]);
  }

  arq >> cont;

  for (k=0; k<cont; k++){
      for (j=0;j<18;j++){
	  arq >> valor;
	  aux1 << valor;
	  aux2 = join_cols(aux2,aux1);
      }

    luz.push_back(aux2);
    aux2.reset();
  }

  for (j=0;j<4;j++){
      arq >> valor;
      aux1 << valor;
      aux2 = join_cols(aux2,aux1);
  }

  aux1 << aux2(0) << aux2(1) << aux2(2);

  ambiente.SetKa(aux1);
  ambiente.SetIa(aux2(3));
  aux2.reset();

  for (j=0;j<9;j++){
      arq >> valor;
      aux1 << valor;
      aux2 = join_cols(aux2,aux1);
  }

  keye << aux2(2)/aux2(0) << aux2(1) 	     << aux2(2)/2 << endr
       << aux2(3) 	  << aux2(5)/aux2(4) << aux2(5)/2 << endr
       << aux2(6) 	  << aux2(7)         << aux2(8)   << endr;

  arq.close();

}


/**
 *  Copyright (C) 2012
 *    Ekaterina Potapova, Andreas Richtsfeld, Johann Prankl, Thomas Mörwald, Michael Zillich
 *    Automation and Control Institute
 *    Vienna University of Technology
 *    Gusshausstraße 25-29
 *    1170 Vienna, Austria
 *    ari(at)acin.tuwien.ac.at
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program 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 General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see http://www.gnu.org/licenses/
 */

/**
 * @file trainSVM.cpp
 * @author Andreas Richtsfeld, Ekaterina Potapova
 * @date January 2014
 * @version 0.1
 * @brief Trains svm based on scaled features.
 */

#include <stdio.h>      /* printf, scanf, puts, NULL */
#include <stdlib.h>     /* srand, rand */
#include <time.h>       /* time */
#include <fstream>

#include <pcl/io/pcd_io.h>

#include <boost/filesystem/fstream.hpp>
#include <boost/filesystem.hpp>

#include "v4r/attention_segmentation/SVMTrainModel.h"

void trainFeatures(double gamma, double C, int kernel, int n_folds, double &RecRate, std::vector<int> &ConfusionTable,
                   std::string &train_ST_file_name_scaled, std::string &model_file_name)
{
  svm::SVMTrainModel svmTrainModel;
  svmTrainModel.setInputFileName(train_ST_file_name_scaled);
  svmTrainModel.setModelFileName(model_file_name);
  svmTrainModel.setKernelType(kernel);
  svmTrainModel.setSVMType(svm::C_SVC);
  svmTrainModel.setGamma(gamma);
  svmTrainModel.setC(C);
  svmTrainModel.setProbability(1);
  svmTrainModel.setCrossValidation(n_folds);
  svmTrainModel.setNoPrint(true);
  svmTrainModel.train(RecRate,ConfusionTable);
}

void trainSVM(std::string &train_ST_file_name_scaled, std::string &model_file_name)
{
  int kernels[4] = {svm::RBF, svm::LINEAR, svm::POLY, svm::SIGMOID};
  std::vector<std::string> kernel_names;
  kernel_names.resize(4);
  kernel_names.at(0) = "rbf";
  kernel_names.at(1) = "linear";
  kernel_names.at(2) = "polynomial";
  kernel_names.at(3) = "sigmoid";

  printf("Classifier: LibSVM\nParameters (LibSVM): KernelType,C,Gamma,RecRate,ConfusionTable\n--\n");

  double RecRateBest = 0;
  int kernelBest;
  std::vector<int> ConfusionTableBest;
  ConfusionTableBest.resize(4);
  int gammaBest = 0;
  int CBest = 0;

  for(int k = 0; k < 1; ++k)
  {
    for(int C = -5; C <= 15; C = C+2)
    {
      for(int gamma = -15; gamma <= 3; gamma = gamma + 2)
      {
        double RecRate;
        std::vector<int> ConfusionTable;
        ConfusionTable.resize(4);
        trainFeatures(pow(2.0,gamma),pow(2.0,C),kernels[k],2,RecRate,ConfusionTable,train_ST_file_name_scaled,model_file_name);

        printf("%s;%8.6f;%8.6f;%8.6f;%3.1f/%3.1f/%3.1f/%3.1f/\n",kernel_names.at(k).c_str(),pow(2.0,C),pow(2.0,gamma),RecRate,
               (double)(ConfusionTable.at(0)),(double)(ConfusionTable.at(1)),(double)(ConfusionTable.at(2)),(double)(ConfusionTable.at(3)));

        if(RecRate>RecRateBest)
        {
          RecRateBest = RecRate;
          kernelBest = k;
          gammaBest = gamma;
          CBest = C;
          ConfusionTableBest = ConfusionTable;
        }
      }
    }
  }

  printf("--\nGlobalBest (LibSVM):\n");
  printf("%s;%8.6f;%8.6f;%8.6f;%3.1f/%3.1f/%3.1f/%3.1f/\n",kernel_names.at(kernelBest).c_str(),pow(2.0,CBest),pow(2.0,gammaBest),RecRateBest,
         (double)(ConfusionTableBest.at(0)),(double)(ConfusionTableBest.at(1)),(double)(ConfusionTableBest.at(2)),(double)(ConfusionTableBest.at(3)));

  printf("Training final classifier...\n");
  trainFeatures(pow(2.0,gammaBest),pow(2.0,CBest),kernels[kernelBest],0,RecRateBest,ConfusionTableBest,train_ST_file_name_scaled,model_file_name);
  printf("%s;%8.6f;%8.6f;%8.6f;%3.1f/%3.1f/%3.1f/%3.1f/\n",kernel_names.at(kernelBest).c_str(),pow(2.0,CBest),pow(2.0,gammaBest),RecRateBest,
         (double)(ConfusionTableBest.at(0)),(double)(ConfusionTableBest.at(1)),(double)(ConfusionTableBest.at(2)),(double)(ConfusionTableBest.at(3)));

}

void printUsage(char *av)
{
  printf("Usage: %s training_data.txt.scaled model.txt\n"
    " Options:\n"
    "   [-h] ... show this help.\n"
    "   training_data.txt.scaled ... filename with scaled training samples\n"
    "   model.txt                ... output model\n", av);
  std::cout << " Example: " << av << " training_data.txt.scaled model.txt" << std::endl;
}

int main(int argc, char *argv[])
{
  if(argc != 3)
  {
    printUsage(argv[0]);
    exit(0);
  }

  std::string train_ST_file_name_scaled = argv[1];
  std::string model_file_name = argv[2];

  trainSVM(train_ST_file_name_scaled,model_file_name);

  return(0);
}


#ifndef __solver_HCod_h__
#define __solver_HCod_h__

#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include <weighted_hqp/cod.hpp>

namespace hcod{
    typedef struct H_structure {
        Eigen::MatrixXd A;
        Eigen::MatrixXd b;
        Eigen::VectorXi btype;

        int mmax;
        int m;
        int r;
        int n;
        int ra;
        int rp;

        Eigen::VectorXi iw;
        Eigen::VectorXi im;
        Eigen::MatrixXd W;
        Eigen::MatrixXd H;
        Eigen::VectorXi fw;
        Eigen::VectorXi fm;

        Eigen::VectorXi active, activeb, idx_nh_vec;
        Eigen::VectorXi bound;

        Eigen::MatrixXd A_act;

        // for weighted function
        Eigen::MatrixXd Wk;
        Eigen::MatrixXd AkWk;
        Eigen::VectorXd sol;

        Eigen::MatrixXd Lj;
        Eigen::MatrixXd Y, Yupj, Ydownj;
        Eigen::MatrixXd Hj_c, Wj_c;

        std::vector<int> mj, nj, rj, rpj, raj;
        std::vector<Eigen::VectorXi> iwj, imj, fwj, fmj;
        std::vector<Eigen::MatrixXd> Aj, Wj, Hj;
        int rupj;

    } h_structure;

    class HCod{
        public:
            HCod(const std::vector<Eigen::MatrixXd> &A, const std::vector<Eigen::MatrixXd> &b, const std::vector<Eigen::VectorXi> &btype, const std::vector<Eigen::VectorXi> &aset_init, const std::vector<Eigen::VectorXi> &aset_bound);
            HCod(const std::vector<Eigen::MatrixXd> &A, const std::vector<Eigen::MatrixXd> &b, const std::vector<Eigen::VectorXi> &btype, const std::vector<Eigen::VectorXi> &aset_init, const std::vector<Eigen::VectorXi> &aset_bound, const std::vector<Eigen::MatrixXd> &W);
            ~HCod(){};

        private:
            void set_h_structure(const unsigned int & index);
            void compute_hcod();
            void clear_submatrix(){
                for (int i=0; i<p_; i++){
                    h_[i].mj.clear();
                    h_[i].imj.clear();
                    h_[i].iwj.clear();
                    h_[i].fwj.clear();
                    h_[i].fmj.clear();
                    h_[i].nj.clear();
                    h_[i].rj.clear();
                    h_[i].rpj.clear();
                    h_[i].raj.clear();
                    h_[i].Aj.clear();
                    h_[i].Hj.clear();
                    h_[i].Wj.clear();
                }
            }

        public:
            void print_h_structure(const unsigned int & index);
            std::vector<H_structure> geth(){
                return h_;
            }
            Eigen::MatrixXd getY(){
                return Y_;
            };

        private:
            std::vector<Eigen::MatrixXd> A_;
            std::vector<Eigen::MatrixXd> b_;
            std::vector<Eigen::VectorXi> btype_;
            std::vector<Eigen::MatrixXd> W_;
            std::vector<Eigen::VectorXi> aset_init_, aset_bound_;

            int p_, nh_;
            std::vector<H_structure> h_;
            Cod* cod_;
            Eigen::MatrixXd  Y_;
            bool _isweighted;
    };
}

#endif


#include "globimap/counting_globimap.hpp"
#include "globimap_test_config.hpp"
#include <algorithm>
#include <chrono>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <limits>
#include <math.h>
#include <string>

#include <highfive/H5File.hpp>
#include <tqdm.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include "archive.h"
#include "loc.hpp"
#include "rasterizer.hpp"
#include "shapefile.hpp"

#include <H5Cpp.h>

namespace fs = std::filesystem;

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;
namespace bgt = boost::geometry::strategy::transform;
typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
typedef bg::model::box<point_t> box_t;
typedef bg::model::polygon<point_t> polygon_t;

typedef std::vector<polygon_t> poly_collection_t;

#ifndef TUM1_ICAML_ORG
const std::string base_path = "/mnt/G/datasets/atlas/";
const std::string vector_base_path = "/mnt/G/datasets/vector/";
const std::string experiments_path =
    "/home/moritz/workspace/bgdm/globimap/experiments/";
#else
const std::string base_path = "/home/moritz/tf/pointclouds_2d/data/";
const std::string experiments_path = "/home/moritz/tf/globimap/experiments/";
const std::string vector_base_path = "/home/moritz/tf/vector/";
#endif

std::vector<std::string> datasets{"twitter_1mio_coords.h5",
                                  "twitter_10mio_coords.h5"};

// std::vector<std::string> datasets{"twitter_200mio_coords.h5",
//                                   "asia_200mio_coords.h5"};
std::vector<std::string> polygon_sets{"tl_2017_us_zcta510",
                                      "Global_LSIB_Polygons_Detailed"};
template <typename T> std::string render_hist(std::vector<T> hist) {
  std::stringstream ss;
  ss << "[";
  for (auto i = 0; i < hist.size(); ++i) {
    ss << hist[i] << ((i < (hist.size() - 1)) ? ", " : "");
  }
  ss << "]";
  return ss.str();
}

template <typename T>
std::string render_stat(const std::string &name, std::vector<T> stat) {
  double stat_min = FLT_MAX;
  double stat_max = 0;
  double stat_mean = 0;
  double stat_std = 0;

  for (double v : stat) {
    stat_min = std::min(stat_min, v);
    stat_max = std::max(stat_max, v);
    stat_mean += v;
  }
  stat_mean /= stat.size();

  for (double v : stat) {
    stat_std += pow((stat_mean - v), 2);
  }
  stat_std /= stat.size();
  stat_std = sqrt(stat_std);

  std::stringstream ss;
  ss << "\"" << name << "\": {\n";
  ss << "\"min\": " << stat_min << ",\n";
  ss << "\"max\": " << stat_max << ",\n";
  ss << "\"mean\": " << stat_mean << ",\n";
  ss << "\"std\": " << stat_std << ",\n";
  ss << "\"hist\": " << render_hist(globimap::make_histogram(stat, 1000))
     << "\n";
  ss << "}";
  return ss.str();
}

int main() {
  uint width = 2 * 8192, height = 2 * 8192;
  std::string exp_name = "polygons_stat";
  mkdir((experiments_path + exp_name).c_str(), 0777);
  for (auto shp : polygon_sets) {
    std::stringstream ss1;
    ss1 << shp << "-" << width << "x" << height;
    auto polyset_name = ss1.str();
    std::stringstream ss;
    ss << vector_base_path << polyset_name;
    auto poly_path = ss.str();

    int poly_count = 0;
    for (auto e : fs::directory_iterator(poly_path)) {
      poly_count += (e.is_regular_file() ? 1 : 0);
    }
    std::vector<uint64_t> polysizes;
    for (auto idx = 0; idx < poly_count; idx++) {
      std::vector<uint64_t> raster;
      std::stringstream ss;
      ss << poly_path << "/" << std::setw(8) << std::setfill('0') << idx;
      auto filename = ss.str();
      std::ifstream ifile(filename, std::ios::binary);
      if (!ifile.is_open()) {
        std::cout << "ERROR file doesn't exist: " << filename << std::endl;
      }
      Archive<std::ifstream> a(ifile);
      a >> raster;
      polysizes.push_back(raster.size());
      ifile.close();
    }
    std::stringstream fss;
    fss << experiments_path << exp_name << "/" << polyset_name << ".json";
    std::ofstream out(fss.str());
    out << "{" << render_stat("polysizes", polysizes) << ",\n";
    out << "\"size\": " << poly_count << "" << std::endl;
    out << "}" << std::endl;

    out.close();
    std::cout << "end: " << shp << std::endl;
  }
};