#include <iostream>
#include <vector>

#include <algorithm>
#include <unordered_map>
#include <set>
#include <utility>
#include <stdexcept>
#include <cmath>
#include <chrono>
#include <exception>
#include <boost/filesystem.hpp>

#include "dynet/lstm.h"
#include "dynet/training.h"
#include "dynet/dynet.h"
#include "dynet/dict.h"

#include <boost/functional/hash.hpp>
#include <boost/program_options.hpp>
#include <boost/serialization/unordered_set.hpp>

#include "lemon_inc.h"
#include "serialization.h"
#include "nn/biaffine_parser.h"
#include "utils.h"

#include "dependency.h"
#include "spine_data.h"
#include "activation_function.h"

int main(int argc, char **argv)
{
    std::string train_path;
    std::string dev_path;
    std::string model_path;
    unsigned n_iteration;

    bool probabilistic;

    bool eval_on_dev;

    bool dropout;
    double dropout_p;

    std::string ignore_dynet_mem;
    std::string ignore_dynet_wd;

    unsigned n_stack;
    unsigned n_layer;
    int lstm_dim;
    int word_dim;
    int pos_dim;

    ActivationFunctionOption activation_function;
    bool best_head = false;

    namespace po = boost::program_options;
    po::options_description desc("Options");
    desc.add_options()
        ("train", po::value<std::string>(&train_path)->required(), "")
        ("model", po::value<std::string>(&model_path)->required(), "")
        ("iteration", po::value<unsigned>(&n_iteration)->default_value(20), "")
        ("eval-on-dev", po::value<bool>(&eval_on_dev)->default_value(false), "")
        ("dev-path", po::value<std::string>(&dev_path)->default_value(""), "")
        // dropout
        ("dropout", po::value<bool>(&dropout)->default_value(false), "")
        ("dropout-p", po::value<double>(&dropout_p)->default_value(0.5), "")
        // dynet
        ("dynet-mem", po::value<std::string>(&ignore_dynet_mem), "")
        ("dynet-weight-decay", po::value<std::string>(&ignore_dynet_wd), "")
        // nn options
        ("activation-function", po::value<ActivationFunctionOption>(&activation_function), "")
        ("lstm-dim", po::value<int>(&lstm_dim)->default_value(125))
        ("word-dim", po::value<int>(&word_dim)->default_value(100))
        ("pos-dim", po::value<int>(&pos_dim)->default_value(25))
        ("n-stack", po::value<unsigned>(&n_stack)->default_value(1u), "")
        ("n-layer", po::value<unsigned>(&n_layer)->default_value(1u), "")
        ("probabilistic", po::value<bool>(&probabilistic)->default_value(false), "")
        ("best-head", po::value<bool>(&best_head)->default_value(false))
    ;

    po::positional_options_description pod;
    pod.add("train", 1);
    pod.add("model", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(pod).run(), vm);
    po::notify(vm);

    if (probabilistic && !best_head)
        throw std::runtime_error("Use --best-head=true in order to use probabilistic network");

    dynet::initialize(argc, argv);

    SpineSettings spine_settings;
    read_object(model_path + ".spine_settings.param", spine_settings);

    std::cerr << "Reading train data..." << std::endl << std::flush;
    SpineData spine_train(spine_settings);
    std::vector<IntSentence> train_data;
    spine_train.read(train_path);
    spine_train.as_int_sentence([&](const IntSentence& s) { train_data.push_back(s); });

    std::vector<IntSentence> dev_data;
    if (eval_on_dev)
    {
        SpineData spine_dev(spine_settings);
        spine_dev.read(dev_path);
        spine_dev.as_int_sentence([&](const IntSentence& s) { dev_data.push_back(s); });
    }

    // TODO: other trainer
    dynet::Model model;
    dynet::AdamTrainer trainer(model);
    trainer.sparse_updates_enabled = false;

    RNNSettings node_rnn_settings;
    node_rnn_settings.padding = true;
    node_rnn_settings.dim = lstm_dim;
    node_rnn_settings.n_stack = n_stack;
    node_rnn_settings.n_layer = n_layer;

    NeuralBiaffineParserSettings nn_settings;
    nn_settings.n_word = spine_settings.word_dict.size();
    nn_settings.n_pos = spine_settings.pos_dict.size();
    nn_settings.word_unknown = spine_settings.word_dict.convert("*UNKNOWN*");
    nn_settings.word_dim = word_dim;
    nn_settings.pos_dim = pos_dim;
    nn_settings.pos_input = true;
    nn_settings.activation_function = activation_function.value;

    NeuralBiaffineParser<dynet::LSTMBuilder> rnn(model, nn_settings, node_rnn_settings);

    for (auto const& sentence : train_data)
    {
        // Count word frequence (used for dropout)
        for (auto const& token : sentence)
            rnn.m_word_count[token.word] += 1;
    }

    double loss = 0.0;

    save_object(model_path + ".parser.nn_settings", nn_settings);
    save_object(model_path + ".parser.rnn_settings", node_rnn_settings);

    std::vector<unsigned> dev_uas;

    for (unsigned iteration = 0 ; iteration <= n_iteration ; ++iteration)
    {
        std::cerr << "Iteration: " << iteration << std::endl;
        std::cerr << std::flush;

        unsigned n_correct = 0;
        unsigned n_total = 0;

        std::random_shuffle(std::begin(train_data), std::end(train_data));

        for (const IntSentence& sentence : train_data)
        {
            dynet::ComputationGraph cg;

            // loss output
            std::vector<Expression> errs;

            if (best_head)
            {
                std::vector<unsigned> correct_indices(sentence.size());
                std::vector<std::vector<Expression>> head_exprs(sentence.size());

                std::vector<std::vector<double>> scores(sentence.size());

                rnn.compute(
                    cg,
                    sentence,
                    [&] (unsigned head, unsigned modifier, double score, dynet::expr::Expression& expr) -> void
                    {
                        if ((int) head == sentence[modifier].head)
                            correct_indices.at(modifier-1) = head_exprs.at(modifier-1).size();
                        head_exprs.at(modifier-1).push_back(expr);

                        if (!probabilistic)
                        {
                            // loss augmented
                            if ((int) head == sentence[modifier].head)
                                score += 1.0;

                            scores.at(modifier-1).push_back(score);
                        }
                    },
                    true, // word dropout
                    dropout,
                    dropout_p
                );

                if (probabilistic)
                {
                    for (unsigned i = 0u ; i < correct_indices.size() ; ++i)
                    {
                        // loss
                        dynet::expr::Expression heads = dynet::expr::concatenate(head_exprs.at(i));
                        errs.push_back(pickneglogsoftmax(heads, correct_indices.at(i)));

                        auto vec = dynet::as_vector(heads.value());
                        int predicted = std::distance(std::begin(vec), std::max_element(std::begin(vec), std::end(vec)));
                        if (predicted == (int) correct_indices.at(i))
                            n_correct += 1;
                    }
                }
                else
                {
                    for (unsigned i = 0u ; i < correct_indices.size() ; ++i)
                    {
                        // loss
                        dynet::expr::Expression heads = dynet::expr::concatenate(head_exprs.at(i));

                        auto vec = dynet::as_vector(heads.value());
                        int predicted = std::distance(std::begin(vec), std::max_element(std::begin(vec), std::end(vec)));
                        if (predicted == (int) correct_indices.at(i))
                        {
                            n_correct += 1;
                        }
                        else
                        {
                            errs.push_back(dynet::expr::pick(heads, predicted) - pick(heads, correct_indices.at(i)));
                        }
                    }
                }
            }
            else
            {
                // MSA decoding
                assert(!probabilistic);
                // TODO
                throw std::runtime_error("unimplemented");
            }
            n_total += sentence.size();

            // backprop
            if (errs.size() > 0)
            {
                Expression sum_errs = dynet::expr::sum(errs);
                loss += as_scalar(cg.get_value(sum_errs.i));
                cg.backward(sum_errs.i);
                trainer.update(1.0);
            }
        }

        trainer.update_epoch();
        trainer.status();

        std::cerr << "E = " << (loss / (double) n_total) << " ppl=" << exp(loss / (double) n_total) << " (acc=" << n_correct / (double) n_total << ")" << std::endl;
        std::cerr << std::flush;

        save_object(model_path + ".parser.param." + std::to_string(iteration), model);

        // Evaluate on dev set
        if (eval_on_dev)
        {
            unsigned n_good_head = 0u;
            unsigned n_total_head = 0u;

            std::cerr << "Evaluating on dev..." << std::endl << std::flush;

            for (auto const& sentence : dev_data)
            {
                dynet::ComputationGraph cg;

                LDigraph lemon_graph;
                LArcMap lemon_weights(lemon_graph);

                for (unsigned i = 0 ; i <= sentence.size() ; ++i)
                {
                    LNode node = lemon_graph.addNode();
                    assert(lemon_graph.id(node) == (int) i);
                }

                rnn.compute(
                    cg,
                    sentence,
                    [&] (unsigned head, unsigned modifier, double score, dynet::expr::Expression& expr) -> void
                    {
                        unused_parameter(expr);
                        LArc lemon_arc = lemon_graph.addArc(
                            lemon_graph.nodeFromId(head),
                            lemon_graph.nodeFromId(modifier)
                        );
                        lemon_weights[lemon_arc] = -score;
                    }
                );

                MSA msa(lemon_graph, lemon_weights);
                msa.run(lemon_graph.nodeFromId(0));

                for (unsigned modifier = 1 ; modifier <= sentence.size() ; ++modifier)
                {
                    auto msa_pred = msa.pred(lemon_graph.nodeFromId(modifier));
                    assert(msa_pred != lemon::INVALID);

                    int predicted = lemon_graph.id(lemon_graph.source(msa_pred));
                    if (predicted == sentence[modifier].head)
                        ++ n_good_head;
                }

                n_total_head += sentence.size();
            }

            std::cerr << " -> UAS: " << n_good_head / (double) n_total_head << std::endl;
            dev_uas.push_back(n_good_head);
        }
        std::cerr << "------" << std::endl;
    }

    if (eval_on_dev)
    {
        unsigned max = std::distance(std::begin(dev_uas), std::max_element(std::begin(dev_uas), std::end(dev_uas)));
        boost::filesystem::copy_file(
                model_path + ".parser.param." + std::to_string(max),
                model_path + ".parser.param",
                boost::filesystem::copy_option::overwrite_if_exists
        );

    }
    else
    {
        // juste resave the last model
        save_object(model_path + ".parser.param", model);
    }
}


#ifndef FAST_GICP_FAST_VGICP_CUDA_IMPL_HPP
#define FAST_GICP_FAST_VGICP_CUDA_IMPL_HPP

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

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/search/kdtree.h>
#include <pcl/registration/registration.h>

#include <fast_gicp/gicp/fast_vgicp_cuda.hpp>
#include <fast_gicp/cuda/fast_vgicp_cuda.cuh>

#include <thrust/host_vector.h>
#include <thrust/device_vector.h>

namespace fast_gicp {

template<typename PointSource, typename PointTarget>
FastVGICPCuda<PointSource, PointTarget>::FastVGICPCuda() : LsqRegistration<PointSource, PointTarget>() {
  this->reg_name_ = "FastVGICPCuda";
  k_correspondences_ = 20;
  voxel_resolution_ = 1.0;
  regularization_method_ = RegularizationMethod::PLANE;
  neighbor_search_method_ = NearestNeighborMethod::CPU_PARALLEL_KDTREE;

  vgicp_cuda_.reset(new cuda::FastVGICPCudaCore());
  vgicp_cuda_->set_resolution(voxel_resolution_);
  vgicp_cuda_->set_kernel_params(0.5, 3.0);
}

template<typename PointSource, typename PointTarget>
FastVGICPCuda<PointSource, PointTarget>::~FastVGICPCuda() {}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setCorrespondenceRandomness(int k) {}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setResolution(double resolution) {
  vgicp_cuda_->set_resolution(resolution);
}

template <typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setKernelWidth(double kernel_width, double max_dist) {
  if (max_dist <= 0.0) {
    max_dist = kernel_width * 5.0;
  }
  vgicp_cuda_->set_kernel_params(kernel_width, max_dist);
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setRegularizationMethod(RegularizationMethod method) {
  regularization_method_ = method;
}

template <typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setNeighborSearchMethod(NeighborSearchMethod method, double radius) {
  vgicp_cuda_->set_neighbor_search_method(method, radius);
}

template <typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setNearestNeighborSearchMethod(NearestNeighborMethod method) {
  neighbor_search_method_ = method;
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::swapSourceAndTarget() {
  vgicp_cuda_->swap_source_and_target();
  input_.swap(target_);
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::clearSource() {
  input_.reset();
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::clearTarget() {
  target_.reset();
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setInputSource(const PointCloudSourceConstPtr& cloud) {
  // the input cloud is the same as the previous one
  if(cloud == input_) {
    return;
  }

  pcl::Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);

  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> points(cloud->size());
  std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointSource& pt) { return pt.getVector3fMap(); });

  vgicp_cuda_->set_source_cloud(points);
  switch(neighbor_search_method_) {
    case NearestNeighborMethod::CPU_PARALLEL_KDTREE: {
      std::vector<int> neighbors = find_neighbors_parallel_kdtree<PointSource>(k_correspondences_, cloud);
      vgicp_cuda_->set_source_neighbors(k_correspondences_, neighbors);
      vgicp_cuda_->calculate_source_covariances(regularization_method_);
    } break;
    case NearestNeighborMethod::GPU_BRUTEFORCE:
      vgicp_cuda_->find_source_neighbors(k_correspondences_);
      vgicp_cuda_->calculate_source_covariances(regularization_method_);
      break;
    case NearestNeighborMethod::GPU_RBF_KERNEL:
      vgicp_cuda_->calculate_source_covariances_rbf(regularization_method_);
      break;
  }
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr& cloud) {
  // the input cloud is the same as the previous one
  if(cloud == target_) {
    return;
  }

  pcl::Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);

  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> points(cloud->size());
  std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointTarget& pt) { return pt.getVector3fMap(); });

  vgicp_cuda_->set_target_cloud(points);
  switch(neighbor_search_method_) {
    case NearestNeighborMethod::CPU_PARALLEL_KDTREE: {
      std::vector<int> neighbors = find_neighbors_parallel_kdtree<PointTarget>(k_correspondences_, cloud);
      vgicp_cuda_->set_target_neighbors(k_correspondences_, neighbors);
      vgicp_cuda_->calculate_target_covariances(regularization_method_);
    } break;
    case NearestNeighborMethod::GPU_BRUTEFORCE:
      vgicp_cuda_->find_target_neighbors(k_correspondences_);
      vgicp_cuda_->calculate_target_covariances(regularization_method_);
      break;
    case NearestNeighborMethod::GPU_RBF_KERNEL:
      vgicp_cuda_->calculate_target_covariances_rbf(regularization_method_);
      break;
  }
  vgicp_cuda_->create_target_voxelmap();
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
  vgicp_cuda_->set_resolution(this->voxel_resolution_);

  LsqRegistration<PointSource, PointTarget>::computeTransformation(output, guess);
}

template<typename PointSource, typename PointTarget>
template<typename PointT>
std::vector<int> FastVGICPCuda<PointSource, PointTarget>::find_neighbors_parallel_kdtree(int k, typename pcl::PointCloud<PointT>::ConstPtr cloud) const {
  pcl::search::KdTree<PointT> kdtree;
  kdtree.setInputCloud(cloud);
  std::vector<int> neighbors(cloud->size() * k);

#pragma omp parallel for schedule(guided, 8)
  for(int i = 0; i < cloud->size(); i++) {
    std::vector<int> k_indices;
    std::vector<float> k_sq_distances;
    kdtree.nearestKSearch(cloud->at(i), k, k_indices, k_sq_distances);

    std::copy(k_indices.begin(), k_indices.end(), neighbors.begin() + i * k);
  }

  return neighbors;
}

template<typename PointSource, typename PointTarget>
double FastVGICPCuda<PointSource, PointTarget>::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) {
  vgicp_cuda_->update_correspondences(trans);
  return vgicp_cuda_->compute_error(trans, H, b);
}

template<typename PointSource, typename PointTarget>
double FastVGICPCuda<PointSource, PointTarget>::compute_error(const Eigen::Isometry3d& trans) {
  return vgicp_cuda_->compute_error(trans, nullptr, nullptr);
}

}  // namespace fast_gicp

#endif


///////////////////////////////////////////////////////////////
//  Copyright 2011 John Maddock. 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_

#include "setup.hpp"
#include "table_type.hpp"

#include <boost/math/special_functions/gamma.hpp>
#include "libs/math/test/test_tgamma_ratio.hpp"

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      ".*",                          // test type(s)
      ".*",                          // test data group
      ".*", 2000, 500);              // test function
   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

template <class T>
void test(T t, const char* p)
{
   test_tgamma_ratio(t, p);
}

BOOST_AUTO_TEST_CASE( test_main )
{
   expected_results();
   ALL_TESTS
}


//==============================================================================
//         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
//==============================================================================
#ifndef BOOST_SIMD_SDK_MEMORY_ALIGNED_ARRAY_FWD_HPP_INCLUDED
#define BOOST_SIMD_SDK_MEMORY_ALIGNED_ARRAY_FWD_HPP_INCLUDED

#include <boost/simd/sdk/config/arch.hpp>
#include <boost/simd/sdk/memory/meta/next_power_of_2.hpp>

namespace boost { namespace simd { namespace memory
{
  /* Default alignment is BOOST_SIMD_ARCH_ALIGNMENT or the largest power of 2
   * smaller than N*sizeof(T), whichever is smaller */
  template< class T
          , std::size_t N
          , std::size_t Align = (BOOST_SIMD_ARCH_ALIGNMENT > (N*sizeof(T)))
                              ? meta::prev_power_of_2_c< N*sizeof(T) >::value
                              : BOOST_SIMD_ARCH_ALIGNMENT
          >
  struct aligned_array;
} } }

#endif


#ifndef LIBSOCKS5_TESTS_UTIL_RANDOM_HPP
#define LIBSOCKS5_TESTS_UTIL_RANDOM_HPP

#include <boost/asio/ip/address.hpp>
#include <boost/asio/ip/tcp.hpp>

#include <algorithm>
#include <cstddef>
#include <limits>
#include <random>
#include <string>
#include <type_traits>

namespace socks5::tests::util {

enum class random_string_options : int {
    alpha      = 1,
    num        = 2,
    sym        = 4,
    alphanum   = alpha | num,
    lower_case = 8,
    upper_case = 16,
    mixed_case = lower_case | upper_case,
    mixed      = alpha | num | lower_case | upper_case | sym,
};

inline constexpr auto operator&(random_string_options lhs,
                                random_string_options rhs) noexcept -> bool {
    return (static_cast<int>(lhs) & static_cast<int>(rhs)) != 0;
}

inline constexpr auto operator|(random_string_options lhs,
                                random_string_options rhs) noexcept
    -> random_string_options {
    return static_cast<random_string_options>(static_cast<int>(lhs) |
                                              static_cast<int>(rhs));
}

namespace detail {

template <random_string_options opts>
inline auto build_random_string_pool() noexcept -> std::string {
    constexpr auto lower_case_alpha = "abcdefghijklmnopqrstuvwxyz";
    constexpr auto upper_case_alpha = "ABCDEFGHIJKLMNOPQRSTUVWXYZ";
    constexpr auto num              = "1234567890";
    constexpr auto sym              = "`~!@#$%^&*()-_=+\"\\/?.>,<";

    std::string pool {};

    if constexpr (opts & random_string_options::alpha) {
        if constexpr (!(opts & random_string_options::lower_case) &&
                      !(opts & random_string_options::upper_case)) {
            pool.append(lower_case_alpha);
        } else {
            if constexpr (opts & random_string_options::lower_case) {
                pool.append(lower_case_alpha);
            }

            if constexpr (opts & random_string_options::upper_case) {
                pool.append(upper_case_alpha);
            }
        }
    }

    if constexpr (opts & random_string_options::num) {
        pool.append(num);
    }

    if constexpr (opts & random_string_options::sym) {
        pool.append(sym);
    }

    return pool;
}

} // namespace detail

template <random_string_options opts = random_string_options::mixed>
inline auto random_string(std::size_t len) -> std::string {
    std::mt19937 rng {std::random_device {}()};
    std::string  ret {};

    auto pool = socks5::tests::util::detail::build_random_string_pool<opts>();

    ret.resize(len);
    std::generate(ret.begin(), ret.end(),
                  [&pool, &rng] { return pool[rng() % pool.size()]; });

    return ret;
}

template <typename T,
          typename = std::enable_if_t<std::is_integral_v<T> ||
                                      std::is_floating_point_v<T>>>
inline auto random(T min = std::numeric_limits<T>::min(),
                   T max = std::numeric_limits<T>::max()) -> T {
    std::mt19937 rng {std::random_device {}()};

    if constexpr (std::is_integral_v<T>) {
        return std::uniform_int_distribution<T> {min, max}(rng);
    } else {
        return std::uniform_real_distribution<T> {min, max}(rng);
    }
}

template <typename Address>
inline auto random_address() -> Address {
    typename Address::bytes_type arr {};

    for (auto &b : arr) {
        b = random<std::decay_t<decltype(b)>>();
    }

    return Address {std::move(arr)};
}

inline auto random_address_v4() -> boost::asio::ip::address_v4 {
    return random_address<boost::asio::ip::address_v4>();
}

inline auto random_address_v6() -> boost::asio::ip::address_v6 {
    return random_address<boost::asio::ip::address_v6>();
}

inline auto random_endpoint_v4() -> boost::asio::ip::tcp::endpoint {
    return {random_address_v4(), random<std::uint16_t>()};
}

inline auto random_endpoint_v6() -> boost::asio::ip::tcp::endpoint {
    return {random_address_v6(), random<std::uint16_t>()};
}

inline auto random_endpoint() -> boost::asio::ip::tcp::endpoint {
    return random<std::uint8_t>(0, 1) ? random_endpoint_v4()
                                      : random_endpoint_v6();
}

} // namespace socks5::tests::util

#endif


#include "sqlTextExporter.h"
#include <iomanip>

#include <boost/date_time/c_local_time_adjustor.hpp>
#include <boost/date_time/posix_time/ptime.hpp>
#include <boost/date_time/time_clock.hpp>

#include "RunningAverage.h"
#include "sqlWeatherStation.h"

#include "version.h"

sqlTextExporter::sqlTextExporter()
{}

sqlTextExporter::~sqlTextExporter()
{}

int sqlTextExporter::utc_offset_seconds()
{
    using namespace boost::posix_time;
    using namespace boost::date_time;

    // boost::date_time::c_local_adjustor uses the C-API to adjust a
    // moment given in utc to the same moment in the local time zone.
    typedef boost::date_time::c_local_adjustor<ptime> local_adj;
    const ptime utc_now = second_clock<ptime>::universal_time();
    const ptime loc_now = local_adj::utc_to_local(utc_now);

    // compute the difference and return the total seconds
    boost::posix_time::time_duration diff = (loc_now - utc_now);
    return diff.total_seconds();
}

time_t sqlTextExporter::time_instance(int day_offset)
{
   time_t now;
   time(&now);

   return now + day_offset*24*60*60;
}

double sqlTextExporter::rain_intensity(time_t now, int seconds_before, RainMap& rain)
{
   time_t time_begin = now - seconds_before;
   double orain = rain[now];
   double irain = 0.0;

   RainMap::const_iterator itlb = rain.lower_bound(time_begin);
   if(itlb != rain.end()) {
      // time found
      time_t time_before  = itlb->first;
      double orain_before = itlb->second;

      // actual length of period
      double delta_sec  = now - time_before;
      if(delta_sec > 0) {
         double delta_rain = orain - orain_before;
         double factor     = double(seconds_before)/delta_sec;
         if(factor > 1.0) factor = 1.0;
         irain = delta_rain*factor;
      }
   }

   if(irain > 200) irain = 0.0;

   return irain;
}

bool sqlTextExporter::export_list(list<op_pid> ids, int utc_offset, double elevation, ostream& out, double& irain_last)
{
   RainMap rain;
   irain_last = 0.0;

   const size_t plen24h = 60*60*24;

   RunningAverage otemp24h(plen24h);
   RunningAverage itemp24h(plen24h);

   out << "# wstation Gnuplot export file. UTC offset used [sec] = " << utc_offset << ". Elevation [m] = " << elevation << endl;
   out << '#'
       << setw(14) << "timestamp  " << " "
       << setw(15) << "itemp[C]"    << " "
       << setw(15) << "ihumi[%]"    << " "
       << setw(15) << "otemp[C]"    << " "
       << setw(15) << "ohumi[%]"    << " "
       << setw(15) << "opres[mbar]" << " "
       << setw(15) << "owspd[m/s]"  << " "
       << setw(15) << "owgus[m/s]"  << " "
       << setw(15) << "owdir[deg]"  << " "
       << setw(15) << "orain[mm]"   << " "
       << setw(15) << "irain1[mm/h]" << " "
       << setw(15) << "irain24[mm/24h]" << " "
       << setw(15) << "timestamp24h" << " "
       << setw(15) << "itemp24h[C]" << " "
       << setw(15) << "otemp24[C]"  << " "
       << setw(15) << "osens[1/0]"  << " "
       << "Time" << endl;

   for(auto& id : ids) {

      op_ptr<sqlWeatherStation> ws(id);

      if(ws->is_valid()) {

         time_t timestamp = ws->time_utc()+utc_offset;

         // use gmtime always, since we adjust the timestamp manually for local time
         const size_t buflen = 32;
         char buffer[buflen];
         strftime(buffer,buflen,"%Y%m%d %H:%M:%S",gmtime(&timestamp));

         double orain = ws->orain();
         if(orain < 0.0)orain = 0.0;
         rain[timestamp] = orain;

         // compute rain intensity over one hour and 24 hours
         double irain1  = rain_intensity(timestamp,  1*3600,rain);
         double irain24 = rain_intensity(timestamp, 24*3600,rain);

         // add to calculation of running averages
         otemp24h.push_back(make_pair(timestamp,ws->otemp()));
         itemp24h.push_back(make_pair(timestamp,ws->itemp()-2.0));

         double ohumi = ws->ohumi();

         out << setw(15) << timestamp << " "
             << setw(15) << ws->itemp()-2.0 << " "
             << setw(15) << ws->ihumi() << " "
             << setw(15) << ws->otemp() << " "
             << setw(15) << ((ohumi>100.0)? 0.0 : ohumi)  << " "
             << setw(15) << ws->opres(elevation) << " "
             << setw(15) << ws->owspd() << " "
             << setw(15) << ws->owgus() << " "
             << setw(15) << ws->owdir_deg() << " "
             << setw(15) << ws->orain() << " "
             << setw(15) << irain1     << " "
             << setw(15) << irain24    << " "
             << setw(15) << otemp24h.time_value()  << " "
             << setw(15) << itemp24h.value()  << " "
             << setw(15) << otemp24h.value()  << " "
             << setw(15) << ws->osens() << " "
             << buffer << endl;

         irain_last = irain1;
      }

   }

   return true;
}

void sqlTextExporter::export_numeric_html(list<op_pid> ids, int utc_offset, double elevation, double irain, ostream& out)
{
   // the most recent instance (may be invalid)
   op_ptr<sqlWeatherStation> ws(ids.back());

   // count invalid instances and keep the most recent valid
   size_t num_invalid=0;
   for(auto& id : ids) {
      op_ptr<sqlWeatherStation> ws_test(id);
      if(!(ws_test->is_valid()))num_invalid++;
      else ws = ws_test;
   }

   time_t timestamp = ws->time_utc()+utc_offset;

   // use gmtime always, since we adjust the timestamp manually for local time
   const size_t buflen = 32;
   char buffer[buflen];
   strftime(buffer,buflen,"%d-%b-%Y",gmtime(&timestamp));
   string date_value = buffer;
   strftime(buffer,buflen,"%H:%M",gmtime(&timestamp));
   string time_value = buffer;

   out << "" << endl;
   out << "" << endl;

   out << "<meta http-equiv=\"Content-Type\" content=\"text/html; charset=iso-8859-1\">" << endl;
   out << "" << endl;
   out << "" << endl;

   out << "Latest values:" << endl;
   out << "
" << endl;

   out << "
" << endl;
   out << "Date:	   " << date_value.c_str() << "	" << endl;
   out << "Temperature:	" << ws->otemp() << " &#176C	" << endl;
   out << "Pressure:	 "<< ws->opres(elevation) <<" hPa	" << endl;
   out << "
" << endl;

   out << "
" << endl;
   out << "Time:	   "<< time_value.c_str() << "	" << endl;
   double ohumi = ws->ohumi();
   out << "Humidity:	" << ((ohumi>100.0)? 0.0 : ohumi) << "%	" << endl;
   out << "Wind-avg.:	  " << ws->owspd() << " m/s	" << endl;
   out << "
" << endl;

   out << "
" << endl;
   out << "Inside Temp:	 " << ws->itemp()-2.0 << " &#176C	" << endl;
   out << "Rain-acc:	  " << ws->orain() << " mm	" << endl;
   out << "Wind-gust:	  " << ws->owgus() << " m/s	" << endl;
   out << "
" << endl;

   out << "
" << endl;
   out << "Inside Humidity:	 "<< ws->ihumi() << " %	" << endl;
   out << "Rain/hour:	  " << irain << " mm/h	" << endl;
   out << "Wind dir:	  " << ws->owdir_deg() << "	" << endl;
   out << "
" << endl;

   out << "

" << endl;

   out << "
" << endl;
   out << "UTC offset:	 "<< utc_offset/60 << " min.	" << endl;
   out << "Elevation	  " << elevation << " m	" << endl;
   out << " 	  " << WSEXPORT_VERSION << " ("<< num_invalid<<") 	" << endl;
   out << "
" << endl;

   out << "
" << endl;
   out << "" << endl;
   out << "" << endl;

}


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <vw/Core/Log.h>
#include <vw/Camera/CAHVOREModel.h>
#include <fstream>
#include <boost/foreach.hpp>

using namespace vw;
using namespace vw::camera;

bool CAHVOREModel::check_line( std::istream& istream, char letter ) {
  char r1,r2;
  istream >> r1 >> r2;
  if ( r1 != letter || r2 != '=' )
    return false;
  return true;
}

// Overloaded constructor - this one reads in the file name
// where the CAHVORE camera model is saved.
CAHVOREModel::CAHVOREModel(std::string const& filename) {

  try {
    std::ifstream input(filename.c_str(), std::ifstream::in);
    input.exceptions(std::ifstream::failbit | std::ifstream::badbit);

    vw_out(InfoMessage, "camera") << "Reading CAHVORE file: "
                                  << filename << ".\n";

    char r1;

    while (true) {
      input.ignore(1024, 'C');
      input >> r1;
      if (r1 == '=')
        break;
    }
    input >> C(0) >> C(1) >> C(2);

    if ( !check_line( input, 'A') )
      vw_throw( IOErr() << "CAHVOREModel: Could not read A vector\n" );
    input >> A(0) >> A(1) >> A(2);

    if ( !check_line( input, 'H') )
      vw_throw( IOErr() << "CAHVOREModel: Could not read H vector\n" );
    input >> H(0) >> H(1) >> H(2);

    if ( !check_line( input, 'V') )
      vw_throw( IOErr() << "CAHVOREModel: Could not read V vector\n" );
    input >> V(0) >> V(1) >> V(2);

    if ( !check_line( input, 'O') )
      vw_throw( IOErr() << "CAHVOREModel: Could not read O vector\n" );
    input >> O(0) >> O(1) >> O(2);

    if ( !check_line( input, 'R') )
      vw_throw( IOErr() << "CAHVOREModel: Could not read R vector\n" );
    input >> R(0) >> R(1) >> R(2);

    if ( !check_line( input, 'E') )
      vw_throw( IOErr() << "CAHVOREModel: could not read R vector\n" );
    input >> E(0) >> E(1) >> E(2);

    if ( !check_line( input, 'T') )
      vw_throw( IOErr() << "CAHVOREModel: could not read T element\n" );
    int T;
    input >> T;

    if ( !check_line( input, 'P') )
      vw_throw( IOErr() << "CAHVOREModel: could not read P element\n" );
    input >> P;

    switch( T ) {
    case 1: P = 1.0; break;
    case 2: P = 0.0; break;
    case 3:
      if ( P < 0 || P > 1 ) vw_throw( ArgumentErr() << "Invalid P value: "
                                      << P << "\n" );
      break;
    default: vw_throw( ArgumentErr() << "Unknown CAHVORE type: " << T << "\n" );
    }

  } catch (const std::ifstream::failure& e) {
    vw_throw( IOErr() << "CAHVOREModel: Could not read file: " << filename << " (" << e.what() << ")" );
  }
}

// Write CAHVOR model to file.
void vw::camera::CAHVOREModel::write(std::string const& filename) {
  try {
    std::ofstream output(filename.c_str(), std::ofstream::out);
    output.exceptions(std::ofstream::failbit | std::ofstream::badbit);
    output.precision(20);

    vw_out(InfoMessage, "camera") << "Writing CAHVORE file: " << filename << "\n";

    output << "C = " << C(0) << " " << C(1) << " " << C(2) << "\n"
           << "A = " << A(0) << " " << A(1) << " " << A(2) << "\n"
           << "H = " << H(0) << " " << H(1) << " " << H(2) << "\n"
           << "V = " << V(0) << " " << V(1) << " " << V(2) << "\n"
           << "O = " << O(0) << " " << O(1) << " " << O(2) << "\n"
           << "R = " << R(0) << " " << R(1) << " " << R(2) << "\n"
           << "E = " << E(0) << " " << E(1) << " " << E(2) << "\n";
    if ( P == 0 ) {
      output << "T = 2\n";
    } else if ( P == 1 ) {
      output << "T = 1\n";
    } else {
      output << "T = 3\n";
    }
    output << "P = " << P << "\n";
  } catch ( const std::ofstream::failure& e ) {
    vw_throw( IOErr() << "CAHVOREModel: Could not write file: " << filename << " (" << e.what() << ")" );
  }
}

vw::Vector3 CAHVOREModel::pixel_to_vector(vw::Vector2 const& pix) const {
  // Based on JPL's cmod_cahvore_2d_to_3d
  Vector3 result;

  // Calculate initial terms
  Vector3 w3 = cross_prod(V - pix[1]*A,
                          H - pix[0]*A);
  Vector3 rp = (1/dot_prod(A,cross_prod(V,H))) * w3;

  double zetap = dot_prod(rp,O);
  Vector3 lambdap3 = rp - zetap*O;
  double lambdap = norm_2(lambdap3);
  double chip = lambdap / zetap;

  if (chip < 1e-8) {
    // Approximations for small angles
    result = O;
  } else {
    // Full calculations

    // Calculate chi using Newton's Method
    double chi = chip;
    double dchi = 1;
    for (int32 n = 0;; ++n) {
      // Checking exit conditions
      if (n > 100)
        vw_throw( PixelToRayErr() << "CAHVOREModel: Did not converge.\n" );
      if (fabs(dchi) < 1e-8)
        break;

      // Compute terms from the current value of chi
      double chi2 = chi * chi;
      double chi3 = chi * chi2;
      double chi4 = chi * chi3;
      double chi5 = chi * chi4;

      // Update chi
      double deriv = (1 + R[0]) + 3*R[1]*chi2 + 5*R[2]*chi4;
      dchi = ((1 + R[0])*chi + R[1]*chi3 + R[2]*chi5 - chip) / deriv;
      chi -= dchi;
    }

    // Compute the incoming ray's angle
    double linchi, theta;
    linchi = P * chi;
    if (P < -1e-15)
      theta = asin(linchi) / P;
    else if (P > 1e-15)
      theta = atan(linchi) / P;
    else
      theta = chi;

    double theta2 = theta*theta;
    double mu = E[2];
    mu = E[1] + theta2*mu;
    mu = E[0] + theta2*mu;

    result = sin(theta)*normalize(lambdap3) + cos(theta)*O;
  }

  return result;
}

Vector2 CAHVOREModel::point_to_pixel(vw::Vector3 const& point) const {
  // Base on JPL's cmod_cahvore_3d_to_2d_general

  // Calculate initial terms
  Vector3 p_c = point - C;
  double zeta = dot_prod(p_c, O);
  Vector3 lambda3 = p_c - zeta * O;
  double lambda = norm_2(lambda3);

  // Calculate theta using Newton's Method
  double theta = atan2(lambda, zeta);
  double dtheta = 1;
  for (int32 n = 0;;++n) {

    // Checking exit conditions
    if (n > 100)
      vw_throw( PointToPixelErr() << "CAHVOREModel: Did not converge.\n" );
    if (fabs(dtheta) < 1e-8)
      break;

    // Compute terms from the current value of theta
    double costh = cos(theta);
    double sinth = sin(theta);
    double theta2 = theta * theta;
    double theta3 = theta * theta2;
    double theta4 = theta * theta3;
    double upsilon = zeta*costh + lambda*sinth
      - (1     - costh) * (E[0] +  E[1]*theta2 +   E[2]*theta4)
      - (theta - sinth) * (      2*E[1]*theta  + 4*E[2]*theta3);

    // Update theta
    dtheta = (
              zeta*sinth - lambda*costh
              - (theta - sinth) * (E[0] + E[1]*theta2 + E[2]*theta4)
              ) / upsilon;
    theta -= dtheta;
  }

  // Check the value of theta
  if ((theta * fabs(P)) > M_PI/2)
    vw_throw( PointToPixelErr() << "CAHVOREModel: Theta out of bounds.\n" );

  // Approximations for small theta
  Vector3 rp;
  if (theta < 1e-8) {
    rp = p_c;
  } else {
    // Full calculations
    double linth, chi;

    linth = P * theta;
    if (P < -1e-15)
      chi = sin(linth) / P;
    else if (P > 1e-15)
      chi = tan(linth) / P;
    else
      chi = theta;

    double chi2 = chi*chi;
    double mu = R[2];
    mu = R[1] + chi2 * mu;
    mu = R[0] + chi2 * mu;
    rp = (lambda / chi) * O + (1+mu)*lambda3;
  }

  // Calculate the projection
  double alpha  = dot_prod(rp, A);
  return Vector2( dot_prod(rp,H) / alpha,
                  dot_prod(rp,V) / alpha );
}

CAHVModel camera::linearize_camera( CAHVOREModel const& camera_model,
                                    Vector2i const& cahvore_image_size,
                                    Vector2i const& cahv_image_size ) {
  // Limit to field of view
  const static double limfov = M_PI * 3/4; // 135 degrees field of view.
  const bool minfov          = true;       // Yes, minimize the common field of view

  CAHVModel cahv_model;
  cahv_model.C = camera_model.C;

  // Record the landmark 2D coordinates around the perimeter of the image
  Vector2 hpts[6], vpts[6];
  hpts[0] = Vector2();
  hpts[1] = Vector2(0,(cahvore_image_size[1]-1)/2);
  hpts[2] = Vector2(0,cahvore_image_size[1]-1);
  hpts[3] = Vector2(cahvore_image_size[0]-1,0);
  hpts[4] = Vector2(cahvore_image_size[0]-1,(cahvore_image_size[1]-1)/2);
  hpts[5] = cahvore_image_size - Vector2(1,1);
  vpts[0] = Vector2();
  vpts[1] = Vector2((cahvore_image_size[0]-1)/2,0);
  vpts[2] = Vector2(cahvore_image_size[0]-1,0);
  vpts[3] = Vector2(0,cahvore_image_size[1]-1);
  vpts[4] = Vector2((cahvore_image_size[0]-1)/2,cahvore_image_size[1]-1);
  vpts[5] = cahvore_image_size - Vector2(1,1);

  // Choose a camera axis in the middle of the image
  Vector2 p2 = (cahvore_image_size-Vector2i(1,1))/2.0;
  cahv_model.A = camera_model.pixel_to_vector( p2 );

  // Compute the original right and down vectors
  Vector3 dn = cross_prod(camera_model.A, camera_model.H);
  Vector3 rt = normalize(cross_prod(dn,   camera_model.A));
  dn = normalize( dn );

  // Adjust the right and down vectors to be orthogonal to new axis
  rt = cross_prod(dn, cahv_model.A);
  dn = normalize(cross_prod(cahv_model.A, rt));
  rt = normalize( rt );

  // Find horizontal and vertical fields of view
  double hmin = 1, hmax = -1;
  BOOST_FOREACH( Vector2 const& loop, hpts ) {
    const Vector3 u3 = camera_model.pixel_to_vector(loop);
    double cs = dot_prod(cahv_model.A, normalize(u3 - dot_prod(dn, u3) * dn));
    if (hmin > cs) hmin = cs;
    if (hmax < cs) hmax = cs;
  }
  double vmin = 1, vmax = -1;
  BOOST_FOREACH( Vector2 const& loop, vpts ) {
    const Vector3 u3 = camera_model.pixel_to_vector(loop);
    double cs = dot_prod(cahv_model.A,normalize(u3 - dot_prod(rt, u3)*rt));
    if (vmin > cs) vmin = cs;
    if (vmax < cs) vmax = cs;
  }

  // Compute the all-encompassing scale factors
  Vector2 cosines;
  if ( minfov ) {
    // use max
    cosines = Vector2(hmax,vmax);
  } else {
    // use min
    cosines = Vector2(hmin,vmin);
  }
  if ( acos(cosines[0]) > limfov ) cosines[0] = cos(limfov);
  if ( acos(cosines[1]) > limfov ) cosines[1] = cos(limfov);
  Vector2 scalars =
    elem_quot(elem_prod(cahv_image_size/2.0,cosines),
              sqrt(Vector2(1,1) - elem_prod(cosines,cosines)));

  // Assign idealized image centers and coordinate angles
  Vector2 centers = (cahv_image_size - Vector2(1,1))/2.0;

  // Construct H and V
  cahv_model.H = scalars[0] * rt + centers[0] * cahv_model.A;
  cahv_model.V = scalars[1] * dn + centers[1] * cahv_model.A;

  return cahv_model;
}


///////////////////////////////////////////////////////////////////////////////
// assign_data.hpp                                                           //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_TOOLS_DETAIL_SERIES_ASSIGN_DATA_HPP_ER_2010
#define BOOST_STATISTICS_TOOLS_DETAIL_SERIES_ASSIGN_DATA_HPP_ER_2010
#include <boost/mpl/foreach.hpp>
#include <boost/mpl/detail/wrapper.hpp>
#include <boost/accumulators/framework/accumulator_set.hpp>
#include <boost/accumulators/framework/accumulator_set.hpp>
#include <boost/statistics/tools/series/data_field.hpp>
#include <boost/statistics/tools/series/traits.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace series{

    // Each call to the unary functor extracts the specified feature from the
    // accumulator (A) and assigns it to the internally refererenced data (D).
    template<typename A,typename D>
    struct data_assigner{

        data_assigner(const A& acc,D& data)
        :acc_( acc ),data_( data ){}

        template<typename Tag>
        void operator()(const boost::mpl::wrapper<Tag>& feature)const
        {
            namespace ns = boost::accumulators;
            get_field<Tag>( data ) = ns::extract_result<Tag>( this->acc );
        }

        template<typename Tag,typename Args>
        void operator()(
            const boost::mpl::wrapper<Tag>& feature,const Args& args)const
        {
            namespace ns = boost::accumulators;
            get_field<Tag>( data ) = ns::extract_result<Tag>( this->acc, args );
        }

        private:
        const &A acc_;
        mutable D& data_

    };

    // Extracts the features specified by the mpl vector F from the accumulator
    // of type A, and assigns them to the data-structure D.
    template<typename F,typename A,typename D>
    void assign_data(const A& acc,D& data){
        typedef data_assigner<F,A,D> assigner_;
        typedef boost::mpl::detail::wrapper<boost::mpl::_> op_;
        boost::mpl::for_each<S,op_>(assigner_(acc,data));
    };

    template<typename F,typename A,typename D>
    void assign_data(const A& acc,const A1&,D& data){
        typedef data_assigner<F,A,D> assigner_;
        typedef boost::mpl::detail::wrapper<boost::mpl::_> op_;
        boost::mpl::for_each<S,op_>(assigner_(acc,data));
    };

}// series
}// detail
}// statistics
}// boost

#endif


// Copyright (c) 2020 fortiss GmbH
//
// Authors: Julian Bernhard, Klemens Esterle, Patrick Hart and
// Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef BARK_GEOMETRY_LINE_HPP_
#define BARK_GEOMETRY_LINE_HPP_

#include <algorithm>
#include <cmath>
#include <limits>
#include <tuple>
#include <utility>
#include <vector>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/tuple/tuple.hpp>

#include "bark/geometry/angle.hpp"
#include "bark/geometry/commons.hpp"

#include "src/spline.h"

namespace bark {
namespace geometry {

//! templated line class with a boost polygon as a member function
template <typename T>
class Line_t : public Shape<bg::model::linestring<T>, T> {
 public:
  Line_t()
      : Shape<bg::model::linestring<T>, T>(Pose(0, 0, 0), std::vector<T>(), 0) {
  }

  explicit Line_t(const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& points);

  virtual Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> ToArray() const;

  virtual std::shared_ptr<Shape<bg::model::linestring<T>, T>> Clone() const;

  //! TODO(@all): do not recompute full s but only add one point
  bool AddPoint(const T& p) {
    return Shape<bg::model::linestring<T>, T>::AddPoint(p) && RecomputeS();
  }

  bool AddPoints(const std::vector<T>& pts) {
    for (const auto& p : pts) {
      Shape<bg::model::linestring<T>, T>::AddPoint(p);
    }
    return RecomputeS();
  }

  auto Length() const {
    return bg::length(Shape<bg::model::linestring<T>, T>::obj_);
  }

  unsigned int size() const {
    return Shape<bg::model::linestring<T>, T>::obj_.size();
  }

  void AppendLinestring(const Line_t& ls) {
    bg::append(Shape<bg::model::linestring<T>, T>::obj_, ls.obj_);
    RecomputeS();
  }

  std::vector<T> GetPointsInSInterval(double begin, double end) const {
    std::vector<T> points;
    uint begin_idx = std::upper_bound(s_.begin(), s_.end(), begin) - s_.begin();
    uint end_idx = std::lower_bound(s_.begin(), s_.end(), end) - s_.begin();
    std::copy(Shape<bg::model::linestring<T>, T>::obj_.begin() + begin_idx,
              Shape<bg::model::linestring<T>, T>::obj_.begin() + end_idx,
              std::back_inserter(points));
    return points;
  }

  virtual bool Valid() const {
    return Shape<bg::model::linestring<T>, T>::Valid() && s_.size() == size();
  }

  void Reverse() {
    boost::geometry::reverse(Shape<bg::model::linestring<T>, T>::obj_);
  }

  typedef typename std::vector<T>::iterator point_iterator;
  typedef typename std::vector<T>::const_iterator const_point_iterator;
  point_iterator begin() {
    return Shape<bg::model::linestring<T>, T>::obj_.begin();
  }
  const_point_iterator begin() const {
    return Shape<bg::model::linestring<T>, T>::obj_.begin();
  }
  point_iterator end() {
    return Shape<bg::model::linestring<T>, T>::obj_.end();
  }
  const_point_iterator end() const {
    return Shape<bg::model::linestring<T>, T>::obj_.end();
  }

  typedef typename std::vector<T>::reverse_iterator reverse_point_iterator;
  typedef typename std::vector<T>::const_reverse_iterator
      const_reverse_point_iterator;  // NOLINT

  reverse_point_iterator rbegin() {
    return Shape<bg::model::linestring<T>, T>::obj_.rbegin();
  }
  const_reverse_point_iterator rbegin() const {
    return Shape<bg::model::linestring<T>, T>::obj_.rbegin();
  }
  reverse_point_iterator rend() {
    return Shape<bg::model::linestring<T>, T>::obj_.rend();
  }
  const_reverse_point_iterator rend() const {
    return Shape<bg::model::linestring<T>, T>::obj_.rend();
  }

  //! local coordinates 0..[total distance] along the lines
  std::vector<double> s_;

  //! @todo free function, s_ private?
  bool RecomputeS() {
    s_.clear();
    // edge case no points
    if (Shape<bg::model::linestring<T>, T>::obj_.empty()) {
      return true;
    } else if (Shape<bg::model::linestring<T>, T>::obj_.size() == 1) {
      // edge case one point
      s_.push_back(0.0);
      return true;
    } else {  // nominal case
      // compute distance from last point to next point and
      // store these on vector s_ (avoid additional tmp vector)
      T last_pt = Shape<bg::model::linestring<T>, T>::obj_.front();
      s_.reserve(Shape<bg::model::linestring<T>, T>::obj_.size());
      double distance_until_now = 0.0;
      for (const T& next_pt : Shape<bg::model::linestring<T>, T>::obj_) {
        distance_until_now += bg::distance(next_pt, last_pt);
        s_.push_back(distance_until_now);
        last_pt = next_pt;
      }
      return true;
    }
  }

  void RemoveDuplicates() {
    bg::unique(Shape<bg::model::linestring<T>, T>::obj_);
    RecomputeS();
  }
  bool operator==(const Line_t& rhs) const {
    return bg::equals(this->obj_, rhs.obj_);
  }
  bool operator!=(const Line_t& rhs) const { return !(rhs == *this); }
};

//! for better usage simple double defines
using LinePoint = Point2d;
using Line = Line_t<LinePoint>;

template <typename T>
inline Line_t<T>::Line_t(
    const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& points)
    : Shape<bg::model::linestring<T>, T>(points, 0) {
  RecomputeS();
}

template <>
inline Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Line::ToArray()
    const {
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> mat(obj_.size(), 2);
  for (uint32_t i = 0; i < obj_.size(); i++) {
    mat.row(i) << bg::get<0>(obj_[i]), bg::get<1>(obj_[i]);
  }
  return mat;
}

template <typename T>
inline std::shared_ptr<Shape<bg::model::linestring<T>, T>> Line_t<T>::Clone()
    const {
  std::shared_ptr<Line_t<T>> new_line = std::make_shared<Line_t<T>>(*this);
  return new_line;
}

template <typename T>
inline Line Reverse(const T& l) {
  T lr = l;
  lr.Reverse();
  return lr;
}

inline double Distance(const Line& line, const Point2d& p) {
  return bg::distance(line.obj_, p);
}

inline double Distance(const Line& line, const Line& line2) {
  return bg::distance(line.obj_, line2.obj_);
}

template <typename T>
inline T Length(const Line& line) {
  return bg::length<T>(line.obj_);
}

inline Line Rotate(const Line& line, double hdg) {
  using boost::geometry::strategy::transform::rotate_transformer;
  rotate_transformer<boost::geometry::radian, double, 2, 2> rotate(hdg);
  Line line_rotated;
  boost::geometry::transform(line.obj_, line_rotated.obj_, rotate);
  line_rotated.RecomputeS();
  return line_rotated;
}

inline Line Translate(const Line& line, double x, double y) {
  using boost::geometry::strategy::transform::translate_transformer;
  translate_transformer<double, 2, 2> translate(x, y);
  Line line_translated;
  boost::geometry::transform(line.obj_, line_translated.obj_, translate);
  return line_translated;
}

inline Line Simplify(const Line& line, double max_distance) {
  Line temp_line;
  boost::geometry::simplify(line.obj_, temp_line.obj_, max_distance);
  temp_line.RecomputeS();
  return temp_line;
}

inline int GetSegmentEndIdx(Line l, double s) {
  std::vector<double>::iterator up =
      std::upper_bound(l.s_.begin(), l.s_.end(), s);
  if (up != l.s_.end()) {
    int retval = up - l.s_.begin();
    return retval;
  } else {
    return l.s_.size() - 1;  // last point if s is larger then line length
  }
}

inline bool CheckSForSegmentIntersection(Line l, double s) {
  int start_it = GetSegmentEndIdx(l, s);
  std::vector<double>::iterator low =
      std::lower_bound(l.s_.begin(), l.s_.end(), s);
  int start_it_low = low - l.s_.begin();
  return start_it != start_it_low;
}

inline Point2d GetPointAtIdx(const Line& l, const uint idx) {
  if (idx > l.obj_.size() - 1) {
    LOG(WARNING) << "idx is outside line";
    return l.obj_.back();
  } else {
    return l.obj_.at(idx);
  }
}

inline Eigen::VectorXd Gradient(Eigen::VectorXd vec) {
  // calculating central difference
  Eigen::VectorXd g(vec.size());
  for (int i = 1; i < vec.size() - 1; i++) {
    g(i) = (vec(i + 1) - vec(i - 1)) / 2;
  }
  // TODO: find better solution for first and last point
  g(0) = g(1);
  g(vec.size() - 1) = g(vec.size() - 2);
  return g;
}

inline Eigen::VectorXd GetCurvature(const Line& l) {
  Eigen::MatrixXd larray = l.ToArray();
  Eigen::VectorXd dx = Gradient(larray.col(0));
  Eigen::VectorXd ddx = Gradient(dx);
  Eigen::VectorXd dy = Gradient(larray.col(1));
  Eigen::VectorXd ddy = Gradient(dy);

  // elementwise, as pow(vector, scalar) does not work
  Eigen::VectorXd curvature(larray.rows());
  for (int i = 0; i < curvature.size(); i++) {
    double n = dx(i) * ddy(i) - ddx(i) * dy(i);
    double r = pow(dx(i), 2) + pow(dy(i), 2);
    double d = pow(r, 1.5);
    if (d == 0) {
      curvature(i) = 0;
    } else {
      curvature(i) = n / d;
    }
  }
  return curvature;
}

inline Point2d GetPointAtS(Line l, double s) {
  const size_t& length = l.obj_.size();
  if (length <= 1) {  // this is an error Line consist of 0 or 1 element
    return Point2d(0, 0);
  } else if (s <= 0.0) {  // edge case begin
    return l.obj_.at(0);
  } else if (s >= l.s_.back()) {  // edge case end
    return l.obj_.at(length - 1);
  } else {  // nominal case
    int segment_end_idx = GetSegmentEndIdx(l, s);
    int segment_begin_idx = segment_end_idx - 1;

    double s_on_segment =
        (s - l.s_.at(segment_begin_idx)) /
        (l.s_.at(segment_end_idx) - l.s_.at(segment_begin_idx));
    double interp_pt_x =
        bg::get<0>(l.obj_.at(segment_begin_idx)) +
        s_on_segment * (bg::get<0>(l.obj_.at(segment_end_idx)) -
                        bg::get<0>(l.obj_.at(segment_begin_idx)));
    double interp_pt_y =
        bg::get<1>(l.obj_.at(segment_begin_idx)) +
        s_on_segment * (bg::get<1>(l.obj_.at(segment_end_idx)) -
                        bg::get<1>(l.obj_.at(segment_begin_idx)));
    return Point2d(interp_pt_x, interp_pt_y);
  }
}

inline double GetTangentAngleAtS(Line l, double s) {
  if (s >= l.s_.back()) {
    Point2d p1 = l.obj_.at(l.obj_.size() - 2);
    Point2d p2 = l.obj_.at(l.obj_.size() - 1);
    double angle =
        atan2(bg::get<1>(p2) - bg::get<1>(p1), bg::get<0>(p2) - bg::get<0>(p1));
    return angle;
  } else if (s <= 0.0) {
    Point2d p1 = l.obj_.at(0);
    Point2d p2 = l.obj_.at(1);
    return atan2(bg::get<1>(p2) - bg::get<1>(p1),
                 bg::get<0>(p2) - bg::get<0>(p1));
  } else {  // not start or end
    int end_segment_it = GetSegmentEndIdx(l, s);
    // check if s is at intersection, if true then calculate the angle
    if (CheckSForSegmentIntersection(l, s)) {
      Point2d p1 = l.obj_.at(end_segment_it - 2);
      Point2d p2 = l.obj_.at(end_segment_it - 1);
      Point2d p3 = l.obj_.at(end_segment_it);
      double sin_mean = 0.5 * (sin(atan2(bg::get<1>(p2) - bg::get<1>(p1),
                                         bg::get<0>(p2) - bg::get<0>(p1))) +
                               sin(atan2(bg::get<1>(p3) - bg::get<1>(p2),
                                         bg::get<0>(p3) - bg::get<0>(p2))));
      double cos_mean = 0.5 * (cos(atan2(bg::get<1>(p2) - bg::get<1>(p1),
                                         bg::get<0>(p2) - bg::get<0>(p1))) +
                               cos(atan2(bg::get<1>(p3) - bg::get<1>(p2),
                                         bg::get<0>(p3) - bg::get<0>(p2))));
      return atan2(sin_mean, cos_mean);
    } else {  // every s not start, end or intersection
      Point2d p1 = l.obj_.at(end_segment_it - 1);
      Point2d p2 = l.obj_.at(end_segment_it);
      return atan2(bg::get<1>(p2) - bg::get<1>(p1),
                   bg::get<0>(p2) - bg::get<0>(p1));
    }
  }
}

inline Point2d GetNormalAtS(const Line& l, double s) {
  double tangent = GetTangentAngleAtS(l, s);
  // rotate unit vector anti-clockwise with angle = tangent by 1/2 pi
  Point2d t(cos(tangent + asin(1)), sin(tangent + asin(1)));
  return t;
}

inline Line GetLineFromSInterval(const Line& line, double begin, double end) {
  Line new_line;
  new_line.AddPoint(GetPointAtS(line, begin));
  std::vector<Point2d> points = line.GetPointsInSInterval(begin, end);
  for (auto const& point : points) {
    new_line.AddPoint(point);
  }
  new_line.AddPoint(GetPointAtS(line, end));
  return new_line;
}

inline Line GetLineShiftedLaterally(const Line& line, double lateral_shift) {
  Line new_line;
  for (const auto& s : line.s_) {
    const Point2d normal = GetNormalAtS(line, s);
    const Point2d point_at_s = GetPointAtS(line, s);
    const Point2d shifted = point_at_s + (normal * lateral_shift);
    new_line.AddPoint(shifted);
  }
  return new_line;
}

inline std::tuple<Point2d, double, uint> GetNearestPointAndS(
    const Line& l, const Point2d& p) {  // GetNearestPoint
  // edge cases: empty or one-point line
  if (l.obj_.empty()) {
    return std::make_tuple(Point2d(0, 0), 0.0, 0);
  } else if (l.obj_.size() == 1) {
    return std::make_tuple(l.obj_.at(0), 0.0, 0);
  }

  // nominal case:
  // check distance to each line segment and find closest segment
  double min_dist = boost::numeric::bounds<double>::highest();
  int min_segment_idx = 0;
  for (uint line_idx = 0; line_idx < l.obj_.size() - 1; ++line_idx) {
    bg::model::linestring<Point2d> current_segment;
    bg::append(current_segment, l.obj_.at(line_idx));
    bg::append(current_segment, l.obj_.at(line_idx + 1));
    double d = bg::comparable_distance(current_segment, p);
    if (d < min_dist) {
      min_dist = d;
      min_segment_idx = line_idx;
    }
  }

  const double a1 = bg::get<0>(l.obj_.at(min_segment_idx));
  const double a2 = bg::get<1>(l.obj_.at(min_segment_idx));
  const double b1 = bg::get<0>(l.obj_.at(min_segment_idx + 1));
  const double b2 = bg::get<1>(l.obj_.at(min_segment_idx + 1));
  const double p1 = bg::get<0>(p);
  const double p2 = bg::get<1>(p);

  const double lambda = -(a1 * b1 + a2 * b2 + a1 * p1 + a2 * p2 - b1 * p1 -
                          b2 * p2 - a1 * a1 - a2 * a2)  // NOLINT
                        / (a1 * a1 - 2 * a1 * b1 + a2 * a2 - 2 * a2 * b2 +
                           b1 * b1 + b2 * b2);  // NOLINT

  // calculate interpolated s value
  double s;
  // double dist;  // unused
  Point2d retval;

  if (lambda < 0) {  // extrapolation front
    s = l.s_.at(min_segment_idx);
    retval = Point2d(a1, a2);
    // debug
    // dist = sqrt(pow(p1 - a1, 2) + pow(p2 - a2, 2));
  } else if (lambda > 1) {  // extrapolation end
    s = l.s_.at(min_segment_idx + 1);
    retval = Point2d(b1, b2);
    // debug
    // dist = sqrt(pow(p1 - b1, 2) + pow(p2 - b2, 2));
  } else {  // real interpolation
    s = (1 - lambda) * l.s_.at(min_segment_idx) +
        lambda * l.s_.at(min_segment_idx + 1);  // NOLINT

    const double s1 =
        (p1 * a1 * a1 - a1 * a2 * b2 + p2 * a1 * a2 - 2 * p1 * a1 * b1 +
         a1 * b2 * b2 - p2 * a1 * b2 + a2 * a2 * b1 - a2 * b1 * b2 -
         p2 * a2 * b1 + p1 * b1 * b1 + p2 * b1 * b2)  // NOLINT
        / (a1 * a1 - 2 * a1 * b1 + a2 * a2 - 2 * a2 * b2 + b1 * b1 +
           b2 * b2);  // NOLINT
    const double s2 =
        (a1 * a1 * b2 - a1 * a2 * b1 + p1 * a1 * a2 - a1 * b1 * b2 -
         p1 * a1 * b2 + p2 * a2 * a2 + a2 * b1 * b1 - p1 * a2 * b1 -
         2 * p2 * a2 * b2 + p1 * b1 * b2 + p2 * b2 * b2)  // NOLINT
        / (a1 * a1 - 2 * a1 * b1 + a2 * a2 - 2 * a2 * b2 + b1 * b1 +
           b2 * b2);  // NOLINT

    // debug
    // dist = sqrt(pow(p1 - s1, 2) + pow(p2 - s2, 2));
    retval = Point2d(s1, s2);
  }

  // debug
  // const double dist_boost = bg::distance(l.obj_, p);

  // return
  return std::make_tuple(retval, s, min_segment_idx);
}
inline Point2d GetNearestPoint(const Line& l, const Point2d& p) {
  return std::get<0>(GetNearestPointAndS(l, p));
}
inline double GetNearestS(const Line& l, const Point2d& p) {
  return std::get<1>(GetNearestPointAndS(l, p));
}
inline uint FindNearestIdx(const Line& l, const Point2d& p) {
  return std::get<2>(GetNearestPointAndS(l, p));
}
//! Point - Line collision checker using boost::intersection
inline bool Collide(const Line& l, const LinePoint& p) {
  std::vector<LinePoint> shape_intersect;
  bg::intersection(l.obj_, p, shape_intersect);
  return !shape_intersect.empty();
}

//! Line - Point collision checker
inline bool Collide(const LinePoint& p, const Line& l) { return Collide(l, p); }

//! Line - Line collision checker using boost::intersection
inline bool Collide(const Line& l1, const Line& l2) {
  std::vector<bg::model::linestring<LinePoint>> shape_intersect;
  bg::intersection(l1.obj_, l2.obj_, shape_intersect);
  return !shape_intersect.empty();
}

// An oriented point can have a linestring (the nearest point on it)
// on the left or right side, left side < 0, right side > 0
inline double SignedDistance(const Line& line, const Point2d& p,
                             const double& orientation) {
  auto closest_point = GetNearestPoint(line, p);
  auto direction_vector = closest_point - p;

  double diff = SignedAngleDiff(
      orientation,
      atan2(bg::get<1>(direction_vector), bg::get<0>(direction_vector)));
  double sign = (diff > 0) ? 1 : ((diff < 0) ? -1 : 0);

  return bg::distance(line.obj_, p) * sign;
}

inline Line AppendLinesNoIntersect(const Line& ls1, const Line& ls2) {
  std::vector<Point2d> intersecting_points;
  bg::intersection(ls1.obj_, ls2.obj_, intersecting_points);
  Line lout;
  if (intersecting_points.size() == 1) {
    // get s value for both lines
    double s_i1 = GetNearestS(ls1, intersecting_points.at(0));
    double s_i2 = GetNearestS(ls2, intersecting_points.at(0));
    double rel_s_i1 = s_i1 / ls1.Length();
    double rel_s_i2 = s_i2 / ls2.Length();

    Line ls1_part, ls2_part;
    if (rel_s_i1 < 0.3) {
      // take latter part of line
      ls1_part = GetLineFromSInterval(ls1, s_i1, ls1.Length());
    } else if (rel_s_i1 > 0.7) {
      // take front part of line
      ls1_part = GetLineFromSInterval(ls1, 0, s_i1);
    } else {
      LOG(WARNING) << "Lines intersecting too much, only appending";
      ls1_part = ls1;
    }

    if (rel_s_i2 < 0.3) {
      // take latter part of line
      ls2_part = GetLineFromSInterval(ls2, s_i2, ls2.Length());
    } else if (rel_s_i2 > 0.7) {
      // take front part of line
      ls2_part = GetLineFromSInterval(ls2, 0, s_i2);
    } else {
      LOG(WARNING) << "Lines intersecting too much, only appending";
      ls2_part = ls2;
    }

    lout = ls1_part;
    lout.AppendLinestring(ls2_part);
  } else if (intersecting_points.size() > 1) {
    // do something
    LOG(ERROR) << "two intersecting points";
    lout = ls1;
    lout.AppendLinestring(ls2);
  } else {
    lout = ls1;
    lout.AppendLinestring(ls2);
  }

  lout.RemoveDuplicates();

  if (boost::geometry::intersects(lout.obj_)) {
    const double tol = 1e-6;
    LOG(WARNING) << "AppendLinesNoIntersect yields self intersecting line, "
                    "will simplify it with "
                 << tol;
    VLOG(5) << "ls1: " << ls1.ToArray();
    VLOG(5) << "ls2: " << ls2.ToArray();
    VLOG(5) << "lout: " << lout.ToArray();

    lout = Simplify(lout, tol);
  }
  return lout;
}

inline Line ConcatenateLinestring(const Line& ls1, const Line& ls2) {
  // Get first and last points
  auto first_point_this = *ls1.begin();
  auto last_point_this = *(ls1.end() - 1);
  auto first_point_other = *ls2.begin();
  auto last_point_other = *(ls2.end() - 1);

  double d_first_first = Distance(first_point_this, first_point_other);
  double d_first_last = Distance(first_point_this, last_point_other);
  double d_last_first = Distance(last_point_this, first_point_other);
  double d_last_last = Distance(last_point_this, last_point_other);

  Line lconcat;
  if (d_first_first <= std::min({d_first_last, d_last_first, d_last_last})) {
    // Reverse this
    lconcat = AppendLinesNoIntersect(Reverse(ls1), ls2);
  } else if (d_first_last <=
             std::min({d_first_first, d_last_first, d_last_last})) {
    // Reverse both
    lconcat = AppendLinesNoIntersect(Reverse(ls1), Reverse(ls2));
  } else if (d_last_first <=
             std::min({d_first_first, d_first_last, d_last_last})) {
    // No reversing
    lconcat = AppendLinesNoIntersect(ls1, ls2);
  } else {
    // Reverse other
    lconcat = AppendLinesNoIntersect(ls1, Reverse(ls2));
  }
  return lconcat;
}

// Subsampling using spline
inline Line SmoothLine(const Line& l, const double ds) {
  if (l.size() < 3) {
    LOG(WARNING) << "cannot subsample line with only 3 points";
    return l;
  } else {
    int num_points = l.Length() / ds;

    tk::spline splineX, splineY;
    std::vector<double> xVec, yVec;
    for (size_t i = 0; i < l.obj_.size(); i++) {
      xVec.push_back(bg::get<0>(l.obj_[i]));
      yVec.push_back(bg::get<1>(l.obj_[i]));
    }
    std::vector<double> sVec(l.s_.begin(), l.s_.end());
    splineX.set_points(sVec, xVec);
    splineY.set_points(sVec, yVec);

    Line lss;
    for (size_t j = 0; j <= num_points; ++j) {
      double x = splineX(j * ds);
      double y = splineY(j * ds);
      lss.AddPoint(geometry::Point2d(x, y));
    }
    if (l.Length() > num_points * ds) {
      lss.AddPoint(l.obj_.at(l.size() - 1));
    }
    return lss;
  }
}

inline Line ComputeCenterLine(const Line& outer_line, const Line& inner_line) {
  if (boost::geometry::intersects(outer_line.obj_)) {
    LOG(WARNING) << "Computing center line, but outer line self-intersects";
  }
  if (boost::geometry::intersects(inner_line.obj_)) {
    LOG(WARNING) << "Computing center line, but inner line self-intersects";
  }

  Line center_line_;
  Line line_more_points = outer_line;
  Line line_less_points = inner_line;
  if (inner_line.obj_.size() > outer_line.obj_.size()) {
    line_more_points = inner_line;
    line_less_points = outer_line;
  }
  for (Point2d& point_loop : line_more_points.obj_) {
    Point2d nearest_point_other =
        geometry::GetNearestPoint(line_less_points, point_loop);
    geometry::Point2d middle_point = (point_loop + nearest_point_other) / 2;
    center_line_.AddPoint(middle_point);
  }
  return center_line_;
}

template <typename T>
std::pair<T, T> MergeBoundingBoxes(std::pair<T, T> bb1, std::pair<T, T> bb2) {
  Line_t<T> line;  // just use a line and add all points
  line.AddPoint(bb1.first);
  line.AddPoint(bb1.second);
  line.AddPoint(bb2.first);
  line.AddPoint(bb2.second);
  return line.BoundingBox();
}

}  // namespace geometry
}  // namespace bark

#endif  // BARK_GEOMETRY_LINE_HPP_


#pragma once

#include <polyfem/ElementBases.hpp>
#include <polyfem/AssemblerUtils.hpp>
#include <polyfem/LocalBoundary.hpp>
#include <polyfem/Mesh.hpp>

#include <polyfem/Common.hpp>

#include <vector>
#include <Eigen/Dense>
#include <memory>

namespace polyfem
{
	class Problem
	{
	public:
		Problem(const std::string &name);
		virtual void init(const Mesh &mesh) {}

		virtual void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const = 0;
		virtual bool is_rhs_zero() const = 0;

		virtual void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const = 0;
		virtual void velocity_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const {}
		virtual void acceleration_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const {}

		virtual void neumann_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const Eigen::MatrixXd &normals, const double t, Eigen::MatrixXd &val) const {}
		virtual void neumann_velocity_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const Eigen::MatrixXd &normals, const double t, Eigen::MatrixXd &val) const {}
		virtual void neumann_acceleration_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const Eigen::MatrixXd &normals, const double t, Eigen::MatrixXd &val) const {}

		virtual void exact(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const {};
		virtual void exact_grad(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const {};

		inline const std::string &name() const { return name_; }

		virtual bool has_exact_sol() const = 0;
		virtual bool is_scalar() const = 0;
		// virtual bool is_mixed() const { return false; }
		virtual bool is_constant_in_time() const { return true; }

		virtual void clear() {}

		virtual bool is_time_dependent() const { return false; }
		virtual void initial_solution(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const {}
		virtual void initial_velocity(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const {}
		virtual void initial_acceleration(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const {}
		virtual void initial_density(const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const {}

		virtual int n_incremental_load_steps(const double diag) const { return 1; }

		virtual void set_parameters(const json &params) {}

		void setup_bc(const Mesh &mesh, const std::vector<ElementBases> &bases, const std::vector<ElementBases> &pressure_bases, std::vector<LocalBoundary> &local_boundary, std::vector<int> &boundary_nodes, std::vector<LocalBoundary> &local_neumann_boundary, std::vector<int> &pressure_boundary_nodes);

		// std::vector<int> &boundary_ids() { return boundary_ids_; }
		// const std::vector<int> &boundary_ids() const { return boundary_ids_; }

		virtual bool is_dimension_dirichet(const int tag, const int dim) const { return true; }

		//here for efficiency reasons
		virtual bool all_dimensions_dirichlet() const { return true; }

		virtual ~Problem() {}

	protected:
		std::vector<int> boundary_ids_;
		std::vector<int> neumann_boundary_ids_;
		std::vector<int> pressure_boundary_ids_;
		std::vector<int> splitting_pressure_boundary_ids_;

	private:
		std::string name_;
	};

	class ProblemFactory
	{
	public:
		static const ProblemFactory &factory();

		std::shared_ptr<Problem> get_problem(const std::string &problem) const;
		inline const std::vector<std::string> &get_problem_names() const { return problem_names_; }

	private:
		ProblemFactory();
		std::map<std::string, std::function<std::shared_ptr<Problem>()>> problems_;
		std::vector<std::string> problem_names_;
	};
} // namespace polyfem


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is 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_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_AREA_HUILLER_EARTH_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_AREA_HUILLER_EARTH_HPP

#include <boost/geometry/strategies/spherical/area_huiller.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace area
{

template
<
    typename PointOfSegment,
    typename CalculationType = void
>
class huiller_earth
    : public huiller<PointOfSegment, CalculationType>
{
public :
    // By default the average earth radius.
    // Uses can specify another radius.
    // Note that the earth is still handled spherically
    inline huiller_earth(double radius = 6372795.0)
        : huiller<PointOfSegment, CalculationType>(radius)
    {}
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename Point>
struct default_strategy<geographic_tag, Point>
{
    typedef huiller_earth<Point> type;
};

} // namespace services

#endif

}} // namespace strategy::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_AREA_HUILLER_EARTH_HPP


#include <ros/ros.h>
#include "ros/package.h"

#include <tf/tf.h>
#include <tf/transform_listener.h>

#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>

#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>

// #include <visualization_msgs/Marker.h>
// #include <visualization_msgs/MarkerArray.h>
#include "visualization_tools/bounding_box_array.h"
#include "visualization_tools/bounding_box.h"

#include <ssd_ros_msgs/BoundingBox.h>
#include <ssd_ros_msgs/BoundingBoxArray.h>
#include <ssd_ros_msgs/SSD.h>
#include <ssd_ros_msgs/Cluster.h>
#include <ssd_ros_msgs/ClusterArray.h>

#include <Eigen/Core>

using namespace std;
using namespace Eigen;

typedef pcl::PointXYZRGB PointA;
//typedef pcl::PointXYZINormal PointA;  // 法線情報・反射強度を利用する場合はNormal型に変更
typedef pcl::PointCloud<PointA> CloudA;
typedef pcl::PointCloud<PointA>::Ptr CloudAPtr;

ros::Publisher pub_cluster_centroid;
ros::Publisher pub_cluster_points;
ros::Publisher pub_cluster_centroid_max;
ros::Publisher pub_cluster_points_max;
ros::Publisher pub_cluster_array;

string target_frame = "/zed_left_camera";
string source_frame = "/velodyne";

bool pc_flag = false;

void getClusterInfo(CloudA pt, PointA& cluster)
{
    Vector3f centroid;
	centroid[0]=pt.points[0].x;
	centroid[1]=pt.points[0].y;
	centroid[2]=pt.points[0].z;

	Vector3f min_p;
	min_p[0]=pt.points[0].x;
	min_p[1]=pt.points[0].y;
    min_p[2]=pt.points[0].z;

	Vector3f max_p;
	max_p[0]=pt.points[0].x;
	max_p[1]=pt.points[0].y;
	max_p[2]=pt.points[0].z;

	for(size_t i=1;i<pt.points.size();i++){
        centroid[0]+=pt.points[i].x;
        centroid[1]+=pt.points[i].y;
        centroid[2]+=pt.points[i].z;
		if (pt.points[i].x<min_p[0]) min_p[0]=pt.points[i].x;
		if (pt.points[i].y<min_p[1]) min_p[1]=pt.points[i].y;
        if (pt.points[i].z<min_p[2]) min_p[2]=pt.points[i].z;

		if (pt.points[i].x>max_p[0]) max_p[0]=pt.points[i].x;
		if (pt.points[i].y>max_p[1]) max_p[1]=pt.points[i].y;
		if (pt.points[i].z>max_p[2]) max_p[2]=pt.points[i].z;
    }

	cluster.x=centroid[0]/(float)pt.points.size();
    cluster.y=centroid[1]/(float)pt.points.size();
    cluster.z=centroid[2]/(float)pt.points.size();
	// cluster.normal_x=max_p[0]-min_p[0];
	// cluster.normal_y=max_p[1]-min_p[1];
	// cluster.normal_z=max_p[2];
}

bool height_detection(float height)
{
    bool flag = false;
    if(0.5<=height && height<=1.7) flag = true;
    return flag;
}

bool width_detection(float width)
{
    bool flag = false;
    if(0.3<= width && width<=0.8) flag = true;
    return flag;
}

bool depth_detection(float depth)
{
    bool flag = false;
    if (0.2 < depth) flag = true;
    return flag;
}

bool size_detection(float size)
{
    bool flag = false;
    if (200<size) flag = true;
    return flag;
}

bool curvature_detection(float curvature)
{
    bool flag = false;
    if (0.10 < curvature) flag = true;
    return flag;
}

bool object_detection(BoundingBox boundingbox)
{
    cout<<"detection"<<endl;
    bool detect = false;

    float height    = boundingbox.height();
    float width     = boundingbox.width();
    float depth     = boundingbox.depth();
    float curvature = boundingbox.curvature();

    bool height_flag    = height_detection(height);
    bool width_flag     = width_detection(width);
    bool depth_flag     = depth_detection(depth);
    bool curvature_flag = curvature_detection(curvature);

    if(curvature_flag && depth_flag && height_flag && width_flag){
        printf("H:%.2f W:%.2f D:%.2f Curv:%.2f\n", height, width, depth, curvature);
        detect = true;
    }

    return detect;
}

void sampling_cpu_clustering(CloudAPtr pcl_in, CloudA& cluster_centroid, CloudA& cluster_pt, CloudA& max_centroid, CloudA& max_pt)
{
    //Downsample//
    pcl::VoxelGrid<PointA> vg;
	CloudAPtr ds_cloud (new CloudA);
	vg.setInputCloud (pcl_in);
	vg.setLeafSize (0.01f, 0.01f, 0.01f);
	vg.filter (*ds_cloud);
    //downsampled point's z =>0
    vector<float> tmp_z;
    tmp_z.resize(ds_cloud->points.size());
	for(int i=0;i<(int)ds_cloud->points.size();i++){
        tmp_z[i]=ds_cloud->points[i].z;
		ds_cloud->points[i].z  = 0.0;
    }
    //Clustering//
    pcl::search::KdTree<PointA>::Ptr tree (new pcl::search::KdTree<PointA>);
    tree->setInputCloud (ds_cloud);
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<PointA> ec;
    ec.setClusterTolerance (0.05); // 15cm
    ec.setMinClusterSize (10);
    ec.setMaxClusterSize (4000);
    ec.setSearchMethod (tree);
    ec.setInputCloud(ds_cloud);
    ec.extract (cluster_indices);
    //reset z value
	for(int i=0;i<(int)ds_cloud->points.size();i++)
        ds_cloud->points[i].z=tmp_z[i];
    //get cluster information//
    size_t num=0;
    cluster_centroid.resize(cluster_indices.size());

    // visualization_toolを利用して検出したclusterが検出したい対象物であるかを判断
    bool detect_cluster=false;

    for(size_t iii=0;iii<cluster_indices.size();iii++){
        //cluster cenrtroid
        CloudAPtr cloud_cluster (new CloudA);
        cloud_cluster->points.resize(cluster_indices[iii].indices.size());
        //cluster points
        cluster_pt.points.resize(cluster_indices[iii].indices.size()+num);
        for(size_t jjj=0;jjj<cluster_indices[iii].indices.size();jjj++){
            int p_num=cluster_indices[iii].indices[jjj];
            cloud_cluster->points[jjj]=ds_cloud->points[p_num];
            cluster_pt.points[num+jjj]=ds_cloud->points[p_num];
        }
        getClusterInfo(*cloud_cluster, cluster_centroid[iii]);

        if (detect_cluster!=true){
            // BoundingBoxArray boundingbox_array;
            // boundingbox_array.push_back<CloudAPtr&>(cloud_cluster);
            // cout << "\033[2J";
            // printf("\033[%d;%dH", 1, 1);
            // cout << boundingbox_array << endl;
            // cout << boundingbox_array.v_bb.curv << endl;
            bool check_cluster = false;
            BoundingBox boundingbox;
            boundingbox.pc2bb(cloud_cluster);
            check_cluster = object_detection(boundingbox);

            if (check_cluster){
                max_pt.points.resize(cloud_cluster->points.size());
                for(size_t jjj=0;jjj<max_pt.points.size();jjj++){
                    max_pt.points[jjj] = cloud_cluster->points[jjj];
                }
                max_centroid.resize(1);
                getClusterInfo(max_pt, max_centroid[0]);
                break;
            }
        }

        // if(iii==0){
        //     max_pt.points.resize(cluster_indices[iii].indices.size()+num);
        //     for(size_t jjj=0;jjj<cluster_indices[iii].indices.size();jjj++){
        //         int p_num=cluster_indices[iii].indices[jjj];
        //         max_pt.points[num+jjj]=ds_cloud->points[p_num];
        //     }
        //     max_centroid.resize(1);
        //     getClusterInfo(*cloud_cluster, max_centroid[iii]);
        // }
        num=cluster_pt.points.size();//save previous size
    }
}

void cluster(const ssd_ros_msgs::SSDConstPtr& pc_msg)
{
    CloudAPtr pcl_cluster_points(new CloudA);
    CloudAPtr pcl_max_points(new CloudA);
    CloudA pcl_cluster_centroid;
    CloudA pcl_max_centroid;

    ssd_ros_msgs::ClusterArray ssd_clusters;

    BoundingBoxArray boundingbox_array;

    int grobal_count = 0;

    for(size_t i=0;i<pc_msg->ssd_array.size();i++)
    {
        int local_count = 0;
        const sensor_msgs::PointCloud2 &pc2_people = pc_msg->ssd_array[i];
        // printf("Num:%d size:%d\n", i+1, pc2_people.width);

        CloudAPtr pcl_people (new CloudA);
        CloudA cluster_points;
        CloudA cluster_centroid;
        CloudA max_centroid;
        CloudA max_points;

        pcl::fromROSMsg(pc2_people, *pcl_people);

		sampling_cpu_clustering(pcl_people, cluster_centroid, cluster_points, max_centroid, max_points);

		for(size_t j=0;j<cluster_centroid.points.size();j++){
            PointA p = cluster_centroid.points[j];
            pcl_cluster_centroid.push_back(p);
            local_count++;
            grobal_count++;
        }
        for(size_t k=0;k<cluster_points.points.size();k++){
            PointA q = cluster_points.points[k];
            pcl_cluster_points->push_back(q);
        }
		// if(0<pcl_cluster_points->points.size()){
		// 	boundingbox_array.push_back<CloudAPtr&>(pcl_cluster_points);
		// }

        // boundingbox_array.push_back<CloudAPtr&>(pcl_cluster_points);

        for(size_t l=0;l<max_centroid.points.size();l++){
            PointA p = max_centroid.points[l];
            pcl_max_centroid.push_back(p);
        }
        for(size_t m=0;m<max_points.points.size();m++){
            PointA q = max_points.points[m];
            pcl_max_points->push_back(q);
        }

        boundingbox_array.push_back<CloudAPtr&>(pcl_max_points);

        if(0<max_points.points.size()){
            ssd_ros_msgs::Cluster ssd_cluster;
            CloudA ssd_centroid;
            CloudA ssd_points;

            for(size_t l=0;l<max_centroid.points.size();l++){
                PointA p = max_centroid.points[l];
                ssd_centroid.push_back(p);
            }
            for(size_t m=0;m<max_points.points.size();m++){
                PointA q = max_points.points[m];
                ssd_points.push_back(q);
            }
            toROSMsg(ssd_centroid, ssd_cluster.centroid);
            toROSMsg(ssd_points, ssd_cluster.points);
            ssd_cluster.centroid.header.frame_id = target_frame;
            ssd_cluster.centroid.header.stamp = ros::Time::now();
            ssd_cluster.points.header.frame_id = target_frame;
            ssd_cluster.points.header.stamp = ros::Time::now();
            ssd_clusters.clusters.push_back(ssd_cluster);
        }

        // if(0<max_points.points.size()){
        //     toROSMsg(pcl_max_centroid, ssd_cluster.centroid);
        //     toROSMsg(pcl_max_points, ssd_cluster.points);
        //     ssd_cluster.centroid.header.frame_id = target_frame;
        //     ssd_cluster.centroid.header.stamp = ros::Time::now();
        //     ssd_cluster.points.header.frame_id = target_frame;
        //     ssd_cluster.points.header.stamp = ros::Time::now();
        //     ssd_clusters.clusters.push_back(ssd_cluster);
        // }

        // printf("Array Num:%d Size:%d Class:%d\n",(int)i+1, (int)pc2_people.width, local_count);
    }

    printf("Total Class:%d\n", grobal_count);
    printf("Array size:%d\n", (int)ssd_clusters.clusters.size());

    sensor_msgs::PointCloud2 cluster_centroid_ros;
    toROSMsg(pcl_cluster_centroid, cluster_centroid_ros);
    cluster_centroid_ros.header.frame_id=target_frame;
    cluster_centroid_ros.header.stamp=ros::Time::now();
    pub_cluster_centroid.publish(cluster_centroid_ros);

    sensor_msgs::PointCloud2 cluster_points_ros;
    toROSMsg(*pcl_cluster_points, cluster_points_ros);
    cluster_points_ros.header.frame_id=target_frame;
    cluster_points_ros.header.stamp=ros::Time::now();
    pub_cluster_points.publish(cluster_points_ros);

    // cout << "\033[2J";
    // printf("\033[%d;%dH", 1, 1);
    // cout << boundingbox_array << endl;
    boundingbox_array.publish();

    sensor_msgs::PointCloud2 max_centroid_ros;
    toROSMsg(pcl_max_centroid, max_centroid_ros);
    max_centroid_ros.header.frame_id = target_frame;
    pub_cluster_centroid_max.publish(max_centroid_ros);

    sensor_msgs::PointCloud2 max_points_ros;
    toROSMsg(*pcl_max_points,max_points_ros);
    max_points_ros.header.frame_id = target_frame;
    pub_cluster_points_max.publish(max_points_ros);

    pub_cluster_array.publish(ssd_clusters);

}

ssd_ros_msgs::SSDConstPtr pc_;
void pcCallback(const ssd_ros_msgs::SSDConstPtr& msg)
{
    // printf("Human Count : %d\n", msg->ssd_array.size());
    pc_ = msg;
    pc_flag = true;
}

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

    ros::Subscriber pc_sub = n.subscribe("/SSD/people",10,pcCallback);

    pub_cluster_centroid=n.advertise<sensor_msgs::PointCloud2>("/cluster/centroid",1);
    pub_cluster_points=n.advertise<sensor_msgs::PointCloud2>("/cluster/points",1);
    pub_cluster_centroid_max=n.advertise<sensor_msgs::PointCloud2>("/cluster/centroid/max",1);
    pub_cluster_points_max=n.advertise<sensor_msgs::PointCloud2>("/cluster/points/max",1);
    pub_cluster_array = n.advertise<ssd_ros_msgs::ClusterArray>("/cluster/array/ssd", 1);

    ros::Rate rate(20);

    while(ros::ok())
    {
        if(pc_flag) cluster(pc_);
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#ifndef BOOST_SIMD_FUNCTION_SCALAR_COT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_COT_HPP_INCLUDED

#include <boost/simd/function/definition/cot.hpp>
#include <boost/simd/arch/common/scalar/function/cot.hpp>

#endif


#include <boost/test/unit_test.hpp>
#include <templ/utils/CartographicMapping.hpp>
#include <templ/symbols/constants/Location.hpp>

BOOST_AUTO_TEST_SUITE(cartographic_mapping)

BOOST_AUTO_TEST_CASE(longitude_latitude_to_metric)
{
    using namespace templ::utils;
    CartographicMapping mapping(CartographicMapping::MOON);
    base::Point point(16, 20.25, 0.0);
    base::Point metricPoint = mapping.latitudeLongitudeToMetric(point);

    BOOST_TEST_MESSAGE("Converted: longitude_latitude: " << point << " to metric: " << metricPoint);

    {
        base::Point location0(-8.00918, 16.00533, 0.0);
        base::Point location1(-8.00085, 17.00359, 0.0);

        base::Point location0metric = mapping.latitudeLongitudeToMetric(location0);
        base::Point location1metric = mapping.latitudeLongitudeToMetric(location1);

        double distance = (location0metric - location1metric).norm();

        BOOST_REQUIRE_MESSAGE(distance < 32E03 && distance > 30E03, "Distance around 30 km, but was " << distance);
    }

    // The mapping is not suited for computation of distance close to poles
    // thus refer to law of cosine based computation in Location
    //
    {
        base::Point location0(-83.34083, 84.64467, 0.0);
        base::Point location1(-83.54570, 87.09851, 0.0);

        base::Point location0metric = mapping.latitudeLongitudeToMetric(location0);
        base::Point location1metric = mapping.latitudeLongitudeToMetric(location1);
        double distance = (location0metric - location1metric).norm();

        //BOOST_REQUIRE_MESSAGE(distance < 10.75E03 && distance > 10.5E03,
        BOOST_TEST_MESSAGE(
                "Distance around 10.5 km, but was " << distance);

        using namespace templ::symbols::constants;
        Location a("a", location0, CartographicMapping::RADIUS_MOON_IN_M);
        Location b("b", location1, CartographicMapping::RADIUS_MOON_IN_M);

        double s_distance = Location::getDistance(a,b);

        BOOST_REQUIRE_MESSAGE(s_distance < 10.75E03 && s_distance > 10.5E03,
                "Distance around 10.5 km, but was " << s_distance);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include <math.h>
#include <stdlib.h>
#include <string>
#include <fstream>
#include <iostream>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
//#include <opencv2/legacy/compat.hpp>

#include "dlib/opencv.h"
#include "dlib/image_processing/frontal_face_detector.h"
#include "dlib/image_processing/render_face_detections.h"
#include "dlib/gui_widgets.h"
#include <dlib/image_processing.h>
#include "faceDetection.h"
#include "util.h"
#include "gestureDetection.h"

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

	cv::VideoCapture cap(0);

	FaceFeatures *face_features = new FaceFeatures();
	FaceData *face_data = new FaceData();
	FacePose *face_pose = new FacePose();
	FaceGesture *face_gesture = new FaceGesture();

	face_gesture->assign(15);

	cv::Mat frame, frame_clr;

	dlib::frontal_face_detector detector = dlib::get_frontal_face_detector();
	dlib::shape_predictor pose_model;

	dlib::deserialize("./res/shape_predictor_68_face_landmarks.dat") >> pose_model;

	std::vector<std::vector<std::vector<double> > > gestures_learned(5);

	//char* file_names[] = {"left2right_fast.txt", "left_rock.txt", "nod.txt", "right_rock.txt", "up2down_fast.txt"};

	read_vector_from_file(argv[1], gestures_learned[0]);
	read_vector_from_file(argv[2], gestures_learned[1]);
	read_vector_from_file(argv[3], gestures_learned[2]);
	read_vector_from_file(argv[4], gestures_learned[3]);
	read_vector_from_file(argv[5], gestures_learned[4]);

	int score_temp_1, score_temp_2, pos;
	std::vector<double> vec_normal(3);
	std::vector<std::vector<double> > vec_current_bin(0);

	while(1) {
		try {
			cap>>frame;
			cv::flip(frame, frame, 1);
			frame.copyTo(frame_clr);
			cv::cvtColor(frame, frame, CV_BGR2GRAY);

			dlib::cv_image<unsigned char> cimg_gray(frame);
			dlib::cv_image<dlib::bgr_pixel> cimg_clr(frame_clr);

			std::vector<dlib::rectangle> faces = detector(cimg_gray);

			std::vector<dlib::full_object_detection> shapes;
			for (unsigned long i = 0; i < faces.size(); ++i)
				shapes.push_back(pose_model(cimg_gray, faces[i]));

			if(faces.size()) {

				dlib::full_object_detection shape = shapes[0];

				face_features->assign(cv::Point(0,0),
					get_mid_point(cv::Point(shape.part(42).x(), shape.part(42).y()),
						cv::Point(shape.part(45).x(), shape.part(45).y())),
					get_mid_point(cv::Point(shape.part(36).x(), shape.part(36).y()),
						cv::Point(shape.part(39).x(), shape.part(39).y())),
					cv::Point(shape.part(30).x(), shape.part(30).y()),
					get_mid_point(cv::Point(shape.part(48).x(), shape.part(48).y()),
						cv::Point(shape.part(54).x(), shape.part(54).y())));

				face_data->assign(face_features);

				face_pose->assign(face_features, face_data);

				std::cout<<face_pose->normal[0]<<" "<<face_pose->normal[1]<<" "<<face_pose->normal[2]<<"\n";

				vec_normal[0] = face_pose->normal[0];
				vec_normal[1] = face_pose->normal[1];
				vec_normal[2] = face_pose->normal[2];

				face_gesture->normal->push(vec_normal);
				//std::cout<<"Filled : "<<face_gesture->normal->get_filled()<<std::endl;

				vec_current_bin = face_gesture->normal->clone();
				//std::cout<<"Size : "<<vec_current_bin.size()<<std::endl;

				score_temp_1 = DTWScore(vec_current_bin, gestures_learned[0]);
				pos = 0;
				std::cout<<score_temp_1<<" ";

				for(int i=1;i<gestures_learned.size();i++) {
					score_temp_2 = DTWScore(vec_current_bin, gestures_learned[i]);
					std::cout<<score_temp_2<<" ";
					if(score_temp_2 < score_temp_1) {
						score_temp_1 = score_temp_2;
						pos = i;
					}
				}

				//std::cout<<"Matched with gesture["<<pos<<"]"<<std::endl;
				std::cout<<argv[pos+1]<<std::endl;
			}
			else {
				std::cout<<"Zero faces"<<std::endl;
			}
		}
		catch(std::exception& e) {
			std::cout<<e.what()<<std::endl;
			break;
		}
	}
	return 0;
}