/*******************************************************************************
 * Copyright (c) 2012, Dougal J. Sutherland (dsutherl@cs.cmu.edu).             *
 * All rights reserved.                                                        *
 *                                                                             *
 * Redistribution and use in source and binary forms, with or without          *
 * modification, are permitted provided that the following conditions are met: *
 *                                                                             *
 *     * Redistributions of source code must retain the above copyright        *
 *       notice, this list of conditions and the following disclaimer.         *
 *                                                                             *
 *     * Redistributions in binary form must reproduce the above copyright     *
 *       notice, this list of conditions and the following disclaimer in the   *
 *       documentation and/or other materials provided with the distribution.  *
 *                                                                             *
 *     * Neither the name of Carnegie Mellon University nor the                *
 *       names of the contributors may be used to endorse or promote products  *
 *       derived from this software without specific prior written permission. *
 *                                                                             *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" *
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE   *
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE  *
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE   *
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR         *
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF        *
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS    *
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN     *
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)     *
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE  *
 * POSSIBILITY OF SUCH DAMAGE.                                                 *
 ******************************************************************************/
#ifndef SDM_CROSSVALIDATE_HPP_
#define SDM_CROSSVALIDATE_HPP_
#include "sdm/basics.hpp"
#include "sdm/kernels/kernel.hpp"
#include "sdm/kernel_projection.hpp"
#include "sdm/log.hpp"
#include "sdm/defaults.hpp"
#include "sdm/sdm_model.hpp"

#include <cmath>

#include <boost/type_traits/is_same.hpp>
#include <boost/static_assert.hpp>

#include <np-divs/np_divs.hpp>

#include <svm.h>

namespace sdm {

// Function to do cross-validation on the passed distributions.
//
// Specifying 0 folds means leave-one-out; specifying 1 fold or more
// folds than there are bags will cause a std::domain_error.
//
// The project_all_data parameter specifies whether to project the entire
// kernel matrix to PSD, or only the training data for a given fold.
//
// The divs parameter, if passed, should be a pointer to a row-major matrix of
// precomputed divergences for the given bags, as given by
// npdivs::np_divs(...)[i].ptr(). If NULL, will be computed.
template <typename Scalar, typename label_type>
double crossvalidate(
    const flann::Matrix<Scalar> *bags, size_t num_bags,
    const std::vector<label_type> &labels,
    const npdivs::DivFunc &div_func,
    const KernelGroup &kernel_group,
    const npdivs::DivParams &div_params,
    size_t folds = 10,
    size_t num_cv_threads = 0,
    bool project_all_data = true,
    bool shuffle_order = true,
    const std::vector<double> &c_vals = sdm::default_c_vals,
    const svm_parameter &svm_params = sdm::default_svm_params,
    size_t tuning_folds = 3,
    const double *divs = NULL);

// Cross-validate directly on precomputed divergences.
template <typename label_type>
double crossvalidate(
    const double *divs, size_t num_bags,
    const std::vector<label_type> &labels,
    const KernelGroup &kernel_group,
    size_t folds = 10,
    size_t num_cv_threads = 0,
    bool project_all_data = true,
    bool shuffle_order = true,
    const std::vector<double> &c_vals = default_c_vals,
    const svm_parameter &svm_params = default_svm_params,
    size_t tuning_folds = 3);

////////////////////////////////////////////////////////////////////////////////
// Helper functions

namespace detail {

// random index < the passed one
ptrdiff_t rand_idx(ptrdiff_t i);

// split a full matrix into testing/training matrices
void copy_from_full_to_split(const double *source,
        double *train, double *test,
        size_t test_start, size_t num_train, size_t num_test);

// worker functor class for doing CV
template <typename label_type>
class cv_worker : boost::noncopyable {
    protected:
        typedef std::pair<size_t, size_t> size_pair;

        const double *divs;
        const size_t num_bags;
        const std::vector<label_type> &labels;
        const boost::ptr_vector<Kernel> *kernels;
        const svm_parameter &svm_params;
        const std::vector<double> c_vals;
        const size_t tuning_folds;
        const bool project_all_data;
        const npdivs::DivParams &div_params;

        boost::mutex &jobs_mutex;
        std::queue<size_pair> &jobs;
        boost::mutex &results_mutex;
        double &score;

        boost::exception_ptr &error;

    public:
        cv_worker(
                const double *divs,
                const size_t num_bags,
                const std::vector<label_type> &labels,
                const boost::ptr_vector<Kernel> *kernels,
                const svm_parameter &svm_params,
                const std::vector<double> c_vals,
                const size_t tuning_folds,
                const bool project_all_data,
                const npdivs::DivParams &div_params,
                boost::mutex &jobs_mutex,
                std::queue<size_pair> &jobs,
                boost::mutex &results_mutex,
                double &score,
                boost::exception_ptr &error)
            :
                divs(divs), num_bags(num_bags), labels(labels),
                kernels(kernels), svm_params(svm_params),
                c_vals(c_vals), tuning_folds(tuning_folds),
                project_all_data(project_all_data), div_params(div_params),
                jobs_mutex(jobs_mutex), jobs(jobs),
                results_mutex(results_mutex), score(score),
                error(error)
    {}

        void operator()() {
            size_pair job;
            double this_score = 0;

            try {
                while (true) {
                    { // get a job
                        boost::mutex::scoped_lock the_lock(jobs_mutex);
                        if (jobs.size() == 0)
                            break;
                        job = jobs.front();
                        jobs.pop();
                    }

                    this_score += do_job(job.first, job.second);
                }

                { // write the result
                    boost::mutex::scoped_lock the_lock(results_mutex);
                    score += this_score;
                }
            } catch (...) {
                error = boost::current_exception();
                return;
            }
        }

        double do_job(size_t test_start, size_t test_end) const {
            // testing is in [test_start, test_end)
            size_t num_test = test_end - test_start;
            size_t num_train = num_bags - num_test;

            // set up training labels
            std::vector<label_type> train_labels(num_train);
            std::copy(labels.begin(), labels.begin() + test_start,
                    train_labels.begin());
            std::copy(labels.begin() + test_end, labels.end(),
                    train_labels.begin() + test_start);

            // tune the kernel parameters
            double *train_km = new double[num_train * num_train];
            double *test_km = new double[num_test * num_train];
            copy_from_full_to_split(divs, train_km, test_km,
                    test_start, num_train, num_test);

            svm_parameter svm_p = svm_params;

            const std::pair<size_t, size_t> &best_config = tune_params(
                    train_km, num_train, train_labels, *kernels, c_vals,
                    svm_p, tuning_folds, div_params.num_threads);

            const Kernel &kernel = (*kernels)[best_config.first];
            svm_p.C = c_vals[best_config.second];

            // project either the whole matrix or just the training

            if (project_all_data) {
                double *full_km = new double[num_bags * num_bags];
                std::copy(divs, divs + num_bags * num_bags, full_km);

                kernel.transformDivergences(full_km, num_bags);
                project_to_symmetric_psd(full_km, num_bags);

                copy_from_full_to_split(full_km, train_km, test_km,
                        test_start, num_train, num_test);

                delete[] full_km;

            } else {
                copy_from_full_to_split(
                    divs, train_km, test_km, test_start, num_train, num_test);

                kernel.transformDivergences(train_km, num_train);
                project_to_symmetric_psd(train_km, num_train);

                kernel.transformDivergences(test_km, num_test, num_train);
            }

            // set up svm parameters
            svm_problem prob;
            prob.l = num_train;
            prob.y = new double[num_train];
            for (size_t i = 0; i < num_train; i++)
                prob.y[i] = double(train_labels[i]);
            detail::store_kernel_matrix(prob, train_km, true);

            // check svm params
            const char* error = svm_check_parameter(&prob, &svm_p);
            if (error != NULL) {
                FILE_LOG(logERROR) << "LibSVM parameter error: " << error;
                BOOST_THROW_EXCEPTION(std::domain_error(error));
            }

            // train!
            svm_model *svm = svm_train(&prob, &svm_p);

            npdivs::DivL2 fake_df; // XXX figure out a better way around this
            SDM<float, label_type> sdm(*svm, prob, fake_df, kernel, div_params,
                    svm_get_nr_class(svm), NULL, num_train);

            // predict!
            std::vector<label_type> preds;
            std::vector< std::vector<double> > vals;
            sdm.predict_from_kerns(test_km, num_test, preds, vals);

            double score = 0;
            if (boost::is_same<label_type, int>::value) {
                // how many did we get right?
                for (size_t i = 0; i < num_test; i++)
                    if (preds[i] == labels[test_start + i])
                        score++;

                FILE_LOG(logINFO) << "CV " <<
                    test_start << " - " << test_end-1 << ": "
                    << score << "/" << num_test
                    << " correct by " << kernel.name() << ", C=" << svm_p.C;

            } else {
                // get sum squared error
                for (size_t i = 0; i < num_test; i++) {
                    double diff = preds[i] - labels[test_start + i];
                    score += diff * diff;
                }

                FILE_LOG(logINFO) << "CV " <<
                    test_start << " - " << test_end << ": "
                    << std::sqrt(score / num_test)
                    << " RMSE by " << kernel.name() << ", C=" << svm_p.C;
            }

            // clean up
            delete[] train_km;
            delete[] test_km;
            for (size_t i = 0; i < num_train; i++)
                delete[] prob.x[i];
            delete[] prob.x;
            delete[] prob.y;
            svm_free_model_content(svm);
            delete svm;

            return score;
        }
};

} // detail

////////////////////////////////////////////////////////////////////////////////
// Template implementations

// cross-validation directly from bags
template <typename Scalar, typename label_type>
double crossvalidate(
    const flann::Matrix<Scalar> *bags, size_t num_bags,
    const std::vector<label_type> &labels,
    const npdivs::DivFunc &div_func,
    const KernelGroup &kernel_group,
    const npdivs::DivParams &div_params,
    size_t folds,
    size_t num_cv_threads,
    bool project_all_data,
    bool shuffle_order,
    const std::vector<double> &c_vals,
    const svm_parameter &svm_params,
    size_t tuning_folds,
    const double *divs)
{
    typedef flann::Matrix<Scalar> Matrix;

    if (folds == 0) {
        folds = num_bags;
    } else if (folds == 1) {
        BOOST_THROW_EXCEPTION(std::domain_error((boost::format(
                    "Can't cross-validate with %d folds...") % folds).str()));
    } else if (folds > num_bags) {
        BOOST_THROW_EXCEPTION(std::domain_error((boost::format(
           "Can't use %d folds with only %d bags.") % folds % num_bags).str()));
    }

    // calculate the full matrix of divergences if necessary
    bool free_divs = false;

    if (divs == NULL) {
        flann::Matrix<double>* divs_mat =
            npdivs::alloc_matrix_array<double>(1, num_bags, num_bags);
        np_divs(bags, num_bags, div_func, divs_mat, div_params, false);

        divs = divs_mat[0].ptr();
        free_divs = true;
        delete[] divs_mat; // doesn't delete content, just the Matrix array
    }

    double acc = crossvalidate(
            divs, num_bags, labels, kernel_group, folds,
            num_cv_threads, project_all_data, shuffle_order,
            c_vals, svm_params, tuning_folds);

    if (free_divs)
        delete[] divs;

    return acc;
}

// cross-validate with precomputed divergences
template <typename label_type>
double crossvalidate(
    const double *divs, size_t num_bags,
    const std::vector<label_type> &labels,
    const KernelGroup &kernel_group,
    size_t folds,
    size_t num_cv_threads,
    bool project_all_data,
    bool shuffle_order,
    const std::vector<double> &c_vals,
    const svm_parameter &svm_params,
    size_t tuning_folds)
{
    // this only works for int or double label_types
    BOOST_STATIC_ASSERT((
        boost::is_same<label_type, int>::value ||
        boost::is_same<label_type, double>::value
    ));

    // check that folds is appropriate
    if (folds == 0) {
        folds = num_bags;
    } else if (folds == 1) {
        BOOST_THROW_EXCEPTION(std::domain_error((boost::format(
                    "Can't cross-validate with %d folds...") % folds).str()));
    } else if (folds > num_bags) {
        BOOST_THROW_EXCEPTION(std::domain_error((boost::format(
           "Can't use %d folds with only %d bags.") % folds % num_bags).str()));
    }

    // copy the svm params so we can change them
    svm_parameter svm_p = svm_params;
    svm_p.kernel_type = PRECOMPUTED;
    // TODO: better handling of svm_type
    if (boost::is_same<label_type, int>::value)
        svm_p.svm_type = C_SVC;
    else
        svm_p.svm_type = EPSILON_SVR;

    // make libSVM log properly
    svm_set_print_string_function(&log_string<logDEBUG4>);

    // ask for the list of kernels to choose from
    const boost::ptr_vector<Kernel> *kernels =
        kernel_group.getTuningVector(divs, num_bags);

    // figure out the numbers of threads to use
    num_cv_threads = npdivs::get_num_threads(num_cv_threads);
    size_t real_cv_threads = std::min(num_cv_threads, folds);

    // don't want to blow up total # of threads when the workers are tuning
    // XXX reorganize so don't need to pass mostly-fake div params
    npdivs::DivParams pred_div_params(3,
        flann::KDTreeSingleIndexParams(), flann::SearchParams(-1),
        std::max(size_t(1), size_t(double(num_cv_threads) / real_cv_threads)));

    // need this for parameter tuning and shuffling
    std::srand(unsigned(std::time(NULL)));

    // reorder the divs/labels, so folds are randomized
    double* shuff_divs = new double[num_bags*num_bags];
    std::vector<label_type> shuff_labels;

    if (shuffle_order) {
        shuff_labels.resize(num_bags);

        size_t* perm = new size_t[num_bags];
        for (size_t i = 0; i < num_bags; i++)
            perm[i] = i;
        std::random_shuffle(perm, perm+num_bags, detail::rand_idx);

        for (size_t i = 0; i < num_bags; i++) {
            size_t p_i = perm[i];
            shuff_labels[p_i] = labels[i];
            for (size_t j = 0; j < num_bags; j++)
                shuff_divs[p_i * num_bags + perm[j]] = divs[i*num_bags + j];
        }

        delete[] perm;

    } else {
        std::copy(divs, divs + num_bags*num_bags, shuff_divs);
        shuff_labels = labels;
    }

    // symmetrize divergence estimates
    symmetrize(shuff_divs, num_bags);

    // do out each fold
    size_t num_test = (size_t) std::ceil(double(num_bags) / folds);

    std::queue< std::pair<size_t, size_t> > jobs;
    double score = 0;
    boost::mutex jobs_mutex, results_mutex;

    if (real_cv_threads <= 1) {
        // don't actually launch separate threads, 'tis a waste

        boost::exception_ptr error;
        detail::cv_worker<label_type> worker(shuff_divs, num_bags, shuff_labels,
                kernels, svm_p, c_vals, tuning_folds,
                project_all_data, pred_div_params, jobs_mutex, jobs,
                results_mutex, score, error);

        for (size_t fold = 0; fold < folds; fold++) {
            size_t test_start = fold * num_test;
            size_t test_end = std::min(test_start + num_test, num_bags);
            if (test_start < test_end)
                score += worker.do_job(test_start, test_end);
        }

    } else {
        // queue up the jobs
        for (size_t fold = 0; fold < folds; fold++) {
            size_t test_start = fold * num_test;
            size_t test_end = std::min(test_start + num_test, num_bags);
            if (test_start < test_end)
                jobs.push(std::make_pair(test_start, test_end));
        }

        // keep worker objects in this ptr_vector to avoid copying but also
        // have them be destroyed when appropriate
        boost::ptr_vector< detail::cv_worker<label_type> > workers;
        std::vector<boost::exception_ptr> errors(real_cv_threads);

        boost::thread_group worker_threads;

        for (size_t i = 0; i < real_cv_threads; i++) {
            workers.push_back(new detail::cv_worker<label_type>(
                shuff_divs, num_bags, shuff_labels, kernels,
                svm_p, c_vals, tuning_folds, project_all_data, pred_div_params,
                jobs_mutex, jobs, results_mutex, score, errors[i]
            ));
            worker_threads.create_thread(boost::ref(workers[i]));
        }

        worker_threads.join_all();
        for (size_t i = 0; i < real_cv_threads; i++)
            if (errors[i])
                boost::rethrow_exception(errors[i]);
    }

    // clean up
    delete kernels;
    delete[] shuff_divs;

    // return accuracy / RMSE
    if (boost::is_same<label_type, int>::value) {
        return score / num_bags;
    } else {
        return std::sqrt(score / num_bags);
    }
}

} // sdm
#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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_TOOLBOX_SWAR_FUNCTIONS_SIMD_COMMON_SORT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_SWAR_FUNCTIONS_SIMD_COMMON_SORT_HPP_INCLUDED

#include <boost/simd/toolbox/swar/functions/sort.hpp>
#include <boost/simd/include/functions/simd/load.hpp>
#include <boost/simd/include/functions/simd/store.hpp>
#include <boost/simd/sdk/memory/aligned_type.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>

#include <algorithm>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::sort_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type stype;
      static const size_t size = boost::simd::meta::cardinal_of<A0>::value;
      BOOST_SIMD_ALIGNED_TYPE(stype) tmp[size];
      store(a0, &tmp[0], 0);
      std::sort(tmp, tmp + size);
      return load<A0>(&tmp[0], 0);
    }
  };
} } }

#endif


#ifndef ENTRY_HPP_INCLUDED
#define ENTRY_HPP_INCLUDED

#include "banyan.hpp"
#include "benes.hpp"
#include <cassert>
#include <map>
#include <boost/log/trivial.hpp>
#include <sstream>
#include <cmath>

// Uncomment the following line for enabling logs
// #define INTERCONNECT_LOGS

using multistage_interconnect::Int;
using multistage_interconnect::UnsignedInt;

template <typename Interconnect>
inline void propagate_impl(Interconnect &interconnect, Int const *packets,
                           Int *output, Int invalid) {
  auto N = 1 << interconnect.n();
  std::vector<Int> packets_vector(packets, packets + N);
  auto &&propagated = interconnect.propagate(packets_vector, invalid);
  for (UnsignedInt i = 0; i < (UnsignedInt) N; ++i) {
    output[i] = propagated[i];
  }
}

struct InterconnectBase {
  virtual float power(int switch_width) const = 0;
  virtual UnsignedInt num_ports() const = 0;
  virtual UnsignedInt latency() const = 0;
  virtual const char *name() const = 0;
  virtual void reset() = 0;
  virtual void propagate(Int const *packets, Int *output,
                         Int invalid) const = 0;
  virtual bool do_apply_permute(Int const *inverse_mapping) = 0;
  virtual bool do_is_route_free(UnsignedInt src, UnsignedInt dest) = 0;
  virtual void copy_from(InterconnectBase *other) = 0;
  virtual InterconnectBase *clone() const = 0;
  virtual ~InterconnectBase() = default;

  virtual UnsignedInt data_req_latency() const {
    return latency();
  }

  virtual UnsignedInt data_read_latency() const {
    return latency() + 1;
  }

  virtual UnsignedInt data_write_latency() const {
    return latency() + 1;
  }

  virtual UnsignedInt total_latency() const {
    return data_req_latency() + data_read_latency();
  }

  template <typename BankType, typename TargetType>
  bool apply_permute(std::map<BankType *, TargetType *> *permute) {
    std::vector<Int> v(num_ports(), -1);
    for (auto it = cbegin(*permute); it != cend(*permute); ++it) {
      v[it->first->id] = it->second->id;
    }
    auto result = do_apply_permute(&v[0]);
    #ifdef INTERCONNECT_LOGS
    BOOST_LOG_TRIVIAL(info) <<
      "apply_permute: this = " << this <<
      " mapping = " << [&] {
        std::ostringstream oss;
        for (auto &i: v) oss << i << ",";
        return oss.str();
      }() <<
      " result = " << result;
    #endif
    return result;
  }

  template <typename BankType, typename TargetType>
  bool is_route_free(BankType *bank, TargetType * target) {
    auto result = do_is_route_free(bank->id, target->id);
    #ifdef INTERCONNECT_LOGS
    BOOST_LOG_TRIVIAL(info) <<
      "is_route_free: this = " << this <<
      " bank = " << bank->id <<
      " target = " << target->id <<
      " result = " << result;
    #endif
    return result;
  }
};

struct Benes : InterconnectBase {
  enum Algo : Int {
    ALGO_DUMMY = 0,
    ALGO_RAGHAVENDRA = 1,
    ALGO_LOOPING_MULTICAST = 2
  };

  float energy_spent = 0;
  multistage_interconnect::Benes impl;
  std::vector<Int> current_inverse_mapping;
  Algo algorithm = ALGO_LOOPING_MULTICAST;
  UnsignedInt trials = 128;

  Benes(UnsignedInt n) : impl{n}, current_inverse_mapping(1 << n, -1) {}

  float power(int switch_width) const override {
    float I_0 = 2.875e-5; //W per byte

    int _N = 1 << this->impl.n();

    return I_0 * 2 * _N * log2(_N) * switch_width;
  }

  UnsignedInt num_ports() const override {
    return current_inverse_mapping.size();
  }

  UnsignedInt latency() const override { return impl.n() * 2 - 1; }

  const char *name() const override {
    return "benes";
  }

  void reset() override {
    impl.reset();
    for (auto &x : current_inverse_mapping)
      x = -1;
  }

  void propagate(Int const *packets, Int *output, Int invalid) const override {
    propagate_impl(impl, packets, output, invalid);
  }

  void set_algorithm(Algo algorithm) { this->algorithm = algorithm; }

  void set_trials(UnsignedInt trials) { this->trials = trials; }

  bool do_apply_permute(Int const *inverse_mapping) override {
    for (UnsignedInt i = 0; i < current_inverse_mapping.size(); ++i) {
      current_inverse_mapping[i] = inverse_mapping[i];
    }

    impl.reset();

    switch (algorithm) {
    case ALGO_DUMMY: {
      return impl.dummy(&current_inverse_mapping[0]);
    }
    case ALGO_RAGHAVENDRA: {
      return impl.raghavendra(&current_inverse_mapping[0]);
    }
    case ALGO_LOOPING_MULTICAST: {
      return impl.looping_multicast(&current_inverse_mapping[0], trials);
    }
    }

    throw std::runtime_error("not valid default algorithm type");
  }

  bool do_is_route_free(UnsignedInt src, UnsignedInt dest) override {
    if (current_inverse_mapping[dest] != -1)
      return false;

    auto temp = current_inverse_mapping;
    temp[dest] = src;

    impl.reset();

    switch (algorithm) {
    case ALGO_DUMMY: {
      return impl.dummy(&temp[0]);
    }
    case ALGO_RAGHAVENDRA: {
      return impl.raghavendra(&temp[0]);
    }
    case ALGO_LOOPING_MULTICAST: {
      return impl.looping_multicast(&temp[0], trials);
    }
    }

    throw std::runtime_error("not valid default algorithm type");
  }

  void copy_from(InterconnectBase *other) override {
    if (this != (Benes *) other) {
      *this = *((Benes *) other);
    }
  }

  Benes *clone() const override { return new Benes(*this); }
};

struct BenesWithCopy : InterconnectBase {
  BenesWithCopy(UnsignedInt n) : n_{n} {}

  float energy_spent = 0;

  float power(int switch_width) const override {
    float I_0 = 2.875e-5; //W per byte

    int _N = 1 << this->n_;

    return I_0 * 4 * _N * log2(_N) * switch_width;
  }

  UnsignedInt latency() const override { return n_ * 4 - 1; }

  const char *name() const override {
    return "benes_with_copy";
  }

  UnsignedInt num_ports() const override { return 1 << n_; }

  void reset() override {}

  void propagate(Int const *, Int *, Int) const override {}

  bool do_apply_permute(Int const *) override { return true; }

  bool do_is_route_free(UnsignedInt, UnsignedInt) override {
    return true;
  }

  void copy_from(InterconnectBase *other) override {
    if (this != (BenesWithCopy *) other) {
      *this = *((BenesWithCopy *) other);
    }
  }

  BenesWithCopy *clone() const override { return new BenesWithCopy(*this); }

private:
  UnsignedInt n_;
};

struct Crossbar : InterconnectBase {
  Crossbar(UnsignedInt n) : n_{n} {}

  float energy_spent = 0;

  float power(int switch_width) const override {
    float I_0 = 2.875e-5; //W per byte

    int _N = 1 << this->n_;

    return I_0 * _N * _N * switch_width;
  }

  UnsignedInt latency() const override { return 1; }

  const char *name() const override {
    return "crossbar";
  }

  UnsignedInt num_ports() const override { return 1 << n_; }

  void reset() override {}

  void propagate(Int const *, Int *, Int) const override {}

  bool do_apply_permute(Int const *) override { return true; }

  bool do_is_route_free(UnsignedInt, UnsignedInt) override {
    return true;
  }

  void copy_from(InterconnectBase *other) override {
    if (this != (Crossbar *) other) {
      *this = *((Crossbar *) other);
    }
  }

  Crossbar *clone() const override { return new Crossbar(*this); }

private:
  UnsignedInt n_;
};

struct Banyan : InterconnectBase {
  multistage_interconnect::Banyan impl;
  std::vector<Int> current_inverse_mapping;

  // expansion is given logarithmically
  UnsignedInt expansion = 0;

  Banyan(UnsignedInt n) : impl{n}, current_inverse_mapping(1 << n, -1) {}

  float energy_spent = 0;

  float power(int switch_width) const override {
    float I_0 = 2.875e-5; //W per byte

    int _N = 1 << this->impl.n();

    return I_0 * _N * log2(_N) * switch_width;
  }

  UnsignedInt latency() const override { return impl.n() + expansion; }

  const char *name() const override {
    return "banyan";
  }

  UnsignedInt num_ports() const override {
    return current_inverse_mapping.size();
  }

  void reset() override {
    impl.reset();
    for (auto &x : current_inverse_mapping)
      x = -1;
  }

  void propagate(Int const *packets, Int *output, Int invalid) const override {
    propagate_impl(impl, packets, output, invalid);
  }

  void set_expansion(UnsignedInt expansion) { this->expansion = expansion; }

  // TODO most probably, we can do it in a faster way without
  // current_inverse_mapping
  bool do_apply_permute(Int const *inverse_mapping) override {
    for (UnsignedInt i = 0; i < current_inverse_mapping.size(); ++i) {
      current_inverse_mapping[i] = inverse_mapping[i];
    }

    impl.reset();

    if (expansion == 0)
      return impl.bit_follow(&current_inverse_mapping[0], true);
    else
      return impl.bit_follow_expansion(&current_inverse_mapping[0], expansion);
  }

  bool do_is_route_free(UnsignedInt src, UnsignedInt dest) override {
    if (expansion == 0)
      return impl.bit_follow(src, dest, true, false);
    else
      return impl.bit_follow_expansion(src, dest, expansion, false);
  }

  void copy_from(InterconnectBase *other) override {
    if (this != (Banyan *) other) {
      *this = *((Banyan *) other);
    }
  }

  Banyan *clone() const override { return new Banyan(*this); }
};

#endif // ENTRY_HPP_INCLUDED


#pragma once

#include <boost/python.hpp>

#include "corridor/corridor.h"

// Phython API
#include "cubic_spline_wrapper.hpp"
#include "utility.hpp"

namespace py = boost::python;

// /////////////////////////////////////////////////////////////////////////////
// Corridor wrapper
// /////////////////////////////////////////////////////////////////////////////

struct CorridorWrapper {
  // CPP object
  corridor::Corridor corridor_;

  CorridorWrapper(const int id, const py::list& node_x,
                  const py::list& node_y) {
    using namespace corridor;
    CartesianPoints2D points = toCartesianPoints(node_x, node_y);
    const RealType distance_left_boundary = 2.0;
    const RealType distance_right_boundary = 2.0;
    corridor_ =
        Corridor(id, points, distance_left_boundary, distance_right_boundary);
  }

  CorridorWrapper(const int id, const py::list& node_x, const py::list& node_y,
                  const py::list& py_first_tangent,
                  const py::list& py_last_tangent) {
    using namespace corridor;
    const CartesianPoints2D points = toCartesianPoints(node_x, node_y);
    const CartesianVector2D first_tangent =
        to_cartesian_vector(py_first_tangent);
    const CartesianVector2D last_tangent = to_cartesian_vector(py_last_tangent);
    const RealType distance_left_boundary = 2.0;
    const RealType distance_right_boundary = 2.0;
    corridor_ = Corridor(id, points, first_tangent, last_tangent,
                         distance_left_boundary, distance_right_boundary);
  }

  py::dict GetCartesianPolylinesLines(const corridor::RealType delta_s) {
    using namespace corridor;
    // Construct Cartesian polylines form corridors reference line and
    // boundaries. delta_s defines the sampling rate for all three lines
    CartesianPoints2D reference_line;
    CartesianPoints2D left_boundary;
    CartesianPoints2D right_boundary;

    corridor_.fillCartesianPolylines(&reference_line, &left_boundary,
                                     &right_boundary, delta_s);

    // convert to python data structures
    const std::pair<py::list, py::list> py_refline =
        to_py_lists(reference_line);
    const std::pair<py::list, py::list> py_left_boundary =
        to_py_lists(left_boundary);
    const std::pair<py::list, py::list> py_right_boundary =
        to_py_lists(right_boundary);

    // Fill dictionary
    py::dict polylines;
    polylines["reference_line_x"] = py_refline.first;
    polylines["reference_line_y"] = py_refline.second;
    polylines["left_boundary_x"] = py_left_boundary.first;
    polylines["left_boundary_y"] = py_left_boundary.second;
    polylines["right_boundary_x"] = py_right_boundary.first;
    polylines["right_boundary_y"] = py_right_boundary.second;
    return polylines;
  }

  py::list ToFrenetStateVector(const py::list& py_cartesian_state_vector,
                               const bool moving_frenet_frame) {
    using namespace corridor;
    CartesianStateVector2D cartesian_state_vector =
        convert(py_cartesian_state_vector);

    FrenetFrame2D frenet_frame =
        corridor_.FrenetFrame(cartesian_state_vector.position());

    FrenetStateVector2D frenet_state = frenet_frame.FromCartesianStateVector(
        cartesian_state_vector, moving_frenet_frame);

    return convert(frenet_state);
  }

  FlatFrenetStateAndCovMat2D ToFrenetState(
      const FlatCartesianStateAndCovMat2D& flat_cartesian_state,
      const bool moving_frenet_frame) {
    using namespace corridor;
    CartesianState2D cartesian_state = Convert(flat_cartesian_state);

    FrenetFrame2D frenet_frame =
        corridor_.FrenetFrame(cartesian_state.position());

    FrenetState2D frenet_state =
        frenet_frame.FromCartesianState(cartesian_state, moving_frenet_frame);

    return Convert(frenet_state);
  }

  py::dict GetFrenetFrame(const cr::RealType target_x,
                          const cr::RealType target_y) {
    using namespace corridor;
    CartesianPoint2D target_point(target_x, target_y);

    FrenetFrame2D frenet_frame = corridor_.FrenetFrame(target_point);

    return to_py_dict(frenet_frame);
  }

  corridor::RealType lengthReferenceLine() {
    return corridor_.lengthReferenceLine();
  }

  corridor::RealType curvatureAt(const corridor::RealType l) {
    return corridor_.curvatureAt(l);
  }
};

void TestCorridorHandle(const CorridorWrapper& corridor_wrapper) {
  std::cout << corridor_wrapper.corridor_.id() << std::endl;
}


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

// Copyright (c) 2020 Tinko Bartels, Berlin, Germany.

// Contributed and/or modified by Tinko Bartels,
//   as part of Google Summer of Code 2020 program.

// 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_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_STATIC_UTIL_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_STATIC_UTIL_HPP

#include <boost/mp11/integral.hpp>
#include <boost/mp11/list.hpp>
#include <boost/mp11/map.hpp>

namespace boost { namespace geometry
{

namespace detail { namespace generic_robust_predicates
{

template
<
    typename LOV,
    typename Void = boost::mp11::mp_same<LOV, void>
>
struct empty_or_void
{
    using type = boost::mp11::mp_empty<LOV>;
};

template <typename LOV>
struct empty_or_void<LOV, boost::mp11::mp_true>
{
    using type = boost::mp11::mp_true;
};

template <typename N>
struct log_2_floor_impl
{
    using type = typename boost::mp11::mp_plus
        <
            typename log_2_floor_impl<boost::mp11::mp_int<N::value / 2>>::type,
            boost::mp11::mp_int<1>
        >;
};

template <>
struct log_2_floor_impl<boost::mp11::mp_int<1>>
{
    using type = boost::mp11::mp_int<0>;
};

template <typename N> using log_2_floor = typename log_2_floor_impl<N>::type;

template <typename N>
struct log_2_ceil_impl
{
private:
    using floor = log_2_floor<N>;
public:
    using type = boost::mp11::mp_int
        <
            (1 << floor::value) == N::value ? floor::value : floor::value + 1
        >;
};

template <typename N> using log_2_ceil = typename log_2_ceil_impl<N>::type;

template <typename S> using is_not_zero = boost::mp11::mp_bool<S::value != 0>;

template
<
    typename M,
    typename K,
    typename Contained = boost::mp11::mp_map_contains<M, K>
>
struct mp_map_at_second_or_void
{
    using type = void;
};

template<typename M, typename K>
struct mp_map_at_second_or_void<M, K, boost::mp11::mp_true>
{
    using type = boost::mp11::mp_second<boost::mp11::mp_map_find<M, K>>;
};

template <typename T> using inc = boost::mp11::mp_int<T::value + 1>;

template <typename L> using indexed = boost::mp11::mp_transform
    <
        boost::mp11::mp_list,
        boost::mp11::mp_iota<boost::mp11::mp_size<L>>,
        L
    >;

template <typename L> using strip_index =
    boost::mp11::mp_transform<boost::mp11::mp_second, L>;

template
<
    typename Map,
    typename Key,
    typename Contains = boost::mp11::mp_map_contains<Map, Key>
>
struct val_or_empty_list
{
    using type = boost::mp11::mp_second<boost::mp11::mp_map_find<Map, Key>>;
};

template
<
    typename Map,
    typename Key
>
struct val_or_empty_list<Map, Key, boost::mp11::mp_false>
{
    using type = boost::mp11::mp_list<>;
};

}} // namespace detail::generic_robust_predicates

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_STATIC_UTIL_HPP


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/type.hpp>
namespace hana = boost::hana;

struct T { };

int main() {
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::alignof_(T{}),
        hana::size_c<alignof(T)>
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::alignof_(hana::type_c<T>),
        hana::size_c<alignof(T)>
    ));

    // make sure we don't read from non-constexpr variables
    {
        auto t = hana::type_c<T>;
        auto x = 1;
        constexpr auto r1 = hana::alignof_(t); (void)r1;
        constexpr auto r2 = hana::alignof_(x); (void)r2;
    }
}


/***************************************************************************************************************************
 * aruco_det.cpp
 * Author: Jario
 * Update Time: 2020.1.14
 *
 * 说明: 单个二维码识别程序，可识别的二维码在Prometheus/Modules/object_detection/config/aruco_images文件夹中
 *      视野里只允许存在一个二维码 且二维码的字典类型要对应
 *      默认二维码的边长为0.2m
 *      1. 【订阅】图像话题 (默认来自web_cam)
 *         /prometheus/camera/rgb/image_raw
 *      2. 【发布】目标位置，发布话题见 Prometheus/Modules/msgs/msg/DetectionInfo.msg
 *         /prometheus/object_detection/aruco_det
 *      3. 【发布】检测结果的可视化图像话题
 *         /prometheus/camera/rgb/image_aruco_det
***************************************************************************************************************************/
#include <pthread.h>
#include <thread>
#include <chrono>
#include <boost/thread/mutex.hpp>
#include <boost/thread/shared_mutex.hpp>

#include <ros/ros.h>
#include <ros/package.h>
#include <yaml-cpp/yaml.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Bool.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include "opencv2/aruco.hpp"
#include "opencv2/aruco/dictionary.hpp"
#include "opencv2/aruco/charuco.hpp"
#include "opencv2/calib3d.hpp"
#include "message_utils.h"

using namespace std;
using namespace cv;

//【订阅】输入图像
image_transport::Subscriber image_subscriber;
//【发布】检测得到的位置与姿态信息
ros::Publisher pose_pub;
//【发布】输入检测结果图像
image_transport::Publisher aruco_pub;

//【订阅】输入开关量
ros::Subscriber switch_subscriber;
// 接收消息，允许暂停检测
bool is_suspanded = false;
// 使用cout打印消息
bool local_print = false;
// 使用prometheus_msgs::Message打印消息
bool message_print = true;
//【发布】调试消息
ros::Publisher message_pub;
std::string msg_node_name;

void CodeRotateByZ(double x, double y, double thetaz, double& outx, double& outy)
{
    double x1 = x;//将变量拷贝一次，保证&x == &outx这种情况下也能计算正确
    double y1 = y;
    double rz = thetaz * CV_PI / 180;
    outx = cos(rz) * x1 - sin(rz) * y1;
    outy = sin(rz) * x1 + cos(rz) * y1;
}
void CodeRotateByY(double x, double z, double thetay, double& outx, double& outz)
{
    double x1 = x;
    double z1 = z;
    double ry = thetay * CV_PI / 180;
    outx = cos(ry) * x1 + sin(ry) * z1;
    outz = cos(ry) * z1 - sin(ry) * x1;
}
void CodeRotateByX(double y, double z, double thetax, double& outy, double& outz)
{
    double y1 = y;//将变量拷贝一次，保证&y == &y这种情况下也能计算正确
    double z1 = z;
    double rx = thetax * CV_PI / 180;
    outy = cos(rx) * y1 - sin(rx) * z1;
    outz = cos(rx) * z1 + sin(rx) * y1;
}
static float static_depth = 0;
static float static_real_x = 0;
static float static_real_y = 0;

// 相机话题中的图像同步相关变量
int frame_width, frame_height;
std_msgs::Header image_header;
cv::Mat cam_image_copy;
boost::shared_mutex mutex_image_callback;
bool image_status = false;
boost::shared_mutex mutex_image_status;

// 图像接收回调函数，接收web_cam的话题，并将图像保存在cam_image_copy中
void cameraCallback(const sensor_msgs::ImageConstPtr& msg)
{
    if (local_print)
        ROS_DEBUG("[ArucoDetector] USB image received.");

    cv_bridge::CvImagePtr cam_image;

    try {
        cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        image_header = msg->header;
    } catch (cv_bridge::Exception& e) {
        if (local_print)
            ROS_ERROR("cv_bridge exception: %s", e.what());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::ERROR, msg_node_name, "cv_bridge exception");
        return;
    }

    if (cam_image) {
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            cam_image_copy = cam_image->image.clone();
        }
        {
            boost::unique_lock<boost::shared_mutex> lockImageStatus(mutex_image_status);
            image_status = true;
        }
        frame_width = cam_image->image.size().width;
        frame_height = cam_image->image.size().height;
    }
    return;
}

// 用此函数查看是否收到图像话题
bool getImageStatus(void)
{
    boost::shared_lock<boost::shared_mutex> lock(mutex_image_status);
    return image_status;
}

void switchCallback(const std_msgs::Bool::ConstPtr& msg)
{
    is_suspanded = !(bool)msg->data;
    // cout << is_suspanded << endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "aruco_det");
    ros::NodeHandle nh("~");
    image_transport::ImageTransport it(nh);
    // 更新频率为30HZ
    ros::Rate loop_rate(30);
    //【发布】识别
    pose_pub = nh.advertise<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/aruco_det", 1);

    // 发布调试消息
    msg_node_name = "/prometheus/message/aruco_det";
    message_pub = nh.advertise<prometheus_msgs::Message>(msg_node_name, 10);

    std::string camera_topic, camera_info;
    if (nh.getParam("camera_topic", camera_topic)) {
        if (local_print)
            ROS_INFO("camera_topic is %s", camera_topic.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "camera_topic is" + camera_topic);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter camera_topic");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter camera_topic");
        camera_topic = "/prometheus/camera/rgb/image_raw";
    }

    if (nh.getParam("camera_info", camera_info)) {
        if (local_print)
            ROS_INFO("camera_info is %s", camera_info.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "camera_info is" + camera_info);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter camera_info");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter camera_info");
        camera_info = "camera_param.yaml";
    }

    // 接收图像的话题
    image_subscriber = it.subscribe(camera_topic.c_str(), 1, cameraCallback);
    // 发布ArUco检测结果的话题
    aruco_pub = it.advertise("/prometheus/camera/rgb/image_aruco_det", 1);

    bool switch_state = is_suspanded;
    // 接收开关话题
    switch_subscriber = nh.subscribe("/prometheus/switch/aruco_det", 10, switchCallback);

    sensor_msgs::ImagePtr msg_ellipse;
    // 开启编号为0的摄像头
    // cv::VideoCapture cap(0);
	cv::Mat frame;

    cv::Mat camera_matrix;
    cv::Mat distortion_coefficients;

    std::string ros_path = ros::package::getPath("prometheus_detection");
    if (local_print)
        cout << "DETECTION_PATH: " << ros_path << endl;
    if (message_print)
        pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "DETECTION_PATH: " + ros_path);
    //读取参数文档camera_param.yaml中的参数值；
    YAML::Node camera_config = YAML::LoadFile(ros_path + "/config/" + camera_info);
    //相机内部参数
    double fx = camera_config["fx"].as<double>();
    double fy = camera_config["fy"].as<double>();
    double cx = camera_config["x0"].as<double>();
    double cy = camera_config["y0"].as<double>();
    //相机畸变系数
    double k1 = camera_config["k1"].as<double>();
    double k2 = camera_config["k2"].as<double>();
    double p1 = camera_config["p1"].as<double>();
    double p2 = camera_config["p2"].as<double>();
    double k3 = camera_config["k3"].as<double>();

    double aruco_det_len = camera_config["aruco_det_len"].as<double>();

    //相机内参
    camera_matrix = cv::Mat(3,3,CV_64FC1,cv::Scalar::all(0));
    camera_matrix.ptr<double>(0)[0]=fx;
    camera_matrix.ptr<double>(0)[2]=cx;
    camera_matrix.ptr<double>(1)[1]=fy;
    camera_matrix.ptr<double>(1)[2]=cy;
    camera_matrix.ptr<double>(2)[2]=1.0f;

    //相机畸变参数
    distortion_coefficients=cv::Mat(5,1,CV_64FC1,cv::Scalar::all(0));
    distortion_coefficients.ptr<double>(0)[0]=k1;
    distortion_coefficients.ptr<double>(1)[0]=k2;
    distortion_coefficients.ptr<double>(2)[0]=p1;
    distortion_coefficients.ptr<double>(3)[0]=p2;
    distortion_coefficients.ptr<double>(4)[0]=k3;

    //ArUco Marker字典选择
    Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
    vector<double> rv(3),tv(3);
    cv::Mat rvec(rv),tvec(tv);
    const auto wait_duration = std::chrono::milliseconds(2000);

    // 生成Aruco码
    // 保存在Prometheus/Modules/object_detection/config/aruco_images文件夹中
    /*******************************************************************************************
    cv::Mat markerImage;
    cv::Mat markerImagePad = cv::Mat::ones(500, 500,  CV_8UC1) * 255;
    for (int i=0; i<250; i++)
    {
        cv::aruco::drawMarker(dictionary, i, 400, markerImage, 1);
        markerImage.copyTo(markerImagePad.rowRange(50, 450).colRange(50, 450));
        cv::imshow("markerImagePad", markerImagePad);
        cv::waitKey(100);
        char img_fn[256];
        sprintf(img_fn, "Prometheus/Modules/object_detection/config/aruco_images/DICT_6X6_250_%d.png", i);
        cv::imwrite(img_fn, markerImagePad);
    }
    *******************************************************************************************/

    while (ros::ok())
	{
        while (!getImageStatus() && ros::ok())
        {
            if (local_print)
                cout << "Waiting for image." << endl;
            if (message_print)
                pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Waiting for image.");
            std::this_thread::sleep_for(wait_duration);
            ros::spinOnce();
        }

        if (switch_state != is_suspanded)
        {
            switch_state = is_suspanded;
            if (!is_suspanded)
            {
                if (local_print)
                    cout << "Start Detection." << endl;
                if (message_print)
                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Start Detection.");
            }
            else
            {
                if (local_print)
                    cout << "Stop Detection." << endl;
                if (message_print)
                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Stop Detection.");
            }
        }

        if (!is_suspanded)
        {

        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            frame = cam_image_copy.clone();
        }

        if (!frame.empty())
        {
            std::vector<int> markerids;
            vector<vector<Point2f> > markerCorners,rejectedCandidate;
            Ptr<cv::aruco::DetectorParameters> parameters=cv::aruco::DetectorParameters::create();
            cv::aruco::detectMarkers(frame,dictionary,markerCorners,markerids,parameters,rejectedCandidate);

            bool deted = false;
            if (markerids.size()>0)
            {
                cv::Mat RoteM, TransM;
                cv::Point3f Theta_C2W;
                cv::Point3f Theta_W2C;
                cv::Point3f Position_OcInW;

                cv::aruco::estimatePoseSingleMarkers(markerCorners,aruco_det_len,camera_matrix,distortion_coefficients,rvec,tvec);

                prometheus_msgs::DetectionInfo pose_now;
                pose_now.header.stamp = ros::Time::now();
                pose_now.detected = true;
                pose_now.frame = 0;
                pose_now.position[0] = tvec.ptr<double>(0)[0];
                pose_now.position[1] = tvec.ptr<double>(0)[1];
                pose_now.position[2] = tvec.ptr<double>(0)[2];

                static_real_x = tvec.ptr<double>(0)[0];
                static_real_y = tvec.ptr<double>(0)[1];
                static_depth  = tvec.ptr<double>(0)[2];

                if (local_print) {
                    cout << "flag_detected: " << int(pose_now.detected) <<endl;
                    cout << "pos_target: [X Y Z] : " << " " << pose_now.position[0] << " [m] "<< pose_now.position[1] <<" [m] "<< pose_now.position[2] <<" [m] "<<endl;
                }
                pose_pub.publish(pose_now);
                deted=true;
            }
            if (!deted)
            {
                prometheus_msgs::DetectionInfo pose_now;
                pose_now.header.stamp = ros::Time::now();
                pose_now.detected = false;
                pose_now.frame = 0;
                pose_now.position[0] = static_real_x;
                pose_now.position[1] = static_real_y;
                pose_now.position[2] = static_depth;

                pose_pub.publish(pose_now);

                if (local_print) {
                    cout << "flag_detected: " << int(pose_now.detected) <<endl;
                    cout << "pos_target: [X Y Z] : " << " " << pose_now.position[0] << " [m] "<< pose_now.position[1] <<" [m] "<< pose_now.position[2] <<" [m] "<<endl;
                }
            }

            Mat img_copy;
            frame.copyTo(img_copy);
            cv::aruco::drawDetectedMarkers(img_copy,markerCorners,markerids);
            msg_ellipse = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img_copy).toImageMsg();
            aruco_pub.publish(msg_ellipse);
            cv::waitKey(1);

		}
        }

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

	  // cap.release();

}


#include "read_rb_scene.hpp"

#include <unordered_set>

#include <Eigen/Geometry>
#include <ghc/fs_std.hpp> // filesystem
#include <igl/edges.h>
#include <igl/facet_components.h>
#include <igl/PI.h>
#include <igl/read_triangle_mesh.h>
#include <igl/remove_unreferenced.h>
#include <tbb/parallel_sort.h>

#include <io/read_obj.hpp>
#include <io/serialize_json.hpp>
#include <logger.hpp>
#include <utils/not_implemented_error.hpp>

namespace ipc::rigid {

template <typename T>
inline bool contains(const std::unordered_set<T>& set, const T& val)
{
    return set.find(val) != set.end();
}

bool read_rb_scene_from_str(const std::string str, std::vector<RigidBody>& rbs)
{
    using nlohmann::json;
    json scene = json::parse(str.c_str());
    return read_rb_scene(scene, rbs);
}

VectorMax3d read_angular_field(const nlohmann::json& field, int dim)
{
    int angular_dim = dim == 2 ? 1 : 3;

    VectorMax3d v;
    if (field.is_number()) {
        assert(dim == 2);
        v.setZero(angular_dim);
        v[0] = field.get<double>();
    } else {
        assert(field.is_array());
        from_json(field, v);
        assert(v.size() >= angular_dim);
        v.conservativeResize(angular_dim);
    }

    // Convert to radians for easy use later
    v *= igl::PI / 180.0;

    return v;
}

bool read_rb_scene(const nlohmann::json& scene, std::vector<RigidBody>& rbs)
{
    using namespace nlohmann;
    int dim = -1, ndof, angular_dim;

    std::unordered_map<std::string, int> rb_name_to_count;

    for (auto& jrb : scene["rigid_bodies"]) {
        // NOTE:
        // All units by default are expressed in standard SI units
        // density: units of Kg/m³ (default density of plastic)
        // position: position of the model origin
        // rotation: degrees as xyz euler angles around the model origin
        // scale: scale the vertices around the model origin
        // linear_velocity: velocity of the body's center
        // angular_velocity: world coordinates in degrees
        // force: applied to center of mass
        // torque: world coordinates in degrees
        json args = R"({
                "mesh": "",
                "vertices": [],
                "edges": [],
                "faces": [],
                "density": 1000.0,
                "is_dof_fixed": [false, false, false, false, false, false],
                "oriented": false,
                "group_id": -1,
                "position": [0.0, 0.0, 0.0],
                "rotation": [0.0, 0.0, 0.0],
                "scale": [1.0, 1.0, 1.0],
                "linear_velocity": [0.0, 0.0, 0.0],
                "angular_velocity": [0.0, 0.0, 0.0],
                "force": [0.0, 0.0, 0.0],
                "torque": [0.0, 0.0, 0.0],
                "enabled": true,
                "type": "dynamic",
                "kinematic_max_time": -1,
                "kinematic_poses": [],
                "split_components": false
            })"_json;
        args.merge_patch(jrb);
        if (args["kinematic_max_time"] < 0) {
            args["kinematic_max_time"] =
                std::numeric_limits<double>::infinity();
        }

        if (!args["enabled"].get<bool>()) {
            continue;
        }

        Eigen::MatrixXd vertices;
        Eigen::MatrixXi faces, edges;
        std::string rb_name;

        std::string mesh_fname = args["mesh"].get<std::string>();
        if (mesh_fname != "") {
            fs::path mesh_path(mesh_fname);
            if (!exists(mesh_path)) {
                // TODO: First check a path relative to the input file
                mesh_path = fs::path(RIGID_IPC_MESHES_DIR) / mesh_path;
            }
            spdlog::info("loading mesh: {:s}", mesh_path.string());
            bool success;
            if (mesh_path.extension() == ".obj") {
                success = read_obj(mesh_path.string(), vertices, edges, faces);
            } else {
                success = igl::read_triangle_mesh(
                    mesh_path.string(), vertices, faces);
                // Initialize edges
                if (faces.size()) {
                    igl::edges(faces, edges);
                }
            }
            assert(faces.size() == 0 || faces.cols() == 3);
            if (!success) {
                return false;
            }

            rb_name = mesh_path.stem().string();
        } else {
            // Assumes that edges contains the edges of the faces too.
            from_json(args["vertices"], vertices);
            from_json(args["edges"], edges);
            from_json(args["faces"], faces);
            rb_name = "RigidBody";
        }

        if (dim == -1) {
            if (vertices.cols() != 0) { // Why would we have an empty body?
                dim = vertices.cols();
                ndof = PoseD::dim_to_ndof(dim);
                angular_dim = dim == 2 ? 1 : 3;
            }
        } else if (dim != vertices.cols()) {
            spdlog::error("Mixing 2D and 3D bodies are not currently allowed.");
            throw NotImplementedError(
                "Mixing 2D and 3D bodies are not currently allowed.");
        }

        if (dim == 2 && faces.size() != 0) {
            spdlog::warn("Ignoring faces for 2D rigid body.");
            // Delete the faces since they will not be uses
            faces.resize(0, 0);
        }

        VectorMax3d position;
        from_json(args["position"], position);
        assert(position.size() >= dim);
        position.conservativeResize(dim);

        VectorMax3d scale;
        // Dimensions overrides a scale argument
        if (args.contains("dimensions")) {
            VectorMax3d initial_dimensions =
                (vertices.colwise().maxCoeff() - vertices.colwise().minCoeff())
                    .cwiseAbs();
            initial_dimensions =
                (initial_dimensions.array() == 0).select(1, initial_dimensions);
            from_json(args["dimensions"], scale);
            assert(scale.size() >= dim);
            scale.conservativeResize(dim);
            scale.array() /= initial_dimensions.array();
        } else if (args["scale"].is_number()) {
            scale.setConstant(dim, args["scale"].get<double>());
        } else {
            from_json(args["scale"], scale);
            assert(scale.size() >= dim);
            scale.conservativeResize(dim);
        }
        vertices *= scale.asDiagonal();

        // Rotate around the models origin NOT the rigid bodies center of mass
        VectorMax3d rotation = read_angular_field(args["rotation"], dim);
        MatrixMax3d R;
        if (rotation.size() == 3) {
            R = (Eigen::AngleAxisd(rotation.z(), Eigen::Vector3d::UnitZ())
                 * Eigen::AngleAxisd(rotation.y(), Eigen::Vector3d::UnitY())
                 * Eigen::AngleAxisd(rotation.x(), Eigen::Vector3d::UnitX()))
                    .toRotationMatrix();
        } else {
            R = Eigen::Rotation2Dd(rotation(0)).toRotationMatrix();
        }
        vertices = vertices * R.transpose();
        rotation.setZero(angular_dim); // Zero initial body rotation

        VectorMax3d linear_velocity;
        from_json(args["linear_velocity"], linear_velocity);
        assert(linear_velocity.size() >= dim);
        linear_velocity.conservativeResize(dim);

        VectorMax3d angular_velocity =
            read_angular_field(args["angular_velocity"], dim);

        VectorMax3d force;
        from_json(args["force"], force);
        assert(force.size() >= dim);
        force.conservativeResize(dim);

        VectorMax3d torque = read_angular_field(args["torque"], dim);

        VectorXb is_dof_fixed;
        if (args["is_dof_fixed"].is_boolean()) {
            is_dof_fixed.setConstant(ndof, args["is_dof_fixed"].get<bool>());
        } else {
            from_json(args["is_dof_fixed"], is_dof_fixed);
            assert(is_dof_fixed.size() >= ndof);
            is_dof_fixed.conservativeResize(ndof);
        }

        double density = args["density"];
        bool is_oriented = args["oriented"];

        int group_id = args["group_id"];

        RigidBodyType rb_type = args["type"];
        double kinematic_max_time = args["kinematic_max_time"];

        std::vector<json> json_kinematic_poses = args["kinematic_poses"];
        std::deque<PoseD> kinematic_poses;
        for (const auto& json_pose : json_kinematic_poses) {
            PoseD pose = PoseD::Zero(dim);
            if (json_pose.contains("position")) {
                from_json(json_pose["position"], pose.position);
            }
            if (json_pose.contains("rotation")) {
                from_json(json_pose["rotation"], pose.rotation);
            }
            kinematic_poses.push_back(pose);
        }

        if (args["split_components"].get<bool>()) {
            // TODO: Handle codimensional edges too
            assert(faces.cols() == 3);
            Eigen::VectorXi C;
            igl::facet_components(faces, C);
            int num_components = C.maxCoeff();
            std::vector<std::vector<int>> CFs(num_components + 1);
            for (int j = 0; j < faces.cols(); j++) {
                for (int i = 0; i < faces.rows(); i++) {
                    CFs[C[i]].push_back(faces(i, j));
                }
            }

            for (int ci = 0; ci < CFs.size(); ci++) {
                Eigen::MatrixXi F = Eigen::Map<Eigen::MatrixXi>(
                    CFs[ci].data(), CFs[ci].size() / 3, 3);
                Eigen::MatrixXd CV;
                Eigen::MatrixXi CF;
                Eigen::VectorXi I;
                igl::remove_unreferenced(vertices, F, CV, CF, I);
                Eigen::MatrixXi CE;
                igl::edges(CF, CE);
                // WARNING: angular velocity and torque will be around the
                // components center of mass not the entire meshes.
                rbs.emplace_back(
                    CV, CE, CF, PoseD(position, VectorMax3d::Zero(angular_dim)),
                    PoseD(linear_velocity, angular_velocity),
                    PoseD(force, torque), density, is_dof_fixed, is_oriented,
                    group_id, rb_type, kinematic_max_time, kinematic_poses);
                rbs.back().name = fmt::format("{}-part{:03d}", rb_name, ci);
            }
        } else {
            rbs.emplace_back(
                vertices, edges, faces,
                PoseD(position, VectorMax3d::Zero(angular_dim)),
                PoseD(linear_velocity, angular_velocity), PoseD(force, torque),
                density, is_dof_fixed, is_oriented, group_id, rb_type,
                kinematic_max_time, kinematic_poses);
            rbs.back().name = rb_name;
        }
    }

    // Adjust the group ids, so the default ones are unique.
    std::unordered_set<int> static_group_ids;
    for (RigidBody& rb : rbs) {
        if (rb.type != RigidBodyType::DYNAMIC) {
            if (rb.group_id >= 0) {
                static_group_ids.insert(rb.group_id);
            }
            // All static bodies will be given the same group id later
            rb.group_id = -1;
        }
    }
    std::vector<int> taken_ids;
    for (RigidBody& rb : rbs) {
        if (rb.group_id >= 0) {
            taken_ids.push_back(rb.group_id);
        }
    }
    tbb::parallel_sort(taken_ids.begin(), taken_ids.end());
    int taken_id_i = 0;
    int id = 0;
    int static_group_id = -1;
    for (RigidBody& rb : rbs) {
        bool in_static_group = rb.type != RigidBodyType::DYNAMIC
            || contains(static_group_ids, rb.group_id);
        if (static_group_id >= 0 && in_static_group) {
            // All static bodies are in the same group
            rb.group_id = static_group_id;
        } else if (rb.group_id < 0) {
            // Find the next free id
            while (taken_id_i < taken_ids.size()
                   && id >= taken_ids[taken_id_i]) {
                if (id == taken_ids[taken_id_i]) {
                    id++;
                }
                taken_id_i++;
            }
            rb.group_id = id++;
            assert(!std::binary_search(
                taken_ids.begin(), taken_ids.end(), rb.group_id));
        }

        assert(rb.group_id >= 0);
        if (static_group_id < 0 && in_static_group) {
            // All static bodies are in the same group
            static_group_id = rb.group_id;
        }
    }

    return true;
}

} // namespace ipc::rigid


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

/**
 * \file
 * \brief core homography template implementations
 */

#include "homography.h"

#include <cmath>

#include <vital/exceptions/math.h>

#include <Eigen/LU>

namespace kwiver {
namespace vital {

namespace //anonymous
{

/// Private helper method for point transformation via homography matrix
template < typename T >
Eigen::Matrix< T, 2, 1 >
h_map_point( Eigen::Matrix< T, 3, 3 > const& h, Eigen::Matrix< T, 2, 1 > const& p )
{
  Eigen::Matrix< T, 3, 1 > out_pt = h * Eigen::Matrix< T, 3, 1 > ( p[0], p[1], 1.0 );

  if ( fabs( out_pt[2] ) <= Eigen::NumTraits< T >::dummy_precision() )
  {
    throw point_maps_to_infinity();
  }
  return Eigen::Matrix< T, 2, 1 > ( out_pt[0] / out_pt[2], out_pt[1] / out_pt[2] );
}

} // end anonymous namespace

/// Construct an identity homography
template < typename T >
homography_< T >
::homography_()
  : h_( matrix_t::Identity() )
{
}

/// Construct from a provided transformation matrix
template < typename T >
homography_< T >
::homography_( Eigen::Matrix< T, 3, 3 > const& mat )
  : h_( mat )
{
}

/// Conversion Copy constructor -- float specialization
template < >
template < >
homography_< float >
::homography_( homography_< float > const& other )
  : h_( other.get_matrix() )
{
}

/// Conversion Copy constructor -- double specialization
template < >
template < >
homography_< double >
::homography_( homography_< double > const& other )
  : h_( other.get_matrix() )
{
}

/// Construct from a generic homography
template < typename T >
homography_< T >
::homography_( homography const& base )
  : h_( base.matrix().template cast< T > () )
{
}

/// Construct from a generic homography -- double specialization
template < >
homography_< double >
::homography_( homography const& base )
  : h_( base.matrix() )
{
}

/// Create a clone of outself as a shared pointer
template < typename T >
transform_2d_sptr
homography_< T >
::clone() const
{
  return std::make_shared< homography_< T > >( *this );
}

/// Get a double-typed copy of the underlying matrix transformation
template < typename T >
Eigen::Matrix< double, 3, 3 >
homography_< T >
::matrix() const
{
  return this->h_.template cast< double > ();
}

/// Specialization for homographies with native double type
template < >
Eigen::Matrix< double, 3, 3 >
homography_< double >
::matrix() const
{
  return this->h_;
}

/// Normalize homography transformation in-place
template < typename T >
homography_sptr
homography_< T >
::normalize() const
{
  matrix_t norm = this->get_matrix();

  if ( fabs( norm( 2, 2 ) ) >= Eigen::NumTraits< T >::dummy_precision() )
  {
    norm /= norm( 2, 2 );
  }
  return homography_sptr( new homography_< T > ( norm ) );
}

/// Inverse the homography transformation returning a new transformation
template < typename T >
homography_sptr
homography_< T >
::inverse() const
{
  matrix_t inv;
  bool isvalid;

  this->h_.computeInverseWithCheck( inv, isvalid );
  if ( ! isvalid )
  {
    throw non_invertible_matrix();
  }
  return homography_sptr( new homography_< T > ( inv ) );
}

/// Map a 2D double-type point using this homography
template < typename T >
Eigen::Matrix< double, 2, 1 >
homography_< T >
::map( Eigen::Matrix< double, 2, 1 > const& p ) const
{
  // Explicitly refer to templated version of method so as to not infinitely
  // recurse.
  Eigen::Matrix< double, 3, 3 > m = h_.template cast< double > ();

  return h_map_point( m, p );
}

/// Map a 2D double-type point using this homography -- double specialization
template < >
Eigen::Matrix< double, 2, 1 >
homography_< double >
::map( Eigen::Matrix< double, 2, 1 > const& p ) const
{
  return h_map_point( h_, p );
}

/// Get the underlying matrix transformation
template < typename T >
typename homography_< T >::matrix_t &
homography_< T >
::get_matrix()
{
  return this->h_;
}

/// Get a const new copy of the underlying matrix transformation.
template < typename T >
typename homography_< T >::matrix_t const &
homography_< T >
::get_matrix() const
{
  return this->h_;
}

/// Map a 2D point using this homography -- generic version
template < typename T >
Eigen::Matrix< T, 2, 1 >
homography_< T >
::map_point( Eigen::Matrix< T, 2, 1 > const& p ) const
{
  return h_map_point< T > ( h_.template cast< T > (), p );
}

/// Map a 2D point using this homography -- float specialization
template < >
Eigen::Matrix< float, 2, 1 >
homography_< float >
::map_point( Eigen::Matrix< float, 2, 1 > const& p ) const
{
  return h_map_point( h_, p );
}

/// Map a 2D point using this homography -- double specialization
template < >
Eigen::Matrix< double, 2, 1 >
homography_< double >
::map_point( Eigen::Matrix< double, 2, 1 > const& p ) const
{
  return h_map_point( h_, p );
}

/// Custom f2f_homography multiplication operator.
template < typename T >
homography_< T >
homography_< T >
::operator*( homography_< T > const& rhs ) const
{
  return homography_< T > ( h_ * rhs.h_ );
}

// ===========================================================================
// Other Functions
// ---------------------------------------------------------------------------

/// homography_<T> output stream operator
template < typename T >
std::ostream&
operator<<( std::ostream& s, homography_< T > const& h )
{
  s << h.get_matrix();
  return s;
}

/// Output stream operator for \p homography instances
std::ostream&
operator<<( std::ostream& s, homography const& h )
{
  s << h.matrix();
  return s;
}

// ===========================================================================
// Template class instantiation
// ---------------------------------------------------------------------------
/// \cond DoxygenSuppress
#define INSTANTIATE_HOMOGRAPHY( T )              \
  template class homography_< T >;               \
  template VITAL_EXPORT std::ostream&            \
  operator<<( std::ostream&,                     \
              homography_< T > const& )

INSTANTIATE_HOMOGRAPHY( float );
INSTANTIATE_HOMOGRAPHY( double );
#undef INSTANTIATE_HOMOGRAPHY
/// \endcond

} } // end vital namespace


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

#include <AnimatedScene.h>
#include <dp/DP.h>
#include <dp/sg/core/GeoNode.h>
#include <dp/sg/core/Group.h>
#include <dp/sg/core/PipelineData.h>
#include <dp/sg/core/Scene.h>
#include <dp/sg/core/Primitive.h>
#include <dp/sg/core/Transform.h>

#include <dp/sg/generator/MeshGenerator.h>
#include <boost/make_shared.hpp>

#include <cmath>

AnimationTime::AnimationTime()
  : m_time(0.0f)
{
}

DEFINE_STATIC_PROPERTY(AnimationTime, Time);

BEGIN_REFLECTION_INFO(AnimationTime)
INIT_STATIC_PROPERTY_RW(AnimationTime, Time, float, Semantic::VALUE, value, value);
END_REFLECTION_INFO

Animator::~Animator()
{
}

AnimatorBase::AnimatorBase()
{
}

DEFINE_STATIC_PROPERTY(AnimatorColor, Time);
DEFINE_STATIC_PROPERTY(AnimatorColor, Color3);
DEFINE_STATIC_PROPERTY(AnimatorColor, Color4);

BEGIN_REFLECTION_INFO(AnimatorColor)
INIT_STATIC_PROPERTY_RW(AnimatorColor, Time, float, Semantic::VALUE, value, value);
INIT_STATIC_PROPERTY_RO(AnimatorColor, Color3, dp::math::Vec3f, Semantic::VALUE, value);
INIT_STATIC_PROPERTY_RO(AnimatorColor, Color4, dp::math::Vec4f, Semantic::VALUE, value);
END_REFLECTION_INFO

AnimatorColorSharedPtr AnimatorColor::create(int x, int y, dp::math::Vec2i const & objectCount)
{
  return(std::shared_ptr<AnimatorColor>(new AnimatorColor(x, y, objectCount)));
}

AnimatorColor::AnimatorColor(int x, int y, dp::math::Vec2i const & objectCount)
  : m_x(x)
  , m_y(y)
  , m_objectCount(objectCount)
{
}

dp::math::Vec3f AnimatorColor::getColor3() const
{
  float sx;
  float ix;
  sx = std::modf((float(m_x) / m_objectCount[0]) + m_time, &ix);
  float sy = float(m_y) / m_objectCount[1];

  return dp::math::Vec3f(sx, sy, sx * sy);
}

dp::math::Vec4f AnimatorColor::getColor4() const
{
  float sx;
  float ix;
  sx = std::modf((float(m_x) / m_objectCount[0]) + m_time, &ix);
  float sy = float(m_y) / m_objectCount[1];

  return dp::math::Vec4f(sx, sy, sx * sy, 1.0f);
}

/************************************************************************/
/* AnimatedScene                                                        */
/************************************************************************/
AnimatedSceneSharedPtr AnimatedScene::create( const dp::math::Vec2f& gridSize, const dp::math::Vec2i& objectCount )
{
  return( std::shared_ptr<AnimatedScene>( new AnimatedScene( gridSize, objectCount ) ) );
}

AnimatedScene::AnimatedScene( const dp::math::Vec2f& gridSize, const dp::math::Vec2i& objectCount )
  : m_gridSize( gridSize )
  , m_objectCount( objectCount )
{
  m_root = dp::sg::core::Group::create();
  m_scene = dp::sg::core::Scene::create();
  m_scene->setRootNode( m_root );
  m_effectSpec = dp::sg::core::getStandardMaterialSpec();
  m_primitive = dp::sg::generator::createSphere( 5, 5, 1.5 );
  //m_primitive = dp::sg::generator::createCube();

  m_itColors = m_effectSpec->findParameterGroupSpec( std::string("standardMaterialParameters") );

  dp::util::FileFinder fileFinder;
  fileFinder.addSearchPath( dp::home() + "/media/dpfx" );
  fileFinder.addSearchPath( dp::home() + "/media/textures" );
  dp::fx::EffectLibrary::instance()->loadEffects( dp::home() + "/media/effects/xml/carpaint.xml", fileFinder );
  dp::fx::EffectLibrary::instance()->loadEffects( dp::home() + "/media/effects/xml/phong.xml", fileFinder );
  dp::fx::EffectLibrary::instance()->loadEffects( dp::home() + "/media/effects/xml/standard_material.xml", fileFinder );
  dp::fx::EffectLibrary::instance()->loadEffects( dp::home() + "/media/effects/xml/thinglass.xml", fileFinder );

  m_carpaint          = dp::sg::core::PipelineData::create( dp::fx::EffectLibrary::instance()->getEffectData("carpaint") );
  m_phong             = dp::sg::core::PipelineData::create( dp::fx::EffectLibrary::instance()->getEffectData("phong") );
  m_standard_material = dp::sg::core::PipelineData::create( dp::fx::EffectLibrary::instance()->getEffectData("standardMaterial") );
  m_thinglass         = dp::sg::core::PipelineData::create( dp::fx::EffectLibrary::instance()->getEffectData("thinglass") );

  m_animationTime = std::shared_ptr<AnimationTime>(new AnimationTime());

  DP_ASSERT( m_carpaint );
  DP_ASSERT( m_phong );
  DP_ASSERT( m_standard_material );
  DP_ASSERT( m_thinglass );

  createGrid( );
}

AnimatedScene::~AnimatedScene()
{

}

dp::sg::core::SceneSharedPtr AnimatedScene::getScene() const
{
  return m_scene;
}

dp::sg::core::PrimitiveSharedPtr AnimatedScene::createPrimitive( size_t x, size_t y )
{
  return m_primitive;
}

void AnimatedScene::linkAnimationColor2(AnimatorColorSharedPtr const & animation, dp::sg::core::PipelineDataSharedPtr const & effect, char const *group, char const *parameter)
{
  m_linkManager.link(m_animationTime, AnimationTime::PID_Time, animation, AnimatorColor::PID_Time);

  dp::fx::EffectSpec::iterator esIt = effect->getEffectSpec()->findParameterGroupSpec(group);
  dp::fx::ParameterGroupSpec::iterator itParameter = (*esIt)->findParameterSpec(parameter);
  dp::util::PropertyId pidParameter = effect->getParameterGroupData(esIt)->getProperty(parameter);

  if ((itParameter->first.getType() & dp::fx::PT_SCALAR_MODIFIER_MASK) == dp::fx::PT_VECTOR3)
  {
    m_linkManager.link(animation, AnimatorColor::PID_Color3, effect->getParameterGroupData(esIt), pidParameter);
  }
  else if ((itParameter->first.getType() & dp::fx::PT_SCALAR_MODIFIER_MASK) == dp::fx::PT_VECTOR4)
  {
    m_linkManager.link(animation, AnimatorColor::PID_Color4, effect->getParameterGroupData(esIt), pidParameter);
  }
  else {
    DP_ASSERT(!"unsupported animated color parameter");
  }
}

dp::sg::core::PipelineDataSharedPtr AnimatedScene::createMaterial( size_t x, size_t y )
{
  dp::sg::core::PipelineDataSharedPtr effect;
  size_t index = (y * m_objectCount[0]) + x;
  switch (index % 5)
  {
  case 0:
    {
      effect = dp::sg::core::PipelineData::create( m_effectSpec );

      dp::sg::core::ParameterGroupDataSharedPtr parameterGroupDataSharedPtr = dp::sg::core::ParameterGroupData::create( *m_itColors );
      effect->setParameterGroupData( parameterGroupDataSharedPtr );
      linkAnimationColor2(AnimatorColor::create(int(x), int(y), m_objectCount), effect, "standardMaterialParameters", "frontDiffuseColor");
    }
    break;
  case 1:
    effect = std::static_pointer_cast<dp::sg::core::PipelineData>(m_carpaint->clone());
    linkAnimationColor2(AnimatorColor::create(int(x), int(y), m_objectCount), effect, "carpaint_parameters", "diffuse");
    break;
  case 2:
    effect = std::static_pointer_cast<dp::sg::core::PipelineData>(m_phong->clone());
    linkAnimationColor2(AnimatorColor::create(int(x), int(y), m_objectCount), effect, "phongParameters", "diffuseColor");
    break;
  case 3:
    effect = std::static_pointer_cast<dp::sg::core::PipelineData>(m_standard_material->clone());
    linkAnimationColor2(AnimatorColor::create(int(x), int(y), m_objectCount), effect, "standardMaterialParameters", "frontDiffuseColor");
    break;
  case 4:
    effect = std::static_pointer_cast<dp::sg::core::PipelineData>(m_thinglass->clone());
    linkAnimationColor2(AnimatorColor::create(int(x), int(y), m_objectCount), effect, "thinglass_parameters", "transparentColor");
    break;
  }

  return effect;
}

dp::sg::core::TransformSharedPtr AnimatedScene::createTransform( size_t x, size_t y )
{
  float posX = float(x) / float(m_objectCount[0]) * m_gridSize[0];
  float posY = float(y) / float(m_objectCount[1]) * m_gridSize[1];
  dp::sg::core::TransformSharedPtr transform = dp::sg::core::Transform::create();
  transform->setTranslation( dp::math::Vec3f( posX, posY, 0.0f) );
  return transform;
}

void AnimatedScene::createGrid()
{
  size_t objectCount = m_objectCount[0] * m_objectCount[1];
  m_primitives.resize( objectCount );
  m_materials.resize( objectCount );
  m_geoNodes.resize( objectCount );
  m_transforms.resize( objectCount );
  m_animators.resize( objectCount );

  size_t index = 0;
  for ( size_t y = 0; y < m_objectCount[1]; ++y )
  {
    for ( size_t x = 0; x < m_objectCount[0]; ++x )
    {
      m_primitives[index] = createPrimitive( x, y );
      m_materials[index] = createMaterial( x, y );
      m_geoNodes[index] = dp::sg::core::GeoNode::create();
      {
        m_geoNodes[index]->setMaterialPipeline( m_materials[index] );
        m_geoNodes[index]->setPrimitive( m_primitives[index] );
      }
      m_transforms[index] = createTransform( x, y );
      {
        m_transforms[index]->addChild( m_geoNodes[index] );
      }
      m_root->addChild( m_transforms[index] );

      ++index;
    }
  }
}

void AnimatedScene::update( float time )
{
  m_animationTime->setTime(time);
  m_linkManager.processLinks();
}

void AnimatedScene::updateTransforms( float time )
{
  size_t index = 0;
  float animationTime = sin(time) * 10;
  for ( size_t y = 0; y < m_objectCount[1]; ++y )
  {
    for ( size_t x = 0; x < m_objectCount[0]; ++x )
    {
      float fx = float(x) / m_objectCount[0] + (animationTime / 10.0f);
      float fy = float(y) / m_objectCount[1] + sin(animationTime);
      float height = sin(fx*fx) * cos(fx*fy*cos(animationTime));
      dp::math::Vec3f translation = m_transforms[index]->getTranslation();
      translation[2] = height * (m_gridSize[0] + m_gridSize[1]) * 0.1f;
      m_transforms[index]->setTranslation(translation);
      ++index;
    }
  }
}


/*=============================================================================
    Copyright (c) 2005-2007 Hartmut Kaiser
                  2007, 2009 Tim Blechmann

    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)
=============================================================================*/
#if !defined(BOOST_HIGH_RESOLUTION_TIMER_HPP)
#define BOOST_HIGH_RESOLUTION_TIMER_HPP

#include <boost/config.hpp>
#include <boost/throw_exception.hpp>

#if _POSIX_C_SOURCE >= 199309L

#include "time.h"

#include <stdexcept>
#include <limits>

namespace boost {

class high_resolution_timer
{
public:
    high_resolution_timer()
    {
        restart();
    }

    void restart()
    {
        int status = clock_gettime(CLOCK_REALTIME, &start_time);

        if (status == -1)
            boost::throw_exception(std::runtime_error("Couldn't initialize start_time"));
    }

    double elapsed() const                  // return elapsed time in seconds
    {
        struct timespec now;

        int status = clock_gettime(CLOCK_REALTIME, &now);

        if (status == -1)
            boost::throw_exception(std::runtime_error("Couldn't get current time"));

        struct timespec diff;

        double ret_sec = double(now.tv_sec - start_time.tv_sec);
        double ret_nsec = double(now.tv_nsec - start_time.tv_nsec);

        while (ret_nsec < 0)
        {
            ret_sec -= 1.0;
            ret_nsec += 1e9;
        }

        double ret = ret_sec + ret_nsec / 1e9;

        return ret;
    }

    double elapsed_max() const   // return estimated maximum value for elapsed()
    {
        return double((std::numeric_limits<double>::max)());
    }

    double elapsed_min() const            // return minimum value for elapsed()
    {
        return 0.0;
    }

private:
    struct timespec start_time;
};

} // namespace boost

#elif defined(__APPLE__)

#import <mach/mach_time.h>

namespace boost {

class high_resolution_timer
{
public:
    high_resolution_timer(void)
    {
        mach_timebase_info_data_t info;

        kern_return_t err = mach_timebase_info(&info);
        if (err)
            throw std::runtime_error("cannot create mach timebase info");

        conversion_factor = (double)info.numer/(double)info.denom;
        restart();
    }

    void restart()
    {
        start = mach_absolute_time();
    }

    double elapsed() const                  // return elapsed time in seconds
    {
        uint64_t now = mach_absolute_time();
        double duration = double(now - start) * conversion_factor;

        return duration
    }

    double elapsed_max() const   // return estimated maximum value for elapsed()
    {
        return double((std::numeric_limits<double>::max)());
    }

    double elapsed_min() const            // return minimum value for elapsed()
    {
        return 0.0;
    }

private:
    uint64_t start;
    double conversion_factor;
};

} // namespace boost

#elif defined(BOOST_WINDOWS)

#include <stdexcept>
#include <limits>
#include <windows.h>

namespace boost {

///////////////////////////////////////////////////////////////////////////////
//
//  high_resolution_timer
//      A timer object measures elapsed time.
//      CAUTION: Windows only!
//
///////////////////////////////////////////////////////////////////////////////
class high_resolution_timer
{
public:
    high_resolution_timer()
    {
        start_time.QuadPart = 0;
        frequency.QuadPart = 0;

        if (!QueryPerformanceFrequency(&frequency))
            boost::throw_exception(std::runtime_error("Couldn't acquire frequency"));

        restart();
    }

    void restart()
    {
        if (!QueryPerformanceCounter(&start_time))
            boost::throw_exception(std::runtime_error("Couldn't initialize start_time"));
    }

    double elapsed() const                  // return elapsed time in seconds
    {
        LARGE_INTEGER now;
        if (!QueryPerformanceCounter(&now))
            boost::throw_exception(std::runtime_error("Couldn't get current time"));

        return double(now.QuadPart - start_time.QuadPart) / frequency.QuadPart;
    }

    double elapsed_max() const   // return estimated maximum value for elapsed()
    {
        return (double((std::numeric_limits<LONGLONG>::max)())
            - double(start_time.QuadPart)) / double(frequency.QuadPart);
    }

    double elapsed_min() const            // return minimum value for elapsed()
    {
        return 1.0 / frequency.QuadPart;
    }

private:
    LARGE_INTEGER start_time;
    LARGE_INTEGER frequency;
};

} // namespace boost

#else

//  For other platforms, simply fall back to boost::timer
#include <boost/timer.hpp>
#include <boost/throw_exception.hpp>

namespace boost {
    typedef boost::timer high_resolution_timer;
}

#endif

#endif  // !defined(BOOST_HIGH_RESOLUTION_TIMER_HPP)