#include <iostream>
#include <Eigen/Eigen>
#include <list>
#include <map>
#include <unordered_set>
#include <math.h>
#include <LentPitShift.h>

#include "SongEmbedder.h"
#include "Chunk.h"
#include "util.h"

using std::list;
using std::map;
using std::unordered_set;
using std::pair;
using std::string;
using std::max;
using Eigen::MatrixXf;
using Eigen::MatrixXi;
using stk::FileWvIn;
using stk::FileWvOut;
using stk::StkFrames;
using stk::LentPitShift;

int get_closest_bin_index(map<int, list<Chunk>> bins, int start);

SongEmbedder::SongEmbedder(Song src, Song dst, int sps, int edge_r, int chunk_r, int match_r) {
  reciptor = src;
  replacer = dst;
  samples_per_slice = sps;
  edge_snazr = edge_r;
  chunk_snazr = chunk_r;
  match_snazr = match_r;
}

double chunkmatch_get_score_wrapper(ChunkMatch& a) {
  return a.get_score();
}

double chunkgroupmatch_get_score_wrapper(ChunkGroupMatch& a) {
  return a.get_score();
}

list<SoundPatch> SongEmbedder::individual_sound_patches(list<Chunk>& res, list<Chunk>& src) {
  list<ChunkMatch> matches = get_matches(res, src);
  list<SoundPatch> ret;
  for (ChunkMatch& m : matches) {
    ret.push_back(SoundPatch(m, replacer));
  }
  return ret;
}

list<SoundPatch> SongEmbedder::group_sound_patches(list<Chunk>& res, list<Chunk>& src) {

  list<ChunkGroup> res_pool = group_chunks(res);
  printf("For result chunks, put %lu chunks into %lu groups\n", res.size(), res_pool.size());
  list<ChunkGroup> src_pool = group_chunks(src);
  printf("For source chunks, put %lu chunks into %lu groups\n", src.size(), src_pool.size());
  list<ChunkGroupMatch> matches = get_matches(res_pool, src_pool);
  list<SoundPatch> ret;
  for (ChunkGroupMatch& m : matches) {
    ret.push_back(SoundPatch(m, replacer));
  }
  return ret;
}

list<ChunkMatch> SongEmbedder::get_matches(list<Chunk> &reciptor_chunks, list<Chunk> &replacer_chunks) {
  //weights for frequency center, frequency margin, and time marin
  ChunkCompare comp (50,1,10);

  std::cout << "matching sound chunks!\n";
  list<ChunkMatch> matches = ChunkMatch::quick_match(reciptor_chunks, replacer_chunks, comp);
  std::cout << "Done with matching!\n";
  if (match_snazr > 0) {
    std::cout << "culling bad matches\n";
    std::cout << "orig len is " << matches.size() << "\n";
    double avg = reverse_gsnaz(matches, chunkmatch_get_score_wrapper, match_snazr);
    std::cout << "only accepted scores <= " << avg << "\n";
    std::cout << "culled len is " << matches.size() << "\n";
  }

  return matches;
}

list<ChunkGroupMatch> SongEmbedder::get_matches(list<ChunkGroup> &res, list<ChunkGroup>& src) {
  //weights for frequency center, frequency margin, and time marin
  ChunkGroupCompare comp (3,9,1);

  std::cout << "matching chunk groups!\n";
  list<ChunkGroupMatch> matches = ChunkGroupMatch::best_match(res, src, comp);
  std::cout << "Done with matching!\n";
  if (match_snazr > 0) {
    std::cout << "culling bad matches\n";
    std::cout << "orig len is " << matches.size() << "\n";
    double avg = reverse_gsnaz(matches, chunkgroupmatch_get_score_wrapper, match_snazr);
    std::cout << "only accepted scores <= " << avg << "\n";
    std::cout << "culled len is " << matches.size() << "\n";
  }

  return matches;
}

void SongEmbedder::funk() {
  if (reciptor.get_file_rate() == replacer.get_file_rate()) {
    list<Chunk> reciptor_chunks = reciptor.make_chunks(samples_per_slice, edge_snazr, chunk_snazr);
    list<Chunk> replacer_chunks = replacer.make_chunks(samples_per_slice, edge_snazr, chunk_snazr);

    std::string fn = "muxed.wav";
    bool useSoundPatch = true;
    bool useGroupMatch = true;
    if (useSoundPatch) {
      list<SoundPatch> patches;
      if (useGroupMatch) {
	patches = group_sound_patches(reciptor_chunks, replacer_chunks);
      }
      else {
	patches = individual_sound_patches(reciptor_chunks, replacer_chunks);
      }
      output_remix(patches, fn);
    }
    else {
      list<ChunkMatch>matches = get_matches(reciptor_chunks, replacer_chunks);
      output_remix(matches, fn);
    }
    //output_remix_alt(samples_per_slice, matches, "muxed_alt.wav");
    std::cout << "completed output\n";
  }
  else {
    //ERROR
    //would have to do weird stuff to work with this
    //because the different sampling rates would manifest in fft
    //would have the frequency bins of both spectrograms not line up
    std::cerr << "unable to mix files, sample rates are different, please use ffmpeg or alternative software to convert songs to same sample rate\n";
  }
}

struct cm_hash {
  size_t operator ()(const ChunkMatch & cm) const {
    return ChunkMatch::match_hash((ChunkMatch&)cm);
  }
};

void SongEmbedder::output_remix(list<ChunkMatch> &matches, std::string fn) {
  list<ChunkMatch> active_start;
  list<ChunkMatch> active_end;
  for (ChunkMatch a_match : matches) {
    active_start.push_front(a_match);
    active_end.push_front(a_match);
  }

  active_start.sort(ChunkMatch::comp_active_start);
  active_end.sort(ChunkMatch::comp_active_end);
  int new_slices = reciptor.get_samples_per_channel() / samples_per_slice;
  int channels = reciptor.get_channels();
  FileWvIn sample_src;
  FileWvOut output;
  StkFrames frame_in (samples_per_slice, channels);
  StkFrames frame_out (samples_per_slice, channels);
  StkFrames zero (0, samples_per_slice, channels);
  StkFrames temp (0, samples_per_slice, channels);

  LentPitShift lent(1, samples_per_slice);

  StkFrames rev_feather (0, samples_per_slice, channels);
  StkFrames reg_feather (0, samples_per_slice, channels);
  int feather_margin = samples_per_slice;
  float feather_val = 0;
  for (int i = 0; i < samples_per_slice; i++) {
    feather_val = std::min(1.0f, (float)i / feather_margin);
    for (int chan = 0; chan < channels; chan++) {
      reg_feather(i, chan) = feather_val;
      rev_feather(samples_per_slice - 1 - i, chan) = feather_val;
    }
  }

  sample_src.openFile(replacer.get_file_name());
  output.openFile(fn, channels, stk::FileWrite::FILE_WAV, stk::Stk::STK_SINT16);

  unordered_set<ChunkMatch,cm_hash> active_chunks;
  for (int i = 0; i < new_slices; i++) {
    while(active_start.size() > 0 && active_start.front().get_active_start() <= i) {
      if (active_start.front().get_active_start() == i) {
	active_chunks.insert(active_start.front());
	active_start.pop_front();
      }
      else if (active_start.front().get_active_start() < 0) {
	//odd case, happens when song matches a chunk which is long enough and close enough to start
	//that it has a negative start time.
	//just tossing it away for now
	active_start.pop_front();
      }
    }

    frame_out *= zero;
    for (ChunkMatch chunk_match : active_chunks) {
      temp *= zero;
      normal_filt(chunk_match, sample_src, frame_in, temp, i);
      //alt_filt(chunk_match, sample_src, temp, i, lent);

      if (chunk_match.get_active_start() == i) {
	temp *= reg_feather;
      }
      if (chunk_match.get_active_end() == i) {
	temp *= rev_feather;
      }

      frame_out += temp;
    }
    //skips sections of silence from start and end of song
    if (active_start.size() != matches.size() && active_end.size() != 0) {
      output.tick(frame_out);
    }

    while(active_end.size() > 0 && active_end.front().get_active_end() == i) {
      active_chunks.erase(active_end.front());
      active_end.pop_front();
    }
  }
  output.closeFile();
}

void SongEmbedder::normal_filt(ChunkMatch& chunk_match, FileWvIn &sample_src, StkFrames &frame_in, StkFrames &out, int i) {
  Chunk match = chunk_match.get_match_chunk();
  int frame_n = (i - chunk_match.get_active_start()) + match.get_time_start();
  fetch_frame(frame_n, samples_per_slice, sample_src, frame_in);
  filter_frame(match, frame_in, out);
}

void SongEmbedder::alt_filt(ChunkMatch& chunk_match, FileWvIn &sample_src, StkFrames &out, int i, LentPitShift &lent)  {
  int channels = sample_src.channelsOut();;
  Chunk match = chunk_match.get_match_chunk();
  Chunk orig = chunk_match.get_orig_chunk();
  ChunkFilter filt = match.get_filter();
  double offset_ratio =  (double)(i - orig.get_time_start()) / (orig.get_time_length());
  int offset = (offset_ratio * match.get_time_length() + match.get_time_start()) * samples_per_slice;
  sample_src.reset();
  sample_src.addTime(offset);

  double compression_ratio = (double)orig.get_time_length() / match.get_time_length();
  int size = samples_per_slice / compression_ratio;
  StkFrames temp_in (size, channels);
  sample_src.tick(temp_in);

  filt.fir_filter_frame(temp_in, temp_in);
  reshape_chunk(temp_in, out, orig, match, lent);
}

static bool sound_p_act_start_wrap(SoundPatch* a, SoundPatch* b) {
  return SoundPatch::comp_active_start(*a,*b);
}

static bool sound_p_act_end_wrap(SoundPatch* a, SoundPatch* b) {
  return SoundPatch::comp_active_end(*a,*b);
}

struct sp_hash {
  size_t operator ()(const SoundPatch* sp) const {
    return SoundPatch::patch_hash(*sp);
  }
};

void SongEmbedder::output_remix(list<SoundPatch>& patches, std::string fn) {
  std::cout << "outputting song\n";
  list<SoundPatch*> active_start;
  list<SoundPatch*> active_end;
  unordered_set<SoundPatch*,sp_hash> active_patches;
  for (SoundPatch& p : patches) {
    active_start.push_front(&p);
    active_end.push_front(&p);
  }

  active_start.sort(sound_p_act_start_wrap);
  active_end.sort(sound_p_act_end_wrap);

  int new_slices = reciptor.get_samples_per_channel() / samples_per_slice;
  int channels = reciptor.get_channels();

  FileWvOut output;
  StkFrames frame_in (samples_per_slice, channels);
  StkFrames frame_out (samples_per_slice, channels);
  StkFrames zero (0, samples_per_slice, channels);
  StkFrames temp (0, samples_per_slice, channels);

  LentPitShift lent(1, samples_per_slice);

  StkFrames rev_feather (0, samples_per_slice, channels);
  StkFrames reg_feather (0, samples_per_slice, channels);
  int feather_margin = samples_per_slice;
  float feather_val = 0;
  for (int i = 0; i < samples_per_slice; i++) {
    feather_val = std::min(1.0f, (float)i / feather_margin);
    for (int chan = 0; chan < channels; chan++) {
      reg_feather(i, chan) = feather_val;
      rev_feather(samples_per_slice - 1 - i, chan) = feather_val;
    }
  }

  output.openFile(fn, channels, stk::FileWrite::FILE_WAV, stk::Stk::STK_SINT16);

  for (int i = 0; i < new_slices; i++) {
    while(active_start.size() > 0 && active_start.front()->get_active_start() <= i) {
      if (active_start.front()->get_active_start() == i) {
	active_patches.insert(active_start.front());
	active_start.pop_front();
      }
      else if (active_start.front()->get_active_start() < 0) {
	//odd case, happens when song matches a chunk which is long enough and close enough to start
	//that it has a negative start time.
	//just tossing it away for now
	active_start.pop_front();
      }
    }

    frame_out *= zero;
    for (SoundPatch* p : active_patches) {
      temp *= zero;

      p->tick_out(i, temp);

      if (p->get_active_start() == i) {
	temp *= reg_feather;
      }
      if (p->get_active_end() == i) {
	temp *= rev_feather;
      }
      frame_out += temp;
    }
    //skips sections of silence from start and end of song
    if (active_start.size() != patches.size() && active_end.size() != 0) {
      output.tick(frame_out);
    }

    while(active_end.size() > 0 && active_end.front()->get_active_end() == i) {
      active_patches.erase(active_end.front());
      active_end.pop_front();
    }
  }
  output.closeFile();
}


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_GGSVP_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_GGSVP_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/detail/array.hpp>
#include <boost/numeric/bindings/is_column_major.hpp>
#include <boost/numeric/bindings/is_complex.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/is_real.hpp>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/utility/enable_if.hpp>

//
// The LAPACK-backend for ggsvp is the netlib-compatible backend.
//
#include <boost/numeric/bindings/lapack/detail/lapack.h>
#include <boost/numeric/bindings/lapack/detail/lapack_option.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace lapack {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end LAPACK-routine.
//
namespace detail {

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * float value-type.
//
inline std::ptrdiff_t ggsvp( const char jobu, const char jobv, const char jobq,
        const fortran_int_t m, const fortran_int_t p, const fortran_int_t n,
        float* a, const fortran_int_t lda, float* b, const fortran_int_t ldb,
        const float tola, const float tolb, fortran_int_t& k,
        fortran_int_t& l, float* u, const fortran_int_t ldu, float* v,
        const fortran_int_t ldv, float* q, const fortran_int_t ldq,
        fortran_int_t* iwork, float* tau, float* work ) {
    fortran_int_t info(0);
    LAPACK_SGGSVP( &jobu, &jobv, &jobq, &m, &p, &n, a, &lda, b, &ldb, &tola,
            &tolb, &k, &l, u, &ldu, v, &ldv, q, &ldq, iwork, tau, work,
            &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * double value-type.
//
inline std::ptrdiff_t ggsvp( const char jobu, const char jobv, const char jobq,
        const fortran_int_t m, const fortran_int_t p, const fortran_int_t n,
        double* a, const fortran_int_t lda, double* b,
        const fortran_int_t ldb, const double tola, const double tolb,
        fortran_int_t& k, fortran_int_t& l, double* u,
        const fortran_int_t ldu, double* v, const fortran_int_t ldv,
        double* q, const fortran_int_t ldq, fortran_int_t* iwork, double* tau,
        double* work ) {
    fortran_int_t info(0);
    LAPACK_DGGSVP( &jobu, &jobv, &jobq, &m, &p, &n, a, &lda, b, &ldb, &tola,
            &tolb, &k, &l, u, &ldu, v, &ldv, q, &ldq, iwork, tau, work,
            &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<float> value-type.
//
inline std::ptrdiff_t ggsvp( const char jobu, const char jobv, const char jobq,
        const fortran_int_t m, const fortran_int_t p, const fortran_int_t n,
        std::complex<float>* a, const fortran_int_t lda,
        std::complex<float>* b, const fortran_int_t ldb, const float tola,
        const float tolb, fortran_int_t& k, fortran_int_t& l,
        std::complex<float>* u, const fortran_int_t ldu,
        std::complex<float>* v, const fortran_int_t ldv,
        std::complex<float>* q, const fortran_int_t ldq, fortran_int_t* iwork,
        float* rwork, std::complex<float>* tau, std::complex<float>* work ) {
    fortran_int_t info(0);
    LAPACK_CGGSVP( &jobu, &jobv, &jobq, &m, &p, &n, a, &lda, b, &ldb, &tola,
            &tolb, &k, &l, u, &ldu, v, &ldv, q, &ldq, iwork, rwork, tau, work,
            &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<double> value-type.
//
inline std::ptrdiff_t ggsvp( const char jobu, const char jobv, const char jobq,
        const fortran_int_t m, const fortran_int_t p, const fortran_int_t n,
        std::complex<double>* a, const fortran_int_t lda,
        std::complex<double>* b, const fortran_int_t ldb, const double tola,
        const double tolb, fortran_int_t& k, fortran_int_t& l,
        std::complex<double>* u, const fortran_int_t ldu,
        std::complex<double>* v, const fortran_int_t ldv,
        std::complex<double>* q, const fortran_int_t ldq,
        fortran_int_t* iwork, double* rwork, std::complex<double>* tau,
        std::complex<double>* work ) {
    fortran_int_t info(0);
    LAPACK_ZGGSVP( &jobu, &jobv, &jobq, &m, &p, &n, a, &lda, b, &ldb, &tola,
            &tolb, &k, &l, u, &ldu, v, &ldv, q, &ldq, iwork, rwork, tau, work,
            &info );
    return info;
}

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to ggsvp.
//
template< typename Value, typename Enable = void >
struct ggsvp_impl {};

//
// This implementation is enabled if Value is a real type.
//
template< typename Value >
struct ggsvp_impl< Value, typename boost::enable_if< is_real< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename MatrixB, typename MatrixU,
            typename MatrixV, typename MatrixQ, typename IWORK, typename TAU,
            typename WORK >
    static std::ptrdiff_t invoke( const char jobu, const char jobv,
            const char jobq, MatrixA& a, MatrixB& b, const real_type tola,
            const real_type tolb, fortran_int_t& k, fortran_int_t& l,
            MatrixU& u, MatrixV& v, MatrixQ& q, detail::workspace3< IWORK,
            TAU, WORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixU >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixV >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixQ >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixB >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixU >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixV >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixQ >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixU >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixV >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixQ >::value) );
        BOOST_ASSERT( bindings::size(work.select(fortran_int_t())) >=
                min_size_iwork( bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_tau( bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_work( bindings::size_column(a),
                bindings::size_row(a), bindings::size_row(b) ));
        BOOST_ASSERT( bindings::size_column(a) >= 0 );
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        BOOST_ASSERT( bindings::size_minor(b) == 1 ||
                bindings::stride_minor(b) == 1 );
        BOOST_ASSERT( bindings::size_minor(q) == 1 ||
                bindings::stride_minor(q) == 1 );
        BOOST_ASSERT( bindings::size_minor(u) == 1 ||
                bindings::stride_minor(u) == 1 );
        BOOST_ASSERT( bindings::size_minor(v) == 1 ||
                bindings::stride_minor(v) == 1 );
        BOOST_ASSERT( bindings::size_row(a) >= 0 );
        BOOST_ASSERT( bindings::size_row(b) >= 0 );
        BOOST_ASSERT( bindings::stride_major(a) >= std::max< std::ptrdiff_t >(1,
                bindings::size_row(a)) );
        BOOST_ASSERT( bindings::stride_major(b) >= std::max< std::ptrdiff_t >(1,
                bindings::size_row(b)) );
        BOOST_ASSERT( jobq == 'Q' || jobq == 'N' );
        BOOST_ASSERT( jobu == 'U' || jobu == 'N' );
        BOOST_ASSERT( jobv == 'V' || jobv == 'N' );
        return detail::ggsvp( jobu, jobv, jobq, bindings::size_row(a),
                bindings::size_row(b), bindings::size_column(a),
                bindings::begin_value(a), bindings::stride_major(a),
                bindings::begin_value(b), bindings::stride_major(b), tola,
                tolb, k, l, bindings::begin_value(u),
                bindings::stride_major(u), bindings::begin_value(v),
                bindings::stride_major(v), bindings::begin_value(q),
                bindings::stride_major(q),
                bindings::begin_value(work.select(fortran_int_t())),
                bindings::begin_value(work.select(real_type())),
                bindings::begin_value(work.select(real_type())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename MatrixA, typename MatrixB, typename MatrixU,
            typename MatrixV, typename MatrixQ >
    static std::ptrdiff_t invoke( const char jobu, const char jobv,
            const char jobq, MatrixA& a, MatrixB& b, const real_type tola,
            const real_type tolb, fortran_int_t& k, fortran_int_t& l,
            MatrixU& u, MatrixV& v, MatrixQ& q, minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( bindings::size_column(a) ) );
        bindings::detail::array<
                real_type > tmp_tau( min_size_tau( bindings::size_column(a) ) );
        bindings::detail::array< real_type > tmp_work( min_size_work(
                bindings::size_column(a), bindings::size_row(a),
                bindings::size_row(b) ) );
        return invoke( jobu, jobv, jobq, a, b, tola, tolb, k, l, u, v, q,
                workspace( tmp_iwork, tmp_tau, tmp_work ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename MatrixA, typename MatrixB, typename MatrixU,
            typename MatrixV, typename MatrixQ >
    static std::ptrdiff_t invoke( const char jobu, const char jobv,
            const char jobq, MatrixA& a, MatrixB& b, const real_type tola,
            const real_type tolb, fortran_int_t& k, fortran_int_t& l,
            MatrixU& u, MatrixV& v, MatrixQ& q, optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        return invoke( jobu, jobv, jobq, a, b, tola, tolb, k, l, u, v, q,
                minimal_workspace() );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array iwork.
    //
    static std::ptrdiff_t min_size_iwork( const std::ptrdiff_t n ) {
        return n;
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array tau.
    //
    static std::ptrdiff_t min_size_tau( const std::ptrdiff_t n ) {
        return n;
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t n,
            const std::ptrdiff_t m, const std::ptrdiff_t p ) {
        return std::max< std::ptrdiff_t >(3*n,std::max< std::ptrdiff_t >(m,p));
    }
};

//
// This implementation is enabled if Value is a complex type.
//
template< typename Value >
struct ggsvp_impl< Value, typename boost::enable_if< is_complex< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename MatrixB, typename MatrixU,
            typename MatrixV, typename MatrixQ, typename IWORK,
            typename RWORK, typename TAU, typename WORK >
    static std::ptrdiff_t invoke( const char jobu, const char jobv,
            const char jobq, MatrixA& a, MatrixB& b, const real_type tola,
            const real_type tolb, fortran_int_t& k, fortran_int_t& l,
            MatrixU& u, MatrixV& v, MatrixQ& q, detail::workspace4< IWORK,
            RWORK, TAU, WORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixU >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixV >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixQ >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixB >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixU >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixV >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixQ >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixU >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixV >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixQ >::value) );
        BOOST_ASSERT( bindings::size(work.select(fortran_int_t())) >=
                min_size_iwork( bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_rwork( bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size(work.select(value_type())) >=
                min_size_tau( bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size(work.select(value_type())) >=
                min_size_work( bindings::size_column(a),
                bindings::size_row(a), bindings::size_row(b) ));
        BOOST_ASSERT( bindings::size_column(a) >= 0 );
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        BOOST_ASSERT( bindings::size_minor(b) == 1 ||
                bindings::stride_minor(b) == 1 );
        BOOST_ASSERT( bindings::size_minor(q) == 1 ||
                bindings::stride_minor(q) == 1 );
        BOOST_ASSERT( bindings::size_minor(u) == 1 ||
                bindings::stride_minor(u) == 1 );
        BOOST_ASSERT( bindings::size_minor(v) == 1 ||
                bindings::stride_minor(v) == 1 );
        BOOST_ASSERT( bindings::size_row(a) >= 0 );
        BOOST_ASSERT( bindings::size_row(b) >= 0 );
        BOOST_ASSERT( bindings::stride_major(a) >= std::max< std::ptrdiff_t >(1,
                bindings::size_row(a)) );
        BOOST_ASSERT( bindings::stride_major(b) >= std::max< std::ptrdiff_t >(1,
                bindings::size_row(b)) );
        BOOST_ASSERT( jobq == 'Q' || jobq == 'N' );
        BOOST_ASSERT( jobu == 'U' || jobu == 'N' );
        BOOST_ASSERT( jobv == 'V' || jobv == 'N' );
        return detail::ggsvp( jobu, jobv, jobq, bindings::size_row(a),
                bindings::size_row(b), bindings::size_column(a),
                bindings::begin_value(a), bindings::stride_major(a),
                bindings::begin_value(b), bindings::stride_major(b), tola,
                tolb, k, l, bindings::begin_value(u),
                bindings::stride_major(u), bindings::begin_value(v),
                bindings::stride_major(v), bindings::begin_value(q),
                bindings::stride_major(q),
                bindings::begin_value(work.select(fortran_int_t())),
                bindings::begin_value(work.select(real_type())),
                bindings::begin_value(work.select(value_type())),
                bindings::begin_value(work.select(value_type())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename MatrixA, typename MatrixB, typename MatrixU,
            typename MatrixV, typename MatrixQ >
    static std::ptrdiff_t invoke( const char jobu, const char jobv,
            const char jobq, MatrixA& a, MatrixB& b, const real_type tola,
            const real_type tolb, fortran_int_t& k, fortran_int_t& l,
            MatrixU& u, MatrixV& v, MatrixQ& q, minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( bindings::size_column(a) ) );
        bindings::detail::array< real_type > tmp_rwork( min_size_rwork(
                bindings::size_column(a) ) );
        bindings::detail::array<
                value_type > tmp_tau( min_size_tau( bindings::size_column(a) ) );
        bindings::detail::array< value_type > tmp_work( min_size_work(
                bindings::size_column(a), bindings::size_row(a),
                bindings::size_row(b) ) );
        return invoke( jobu, jobv, jobq, a, b, tola, tolb, k, l, u, v, q,
                workspace( tmp_iwork, tmp_rwork, tmp_tau, tmp_work ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename MatrixA, typename MatrixB, typename MatrixU,
            typename MatrixV, typename MatrixQ >
    static std::ptrdiff_t invoke( const char jobu, const char jobv,
            const char jobq, MatrixA& a, MatrixB& b, const real_type tola,
            const real_type tolb, fortran_int_t& k, fortran_int_t& l,
            MatrixU& u, MatrixV& v, MatrixQ& q, optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        return invoke( jobu, jobv, jobq, a, b, tola, tolb, k, l, u, v, q,
                minimal_workspace() );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array iwork.
    //
    static std::ptrdiff_t min_size_iwork( const std::ptrdiff_t n ) {
        return n;
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array rwork.
    //
    static std::ptrdiff_t min_size_rwork( const std::ptrdiff_t n ) {
        return 2*n;
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array tau.
    //
    static std::ptrdiff_t min_size_tau( const std::ptrdiff_t n ) {
        return n;
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t n,
            const std::ptrdiff_t m, const std::ptrdiff_t p ) {
        return std::max< std::ptrdiff_t >(3*n,std::max< std::ptrdiff_t >(m,p));
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. In
// addition, if applicable, they are overloaded for user-defined workspaces.
// Calls to these functions are passed to the ggsvp_impl classes. In the
// documentation, most overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for ggsvp. Its overload differs for
// * User-defined workspace
//
template< typename MatrixA, typename MatrixB, typename MatrixU,
        typename MatrixV, typename MatrixQ, typename Workspace >
inline typename boost::enable_if< detail::is_workspace< Workspace >,
        std::ptrdiff_t >::type
ggsvp( const char jobu, const char jobv, const char jobq, MatrixA& a,
        MatrixB& b, const typename remove_imaginary<
        typename bindings::value_type< MatrixA >::type >::type tola,
        const typename remove_imaginary< typename bindings::value_type<
        MatrixA >::type >::type tolb, fortran_int_t& k,
        fortran_int_t& l, MatrixU& u, MatrixV& v, MatrixQ& q,
        Workspace work ) {
    return ggsvp_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( jobu, jobv, jobq, a, b, tola, tolb, k,
            l, u, v, q, work );
}

//
// Overloaded function for ggsvp. Its overload differs for
// * Default workspace-type (optimal)
//
template< typename MatrixA, typename MatrixB, typename MatrixU,
        typename MatrixV, typename MatrixQ >
inline typename boost::disable_if< detail::is_workspace< MatrixQ >,
        std::ptrdiff_t >::type
ggsvp( const char jobu, const char jobv, const char jobq, MatrixA& a,
        MatrixB& b, const typename remove_imaginary<
        typename bindings::value_type< MatrixA >::type >::type tola,
        const typename remove_imaginary< typename bindings::value_type<
        MatrixA >::type >::type tolb, fortran_int_t& k,
        fortran_int_t& l, MatrixU& u, MatrixV& v, MatrixQ& q ) {
    return ggsvp_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( jobu, jobv, jobq, a, b, tola, tolb, k,
            l, u, v, q, optimal_workspace() );
}

} // namespace lapack
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


#include <cstdlib>
#include <csetjmp>
#include <csignal>
#include <iostream>
#include <memory>
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <SOIL.h>
#include <boost/asio/thread_pool.hpp>
#include "scene.h"
#include "camera.h"
#include "envmap.h"
#include "make_scene.h"
#include <chrono>

struct ExitGuard {
	~ExitGuard() {
		std::cerr << "exiting gracefully..." << std::endl;
	}
} _;

std::jmp_buf sigint;

constexpr int32_t width = 1920;
constexpr int32_t height = 1080;
constexpr float fov = 140.0;

using std::chrono::high_resolution_clock;
using std::chrono::duration;
using std::chrono::duration_cast;

int main(int argc, char **argv) {
	if (argc < 2) {
		std::cerr << argv[0] << " < <env_prefix>" << std::endl;
		return EXIT_FAILURE;
	}
	boost::asio::thread_pool pool(8);
	Scene scene;
	Camera *cam = make_scene(scene, pool.get_executor());
	cam->depth = 5;
	const float px = glm::tan(fov / 2.0 * 3.14159265359 / 180.0) / width;
	for (int32_t y = -height/2; y < height/2; y++)
		for (int32_t x = -width/2; x < width/2; x++)
			cam->rays.emplace_back(glm::normalize(glm::vec3{x*px, -y*px, -1}));
	if (setjmp(sigint) == 0) {
		std::signal(SIGINT,  [](int) { std::longjmp(sigint, 1); });
		std::signal(SIGTERM, [](int) { std::longjmp(sigint, 1); });
		while (true) {
			float yaw, pitch, roll;
			std::cin >> yaw >> pitch >> roll;
			if (std::cin.fail()) {
				return EXIT_SUCCESS;
			}
			glm::mat4 transform = cam->get_transform();
			transform = glm::rotate(transform, glm::radians(yaw),   {0, 1, 0});
			transform = glm::rotate(transform, glm::radians(pitch), {1, 0, 0});
			transform = glm::rotate(transform, glm::radians(roll),  {0, 0, 1});
			cam->set_transform(transform);
			high_resolution_clock::time_point t1 = high_resolution_clock::now();
			cam->trace_rays(
					scene,
					[t1](std::vector<glm::vec3> results) {
						high_resolution_clock::time_point t2 = high_resolution_clock::now();
						duration<double> time_span = duration_cast<duration<double>>(t2 - t1);
						std::cerr << "time: " << time_span.count() << " seconds" << std::endl;
						uint8_t buffer[width*height*4];
						for (int32_t i = 0; i < static_cast<int32_t>(results.size()); i++) {
							buffer[4*i+0] = results[i].x * 255.0 + 0.5;
							buffer[4*i+1] = results[i].y * 255.0 + 0.5;
							buffer[4*i+2] = results[i].z * 255.0 + 0.5;
							buffer[4*i+3] = 255;
						}
						SOIL_save_image("output.tga", SOIL_SAVE_TYPE_TGA, width, height, 4, buffer);
					});
		}
	}
	return EXIT_FAILURE;
}


//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#ifndef FIT_SELECTION_GENERATOR_HOST_HPP_
#define FIT_SELECTION_GENERATOR_HOST_HPP_

#include "clotho/cuda/curand_state_pool.hpp"

#include "clotho/cuda/data_spaces/basic_data_space.hpp"

#include "clotho/cuda/recombination/recombine_parents.hpp"
#include "clotho/recombination/sequence_bias_parameter.hpp"

#include "clotho/cuda/device_state_object.hpp"

#include "clotho/random/seed_parameter.hpp"
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/discrete_distribution.hpp>
#include <boost/random/bernoulli_distribution.hpp>

#include "clotho/cuda/selection/fit_selection_generator_def.hpp"

#include "clotho/utility/bit_helper.hpp"

struct host_discrete_distribution {};

template < class IntType, class RealType >
class FitSelectionGenerator< IntType, RealType, host_discrete_distribution > : public clotho::utility::iStateObject {
public:
//    typedef basic_data_space< IntType >                     event_space_type;
    typedef IntType                                       event_space_type;

//    typedef clotho::cuda::curand_state_pool             state_pool_type;
//
//    typedef DiscreteDistribution< IntType, RealType >   discrete_dist_type;
    typedef boost::random::mt19937                                       random_engine_type;
    typedef boost::random::discrete_distribution< IntType, RealType >   discrete_dist_type;
    typedef boost::random::bernoulli_distribution< double >             bernoulli_type;
    typedef sequence_bias_parameter< RealType >         sequence_bias_type;

    typedef clotho::utility::BitHelper< IntType >      bit_helper_type;

    FitSelectionGenerator( boost::property_tree::ptree & config ) :
         m_seq_bias( config )
        , dParentIndices(NULL)
        , localParentIndices( NULL )
        , localFitness( NULL )
        , m_parent_size( 0 )
        , m_current_parent_size(0)
        , m_fitness_capacity(0)
    {
        seed_parameter< > seed( config );

        m_rng.seed( seed.m_seed );
    }

    void generate( RealType * fitness, unsigned int N ) {

        buildParentIndices( fitness, N );

        cudaError_t err = cudaMemcpy(dParentIndices, localParentIndices, N * sizeof( IntType ), cudaMemcpyHostToDevice );
        if( err != cudaSuccess ) {
            std::cerr << "ERROR: " << cudaGetErrorString( err ) << std::endl;
            assert( false );
        }

    }

    template < class PopulationSpaceType >
    void generate_and_recombine( PopulationSpaceType * parent_pop, PopulationSpaceType * child_pop, RealType * fitness, unsigned int N ) {
        generate( fitness, N );

        clotho::utility::algo_version< 3 > * v = NULL;
        recombine_parents_kernel<<< 10, 1024 >>>( parent_pop->sequences.get_device_space()
                                                , dParentIndices
                                                , child_pop->sequences.get_device_space()
                                                , v);
        CHECK_LAST_KERNEL_EXEC
    }

    event_space_type * get_device_space() {
        return dParentIndices;
    }

    void get_state( boost::property_tree::ptree & state ) {
        boost::property_tree::ptree evts;

        for( unsigned int i = 0; i < m_parent_size; ++i ) {
            clotho::utility::add_value_array( evts, localParentIndices[i] );
        }

        state.add_child( "events", evts );
    }

    virtual ~FitSelectionGenerator() {
        if( localParentIndices != NULL ) {
            delete [] localParentIndices;
            cudaFree( dParentIndices );
        }

        if( localFitness != NULL ) {
            delete [] localFitness;
        }
    }

protected:

    void resize( unsigned int N ) {
        if( N > m_parent_size ) {
            if( localParentIndices != NULL ) {
                delete [] localParentIndices;
                cudaFree( dParentIndices );
            }

            assert( cudaMalloc( (void **) &dParentIndices, N * sizeof(IntType ) ) == cudaSuccess );

            localParentIndices = new IntType[ N ];
            m_parent_size = N;
        }

        if( N > m_fitness_capacity ) {
            if( localFitness != NULL ) {
                delete [] localFitness;
            }

            localFitness = new RealType[ N ];

            m_fitness_capacity = N;
        }
    }

    void buildParentIndices( RealType * fitness, unsigned int N ) {
        resize( N );

        if( m_current_parent_size == 0 ) {
            for( unsigned int i = 0; i < N; ++i ) {
                localParentIndices[ i ] = 0;
            }
        } else {

            cudaError_t err = cudaMemcpy( localFitness, fitness, m_current_parent_size * sizeof( RealType ), cudaMemcpyDeviceToHost );
            if( err != cudaSuccess ) {
                std::cerr << "ERROR: " << cudaGetErrorString( err ) << std::endl;
                assert(false);
            }

            discrete_dist_type dist( localFitness, localFitness + m_current_parent_size );

            for( unsigned int i = 0; i < N; ++i ) {

                // sequence offset
                localParentIndices[ i ] = 2 * dist( m_rng );

                if( m_bern( m_rng ) ) {
                    localParentIndices[ i ] += 1;
                }
            }
        }

        m_current_parent_size = N / 2;
    }

    random_engine_type  m_rng;
    sequence_bias_type  m_seq_bias;
    bernoulli_type  m_bern;

    IntType         * dParentIndices;
    IntType         * localParentIndices;

    RealType        * localFitness;

    unsigned int m_parent_size;
    unsigned int m_current_parent_size, m_fitness_capacity;
};

#endif  // FIT_SELECTION_GENERATOR_HOST_HPP_


/*
 * Copyright 2018 Autoware Foundation. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *
 * v1.0 Yukihiro Saito
 */

#include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp"
#include "multi_object_tracker/utils/utils.hpp"
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>

PedestrianTracker::PedestrianTracker(
  const ros::Time & time, const autoware_perception_msgs::DynamicObject & object)
: Tracker(time, object.semantic.type),
  filtered_posx_(object.state.pose_covariance.pose.position.x),
  filtered_posy_(object.state.pose_covariance.pose.position.y),
  pos_filter_gain_(0.2),
  filtered_vx_(0.0),
  filtered_vy_(0.0),
  v_filter_gain_(0.6),
  area_filter_gain_(0.8),
  last_measurement_posx_(object.state.pose_covariance.pose.position.x),
  last_measurement_posy_(object.state.pose_covariance.pose.position.y),
  last_update_time_(time),
  last_measurement_time_(time)
{
  object_ = object;
  // area
  filtered_area_ = utils::getArea(object.shape);
}

bool PedestrianTracker::predict(const ros::Time & time)
{
  double dt = (time - last_update_time_).toSec();
  if (dt < 0.0) dt = 0.0;
  filtered_posx_ += filtered_vx_ * dt;
  filtered_posy_ += filtered_vy_ * dt;
  last_update_time_ = time;
  return true;
}
bool PedestrianTracker::measure(
  const autoware_perception_msgs::DynamicObject & object, const ros::Time & time)
{
  int type = object.semantic.type;
  bool is_changed_unknown_object = false;
  if (type == autoware_perception_msgs::Semantic::UNKNOWN) {
    type = getType();
    is_changed_unknown_object = true;
  }
  object_ = object;
  setType(type);

  // area
  filtered_area_ =
    area_filter_gain_ * filtered_area_ + (1.0 - area_filter_gain_) * utils::getArea(object.shape);

  // vx,vy
  double dt = (time - last_measurement_time_).toSec();
  last_measurement_time_ = time;
  last_update_time_ = time;
  if (0.0 < dt) {
    double current_vel = std::sqrt(
      (object.state.pose_covariance.pose.position.x - last_measurement_posx_) *
        (object.state.pose_covariance.pose.position.x - last_measurement_posx_) +
      (object.state.pose_covariance.pose.position.y - last_measurement_posy_) *
        (object.state.pose_covariance.pose.position.y - last_measurement_posy_));
    const double max_vel = 15.0; /* [m/s]*/
    const double vel_scale =
      current_vel < 0.01 ? 1.0 : std::min(max_vel, current_vel) / current_vel;
    if (is_changed_unknown_object) {
      filtered_vx_ =
        0.9 * filtered_vx_ +
        (1.0 - 0.9) *
          ((object.state.pose_covariance.pose.position.x - last_measurement_posx_) / dt) *
          vel_scale;
      filtered_vy_ =
        0.9 * filtered_vy_ +
        (1.0 - 0.9) *
          ((object.state.pose_covariance.pose.position.y - last_measurement_posy_) / dt) *
          vel_scale;
    } else {
      filtered_vx_ =
        v_filter_gain_ * filtered_vx_ +
        (1.0 - v_filter_gain_) *
          ((object.state.pose_covariance.pose.position.x - last_measurement_posx_) / dt) *
          vel_scale;
      filtered_vy_ =
        v_filter_gain_ * filtered_vy_ +
        (1.0 - v_filter_gain_) *
          ((object.state.pose_covariance.pose.position.y - last_measurement_posy_) / dt) *
          vel_scale;
      v_filter_gain_ = std::min(0.9, v_filter_gain_ + 0.05);
    }
  }

  // pos x, pos y
  // filtered_posx_ = object.state.pose_covariance.pose.position.x;
  // filtered_posy_ = object.state.pose_covariance.pose.position.y;
  last_measurement_posx_ = object.state.pose_covariance.pose.position.x;
  last_measurement_posy_ = object.state.pose_covariance.pose.position.y;
  filtered_posx_ = pos_filter_gain_ * filtered_posx_ +
                   (1.0 - pos_filter_gain_) * object.state.pose_covariance.pose.position.x;
  filtered_posy_ = pos_filter_gain_ * filtered_posy_ +
                   (1.0 - pos_filter_gain_) * object.state.pose_covariance.pose.position.y;
  pos_filter_gain_ = std::min(0.8, pos_filter_gain_ + 0.05);

  return true;
}

bool PedestrianTracker::getEstimatedDynamicObject(
  const ros::Time & time, autoware_perception_msgs::DynamicObject & object)
{
  object = object_;
  object.id = unique_id::toMsg(getUUID());
  object.semantic.type = getType();
  double dt = (time - last_update_time_).toSec();
  if (dt < 0.0) dt = 0.0;

  object.state.pose_covariance.pose.position.x = filtered_posx_;
  object.state.pose_covariance.pose.position.y = filtered_posy_;
  object.state.pose_covariance.pose.position.x += filtered_vx_ * dt;
  object.state.pose_covariance.pose.position.y += filtered_vy_ * dt;

  object.state.orientation_reliable = false;

  double roll, pitch, yaw;
  tf2::Quaternion quaternion;
  tf2::fromMsg(object.state.pose_covariance.pose.orientation, quaternion);
  tf2::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
  object.state.twist_covariance.twist.linear.x =
    filtered_vx_ * std::cos(-yaw) - filtered_vy_ * std::sin(-yaw);
  object.state.twist_covariance.twist.linear.y =
    filtered_vx_ * std::sin(-yaw) + filtered_vy_ * std::cos(-yaw);

  object.state.twist_reliable = true;

  return true;
}

// geometry_msgs::Point PedestrianTracker::getPosition(const ros::Time &time)
// {
//     geometry_msgs::Point position;
//     position.x = filtered_posx_;
//     position.y = filtered_posy_;
//     position.z = object_.state.pose_covariance.pose.position.z;
//     return position;
// }

// double PedestrianTracker::getArea()
// {
//     return filtered_area_;
// }


#include "operators_c.h"
#include "operators.h"
#include <Eigen/Dense>
#include <iostream>

using namespace std;

static dfloat_t *to_c(VectorXd &v)
{
  dfloat_t *vdata = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * v.size());
#if USE_DFLOAT_DOUBLE == 1
  Eigen::Map<Eigen::VectorXd>(vdata, v.size()) = v.cast<dfloat_t>();
#else
  Eigen::Map<Eigen::VectorXf>(vdata, v.size()) = v.cast<dfloat_t>();
#endif
  return vdata;
}

static dfloat_t *to_c(MatrixXd &m)
{
  dfloat_t *mdata = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * m.size());
#if USE_DFLOAT_DOUBLE == 1
  Eigen::Map<Eigen::MatrixXd>(mdata, m.rows(), m.cols()) = m.cast<dfloat_t>();
#else
  Eigen::Map<Eigen::MatrixXf>(mdata, m.rows(), m.cols()) = m.cast<dfloat_t>();
#endif
  return mdata;
}

static uintloc_t *to_c(MatrixXu32 &m)
{
  uintloc_t *mdata =
      (uintloc_t *)asd_malloc_aligned(sizeof(uintloc_t) * m.size());
  Eigen::Map<Eigen::Matrix<uintloc_t, Eigen::Dynamic, Eigen::Dynamic>>(
      mdata, m.rows(), m.cols()) = m.cast<uintloc_t>();
  return mdata;
}

host_operators_t *host_operators_new_2D(int N, int M, uintloc_t E,
                                        uintloc_t *EToE, uint8_t *EToF,
                                        uint8_t *EToO, double *EToVX)
{
  host_operators_t *ops =
      (host_operators_t *)asd_malloc(sizeof(host_operators_t));

  ref_elem_data *ref_data = build_ref_ops_2D(N, M, M);

  VectorXd wq = ref_data->wq;

  // nodal
  MatrixXd Dr = ref_data->Dr;
  MatrixXd Ds = ref_data->Ds;
  MatrixXd Vq = ref_data->Vq;

  VectorXd wfq = ref_data->wfq;
  VectorXd nrJ = ref_data->nrJ;
  VectorXd nsJ = ref_data->nsJ;
  MatrixXd Vfqf = ref_data->Vfqf;
  MatrixXd Vfq = ref_data->Vfq;
  MatrixXd Pq = ref_data->Pq;

  MatrixXd MM = Vq.transpose() * wq.asDiagonal() * Vq;
  MatrixXd MMfq = Vfq.transpose() * wfq.asDiagonal();
  MatrixXd Lq = mldivide(MM, MMfq);
  MatrixXd VqLq = Vq * Lq;
  MatrixXd VqPq = Vq * Pq;
  MatrixXd VfPq = Vfq * Pq;
  MatrixXd Drq = Vq * Dr * Pq - .5 * Vq * Lq * nrJ.asDiagonal() * Vfq * Pq;
  MatrixXd Dsq = Vq * Ds * Pq - .5 * Vq * Lq * nsJ.asDiagonal() * Vfq * Pq;

  ops->dim = 2;

  ops->N = N;
  ops->M = M;

  ops->Np = (int)ref_data->r.size();
  ops->Nq = (int)ref_data->rq.size();

  // printf("Num cubature points Nq = %d\n",ops->Nq);

  ops->Nfp = N + 1;
  ops->Nfq = (int)ref_data->ref_rfq.size();

  ops->Nfaces = ref_data->Nfaces;
  ops->Nvgeo = 4;
  ops->Nfgeo = 3;

  ops->wq = to_c(wq);
  ops->nrJ = to_c(nrJ);
  ops->nsJ = to_c(nsJ);

  ops->Drq = to_c(Drq);
  ops->Dsq = to_c(Dsq);

  ops->Vq = to_c(Vq);
  ops->Pq = to_c(Pq);

  ops->VqLq = to_c(VqLq);
  ops->VqPq = to_c(VqPq);
  ops->VfPq = to_c(VfPq);
  ops->Vfqf = to_c(Vfqf);

  Map<MatrixXd> EToVXmat(EToVX, 2 * 3, E);

  if (sizeof(uintloc_t) != sizeof(uint32_t))
  {
    cerr << "Need to update build maps to support different integer types"
         << endl;
    std::abort();
  }
  Map<MatrixXu32> mapEToE(EToE, 3, E);
  Map<MatrixXu8> mapEToF(EToF, 3, E);
  Map<MatrixXu8> mapEToO(EToO, 3, E);

  geo_elem_data *geo_data = build_geofacs_2D(ref_data, EToVXmat);
  map_elem_data *map_data = build_maps_2D(ref_data, mapEToE, mapEToF, mapEToO);

  const int Nvgeo = ops->Nvgeo;
  const int Nfgeo = ops->Nfgeo;
  const int Nfaces = ref_data->Nfaces;
  const int Nq = ops->Nq;
  const int Nfq = ops->Nfq;

  ops->xyzq =
      (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nq * 3 * E);
  ops->xyzf = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nfq *
                                             ops->Nfaces * 3 * E);
  ops->vgeo =
      (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nq * Nvgeo * E);
  ops->vfgeo = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nfq *
                                              Nfaces * Nvgeo * E);
  ops->fgeo = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nfq *
                                             Nfgeo * Nfaces * E);

  for (uintloc_t e = 0; e < E; ++e)
  {
    for (int n = 0; n < Nq; ++n)
    {
      ops->xyzq[n + 0 * Nq + e * Nq * 3] = (dfloat_t)geo_data->xq(n, e);
      ops->xyzq[n + 1 * Nq + e * Nq * 3] = (dfloat_t)geo_data->yq(n, e);
      // ops->xyzq[n + 2*Nq + e*Nq*3] = (dfloat_t)geo_data->zq(n,e);

      ops->vgeo[e * Nq * Nvgeo + 0 * Nq + n] = (dfloat_t)geo_data->rxJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 1 * Nq + n] = (dfloat_t)geo_data->ryJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 2 * Nq + n] = (dfloat_t)geo_data->sxJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 3 * Nq + n] = (dfloat_t)geo_data->syJ(n, e);
    }
  }

  for (uintloc_t e = 0; e < E; ++e)
  {
    for (int n = 0; n < Nfq * Nfaces; ++n)
    {
      ops->xyzf[n + 0 * Nfq * Nfaces + e * Nfq * Nfaces * 3] =
          (dfloat_t)geo_data->xf(n, e);
      ops->xyzf[n + 1 * Nfq * Nfaces + e * Nfq * Nfaces * 3] =
          (dfloat_t)geo_data->yf(n, e);

      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 0 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->rxJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 1 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->ryJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 2 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->sxJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 3 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->syJf(n, e);

      ops->fgeo[e * Nfq * Nfaces * Nfgeo + 0 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->nxJ(n, e);
      ops->fgeo[e * Nfq * Nfaces * Nfgeo + 1 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->nyJ(n, e);
      ops->fgeo[e * Nfq * Nfaces * Nfgeo + 2 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->sJ(n, e);
    }
  }

  ops->Jq = to_c(geo_data->J);
  ops->mapPq = to_c(map_data->mapPq);
  //  ops->mapPqNoFields = to_c(map_data->mapPq);  // save

  /*
  // JC: FIX LATER - only valid for tri2.msh
  ops->mapPq[0] = 7;
  ops->mapPq[1] = 6;
  ops->mapPq[2] = 9;
  ops->mapPq[3] = 8;
  ops->mapPq[6] = 1;
  ops->mapPq[7] = 0;
  ops->mapPq[8] = 3;
  ops->mapPq[9] = 2;
  */

  //  for(int i = 0; i < Nfq*Nfaces*E; ++i){
  //    printf("mapPq(%d) = %d\n",i,ops->mapPq[i]);
  //  }
  ops->Fmask = to_c(map_data->fmask);

  delete ref_data;
  delete geo_data;
  delete map_data;

  return ops;
}

host_operators_t *host_operators_new_3D(int N, int M, uintloc_t E,
                                        uintloc_t *EToE, uint8_t *EToF,
                                        uint8_t *EToO, double *EToVX)
{
  host_operators_t *ops =
      (host_operators_t *)asd_malloc(sizeof(host_operators_t));

  ref_elem_data *ref_data = build_ref_ops_3D(N, M, M);

  VectorXd wq = ref_data->wq;

  // nodal
  MatrixXd Dr = ref_data->Dr;
  MatrixXd Ds = ref_data->Ds;
  MatrixXd Dt = ref_data->Dt;
  MatrixXd Vq = ref_data->Vq;
  MatrixXd Pq = ref_data->Pq;
  MatrixXd Vfqf = ref_data->Vfqf;
  MatrixXd Vfq = ref_data->Vfq;

  VectorXd wfq = ref_data->wfq;
  VectorXd nrJ = ref_data->nrJ;
  VectorXd nsJ = ref_data->nsJ;
  VectorXd ntJ = ref_data->ntJ;

  MatrixXd MM = Vq.transpose() * wq.asDiagonal() * Vq;
  MatrixXd MMfq = Vfq.transpose() * wfq.asDiagonal();
  MatrixXd Lq = mldivide(MM, MMfq);
  MatrixXd VqLq = Vq * Lq;
  MatrixXd VqPq = Vq * Pq;
  MatrixXd VfPq = Vfq * Pq;
  MatrixXd Drq = Vq * Dr * Pq - .5 * Vq * Lq * nrJ.asDiagonal() * Vfq * Pq;
  MatrixXd Dsq = Vq * Ds * Pq - .5 * Vq * Lq * nsJ.asDiagonal() * Vfq * Pq;
  MatrixXd Dtq = Vq * Dt * Pq - .5 * Vq * Lq * ntJ.asDiagonal() * Vfq * Pq;

  MatrixXd Drstq(Drq.rows(),3*Drq.cols());
  Drstq << Drq,Dsq,Dtq;

  /*
  cout << "VqPq = " << endl << VqPq << endl;
  cout << "VqLq = " << endl << VqLq << endl;
  cout << "VfPq = " << endl << VfPq << endl;

  cout << "Drq = " << endl << Drq << endl;
  cout << "nrJ = " << endl << nrJ << endl;
  cout << "Dsq = " << endl << Dsq << endl;
  cout << "nsJ = " << endl << nsJ << endl;
  cout << "Dtq = " << endl << Dtq << endl;
  cout << "ntJ = " << endl << ntJ << endl;
  */

  ops->dim = 3;

  ops->N = N;
  ops->M = M;

  ops->Np = (int)ref_data->r.size();
  ops->Nq = (int)ref_data->rq.size();

  ops->Nfp = (N + 1) * (N + 2) / 2;
  ops->Nfq = (int)ref_data->ref_rfq.size();

  ops->Nfaces = ref_data->Nfaces;
  ops->Nvgeo = 9;
  ops->Nfgeo = 4;

  ops->wq = to_c(wq);
  ops->nrJ = to_c(nrJ);
  ops->nsJ = to_c(nsJ);
  ops->ntJ = to_c(ntJ);

  ops->Drq = to_c(Drq);
  ops->Dsq = to_c(Dsq);
  ops->Dtq = to_c(Dtq);

  ops->Drstq = to_c(Drstq);

  ops->Vq = to_c(Vq);
  ops->Pq = to_c(Pq);

  ops->VqLq = to_c(VqLq);
  ops->VqPq = to_c(VqPq);
  ops->VfPq = to_c(VfPq);
  ops->Vfqf = to_c(Vfqf);

  Map<MatrixXd> EToVXmat(EToVX, 3 * 4, E);

  if (sizeof(uintloc_t) != sizeof(uint32_t))
  {
    cerr << "Need to update build maps to support different integer types"
         << endl;
    std::abort();
  }
  Map<MatrixXu32> mapEToE(EToE, 4, E);
  Map<MatrixXu8> mapEToF(EToF, 4, E);
  Map<MatrixXu8> mapEToO(EToO, 4, E);

  geo_elem_data *geo_data = build_geofacs_3D(ref_data, EToVXmat);
  map_elem_data *map_data = build_maps_3D(ref_data, mapEToE, mapEToF, mapEToO);

  const int Nvgeo = ops->Nvgeo;
  const int Nfgeo = ops->Nfgeo;
  const int Nfaces = ref_data->Nfaces;
  const int Nq = ops->Nq;
  const int Nfq = ops->Nfq;

  ops->xyzq =
      (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nq * 3 * E);
  ops->xyzf = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nfq *
                                             ops->Nfaces * 3 * E);
  ops->vgeo =
      (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nq * Nvgeo * E);
  ops->vfgeo = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nfq *
                                              Nfaces * Nvgeo * E);
  ops->fgeo = (dfloat_t *)asd_malloc_aligned(sizeof(dfloat_t) * ops->Nfq *
                                             Nfgeo * Nfaces * E);

  for (uintloc_t e = 0; e < E; ++e)
  {
    for (int n = 0; n < Nq; ++n)
    {
      ops->xyzq[n + 0 * Nq + e * Nq * 3] = (dfloat_t)geo_data->xq(n, e);
      ops->xyzq[n + 1 * Nq + e * Nq * 3] = (dfloat_t)geo_data->yq(n, e);
      ops->xyzq[n + 2 * Nq + e * Nq * 3] = (dfloat_t)geo_data->zq(n, e);

      ops->vgeo[e * Nq * Nvgeo + 0 * Nq + n] = (dfloat_t)geo_data->rxJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 1 * Nq + n] = (dfloat_t)geo_data->ryJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 2 * Nq + n] = (dfloat_t)geo_data->rzJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 3 * Nq + n] = (dfloat_t)geo_data->sxJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 4 * Nq + n] = (dfloat_t)geo_data->syJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 5 * Nq + n] = (dfloat_t)geo_data->szJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 6 * Nq + n] = (dfloat_t)geo_data->txJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 7 * Nq + n] = (dfloat_t)geo_data->tyJ(n, e);
      ops->vgeo[e * Nq * Nvgeo + 8 * Nq + n] = (dfloat_t)geo_data->tzJ(n, e);
    }
  }

  for (uintloc_t e = 0; e < E; ++e)
  {
    for (int n = 0; n < Nfq * Nfaces; ++n)
    {
      ops->xyzf[n + 0 * Nfq * Nfaces + e * Nfq * Nfaces * 3] =
          (dfloat_t)geo_data->xf(n, e);
      ops->xyzf[n + 1 * Nfq * Nfaces + e * Nfq * Nfaces * 3] =
          (dfloat_t)geo_data->yf(n, e);
      ops->xyzf[n + 2 * Nfq * Nfaces + e * Nfq * Nfaces * 3] =
          (dfloat_t)geo_data->zf(n, e);

      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 0 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->rxJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 1 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->ryJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 2 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->rzJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 3 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->sxJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 4 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->syJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 5 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->szJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 6 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->txJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 7 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->tyJf(n, e);
      ops->vfgeo[e * Nfq * Nfaces * Nvgeo + 8 * Nfq * Nfaces + n] =
          (dfloat_t)geo_data->tzJf(n, e);

      ops->fgeo[e * Nfq * Nfaces * Nfgeo + 0 * Nfq * Nfaces + n] =
        (dfloat_t)geo_data->nxJ(n, e);
      ops->fgeo[e * Nfq * Nfaces * Nfgeo + 1 * Nfq * Nfaces + n] =
        (dfloat_t)geo_data->nyJ(n, e);
      ops->fgeo[e * Nfq * Nfaces * Nfgeo + 2 * Nfq * Nfaces + n] =
        (dfloat_t)geo_data->nzJ(n, e);
      ops->fgeo[e * Nfq * Nfaces * Nfgeo + 3 * Nfq * Nfaces + n] =
        (dfloat_t)geo_data->sJ(n, e);
      //      if (e==99){
      //        printf("n = %d, nxJ = %f\n", n, (dfloat_t) geo_data->nxJ(n,e));
      //      }
    }
  }

  ops->Jq = to_c(geo_data->J);

  ops->mapPq = to_c(map_data->mapPq);
  ops->Fmask = to_c(map_data->fmask);

  delete ref_data;
  delete geo_data;
  delete map_data;

  return ops;
}

void host_operators_free(host_operators_t *ops)
{
  asd_free_aligned(ops->vgeo);
  asd_free_aligned(ops->fgeo);
  asd_free_aligned(ops->Jq);

  asd_free_aligned(ops->mapPq);
  asd_free_aligned(ops->Fmask);

  asd_free_aligned(ops->nrJ);
  asd_free_aligned(ops->nsJ);

  asd_free_aligned(ops->Drq);
  asd_free_aligned(ops->Dsq);

  if (ops->dim == 3)
  {
    asd_free_aligned(ops->ntJ);
    asd_free_aligned(ops->Dtq);
  }

  asd_free_aligned(ops->Vq);
  asd_free_aligned(ops->Pq);

  asd_free_aligned(ops->VqLq);
  asd_free_aligned(ops->VqPq);
  asd_free_aligned(ops->VfPq);
  asd_free_aligned(ops->Vfqf);
}


#include <iostream>

#include <boost/program_options.hpp>

#include "jsonmodel.h"
#include "helpers.h"

using namespace mht;
using namespace helpers;

int main(int argc, char** argv) {
	namespace po = boost::program_options;

	std::string modelFilename;
	std::string solutionFilename;
	std::string outputFilename("graph.dot");

	// Declare the supported options.
	po::options_description description("Allowed options");
	description.add_options()
	    ("help", "produce help message")
	    ("model,m", po::value<std::string>(&modelFilename), "filename of model stored as Json file")
	    ("solution,s", po::value<std::string>(&solutionFilename), "(optional) filename where the tracking solution (as links) is stored as Json file")
	    ("output,o", po::value<std::string>(&outputFilename), "filename where the graphviz DOT print of the graph should go")
	;

	po::variables_map variableMap;
	po::store(po::parse_command_line(argc, argv, description), variableMap);
	po::notify(variableMap);

	if (variableMap.count("help")) {
	    std::cout << description << std::endl;
	    return 1;
	}

	if (!variableMap.count("model") || !variableMap.count("output")) {
	    std::cout << "Model and Output filenames have to be specified!" << std::endl;
	    std::cout << description << std::endl;
	} else {
	    JsonModel model;
		model.readFromJson(modelFilename);
		WeightsType weights(model.computeNumWeights());
		model.initializeOpenGMModel(weights);

		// print with given solution if any
		if(solutionFilename.size() > 0)
		{
			model.setJsonGtFile(solutionFilename);
			Solution solution = model.getGroundTruth();
			model.toDot(outputFilename, &solution);
		}
		else
		{
			model.toDot(outputFilename);
		}
	}
}


/**
    This node uses the SR305 camera to detect local blocking. The view is divided into three zones,
    left, center, and right. Within each zone, we report 1) maximum length of clear path confirmed,
    and 2) length to any detected obstacles.
*/
#include "ros/ros.h"
#include <ros/console.h>
// #include "std_msgs/String.h"
// #include <sstream>
#include <sensor_msgs/point_cloud_conversion.h>
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/String.h>
#include <bdbd/RoadBlocking.h>
#include <bdbd/RoadBlockingN.h>
#include <iostream>
#include <chrono>
#include <vector>
#include <array>
#include <iomanip>
#include <algorithm>
#include <string>
#include <stdio.h>
#include <math.h>
#include "libcpp/fitting3d.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;

class DetectBlocking
{
    // *** Configuration constants
    // These determine the membership in depth buckets
    const int angle_divisions = 52;
    const int depth_divisions = 52;
    const int angle_road_combine = 2;
    const int depth_road_combine = 1;
    const int point_plane_skip = 3;
    const int MIN_PLANE_POINTS = 25;
    const int MAX_PLANE_DENORMAL = 0.95;
    const float min_depth = 0.25;
    const float min_depth_division = 0.20;
    const float max_road_depth = 0.40; // We cannot reliably see road past this depth
    const float max_depth = 2.00;
    const float min_height = .04;
    const float max_height = .16;
    const float max_distance_road = 0.03; // distance from road plane for allowable road
    const float min_distance_obstacle = 0.05; // distance from road plane to declare an obstacle
    const float min_angle = -0.45;
    const float max_angle = 0.45;
    const float max_obstacle = 2.0;
    const float BIG_DEPTH = 2.0;
    const float min_obstacle_count = 10; // point count in bucket to recognize an obstacle
    const float min_road_count = 2; // point count in bucket to recognize road
    const float eps = 0.01; // added to max when no obstacles detected
    const int max_points = 10000; // The maximum number of PointCloud points to process
    const std::string target_frame = "sr305_color_frame";
    const bool USE_ROAD_BLOCKING = true;

    // *** Calculated constants
    const float delta_angle = (max_angle - min_angle) / (angle_divisions - 3.0);

    // *** Class variables
    vector<float> angle_angles;
    int frame_count = 0;
    ros::NodeHandle nh_;
    ros::Subscriber pc_sub;
    ros::Publisher rb_pub;
    ros::Publisher rbn_pub;
    ros::Publisher ls_pub;

    public:
    DetectBlocking()
    {
        // Subscribe to input video feed and publish output feed
        pc_sub = nh_.subscribe("/sr305/depth/color/points", 1, &DetectBlocking::pcCb, this);
        rb_pub = nh_.advertise<bdbd::RoadBlocking>("detectBlocking/roadBlocking", 1);
        rbn_pub = nh_.advertise<bdbd::RoadBlockingN>("detectBlocking/roadBlockingN", 1);
        ls_pub = nh_.advertise<sensor_msgs::LaserScan>("detectBlocking/scan", 1);

        for (int j = 0; j < angle_divisions; j++) {
            float division_angle = min_angle + (j - 1) * delta_angle + delta_angle/2.0;
            angle_angles.push_back(division_angle);
        }
    }

    ~DetectBlocking()
    {
    }

    void pcCb(const sensor_msgs::PointCloud2ConstPtr& pc2msg)
    {
        auto start = chrono::steady_clock::now();
        sensor_msgs::PointCloud pcmsg;
        sensor_msgs::convertPointCloud2ToPointCloud(*pc2msg, pcmsg);
        auto middle = chrono::steady_clock::now();
        vector<vector<vector<geometry_msgs::Point32>>> pointMatrix(depth_divisions, vector<vector<geometry_msgs::Point32>>(angle_divisions));
        vector<vector<vector<float>>> depthMatrix(depth_divisions, vector<vector<float>>(angle_divisions));
        vector<geometry_msgs::Point32> region_obstacles(3);

        frame_count++;

        int roadPointCount = 0;
        vector<Eigen::Vector3f> roadPlanePoints;

        geometry_msgs::Point32 lowestRoad, highestRoad, farthestRoad;
        lowestRoad.y = 100.;
        float farthestDistance = 0.0;

        // Divide all points into buckets by depth, tangent
        // Assumed coordinate system: x positive right, y positive down, z positive forward.
        int indexDelta = (pcmsg.points.size() / max_points) + 1;
        // ROS_WARN_STREAM("Point cloud points " << pcmsg.points.size() << " delta " <<indexDelta);
        for (int i = 0; i < pcmsg.points.size(); i += indexDelta)
        {
            auto point = pcmsg.points[i];
            auto x = point.x;
            auto z = point.z;
            if (z < .001) {
                ROS_WARN_STREAM("Small or negative distance in point:" <<  point);
                continue;
            }
            float depth = sqrt(x * x + z * z);

            int iz = 1 + int(float(depth_divisions - 2) * (depth - min_depth_division) / (max_depth - min_depth_division));
            if (iz < 0) {
                iz = 0;
            } else if (iz > depth_divisions - 1) {
                iz = depth_divisions - 1;
            }

            float a = atan(x / z);
            int ja = 1 + int(float(angle_divisions - 2) * (a - min_angle) / (max_angle - min_angle));
            if (ja < 0) {
                ja = 0;
            } else if (ja > angle_divisions - 1) {
                ja = angle_divisions - 1;
            }

            pointMatrix[iz][ja].push_back(point);
            depthMatrix[iz][ja].push_back(depth);
        }

        // calculate the average height in each bucket
        Eigen::MatrixXd bucketHeight(depth_divisions, angle_divisions);
        for (int i = 0; i < depth_divisions; i++) {
            for (int j = 0; j < angle_divisions; j++) {
                auto points = pointMatrix[i][j];
                double height_sum = 0.0;
                for (auto& point: points) {
                    height_sum += point.y;
                }
                bucketHeight(i, j) = points.size() != 0.0 ? height_sum / points.size() : 0.0;
            }
        }

        // save points for plane analysis
        for (int i = 1; USE_ROAD_BLOCKING && i < depth_divisions - 1; i++) {
            for (int j = 1; j < angle_divisions - 1; j++) {
                if (
                    bucketHeight(i, j) > min_height &&
                    bucketHeight(i, j) < max_height &&
                    abs(bucketHeight(i, j) - bucketHeight(i-1, j)) < .2 * max_distance_road
                ) {
                    // This point is probably valid road
                    auto points = pointMatrix[i][j];
                    // only use some points for plane calculation to reduce CPU load
                    for (int k = 0; k < points.size(); k += point_plane_skip) {
                        auto point = points[k];
                        roadPlanePoints.push_back(Eigen::Vector3f(point.x, point.y, point.z));
                    }
                }
            }
        }

        // Calculate the road plane

        bool validPlane = false;
        pair<Eigen::Vector3f, Eigen::Vector3f> plane;
        if (USE_ROAD_BLOCKING) {
            if (roadPlanePoints.size() > MIN_PLANE_POINTS) {
                validPlane = true;
                plane = best_plane_from_points<Eigen::Vector3f>(roadPlanePoints);
            } else {
                ROS_WARN_STREAM("Insufficient points to determine road plane: " << roadPlanePoints.size());
                validPlane = false;
            }

            // plane should always point in one direction
            if (plane.second[1] < 0.0) {
                plane.second *= -1.0;
            }

            // check reasonableness of plane
            if (plane.second[1] < MAX_PLANE_DENORMAL) {
                ROS_WARN_STREAM("Direction of road plane is not reasonable" << plane.second[1]);
                validPlane = false;
            }

        }
        if (!validPlane) {
            plane.first = Eigen::Vector3f(0.0, (max_height - min_height)/2., 0.0);
            plane.second = Eigen::Vector3f(0.0, 1.0, 0.0);
        }

        // obstacles

        vector<float> angle_obstacle_depth(angle_divisions, BIG_DEPTH);
        vector<float> angle_obstacle_height_raw(angle_divisions, 0.0);
        vector<float> angle_obstacle_height_relative(angle_divisions, 0.0);

        for (int i = 0; i < depth_divisions; i++) {
            for (int j = 0; j < angle_divisions; j++) {
                vector<geometry_msgs::Point32> obstacles_in_bucket;
                double obstacles_depth_sum = 0.0;
                double obstacles_distance_sum = 0.0;
                double obstacles_height_sum = 0.0;
                auto points = pointMatrix[i][j];
                auto depths = depthMatrix[i][j];
                for (int k = 0; k < points.size(); k++) {
                    auto point = points[k];
                    auto depth = depths[k];
                    // calculate distance from plane to point
                    Eigen::Vector3f p(point.x, point.y, point.z);
                    auto distance = plane.second.dot(plane.first - p);
                    if (distance > min_distance_obstacle) {
                        // This point is an obstacle (above the road plane)
                        obstacles_in_bucket.push_back(point);
                        obstacles_distance_sum += distance;
                        obstacles_height_sum += point.y;
                        obstacles_depth_sum += depth;
                    }
                }

                // We expect a minimum number of detected obstacle points to declare valid
                if (obstacles_in_bucket.size() >= min_obstacle_count) {
                    float obstacles_depth = obstacles_depth_sum / obstacles_in_bucket.size();
                    if (obstacles_depth < angle_obstacle_depth[j]) {
                        // ROS_WARN_STREAM("j " << j << " count " << obstacles_in_bucket.size() << " sum " << obstacles_depth_sum << " depth " << obstacles_depth);
                        float obstacles_distance = obstacles_distance_sum / obstacles_in_bucket.size();
                        float obstacles_height = obstacles_height_sum / obstacles_in_bucket.size();
                        angle_obstacle_depth[j] = obstacles_depth;
                        angle_obstacle_height_raw[j] = obstacles_height;
                        angle_obstacle_height_relative[j] = obstacles_distance;
                    }
                }
            }
        }

        // roads

        vector<float> angle_road_start(angle_divisions, BIG_DEPTH);
        vector<float> angle_road_end(angle_divisions, 0.0);
        vector<bool> angle_road_continuous(angle_divisions, true); // is the road unbroken?
        vector<float> angle_road_height_raw(angle_divisions, 0.0);
        vector<float> angle_road_height_relative(angle_divisions, 0.0);
        vector<vector<float>> depth_sums_in_buckets(depth_divisions, vector<float>(angle_divisions, 0.0));
        vector<vector<float>> distance_sums_in_buckets(depth_divisions, vector<float>(angle_divisions, 0.0));
        vector<vector<float>> height_sums_in_buckets(depth_divisions, vector<float>(angle_divisions, 0.0));
        vector<vector<int>> counts_in_buckets(depth_divisions, vector<int>(angle_divisions, 0));
        for (int i = 0; USE_ROAD_BLOCKING && i < depth_divisions; i++) {
            for (int j = 0; j < angle_divisions; j++) {
                auto points = pointMatrix[i][j];
                auto depths = depthMatrix[i][j];

                for (int k = 0; k < points.size(); k++) {
                    auto point = points[k];
                    auto depth = depths[k];
                    // calculate distance from plane to point
                    Eigen::Vector3f p(point.x, point.y, point.z);
                    auto distance = plane.second.dot(plane.first - p);

                    // Is this valid road?
                    if (validPlane &&
                        abs(distance) < max_distance_road &&
                         // ? why do I need these checks?
                        point.y > min_height &&
                        point.y < max_height)
                    {
                        // This point is valid road
                        counts_in_buckets[i][j]++;
                        distance_sums_in_buckets[i][j] += distance;
                        depth_sums_in_buckets[i][j] += depth;
                        height_sums_in_buckets[i][j] += point.y;
                    }
                }
            }
        }

        // Average values to reduce noise
        vector<vector<float>> avg_depth_sums_in_buckets(depth_divisions, vector<float>(angle_divisions, 0.0));
        vector<vector<float>> avg_distance_sums_in_buckets(depth_divisions, vector<float>(angle_divisions, 0.0));
        vector<vector<float>> avg_height_sums_in_buckets(depth_divisions, vector<float>(angle_divisions, 0.0));
        vector<vector<int>> avg_counts_in_buckets(depth_divisions, vector<int>(angle_divisions, 0));
        for (int i = 1; USE_ROAD_BLOCKING && i < depth_divisions - 1; i++) {
            for (int j = 1; j < angle_divisions - 1; j++) {

                for (int ii = i - depth_road_combine; ii <= i + depth_road_combine; ii++) {
                    if (ii < 0 || ii > depth_divisions - 1) continue;
                    for (int jj = j - angle_road_combine; jj <= j + angle_road_combine; jj++) {
                        if (jj < 0 || jj > depth_divisions - 1) continue;

                        avg_counts_in_buckets[i][j] += counts_in_buckets[ii][jj];
                        avg_depth_sums_in_buckets[i][j] += depth_sums_in_buckets[ii][jj];
                        avg_distance_sums_in_buckets[i][j] += distance_sums_in_buckets[ii][jj];
                        avg_height_sums_in_buckets[i][j] += height_sums_in_buckets[ii][jj];
                    }
                }

                // Calculate roads per angle, requiring minimum bucket count
                if (avg_counts_in_buckets[i][j] >= min_road_count) {
                    float fcount = (float)(avg_counts_in_buckets[i][j]);
                    float roads_depth = avg_depth_sums_in_buckets[i][j] / fcount;
                    angle_road_start[j] = min(angle_road_start[j], roads_depth);
                    if (angle_road_continuous[j]) {
                        angle_road_end[j] = max(angle_road_end[j], roads_depth);
                        angle_road_height_raw[j] = avg_height_sums_in_buckets[i][j] / fcount;
                        angle_road_height_relative[j] = avg_distance_sums_in_buckets[i][j] / fcount;
                    }
                    /*
                    ROS_WARN_STREAM(
                        "i " << i <<
                        " j " << j <<
                        " count " << avg_counts_in_buckets[i][j] <<
                        " sum " << avg_depth_sums_in_buckets[i][j] <<
                        " roads_depth " << roads_depth <<
                        " angle_road_start[j] " << angle_road_start[j] <<
                        " angle_road_end[j] " << angle_road_end[j] <<
                        " angle_road_continuous[j] " << angle_road_continuous[j]
                    );
                    */
                } else {
                    // No road, end road if it is started.
                    if (angle_road_start[j] < BIG_DEPTH) {
                        angle_road_continuous[j] = false;
                    }
                }
            }
        }

        bdbd::RoadBlockingN rbn;
        for (int j = 0; j < angle_divisions; j++) {
            rbn.angle.push_back(angle_angles[j]);
            rbn.obstacle_depth.push_back(angle_obstacle_depth[j]);
            rbn.obstacle_height_raw.push_back(angle_obstacle_height_raw[j]);
            rbn.obstacle_height_relative.push_back(angle_obstacle_height_relative[j]);
            rbn.road_start.push_back(angle_road_start[j]);
            rbn.road_end.push_back(angle_road_end[j]);
            rbn.road_height_raw.push_back(angle_road_height_raw[j]);
            rbn.road_height_relative.push_back(angle_road_height_relative[j]);
        }
        auto rosnow = ros::Time::now();
        double now = (double)rosnow.sec + 1.e-9 * (double)rosnow.nsec;
        // cout << setprecision(15) << "now sec " << rosnow.sec << " nsec " << rosnow.nsec << " double now " << now << "\n";
        rbn.rostime = now;
        rbn_pub.publish(rbn);

        // Publish LaserScan

        // build laserscan output
        sensor_msgs::LaserScan scan_output;
        scan_output.header = pc2msg->header;
        // The LaserScan frame is rotated relative to the depth frame
        scan_output.header.frame_id = target_frame;
        scan_output.angle_min = min_angle;
        scan_output.angle_max = max_angle;
        scan_output.angle_increment = delta_angle;
        scan_output.time_increment = 0.0;
        scan_output.scan_time = 0.0;
        scan_output.range_min = min_depth;
        scan_output.range_max = max_depth;

        // range is the distance to an impenetrable dropoff or obstacle
        for (int jj = 1; jj < angle_divisions - 1; jj++) {
            // flip angle coordinates
            int j = angle_divisions - jj - 1;
            float range = min_depth;
            if (USE_ROAD_BLOCKING) {
                if (angle_road_start[j] < min_depth) {
                    // We see at least some road
                    if (angle_road_end[j] < max_road_depth) {
                        // There is at least some missing road in front of us
                        range = min(angle_road_end[j], angle_obstacle_depth[j]);
                        if (angle_obstacle_depth[j] > max_road_depth + .05) {
                            // This blocking is only due to a potential dropoff
                            ROS_WARN_STREAM("Road dropoff at " << range << " obstacle at " << angle_obstacle_depth[j]);
                        }
                    }
                    else {
                        // We cannot detect road length, but show any obstacles
                        range = angle_obstacle_depth[j];
                        // ROS_WARN_STREAM("Obstacle blocking at " << range);
                    }
                }
                range = min(max_depth + eps, max(min_depth + eps, range));
            } else {
                range = min(max_depth + eps, angle_obstacle_depth[j]);
            }
            scan_output.ranges.push_back(range);
            // ROS_WARN_STREAM("Laser scan j: " << j << " angle: " << angle_angles[j] << " range: " << range);
        }
        ls_pub.publish(scan_output);

        // if (USE_ROAD_BLOCKING)
        {
            // interpret results as left, center, right regions
            // left is region 0, center 1, right 2
            vector<float> road_depths(3, 0.0);
            vector<float> obstacle_depths(3, max_obstacle);
            for (int j = 0; j < angle_divisions; j++) {
                int region;
                if (j < angle_divisions / 3)
                    region = 0;
                else if (j >= 2 * angle_divisions / 3)
                    region = 2;
                else
                    region = 1;
                obstacle_depths[region] = min(obstacle_depths[region], angle_obstacle_depth[j]);
                if (USE_ROAD_BLOCKING) {
                    if (angle_road_start[j] < min_depth) {
                        road_depths[region] = max(road_depths[region], angle_road_end[j]);
                    }
                }
                else {
                    road_depths[region] = max_road_depth;
                }
            }
            bdbd::RoadBlocking rb;
            rb.rostime = now;

            rb.leftRoadDepth = road_depths[0];
            rb.centerRoadDepth = road_depths[1];
            rb.rightRoadDepth = road_depths[2];
            rb.leftObstacleDepth = obstacle_depths[0];
            rb.centerObstacleDepth = obstacle_depths[1];
            rb.rightObstacleDepth = obstacle_depths[2];
            rb_pub.publish(rb);
        }
    }
};

int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line.
*/
    ros::init(argc, argv, "detectBlocking");
    ROS_INFO("detectBlocking starting up");
    DetectBlocking pp;
    ros::spin();
    return(0);
}


#ifndef ASLAM_BACKEND_SPARSE_MATRIX_FUNCTIONS_HPP
#define ASLAM_BACKEND_SPARSE_MATRIX_FUNCTIONS_HPP

#include <sparse_block_matrix/linear_solver.h>
#include <Eigen/Dense>
#include <boost/shared_ptr.hpp>

namespace aslam {
namespace backend {

void applySchurComplement(sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd>& H, const Eigen::VectorXd& e,
                          double lambda, int marginalizedStartingBlock, bool doLevenberg,
                          sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd>& A,
                          std::vector<Eigen::MatrixXd>& invVi, Eigen::VectorXd& b);

void buildDsi(int i, sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd>& H, const Eigen::VectorXd& e,
              int marginalizedStartingBlock, const Eigen::MatrixXd& invVi, const Eigen::VectorXd& dx,
              Eigen::VectorXd& outDsi);

}  // namespace backend
}  // namespace aslam

#endif /* ASLAM_BACKEND_SPARSE_MATRIX_FUNCTIONS_HPP */


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

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

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

// This file was modified by Oracle on 2021.
// Modifications copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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_STRATEGY_CARTESIAN_SIDE_ROBUST_HPP
#define BOOST_GEOMETRY_STRATEGY_CARTESIAN_SIDE_ROBUST_HPP

#include <boost/geometry/core/config.hpp>
#include <boost/geometry/strategy/cartesian/side_non_robust.hpp>

#include <boost/geometry/strategies/side.hpp>

#include <boost/geometry/util/select_most_precise.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/precise_math.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

struct epsilon_equals_policy
{
public:
    template <typename Policy, typename T1, typename T2>
    static bool apply(T1 const& a, T2 const& b, Policy const& policy)
    {
        return boost::geometry::math::detail::equals_by_policy(a, b, policy);
    }
};

struct fp_equals_policy
{
public:
    template <typename Policy, typename T1, typename T2>
    static bool apply(T1 const& a, T2 const& b, Policy const&)
    {
        return a == b;
    }
};

/*!
\brief Adaptive precision predicate to check at which side of a segment a point lies:
    left of segment (>0), right of segment (< 0), on segment (0).
\ingroup strategies
\tparam CalculationType \tparam_calculation (numeric_limits<ct>::epsilon() and numeric_limits<ct>::digits must be supported for calculation type ct)
\tparam Robustness std::size_t value from 0 (fastest) to 3 (default, guarantees correct results).
\details This predicate determines at which side of a segment a point lies using an algorithm that is adapted from orient2d as described in "Adaptive Precision Floating-Point Arithmetic and Fast Robust Geometric Predicates" by Jonathan Richard Shewchuk ( https://dl.acm.org/citation.cfm?doid=237218.237337 ). More information and copies of the paper can also be found at https://www.cs.cmu.edu/~quake/robust.html . It is designed to be adaptive in the sense that it should be fast for inputs that lead to correct results with plain float operations but robust for inputs that require higher precision arithmetics.
 */
template
<
    typename CalculationType = void,
    typename EqualsPolicy = epsilon_equals_policy,
    std::size_t Robustness = 3
>
struct side_robust
{

    template <typename CT>
    struct epsilon_policy
    {
        using Policy = boost::geometry::math::detail::equals_factor_policy<CT>;

        epsilon_policy() {}

        template <typename Type>
        epsilon_policy(Type const& a, Type const& b, Type const& c, Type const& d)
            : m_policy(a, b, c, d)
        {}
        Policy m_policy;

    public:

        template <typename T1, typename T2>
        bool apply(T1 a, T2 b) const
        {
            return EqualsPolicy::apply(a, b, m_policy);
        }
    };

public:

    typedef cartesian_tag cs_tag;

    //! \brief Computes the sign of the CCW triangle p1, p2, p
    template
    <
        typename PromotedType,
        typename P1,
        typename P2,
        typename P,
        typename EpsPolicyInternal,
        std::enable_if_t<std::is_fundamental<PromotedType>::value, int> = 0
    >
    static inline PromotedType side_value(P1 const& p1,
                                          P2 const& p2,
                                          P const& p,
                                          EpsPolicyInternal& eps_policy)
    {
        using vec2d = ::boost::geometry::detail::precise_math::vec2d<PromotedType>;
        vec2d pa;
        pa.x = get<0>(p1);
        pa.y = get<1>(p1);
        vec2d pb;
        pb.x = get<0>(p2);
        pb.y = get<1>(p2);
        vec2d pc;
        pc.x = get<0>(p);
        pc.y = get<1>(p);
        return ::boost::geometry::detail::precise_math::orient2d
            <PromotedType, Robustness>(pa, pb, pc, eps_policy);
    }

    template
    <
        typename PromotedType,
        typename P1,
        typename P2,
        typename P,
        typename EpsPolicyInternal,
        std::enable_if_t<!std::is_fundamental<PromotedType>::value, int> = 0
    >
    static inline auto side_value(P1 const& p1, P2 const& p2, P const& p,
                                  EpsPolicyInternal&)
    {
        return side_non_robust<>::apply(p1, p2, p);
    }

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    template
    <
        typename P1,
        typename P2,
        typename P
    >
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        using coordinate_type = typename select_calculation_type_alt
            <
                CalculationType,
                P1,
                P2,
                P
            >::type;

        using promoted_type = typename select_most_precise
            <
                coordinate_type,
                double
            >::type;

        epsilon_policy<promoted_type> epsp;
        promoted_type sv = side_value<promoted_type>(p1, p2, p, epsp);
        promoted_type const zero = promoted_type();

        return epsp.apply(sv, zero) ? 0
            : sv > zero ? 1
            : -1;
    }

#endif

};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_CARTESIAN_SIDE_ROBUST_HPP


#ifndef DEF_DATATYPES_H
#define DEF_DATATYPES_H

#include <iostream>
#include <iomanip>
#include <vector>
#include <boost/graph/adjacency_list.hpp>

#define M    32  // Maximum number of cores
#define NMax 20000 // Maximum supported taskset size
#define FIXED_FLOAT(x) std::fixed<<std::setprecision(2)<<(x)
#define DEBUG

/*
 * 1. NotReady - Predecessors not yet finished, cannot execute
 * 2. Ready    - Predecessors finished, but not enough resources
 * 3. Active   - Execution under progress
 * 4. Finished - Completed execution already
 */
enum StatusType {NotReady,Ready,Finished,Active};

struct NodeType {
    StatusType status;
    double estart;  /* Earliest possible start time that satisfies the precedence constraints */

    /* Execution type characteristics (static)
     * execution time is modelled \tau_j(m) = \tau_j(1) * m^(-d_{j}).
     * where 0 < d_{j} < 1
     */
    double dj;      /* Exponent 0 < dj < 1 */
    double tau1;
    int id;

    /* Execution type characteristics (Dynamic)
     * mainly start and finish time in the schedule
     */
    int alloc;
    double start_time;
    double stop_time;
    double C;        /* computation requirement, uninitialized value = -1 */
};

/* Only used for dependency modelling */
struct EdgeType {
    int edgeNum;
};

/* Event types for statistics collection and display */
enum EventType {Stop=1,Start=0};

/* <time stamp, EeventType, alloc, id> */
typedef std::tuple<double,EventType,int,int> Events;

typedef boost::adjacency_list<boost::vecS,boost::vecS,boost::bidirectionalS,NodeType,EdgeType> DAGType;
typedef boost::graph_traits<DAGType>::vertex_descriptor DAGVertexType;
typedef boost::graph_traits<DAGType>::edge_descriptor DAGEdgeType;
typedef boost::graph_traits<DAGType>::out_edge_iterator DAGOutEdgeIterator;
typedef boost::graph_traits<DAGType>::in_edge_iterator DAGInEdgeIterator;
typedef boost::graph_traits<DAGType>::vertex_iterator DAGVertexIterator;
typedef boost::graph_traits<DAGType>::degree_size_type DAGDegSizeType;

class DAGVertexPW {
     public:
       DAGVertexPW(DAGType& g) : myGraph(g) {}
       template <class VertexOrEdge>
       void operator()(std::ostream& os, const VertexOrEdge& v) const {
            os << "[status=\"{";
            switch(myGraph[v].status) {
                case NotReady   : os << "NotReady,"; break;
                case Ready      : os << "Ready,"; break;
                case Finished   : os << "Finished,"; break;
                case Active     : os << "Active"; break;
            }
            os << "id:" << myGraph[v].id;
            // os << ",estart:" << FIXED_FLOAT(myGraph[v].estart);
            // os << ",alloc:" << myGraph[v].alloc;
            // os << ",start:" << FIXED_FLOAT(myGraph[v].start_time);
            // os << ",stop:" << FIXED_FLOAT(myGraph[v].stop_time);
            // os << ",C:" << FIXED_FLOAT(myGraph[v].C);
            os << ",dj:" << FIXED_FLOAT(myGraph[v].dj);
            os << ",tau1:" << FIXED_FLOAT(myGraph[v].tau1);
            os << "}\"]";
       }
    private:
        DAGType& myGraph;
};

class DAGEdgePW {
    public:
       DAGEdgePW(DAGType& g) : myGraph(g) {}
       template <class VertexOrEdge>
       void operator()(std::ostream& os, const VertexOrEdge& e) const {
       }
    private:
        DAGType& myGraph;
};

#endif


#pragma once

#include <boost/dynamic_bitset.hpp>
#include <cassert>
#include <csim/stat_ops.hpp>
#include <csim/stat_ops_dm.hpp>
#include <iostream>
#include <regex>
#include <vector>

#ifdef _USE_GPU
#include <gpusim/stat_ops.h>
#endif

#include "pauli_operator.hpp"
#include "state.hpp"
#include "type.hpp"

class DllExport Observable {
private:
    std::vector<MultiQubitPauliOperator> _pauli_terms;
    std::vector<CPPCTYPE> _coef_list;
    std::unordered_map<std::string, ITYPE> _term_dict;

    CPPCTYPE calc_coef(const MultiQubitPauliOperator& a,
        const MultiQubitPauliOperator& b) const;

public:
    Observable(){};

    /**
     * Observable が保持する PauliOperator の個数を返す
     * @return Observable が保持する PauliOperator の個数
     */
    UINT get_term_count() const { return (UINT)_pauli_terms.size(); }

    /**
     * Observable の指定した添字に対応するPauliOperatorを返す
     * @param[in] index
     * Observable が保持するPauliOperatorのリストの添字
     * @return 指定したindexにあるPauliOperator
     */
    std::pair<CPPCTYPE, MultiQubitPauliOperator> get_term(
        const UINT index) const;

    /**
     * Observable が保持するPauliOperatorをunordered_mapとして返す
     * @return Observable が保持するunordered_map
     */
    std::unordered_map<std::string, ITYPE> get_dict() const;

    /**
     * PauliOperatorを内部で保持するリストの末尾に追加する。
     *
     * @param[in] mpt 追加するPauliOperatorのインスタンス
     */
    void add_term(const CPPCTYPE coef, MultiQubitPauliOperator op);

    /**
     * パウリ演算子の文字列と係数の組をObservable に追加する。
     *
     * @param[in] coef pauli_stringで作られるPauliOperatorの係数
     * @param[in] pauli_string
     * パウリ演算子と掛かるindexの組からなる文字列。(example: "X 1 Y 2 Z 5")
     */
    void add_term(const CPPCTYPE coef, std::string s);

    void add_term(const std::vector<CPPCTYPE> coef_list,
        std::vector<MultiQubitPauliOperator> pauli_terms);

    void remove_term(UINT index);

    /**
     * Observable のある量子状態に対応するエネルギー(期待値)を計算して返す
     *
     * @param[in] state 期待値をとるときの量子状態
     * @return 入力で与えた量子状態に対応するObservable の期待値
     */
    CPPCTYPE get_expectation_value(const QuantumStateBase* state) const;

    /**
     * Observable によってある状態が別の状態に移る遷移振幅を計算して返す
     *
     * @param[in] state_bra 遷移先の量子状態
     * @param[in] state_ket 遷移前の量子状態
     * @return 入力で与えた量子状態に対応するObservable の遷移振幅
     */
    CPPCTYPE get_transition_amplitude(const QuantumStateBase* state_bra,
        const QuantumStateBase* state_ket) const;

    /**
     * ヒープに確保した Observable を返す
     *
     * @return ヒープに確保した Observable へのポインタ
     */
    Observable* copy() const;

    Observable operator+(const Observable& target) const;

    Observable& operator+=(const Observable& target);

    Observable operator-(const Observable& target) const;

    Observable& operator-=(const Observable& target);

    Observable operator*(const Observable& target) const;

    Observable operator*(const CPPCTYPE& target) const;

    Observable& operator*=(const Observable& target);

    Observable& operator*=(const CPPCTYPE& target);

    /**
     * Observableを文字列に変換する
     */
    std::string to_string() const;
};