//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Alexander Sokolov <asokolov@nil.foundation>
// Copyright (c) 2020 Nikita Kaskov <nbering@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_DETAIL_UNBOUNDED_SHIFT_HPP
#define CRYPTO3_DETAIL_UNBOUNDED_SHIFT_HPP

#include <boost/assert.hpp>

#include <boost/crypto3/detail/stream_endian.hpp>

namespace boost {
    namespace crypto3 {
        namespace detail {

            template<int Shift, typename T>
            struct unbounded_shifter {
                static T shl(T x) {
                    return unbounded_shifter<Shift - 1, T>::shl(T(x << 1));
                }

                static T shr(T x) {
                    return unbounded_shifter<Shift - 1, T>::shr(T(x >> 1));
                }
            };

            template<typename T>
            struct unbounded_shifter<0, T> {
                static T shl(T x) {
                    return x;
                }

                static T shr(T x) {
                    return x;
                }
            };

            template<int Shift, typename T>
            T unbounded_shl(T x) {
                return unbounded_shifter<Shift, T>::shl(x);
            }

            template<int Shift, typename T>
            T unbounded_shr(T x) {
                return unbounded_shifter<Shift, T>::shr(x);
            }

            template<typename T>
            T unbounded_shl(T x, std::size_t n) {
                return x << n;
            }

            template<typename T>
            T unbounded_shr(T x, std::size_t n) {
                return x >> n;
            }
            // FIXME: it wouldn't work when Shift == sizeof(T) * CHAR_BIT
            template<int Shift, typename T>
            T low_bits(T x) {
                T highmask = unbounded_shl<Shift, T>(~T());
                return T(x & ~highmask);
            }

            template<size_t Shift, size_t TypeBits, typename T>
            T low_bits(T x) {
                constexpr size_t real_shift = TypeBits - Shift;
                T lowmask = ((bool)Shift) * unbounded_shr<real_shift, T>(~T());
                return x & lowmask;
            }

            template<size_t type_bits, typename T>
            T low_bits(T x, size_t shift) {
                T lowmask = ((bool)shift) * unbounded_shr<T>(~T(), type_bits - shift);
                return x & lowmask;
            }

            template<size_t type_bits, typename T>
            T high_bits(T x, size_t shift) {
                T highmask = ((bool)shift) * unbounded_shl<T>(~T(), type_bits - shift);
                return x & highmask;
            }
        }    // namespace detail
    }        // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_HASH_DETAIL_UNBOUNDED_SHIFT_HPP


#include <boost/intrusive/avltree.hpp>


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

/**
 * @file xmg_simulate.hpp
 *
 * @brief XMG simulation
 *
 * @author Mathias Soeken
 * @since  2.3
 */

#ifndef XMG_SIMULATE_HPP
#define XMG_SIMULATE_HPP

#include <map>
#include <unordered_map>

#include <boost/dynamic_bitset.hpp>

#include <core/properties.hpp>
#include <core/utils/timer.hpp>
#include <classical/utils/truth_table_utils.hpp>
#include <classical/xmg/xmg.hpp>
#include <classical/xmg/xmg_dfs.hpp>

#include <cuddObj.hh>

namespace cirkit
{

/******************************************************************************
 * Abstract class for simulators                                              *
 ******************************************************************************/

template<typename T>
class xmg_simulator
{
public:
  /**
   * @brief Simulator routine when input is encountered
   *
   * @param node XMG node reference in the `mig' graph
   * @param name Name of that node
   * @param pos  Position of that node (usually wrt. to the input vector)
   * @param xmg  XMG
   */
  virtual T get_input( const xmg_node& node, const xmg_graph& xmg ) const = 0;
  virtual T get_constant() const = 0;
  virtual T invert( const T& v ) const = 0;
  virtual T xor_op( const xmg_node& node, const T& v1, const T& v2 ) const = 0;
  virtual T maj_op( const xmg_node& node, const T& v1, const T& v2, const T& v3 ) const = 0;

  virtual bool terminate( const xmg_node& node, const xmg_graph& xmg ) const
  {
    return false;
  }
};

/******************************************************************************
 * Several simulator implementations                                          *
 ******************************************************************************/

class xmg_pattern_simulator : public xmg_simulator<bool>
{
public:
  xmg_pattern_simulator( const boost::dynamic_bitset<>& pattern );

  bool get_input( const xmg_node& node, const xmg_graph& xmg ) const;
  bool get_constant() const;
  bool invert( const bool& v ) const;
  bool xor_op( const xmg_node& node, const bool& v1, const bool& v2 ) const;
  bool maj_op( const xmg_node& node, const bool& v1, const bool& v2, const bool& v3 ) const;

private:
  boost::dynamic_bitset<> pattern;
};

class xmg_tt_simulator : public xmg_simulator<tt>
{
public:
  tt get_input( const xmg_node& node, const xmg_graph& xmg ) const;
  tt get_constant() const;
  tt invert( const tt& v ) const;
  tt xor_op( const xmg_node& node, const tt& v1, const tt& v2 ) const;
  tt maj_op( const xmg_node& node, const tt& v1, const tt& v2, const tt& v3 ) const;
};

class xmg_bdd_simulator : public xmg_simulator<BDD>
{
public:
  xmg_bdd_simulator() : mgr( Cudd() ) {}
  xmg_bdd_simulator( const Cudd& mgr ) : mgr( mgr ) {}

  BDD get_input( const xmg_node& node, const xmg_graph& xmg ) const;
  BDD get_constant() const;
  BDD invert( const BDD& v ) const;
  BDD xor_op( const xmg_node& node, const BDD& v1, const BDD& v2 ) const;
  BDD maj_op( const xmg_node& node, const BDD& v1, const BDD& v2, const BDD& v3 ) const;

protected:
  Cudd mgr;
};

template<typename T>
class xmg_partial_node_assignment_simulator : public xmg_simulator<T>
{
public:
  xmg_partial_node_assignment_simulator( const xmg_simulator<T>& total_simulator,
                                         const std::map<xmg_node, T>& assignment,
                                         const T& default_value )
    : total_simulator( total_simulator ),
      assignment( assignment ),
      default_value( default_value ) {}

  T get_input( const xmg_node& node, const xmg_graph& xmg ) const
  {
    auto it = assignment.find( node );
    if ( it == assignment.end() )
    {
      std::cout << "[w] no assignment given for '" << node << "', assume default" << std::endl;
      return default_value;
    }
    else
    {
      return it->second;
    }
  }

  T get_constant() const { return total_simulator.get_constant(); }
  T invert( const T& v ) const { return total_simulator.invert( v ); }
  T xor_op( const xmg_node& node, const T& v1, const T& v2 ) const
  {
    auto it = assignment.find( node );
    if ( it == assignment.end() )
    {
      return total_simulator.xor_op( node, v1, v2 );
    }
    else
    {
      return it->second;
    }
  }
  T maj_op( const xmg_node& node, const T& v1, const T& v2, const T& v3 ) const
  {
    auto it = assignment.find( node );
    if ( it == assignment.end() )
    {
      return total_simulator.maj_op( node, v1, v2, v3 );
    }
    else
    {
      return it->second;
    }
  }

  bool terminate( const xmg_node& node, const xmg_graph& xmg ) const
  {
    return assignment.find( node ) != assignment.end();
  }

private:
  const xmg_simulator<T>& total_simulator;
  const std::map<xmg_node, T>& assignment;
  T default_value;
};

/******************************************************************************
 * DFS visitor for actual simulation                                          *
 ******************************************************************************/

using xmg_node_color_map = std::map<xmg_node, boost::default_color_type>;

template<typename T>
struct simulate_xmg_node_visitor : public xmg_dfs_visitor
{
public:
  simulate_xmg_node_visitor( const xmg_graph& xmg, const xmg_simulator<T>& simulator, std::map<xmg_node, T>& node_values )
    : xmg_dfs_visitor( xmg ),
      simulator( simulator ),
      node_values( node_values ) {}

  void finish_input( const xmg_node& node, const xmg_graph& xmg )
  {
    node_values[node] = simulator.get_input( node, xmg );
  }

  void finish_constant( const xmg_node& node, const xmg_graph& xmg )
  {
    node_values[node] = simulator.get_constant();
  }

  void finish_xor_node( const xmg_node& node, const xmg_function& a, const xmg_function& b, const xmg_graph& xmg )
  {
    T ta = node_values[a.node];
    T tb = node_values[b.node];

    node_values[node] = simulator.xor_op( node,
                                          a.complemented ? simulator.invert( ta ) : ta,
                                          b.complemented ? simulator.invert( tb ) : tb );
  }

  void finish_maj_node( const xmg_node& node, const xmg_function& a, const xmg_function& b, const xmg_function& c, const xmg_graph& xmg )
  {
    T ta = node_values[a.node];
    T tb = node_values[b.node];
    T tc = node_values[c.node];

    node_values[node] = simulator.maj_op( node,
                                          a.complemented ? simulator.invert( ta ) : ta,
                                          b.complemented ? simulator.invert( tb ) : tb,
                                          c.complemented ? simulator.invert( tc ) : tc );
  }

private:
  const xmg_simulator<T>& simulator;
  std::map<xmg_node, T>& node_values;
};

/******************************************************************************
 * Methods to trigger simulation                                              *
 ******************************************************************************/

template<typename T>
T simulate_xmg_node( const xmg_graph& xmg, const xmg_node& node,
                     const xmg_simulator<T>& simulator,
                     xmg_node_color_map& colors,
                     std::map<xmg_node, T>& node_values )
{
  boost::depth_first_visit( xmg.graph(), node,
                            simulate_xmg_node_visitor<T>( xmg, simulator, node_values ),
                            boost::make_assoc_property_map( colors ),
                            [&simulator, &xmg]( const xmg_node& node, const xmg_graph::graph_t& g ) { return simulator.terminate( node, xmg ); } );

  return node_values[node];
}

template<typename T>
T simulate_xmg_node( const xmg_graph& xmg, const xmg_node& node,
                     const xmg_simulator<T>& simulator )
{
  xmg_node_color_map colors;
  std::map<xmg_node, T> node_values;

  return simulate_xmg_node<T>( xmg, node, simulator, colors, node_values );
}

template<typename T>
T simulate_xmg_function( const xmg_graph& xmg, const xmg_function& f,
                         const xmg_simulator<T>& simulator,
                         xmg_node_color_map& colors,
                         std::map<xmg_node, T>& node_values )
{
  T value = simulate_xmg_node<T>( xmg, f.node, simulator, colors, node_values );
  return f.complemented ? simulator.invert( value ) : value;
}

template<typename T>
T simulate_xmg_function( const xmg_graph& xmg, const xmg_function& f,
                         const xmg_simulator<T>& simulator )
{
  T value = simulate_xmg_node<T>( xmg, f.node, simulator );
  return f.complemented ? simulator.invert( value ) : value;
}

template<typename T>
std::map<xmg_function, T> simulate_xmg( const xmg_graph& xmg, const xmg_simulator<T>& simulator,
                                        const properties::ptr& settings = properties::ptr(),
                                        const properties::ptr& statistics = properties::ptr() )
{
  /* settings */
  const auto verbose = get( settings, "verbose", false );

  /* timer */
  properties_timer t( statistics );

  xmg_node_color_map colors;
  std::map<xmg_node, T> node_values;

  std::map<xmg_function, T> results;

  for ( const auto& o : xmg.outputs() )
  {
    if ( verbose )
    {
      std::cout << "[i] simulate '" << o.second << "'" << std::endl;
    }
    T value = simulate_xmg_node<T>( xmg, o.first.node, simulator, colors, node_values );

    /* value may need to be inverted */
    results[o.first] = o.first.complemented ? simulator.invert( value ) : value;
  }

  set( statistics, "node_values", node_values );

  return results;
}

/******************************************************************************
 * Utility functions for simulation                                           *
 ******************************************************************************/

boost::dynamic_bitset<> xmg_simulate_pattern( const xmg_graph& xmg, const boost::dynamic_bitset<>& pattern );

}

#endif

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


#ifndef CREDIBLE_BALL_HPP
#define CREDIBLE_BALL_HPP

#include <Eigen/Dense>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <set>

#include "../lossfunction/BinderLoss.hpp"
#include "../lossfunction/LossFunction.hpp"
#include "../lossfunction/VariationInformation.hpp"

class CredibleBall {
 private:
  LossFunction* loss_function;     // metric to compute the region
  Eigen::MatrixXi mcmc_sample;     // MCMC matrix of clusters
  Eigen::VectorXi point_estimate;  // output of the greedy algorithm
  set<int, greater<int>>
      credibleBall;     // set of index clusters inside the credible region
  int T;                // number of clusters, ie mcmc_sample.rows
  int N;                // dimension of cluster, ie mcmc_sample.cols
  double alpha;         // level of the credible ball region
  double radius;        // radius of the credible ball
  double prob;          // estimated probability
  double vlb_distance;  // distance to the vertical lower bounds
  double vub_distance;  // distance to the vertical upper bounds
  double hb_distance;   // distance to the horizontal bounds

 public:
  CredibleBall(LOSS_FUNCTION loss_type_, Eigen::MatrixXi& mcmc_sample_,
               double alpha_, Eigen::VectorXi& point_estimate_);
  ~CredibleBall();

  void calculateRegion(
      double rate);    // calculate the points in the credible region
  double getRadius();  // returns the calculated radius
  Eigen::VectorXi VerticalUpperBound();  // index of cluusters of the VUB
  Eigen::VectorXi VerticalLowerBound();  // index of the clusters of the VLB
  Eigen::VectorXi HorizontalBound();     // index of the clusters of the HB
  void sumary(
      Eigen::VectorXi HB, Eigen::VectorXi VUB, Eigen::VectorXi VLB,
      string filename);  // generates a file with the bounds information

 protected:
  void populateCredibleSet();  // populate the credibleBall
  int count_cluster_row(int index);
  int getMinimalCrdinality();
  int getMaximalCardinality();
  double getMaxDistance(set<int, greater<int>> vec1);
  double getMinDistance();
  void saveSelectedClusters(set<int, greater<int>>& vec1, double& max_distance,
                            set<int, greater<int>>& vlb);
};
#endif  // CREDIBLE_BALL_HPP


#include "lib/PupilTracker.h"

#include <iostream>

#include <boost/foreach.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <tbb/tbb.h>

#include <lib/cvx.h>

namespace
{
  struct section_guard
  {
    std::string name;
    pupiltracker::tracker_log& log;
    pupiltracker::timer t;
    section_guard(const std::string& name, pupiltracker::tracker_log& log) : name(name), log(log), t() {  }
    ~section_guard() { log.add(name, t); }
    operator bool() const {return false;}
  };

  inline section_guard make_section_guard(const std::string& name, pupiltracker::tracker_log& log)
  {
    return section_guard(name,log);
  }
}

#define SECTION(A,B) if (const section_guard& _section_guard_ = make_section_guard( A , B )) {} else

class HaarSurroundFeature
{
public:
  HaarSurroundFeature(int r1, int r2) : r_inner(r1), r_outer(r2)
  {

    // Number of pixels in each part of the kernel
    int count_inner = r_inner*r_inner;
    int count_outer = r_outer*r_outer - r_inner*r_inner;

    // Frobenius normalized values
    //
    // Want norm = 1 where norm = sqrt(sum(pixelvals^2)), so:
    //  sqrt(count_inner*val_inner^2 + count_outer*val_outer^2) = 1
    //
    // Also want sum(pixelvals) = 0, so:
    //  count_inner*val_inner + count_outer*val_outer = 0
    //
    // Solving both of these gives:
    //val_inner = std::sqrt( (float)count_outer/(count_inner*count_outer + sq(count_inner)) );
    //val_outer = -std::sqrt( (float)count_inner/(count_inner*count_outer + sq(count_outer)) );

    // Square radius normalised values
    //
    // Want the response to be scale-invariant, so scale it by the number of pixels inside it:
    //  val_inner = 1/count = 1/r_outer^2
    //
    // Also want sum(pixelvals) = 0, so:
    //  count_inner*val_inner + count_outer*val_outer = 0
    //
    // Hence:
    val_inner = 1.0 / (r_inner*r_inner);
    val_outer = -val_inner*count_inner/count_outer;

  }

  float val_inner, val_outer;
  int r_inner, r_outer;
};

cv::RotatedRect fitEllipse(const std::vector<pupiltracker::EdgePoint>& edgePoints)
{
  std::vector<cv::Point2f> points;
  points.reserve(edgePoints.size());

  BOOST_FOREACH(const pupiltracker::EdgePoint& e, edgePoints)
  points.push_back(e.point);

  return cv::fitEllipse(points);
}

bool pupiltracker::findPupilEllipse(
  const pupiltracker::TrackerParams& params,
  const cv::Mat& m,

  pupiltracker::findPupilEllipse_out& out,
  pupiltracker::tracker_log& log
)
{
  // --------------------
  // Convert to greyscale
  // --------------------

  cv::Mat_<uchar> mEye;

  SECTION("Grey and crop", log)
  {
    // Pick one channel if necessary, and crop it to get rid of borders
    if (m.channels() == 1)
    {
      mEye = m;
    }
    else
    {
      throw std::runtime_error("Oops! We only support mono image...");
    }
  }

  cv::Mat_<int32_t> mEyeIntegral;
  int padding = 2*params.Radius_Max;

  SECTION("Integral image", log)
  {
    cv::Mat mEyePad;
    // Need to pad by an additional 1 to get bottom & right edges.
    cv::copyMakeBorder(mEye, mEyePad, padding, padding, padding, padding, cv::BORDER_REPLICATE);
    cv::integral(mEyePad, mEyeIntegral);
  }

  cv::Point2f pHaarPupil;
  int haarRadius = 0;

  SECTION("Haar responses", log)
  {
    const int rstep = 15;
    const int ystep = 15;
    const int xstep = 15;

    float minResponse = std::numeric_limits<float>::infinity();

    for (int r = params.Radius_Min; r < params.Radius_Max; r+=rstep)
    {
      // Get Haar feature
      int r_inner = r;
      int r_outer = 3*r;
      HaarSurroundFeature f(r_inner, r_outer);

      // Use TBB for rows
      std::pair<float,cv::Point2f> minRadiusResponse = tbb::parallel_reduce(
        tbb::blocked_range<int>(0, (mEye.rows-r - r - 1)/ystep + 1, ((mEye.rows-r - r - 1)/ystep + 1) / 8),
        std::make_pair(std::numeric_limits<float>::infinity(), UNKNOWN_POSITION),
        [&] (tbb::blocked_range<int> range, const std::pair<float,cv::Point2f>& minValIn) -> std::pair<float,cv::Point2f>
        {
          std::pair<float,cv::Point2f> minValOut = minValIn;
          for (int i = range.begin(), y = r + range.begin()*ystep; i < range.end(); i++, y += ystep)
          {

            int* row1_inner = mEyeIntegral[y+padding - r_inner];
            int* row2_inner = mEyeIntegral[y+padding + r_inner + 1];
            int* row1_outer = mEyeIntegral[y+padding - r_outer];
            int* row2_outer = mEyeIntegral[y+padding + r_outer + 1];

            int* p00_inner = row1_inner + r + padding - r_inner;
            int* p01_inner = row1_inner + r + padding + r_inner + 1;
            int* p10_inner = row2_inner + r + padding - r_inner;
            int* p11_inner = row2_inner + r + padding + r_inner + 1;

            int* p00_outer = row1_outer + r + padding - r_outer;
            int* p01_outer = row1_outer + r + padding + r_outer + 1;
            int* p10_outer = row2_outer + r + padding - r_outer;
            int* p11_outer = row2_outer + r + padding + r_outer + 1;

            for (int x = r; x < mEye.cols - r; x+=xstep)
            {
              int sumInner = *p00_inner + *p11_inner - *p01_inner - *p10_inner;
              int sumOuter = *p00_outer + *p11_outer - *p01_outer - *p10_outer - sumInner;

              float response = f.val_inner * sumInner + f.val_outer * sumOuter;

              if (response < minValOut.first)
              {
                minValOut.first = response;
                minValOut.second = cv::Point(x,y);
              }

              p00_inner += xstep;
              p01_inner += xstep;
              p10_inner += xstep;
              p11_inner += xstep;

              p00_outer += xstep;
              p01_outer += xstep;
              p10_outer += xstep;
              p11_outer += xstep;
            }
          }
          return minValOut;
        },
        [] (const std::pair<float,cv::Point2f>& x, const std::pair<float,cv::Point2f>& y) -> std::pair<float,cv::Point2f>
        {
          if (x.first < y.first)
          return x;
          else
          return y;
        }
      );

      if (minRadiusResponse.first < minResponse)
      {
        minResponse = minRadiusResponse.first;
        // Set return values
        pHaarPupil = minRadiusResponse.second;
        haarRadius = r;
      }
    }
  }
  // Paradoxically, a good Haar fit won't catch the entire pupil, so expand it a bit
  haarRadius = (int)(haarRadius * SQRT_2);

  // ---------------------------
  // Pupil ROI around Haar point
  // ---------------------------
  cv::Rect roiHaarPupil = cvx::roiAround(cv::Point(pHaarPupil.x, pHaarPupil.y), haarRadius);
  cv::Mat_<uchar> mHaarPupil;
  cvx::getROI(mEye, mHaarPupil, roiHaarPupil);

  out.roiHaarPupil = roiHaarPupil;
  out.mHaarPupil = mHaarPupil;

  // --------------------------------------------------
  // Get histogram of pupil region, segment with KMeans
  // --------------------------------------------------

  const int bins = 256;

  cv::Mat_<float> hist;
  SECTION("Histogram", log)
  {
    int channels[] = {0};
    int sizes[] = {bins};
    float range[2] = {0, 256};
    const float* ranges[] = {range};
    cv::calcHist(&mHaarPupil, 1, channels, cv::Mat(), hist, 1, sizes, ranges);
  }

  out.histPupil = hist;

  float threshold;
  SECTION("KMeans", log)
  {
    // Try various candidate centres, return the one with minimal label distance
    float candidate0[2] = {0, 0};
    float candidate1[2] = {128, 255};
    float bestDist = std::numeric_limits<float>::infinity();
    float bestThreshold = std::numeric_limits<float>::quiet_NaN();

    for (int i = 0; i < 2; i++)
    {
      cv::Mat_<uchar> labels;
      float centres[2] = {candidate0[i], candidate1[i]};
      float dist = cvx::histKmeans(hist, 0, 256, 2, centres, labels, cv::TermCriteria(cv::TermCriteria::COUNT, 50, 0.0));

      float thisthreshold = (centres[0] + centres[1])/2;
      if (dist < bestDist && boost::math::isnormal(thisthreshold))
      {
        bestDist = dist;
        bestThreshold = thisthreshold;
      }
    }

    if (!boost::math::isnormal(bestThreshold))
    {
      // If kmeans gives a degenerate solution, exit early
      return false;
    }

    threshold = bestThreshold;
  }

  cv::Mat_<uchar> mPupilThresh;
  SECTION("Threshold", log)
  {
    cv::threshold(mHaarPupil, mPupilThresh, threshold, 255, cv::THRESH_BINARY_INV);
  }

  out.threshold = threshold;
  out.mPupilThresh = mPupilThresh;

  // ---------------------------------------------
  // Find best region in the segmented pupil image
  // ---------------------------------------------

  cv::Rect bbPupilThresh;
  cv::RotatedRect elPupilThresh;

  SECTION("Find best region", log)
  {
    cv::Mat_<uchar> mPupilContours = mPupilThresh.clone();
    std::vector<std::vector<cv::Point> > contours;
    cv::findContours(mPupilContours, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);

    if (contours.size() == 0)
    return false;

    std::vector<cv::Point>& maxContour = contours[0];
    float maxContourArea = cv::contourArea(maxContour);
    BOOST_FOREACH(std::vector<cv::Point>& c, contours)
    {
      float area = cv::contourArea(c);
      if (area > maxContourArea)
      {
        maxContourArea = area;
        maxContour = c;
      }
    }

    cv::Moments momentsPupilThresh = cv::moments(maxContour);

    bbPupilThresh = cv::boundingRect(maxContour);
    elPupilThresh = cvx::fitEllipse(momentsPupilThresh);

    // Shift best region into eye coords (instead of pupil region coords), and get ROI
    bbPupilThresh.x += roiHaarPupil.x;
    bbPupilThresh.y += roiHaarPupil.y;
    elPupilThresh.center.x += roiHaarPupil.x;
    elPupilThresh.center.y += roiHaarPupil.y;
  }

  out.bbPupilThresh = bbPupilThresh;
  out.elPupilThresh = elPupilThresh;

  // ------------------------------
  // Find edges in new pupil region
  // ------------------------------

  cv::Mat_<uchar> mPupil, mPupilOpened, mPupilBlurred, mPupilEdges;
  cv::Mat_<float> mPupilSobelX, mPupilSobelY;
  cv::Rect bbPupil;
  cv::Rect roiPupil = cvx::roiAround(cv::Point(elPupilThresh.center.x, elPupilThresh.center.y), haarRadius);
  SECTION("Pupil preprocessing", log)
  {
    const int padding = 3;

    cv::Rect roiPadded(roiPupil.x-padding, roiPupil.y-padding, roiPupil.width+2*padding, roiPupil.height+2*padding);
    // First get an ROI around the approximate pupil location
    cvx::getROI(mEye, mPupil, roiPadded, cv::BORDER_REPLICATE);

    cv::Mat morphologyDisk = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5));
    cv::morphologyEx(mPupil, mPupilOpened, cv::MORPH_OPEN, morphologyDisk, cv::Point(-1,-1), 2);

    if (params.CannyBlur > 0)
    {
      cv::GaussianBlur(mPupilOpened, mPupilBlurred, cv::Size(), params.CannyBlur);
    }
    else
    {
      mPupilBlurred = mPupilOpened;
    }

    cv::Sobel(mPupilBlurred, mPupilSobelX, CV_32F, 1, 0, 3);
    cv::Sobel(mPupilBlurred, mPupilSobelY, CV_32F, 0, 1, 3);

    cv::Canny(mPupilBlurred, mPupilEdges, params.CannyThreshold1, params.CannyThreshold2);

    cv::Rect roiUnpadded(padding,padding,roiPupil.width,roiPupil.height);
    mPupil = cv::Mat(mPupil, roiUnpadded);
    mPupilOpened = cv::Mat(mPupilOpened, roiUnpadded);
    mPupilBlurred = cv::Mat(mPupilBlurred, roiUnpadded);
    mPupilSobelX = cv::Mat(mPupilSobelX, roiUnpadded);
    mPupilSobelY = cv::Mat(mPupilSobelY, roiUnpadded);
    mPupilEdges = cv::Mat(mPupilEdges, roiUnpadded);

    bbPupil = cvx::boundingBox(mPupil);
  }

  out.roiPupil = roiPupil;
  out.mPupil = mPupil;
  out.mPupilOpened = mPupilOpened;
  out.mPupilBlurred = mPupilBlurred;
  out.mPupilSobelX = mPupilSobelX;
  out.mPupilSobelY = mPupilSobelY;
  out.mPupilEdges = mPupilEdges;

  // -----------------------------------------------
  // Get points on edges, optionally using starburst
  // -----------------------------------------------

  std::vector<cv::Point2f> edgePoints;

  if (params.StarburstPoints > 0)
  {
    SECTION("Starburst", log)
    {
      // Starburst from initial pupil approximation, stopping when an edge is hit.
      // Collect all edge points into a vector

      // The initial pupil approximations are:
      //    Centre of mass of thresholded region
      //    Halfway along the major axis (calculated form second moments) in each direction

      tbb::concurrent_vector<cv::Point2f> edgePointsConcurrent;

      cv::Vec2f elPupil_majorAxis = cvx::majorAxis(elPupilThresh);
      std::vector<cv::Point2f> centres;
      centres.push_back(elPupilThresh.center - cv::Point2f(roiPupil.tl().x, roiPupil.tl().y));
      centres.push_back(elPupilThresh.center - cv::Point2f(roiPupil.tl().x, roiPupil.tl().y) + cv::Point2f(elPupil_majorAxis));
      centres.push_back(elPupilThresh.center - cv::Point2f(roiPupil.tl().x, roiPupil.tl().y) - cv::Point2f(elPupil_majorAxis));

      BOOST_FOREACH(const cv::Point2f& centre, centres) {
        tbb::parallel_for(0, params.StarburstPoints, [&] (int i) {
          float theta = i * 2*PI/params.StarburstPoints;

          // Initialise centre and direction vector
          cv::Point2f pDir((float)std::cos(theta), (float)std::sin(theta));

          int t = 1;
          cv::Point p = centre + (t * pDir);
          while(p.inside(bbPupil))
          {
            uchar val = mPupilEdges(p);

            if (val > 0)
            {
              float dx = mPupilSobelX(p);
              float dy = mPupilSobelY(p);

              float cdirx = p.x - (elPupilThresh.center.x - roiPupil.x);
              float cdiry = p.y - (elPupilThresh.center.y - roiPupil.y);

              // Check edge direction
              float dirCheck = dx*cdirx + dy*cdiry;

              if (dirCheck > 0)
              {
                // We've hit an edge
                edgePointsConcurrent.push_back(cv::Point2f(p.x + 0.5f, p.y + 0.5f));
                break;
              }
            }

            ++t;
            p = centre + (t * pDir);
          }
        });
      }

      edgePoints = std::vector<cv::Point2f>(edgePointsConcurrent.begin(), edgePointsConcurrent.end());

      // Remove duplicate edge points
      std::sort(edgePoints.begin(), edgePoints.end(), [] (const cv::Point2f& p1, const cv::Point2f& p2) -> bool {
        if (p1.x == p2.x)
        return p1.y < p2.y;
        else
        return p1.x < p2.x;
      });
      edgePoints.erase( std::unique( edgePoints.begin(), edgePoints.end() ), edgePoints.end() );

      if (edgePoints.size() < params.StarburstPoints/2)
      return false;
    }
  }
  else
  {
    SECTION("Non-zero value finder", log)
    {
      for(int y = 0; y < mPupilEdges.rows; y++)
      {
        uchar* val = mPupilEdges[y];
        for(int x = 0; x < mPupilEdges.cols; x++, val++)
        {
          if(*val == 0)
          continue;

          edgePoints.push_back(cv::Point2f(x + 0.5f, y + 0.5f));
        }
      }
    }
  }

  // ---------------------------
  // Fit an ellipse to the edges
  // ---------------------------

  cv::RotatedRect elPupil;
  std::vector<cv::Point2f> inliers;
  SECTION("Ellipse fitting", log)
  {
    // Desired probability that only inliers are selected
    const float p = 0.999;
    // Probability that a point is an inlier
    float w = params.PercentageInliers/100.0;
    // Number of points needed for a model
    const int n = 5;

    if (params.PercentageInliers == 0)
    return false;

    if (edgePoints.size() >= n) // Minimum points for ellipse
    {
      // RANSAC!!!

      float wToN = std::pow(w,n);
      int k = static_cast<int>(std::log(1-p)/std::log(1 - wToN)  + 2*std::sqrt(1 - wToN)/wToN);

      out.ransacIterations = k;

      log.add("k", k);

      //size_t threshold_inlierCount = std::max<size_t>(n, static_cast<size_t>(out.edgePoints.size() * 0.7));

      // Use TBB for RANSAC
      struct EllipseRansac_out {
        std::vector<cv::Point2f> bestInliers;
        cv::RotatedRect bestEllipse;
        float bestEllipseGoodness;
        int earlyRejections;
        bool earlyTermination;

        EllipseRansac_out() : bestEllipseGoodness(-std::numeric_limits<float>::infinity()), earlyTermination(false), earlyRejections(0) {}
      };
      struct EllipseRansac {
        const TrackerParams& params;
        const std::vector<cv::Point2f>& edgePoints;
        int n;
        const cv::Rect& bb;
        const cv::Mat_<float>& mDX;
        const cv::Mat_<float>& mDY;
        int earlyRejections;
        bool earlyTermination;

        EllipseRansac_out out;

        EllipseRansac(
          const TrackerParams& params,
          const std::vector<cv::Point2f>& edgePoints,
          int n,
          const cv::Rect& bb,
          const cv::Mat_<float>& mDX,
          const cv::Mat_<float>& mDY)
          : params(params), edgePoints(edgePoints), n(n), bb(bb), mDX(mDX), mDY(mDY), earlyTermination(false), earlyRejections(0)
          {
          }

          EllipseRansac(EllipseRansac& other, tbb::split)
          : params(other.params), edgePoints(other.edgePoints), n(other.n), bb(other.bb), mDX(other.mDX), mDY(other.mDY), earlyTermination(other.earlyTermination), earlyRejections(other.earlyRejections)
          {
            //std::cout << "Ransac split" << std::endl;
          }

          void operator()(const tbb::blocked_range<size_t>& r)
          {
            if (out.earlyTermination)
            return;
            //std::cout << "Ransac start (" << (r.end()-r.begin()) << " elements)" << std::endl;
            for( size_t i=r.begin(); i!=r.end(); ++i )
            {
              // Ransac Iteration
              // ----------------
              std::vector<cv::Point2f> sample;
              if (params.Seed >= 0)
              sample = randomSubset(edgePoints, n, static_cast<unsigned int>(i + params.Seed));
              else
              sample = randomSubset(edgePoints, n);

              cv::RotatedRect ellipseSampleFit = fitEllipse(sample);
              // Normalise ellipse to have width as the major axis.
              if (ellipseSampleFit.size.height > ellipseSampleFit.size.width)
              {
                ellipseSampleFit.angle = std::fmod(ellipseSampleFit.angle + 90, 180);
                std::swap(ellipseSampleFit.size.height, ellipseSampleFit.size.width);
              }

              cv::Size s = ellipseSampleFit.size;
              // Discard useless ellipses early
              if (!ellipseSampleFit.center.inside(bb)
              || s.height > params.Radius_Max*2
              || s.width > params.Radius_Max*2
              || s.height < params.Radius_Min*2 && s.width < params.Radius_Min*2
              || s.height > 2*s.width
              || s.width > 2*s.height
            )
            {
              // Bad ellipse! Go to your room!
              continue;
            }

            // Use conic section's algebraic distance as an error measure
            ConicSection conicSampleFit(ellipseSampleFit);

            // Check if sample's gradients are correctly oriented
            if (params.EarlyRejection)
            {
              bool gradientCorrect = true;
              BOOST_FOREACH(const cv::Point2f& p, sample)
              {
                cv::Point2f grad = conicSampleFit.algebraicGradientDir(p);
                float dx = mDX(cv::Point(p.x, p.y));
                float dy = mDY(cv::Point(p.x, p.y));

                float dotProd = dx*grad.x + dy*grad.y;

                gradientCorrect &= dotProd > 0;
              }
              if (!gradientCorrect)
              {
                out.earlyRejections++;
                continue;
              }
            }

            // Assume that the sample is the only inliers

            cv::RotatedRect ellipseInlierFit = ellipseSampleFit;
            ConicSection conicInlierFit = conicSampleFit;
            std::vector<cv::Point2f> inliers, prevInliers;

            // Iteratively find inliers, and re-fit the ellipse
            for (int i = 0; i < params.InlierIterations; ++i)
            {
              // Get error scale for 1px out on the minor axis
              cv::Point2f minorAxis(-std::sin(PI/180.0*ellipseInlierFit.angle), std::cos(PI/180.0*ellipseInlierFit.angle));
              cv::Point2f minorAxisPlus1px = ellipseInlierFit.center + (ellipseInlierFit.size.height/2 + 1)*minorAxis;
              float errOf1px = conicInlierFit.distance(minorAxisPlus1px);
              float errorScale = 1.0f/errOf1px;

              // Find inliers
              inliers.reserve(edgePoints.size());
              const float MAX_ERR = 2;
              BOOST_FOREACH(const cv::Point2f& p, edgePoints)
              {
                float err = errorScale*conicInlierFit.distance(p);

                if (err*err < MAX_ERR*MAX_ERR)
                inliers.push_back(p);
              }

              if (inliers.size() < n) {
                inliers.clear();
                continue;
              }

              // Refit ellipse to inliers
              ellipseInlierFit = fitEllipse(inliers);
              conicInlierFit = ConicSection(ellipseInlierFit);

              // Normalise ellipse to have width as the major axis.
              if (ellipseInlierFit.size.height > ellipseInlierFit.size.width)
              {
                ellipseInlierFit.angle = std::fmod(ellipseInlierFit.angle + 90, 180);
                std::swap(ellipseInlierFit.size.height, ellipseInlierFit.size.width);
              }
            }
            if (inliers.empty())
            continue;

            // Discard useless ellipses again
            s = ellipseInlierFit.size;
            if (!ellipseInlierFit.center.inside(bb)
            || s.height > params.Radius_Max*2
            || s.width > params.Radius_Max*2
            || s.height < params.Radius_Min*2 && s.width < params.Radius_Min*2
            || s.height > 2*s.width
            || s.width > 2*s.height
          )
          {
            // Bad ellipse! Go to your room!
            continue;
          }

          // Calculate ellipse goodness
          float ellipseGoodness = 0;
          if (params.ImageAwareSupport)
          {
            BOOST_FOREACH(cv::Point2f& p, inliers)
            {
              cv::Point2f grad = conicInlierFit.algebraicGradientDir(p);
              float dx = mDX(p);
              float dy = mDY(p);

              float edgeStrength = dx*grad.x + dy*grad.y;

              ellipseGoodness += edgeStrength;
            }
          }
          else
          {
            ellipseGoodness = inliers.size();
          }

          if (ellipseGoodness > out.bestEllipseGoodness)
          {
            std::swap(out.bestEllipseGoodness, ellipseGoodness);
            std::swap(out.bestInliers, inliers);
            std::swap(out.bestEllipse, ellipseInlierFit);

            // Early termination, if 90% of points match
            if (params.EarlyTerminationPercentage > 0 && out.bestInliers.size() > params.EarlyTerminationPercentage*edgePoints.size()/100)
            {
              earlyTermination = true;
              break;
            }
          }

        }
        //std::cout << "Ransac end" << std::endl;
      }

      void join(EllipseRansac& other)
      {
        //std::cout << "Ransac join" << std::endl;
        if (other.out.bestEllipseGoodness > out.bestEllipseGoodness)
        {
          std::swap(out.bestEllipseGoodness, other.out.bestEllipseGoodness);
          std::swap(out.bestInliers, other.out.bestInliers);
          std::swap(out.bestEllipse, other.out.bestEllipse);
        }
        out.earlyRejections += other.out.earlyRejections;
        earlyTermination |= other.earlyTermination;

        out.earlyTermination = earlyTermination;
      }
    };

    EllipseRansac ransac(params, edgePoints, n, bbPupil, out.mPupilSobelX, out.mPupilSobelY);
    try
    {
      tbb::parallel_reduce(tbb::blocked_range<size_t>(0,k,k/8), ransac);
    }
    catch (std::exception& e)
    {
      const char* c = e.what();
      std::cerr << e.what() << std::endl;
    }
    inliers = ransac.out.bestInliers;
    log.add("goodness", ransac.out.bestEllipseGoodness);

    out.earlyRejections = ransac.out.earlyRejections;
    out.earlyTermination = ransac.out.earlyTermination;

    cv::RotatedRect ellipseBestFit = ransac.out.bestEllipse;
    ConicSection conicBestFit(ellipseBestFit);
    BOOST_FOREACH(const cv::Point2f& p, edgePoints)
    {
      cv::Point2f grad = conicBestFit.algebraicGradientDir(p);
      float dx = out.mPupilSobelX(p);
      float dy = out.mPupilSobelY(p);

      out.edgePoints.push_back(EdgePoint(p, dx*grad.x + dy*grad.y));
    }

    elPupil = ellipseBestFit;
    elPupil.center.x += roiPupil.x;
    elPupil.center.y += roiPupil.y;
  }

  if (inliers.size() == 0)
  return false;

  cv::Point2f pPupil = elPupil.center;

  out.pPupil = pPupil;
  out.elPupil = elPupil;
  out.inliers = inliers;

  return true;
}

return false;
}


/**
 * Demo program for simulation library
 * A virtual camera generates simulated point clouds
 * No visual output, point clouds saved to file
 *
 * three different demo modes:
 * 0 - static camera, 100 poses
 * 1 - circular camera flying around the scene, 16 poses
 * 2 - camera translates between 2 poses using slerp, 20 poses
 * pcl_sim_terminal_demo 2 ../../../../kmcl/models/table_models/meta_model.ply
 */

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <boost/shared_ptr.hpp>
#ifdef _WIN32
# define WIN32_LEAN_AND_MEAN
# include <windows.h>
#endif

#include <kinect_sim/camera_constants.h>
#include <kinect_sim/simulation_io.hpp>

using namespace Eigen;
using namespace pcl;
using namespace pcl::console;
using namespace pcl::io;
using namespace pcl::simulation;
using namespace std;

SimExample::Ptr simexample;

void printHelp (int argc, char **argv)
{
  print_error ("Syntax is: %s <mode 1,2 or 3> <filename>\n", argv[0]);
  print_info ("acceptable filenames include vtk, obj and ply. ply can support colour\n");
}

// Output the simulated output to file:
void write_sim_output(string fname_root){
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);
  bool write_cloud=true;
  bool demo_other_stuff=true;

  if (write_cloud)
  {
    // Read Color Buffer from the GPU before creating PointCloud:
    // By default the buffers are not read back from the GPU
    simexample->rl_->getColorBuffer ();
    simexample->rl_->getDepthBuffer ();
    // Add noise directly to the CPU depth buffer
    simexample->rl_->addNoise ();

    // Optional argument to save point cloud in global frame:
    // Save camera relative:
    //simexample->rl_->getPointCloud(pc_out);
    // Save in global frame - applying the camera frame:
    //simexample->rl_->getPointCloud(pc_out,true,simexample->camera_->getPose());
    // Save in local frame
    simexample->rl_->getPointCloud (pc_out,false,simexample->camera_->getPose ());
    // TODO: what to do when there are more than one simulated view?

    if (pc_out->points.size()>0){
    std::cout << pc_out->points.size() << " points written to file\n";

    pcl::PCDWriter writer;
    //writer.write ( string (fname_root + ".pcd"), *pc_out,	false);  /// ASCII
    writer.writeBinary (  string (fname_root + ".pcd")  , *pc_out);
    //cout << "finished writing file\n";
    }else{
    std::cout << pc_out->points.size() << " points in cloud, not written\n";
    }
  }
  if (demo_other_stuff && write_cloud)
  {
    //simexample->write_score_image (simexample->rl_->getScoreBuffer (),
    //   		   string (fname_root + "_score.png") );
    simexample->write_rgb_image (simexample->rl_->getColorBuffer (),
				 string (fname_root + "_rgb.png") );
    simexample->write_depth_image (simexample->rl_->getDepthBuffer (),
				    string (fname_root + "_depth.png") );
    //simexample->write_depth_image_uint (simexample->rl_->getDepthBuffer (),
    //                                string (fname_root + "_depth_uint.png") );

    // Demo interacton with RangeImage:
    pcl::RangeImagePlanar rangeImage;
    simexample->rl_->getRangeImagePlanar (rangeImage);
  }
}

// A 'halo' camera - a circular ring of poses all pointing at a center point
// @param: focus_center: the center points
// @param: halo_r: radius of the ring
// @param: halo_dz: elevation of the camera above/below focus_center's z value
// @param: n_poses: number of generated poses
void
generate_halo(
  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > &poses,
  Eigen::Vector3d focus_center,double halo_r,double halo_dz,int n_poses){

  for (double t=0;t < (2*M_PI);t =t + (2*M_PI)/ ((double) n_poses) ){
    double x = halo_r*cos(t);
    double y = halo_r*sin(t);
    double z = halo_dz;
    double pitch =atan2( halo_dz,halo_r);
    double yaw = atan2(-y,-x);

    Eigen::Isometry3d pose;
    pose.setIdentity();
    Eigen::Matrix3d m;
    m = AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
	* AngleAxisd(pitch, Eigen::Vector3d::UnitY())
	* AngleAxisd(0, Eigen::Vector3d::UnitZ());

    pose *=m;
    Vector3d v(x,y,z);
    v += focus_center;
    pose.translation() = v;
    poses.push_back(pose);
  }
  return ;
}

int
main (int argc, char** argv)
{
  // 1. Parse arguments:
  print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);
  if (argc < 3)
  {
    printHelp (argc, argv);
    return (-1);
  }
  int mode=atoi(argv[1]);

  // 2 Construct the simulation method:
  int width = kCameraWidth;
  int height = kCameraHeight;
  simexample = SimExample::Ptr (new SimExample (argc, argv, height,width ));

  // 3 Generate a series of simulation poses:
  // -0 100 fixed poses
  // -1 a 'halo' camera
  // -2 slerp between two different poses
  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
  if (mode==0){
    // Create a pose:
    Eigen::Isometry3d pose;
    pose.setIdentity();
    Matrix3d m;
    //ypr:
    //m = AngleAxisd(-9.14989, Vector3d::UnitZ())     * AngleAxisd(0.20944, Vector3d::UnitY())    * AngleAxisd(0, Vector3d::UnitX());
    m = AngleAxisd(-9.149, Vector3d::UnitZ())     * AngleAxisd(0, Vector3d::UnitY())    * AngleAxisd(0, Vector3d::UnitX());
    pose *= m;
    Vector3d v;
    //v << 1.31762, 0.382931, 1.89533;
    v << 3.31762, 0.082931, 1.89533;
    pose.translation() = v;
    for (int i=0;i< 10;i++){ // duplicate the pose 100 times
      poses.push_back(pose);
    }
  }else if(mode==1){
    //Eigen::Vector3d focus_center(0,0,1.3);
    Eigen::Vector3d focus_center(0,0,0);
    double halo_r = 5;//4
    double halo_dz = 0.1;//2
    int n_poses=16;//16
    generate_halo(poses,focus_center,halo_r,halo_dz,n_poses);
  }else if (mode==2){
    Eigen::Isometry3d pose1;
    pose1.setIdentity();
    //pose1.translation() << 1,0.75,2;
    pose1.translation() << 10,-10,2;
    Eigen::Matrix3d rot1;
    rot1 = AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())
	* AngleAxisd(M_PI/10, Eigen::Vector3d::UnitY())
	* AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
    pose1.rotate(rot1);

    Eigen::Isometry3d pose2;
    pose2.setIdentity();
    pose2.translation() << 1,-1,3;
    Eigen::Matrix3d rot2;
    rot2 = AngleAxisd(3*M_PI/4, Eigen::Vector3d::UnitZ())
	* AngleAxisd(M_PI/4, Eigen::Vector3d::UnitY())
	* AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
    pose2.rotate(rot2);

    int n_poses = 20;
    for (double i=0; i<=1;i+= 1/((double) n_poses -1) ){
      Eigen::Quaterniond rot3;
      Eigen::Quaterniond r1(pose1.rotation());
      Eigen::Quaterniond r2(pose2.rotation());
      rot3 = r1.slerp(i,r2);
      Eigen::Isometry3d pose;
      pose.setIdentity();
      Eigen::Vector3d trans3 = (1-i)*pose1.translation() +  i*pose2.translation();
      pose.translation() <<  trans3[0] ,trans3[1] ,trans3[2];
      pose.rotate(rot3);
      poses.push_back(pose);
    }
  }

  // 4 Do the simulation and write the output:
  double tic_main = getTime();
  for (size_t i=0;i < poses.size();i++){
    std::stringstream ss;
    ss.precision(20);
    ss << "sim_cloud_" << i;// << ".pcd";
    double tic = getTime();
    simexample->doSim(poses[i]);

    write_sim_output(ss.str());
    cout << (getTime() -tic) << " sec\n";
  }
  cout << poses.size() << " poses simulated in " << (getTime() -tic_main) << "seconds\n";
  cout << (poses.size()/ (getTime() -tic_main) ) << "Hz on average\n";

  return 0;
}


//---------------------------------------------------------------------------//
//!
//! \file   Utility_UnitTraits.hpp
//! \author Alex Robinson
//! \brief  Unit traits class specializations
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_UNIT_TRAITS_HPP
#define UTILITY_UNIT_TRAITS_HPP

// Boost Includes
#include <boost/units/make_system.hpp>
#include <boost/units/unit.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/io.hpp>
#include <boost/units/dimensionless_unit.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_same.hpp>

// FRENSIE Includes
#include "Utility_UnitTraitsDecl.hpp"

namespace Utility{

/*! The partial specialization of UnitTraits for boost::units::unit
 * \ingroup unit_traits
 */
template<typename Dim, typename Sys>
struct UnitTraits<boost::units::unit<Dim,Sys> >
{
  typedef boost::units::unit<Dim,Sys> Unit;
  typedef Dim Dimension;
  typedef Sys System;

  typedef typename boost::units::power_typeof_helper<Unit,boost::units::static_rational<-1> >::type InverseUnit;

  template<boost::units::integer_type N, boost::units::integer_type D = 1>
  struct GetUnitToPowerType
  {
    typedef typename boost::units::power_typeof_helper<Unit,boost::units::static_rational<N,D> >::type type;
  };

  template<typename OtherUnit>
  struct GetMultipliedUnitType
  {
    typedef typename boost::mpl::if_<boost::is_same<OtherUnit,void>,Unit,typename boost::units::multiply_typeof_helper<Unit,OtherUnit>::type>::type type;
  };

  template<typename T>
  struct GetQuantityType
  { typedef boost::units::quantity<Unit,T> type; };

  static inline std::string name()
  { return boost::units::name_string( Unit() ); }

  static inline std::string symbol()
  { return boost::units::symbol_string( Unit() ); }
};

/*! The specialization of UnitTraits for void (no units)
 *
 * Note that void (no units) is different from dimensionless, which has a
 * distinct boost::units::unit class.
 * \ingroup unit_traits
 */
template<>
struct UnitTraits<void>
{
  //! The dimensionless unit
  //typedef boost::units::dimensionless_unit<boost::units::make_system<>::type>::type UnitType;
  typedef void Unit;
  typedef void Dimension;
  typedef void System;
  typedef void InverseUnit;

  template<boost::units::integer_type N, boost::units::integer_type D = 1>
  struct GetUnitToPowerType
  {
    typedef void type;
  };

  template<typename OtherUnit>
  struct GetMultipliedUnitType
  {
    typedef OtherUnit type;
  };

  template<typename T>
  struct GetQuantityType
  { typedef T type; };

  static inline std::string name()
  { return "void"; }

  static inline std::string symbol()
  { return "0"; }
};

} // end Utility namespace

#endif // end UTILITY_UNIT_TRAITS_HPP

//---------------------------------------------------------------------------//
// end Utility_UnitTraits.hpp
//---------------------------------------------------------------------------//


#include <boost/numeric/interval/arith2.hpp>


#include <stdio.h>
#include <iostream>
#include <boost/shared_ptr.hpp>
#include <lcm/lcm-cpp.hpp>
#include "particle_filter.hpp"

using namespace Eigen;
using namespace std;
using namespace boost;

class StatePub{
  public:
  StatePub(boost::shared_ptr<lcm::LCM> &lcm_);
  ~StatePub() {}
  boost::shared_ptr<lcm::LCM> lcm_;

  void demoParticleFilter();
  private:
  ParticleFilter* pf;
};

StatePub::StatePub(boost::shared_ptr<lcm::LCM> &lcm_): lcm_(lcm_){
}

void StatePub::demoParticleFilter(){
  int64_t current_timestamp =0;

  int rng_seed = 1;
  double resample_threshold =0.5;
  int N_p =1000;
  std::vector <double> initial_var;
  initial_var.push_back(0.01);
  initial_var.push_back(0.01);
  initial_var.push_back(0.001);
  initial_var.push_back(0.001);

  double elapsed_time = 0.1;
  std::vector <double> success_var;
  success_var.push_back(0.0001);
  success_var.push_back(0.0001);
  success_var.push_back(0.000001);
  success_var.push_back(0.000001);

  std::vector <double> failure_var;
  failure_var.push_back(0.001);
  failure_var.push_back(0.001);
  failure_var.push_back(0.0001);
  failure_var.push_back(0.0001);

  Eigen::Isometry3d init_pose;
  init_pose.setIdentity();
  pf = new ParticleFilter(lcm_->getUnderlyingLCM(), N_p,init_pose,
            initial_var, rng_seed,resample_threshold);

  for (size_t i=0;i < 100; i++){
    pf_state odom_diff;
    odom_diff.pose.setIdentity();
    odom_diff.velocity.setIdentity();
    if (i < 50){
      std::cout << "Successful VO\n";
      odom_diff.pose.translation().x() = 0.1;
      pf->MoveParticles(odom_diff,success_var,elapsed_time,0); //success
    }else{
      std::cout << "Drifting VO\n";
      pf->MoveParticles(odom_diff,failure_var,elapsed_time,1); //failed motion estimation
    }

    std::vector<float> loglikelihoods;
    loglikelihoods.assign (N_p,0);
    pf->LogLikelihoodParticles(loglikelihoods);

    pf->SendParticlesLCM(current_timestamp,0);//vo_estimate_status);

    double ESS;
    ESS= pf->ConsiderResample();
    std::cerr << i << ": " << ESS/N_p << " is ESS | " << current_timestamp << " iteration\n";
    double fraction_sleep =0.1;
    usleep( 1000.0*1000.0*fraction_sleep);
  }
}

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

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good())
    return -1;

  StatePub app(lcm);
  app.demoParticleFilter();
  return 0;
}


#include <iostream>                    // std::cout
#include <fstream>                     // std::ifstream, std::ofstream
#include <filesystem>                  // std::filesystem
#include <string>                      // std::string
#include <sstream>                     // std::stringstream
#include <tuple>                       // std::tuple
#include <vector>                      // std::vector
#include <utility>                     // std::pair
#include <iomanip>                     // std::setprecision
// For External Library
#include <torch/torch.h>               // torch
#include <opencv2/opencv.hpp>          // cv::Mat
#include <boost/program_options.hpp>   // boost::program_options
// For Original Header
#include "networks.hpp"                // YOLOv2
#include "detector.hpp"                // YOLODetector
#include "transforms.hpp"              // transforms_Compose
#include "datasets.hpp"                // datasets::ImageFolderPairWithPaths
#include "dataloader.hpp"              // DataLoader::ImageFolderPairWithPaths
#include "visualizer.hpp"              // visualizer

// Define Namespace
namespace fs = std::filesystem;
namespace po = boost::program_options;

// --------------------
// Detection Function
// --------------------
void detect(po::variables_map &vm, torch::Device &device, YOLOv2 &model, std::vector<transforms_Compose> &transformI, std::vector<transforms_Compose> &transformD, const std::vector<std::string> class_names, const std::vector<std::tuple<float, float>> anchors){

    constexpr std::pair<float, float> output_range = {0.0, 1.0};  // range of the value in output images

    // (0) Initialization and Declaration
    size_t BB_n;
    float prob;
    std::string path, result_dir, fname;
    std::string dataroot;
    std::string class_name;
    std::stringstream ss;
    std::ofstream ofs;
    std::tuple<torch::Tensor, torch::Tensor, std::vector<std::string>, std::vector<std::string>> data;
    torch::Tensor imageI, imageD, output;
    torch::Tensor ids, coords, probs;
    std::tuple<torch::Tensor, torch::Tensor, torch::Tensor> detect_result;
    cv::Mat imageO;
    datasets::ImageFolderPairWithPaths dataset;
    DataLoader::ImageFolderPairWithPaths dataloader;

    // (1) Get Detection Dataset
    dataroot = "datasets/" + vm["dataset"].as<std::string>() + '/' + vm["detect_dir"].as<std::string>();
    dataset = datasets::ImageFolderPairWithPaths(dataroot, dataroot, transformI, transformD);
    dataloader = DataLoader::ImageFolderPairWithPaths(dataset, /*batch_size_=*/1, /*shuffle_=*/false, /*num_workers_=*/0);
    std::cout << "total detect images : " << dataset.size() << std::endl << std::endl;

    // (2) Get Model
    path = "checkpoints/" + vm["dataset"].as<std::string>() + "/models/epoch_" + vm["detect_load_epoch"].as<std::string>() + ".pth";
    torch::load(model, path, device);

    // (3) Set Detector
    auto detector = YOLODetector(anchors, (long int)vm["class_num"].as<size_t>(), vm["prob_thresh"].as<float>(), vm["nms_thresh"].as<float>());
    std::vector<std::tuple<unsigned char, unsigned char, unsigned char>> label_palette = detector.get_label_palette();

    // (4) Tensor Forward
    model->eval();
    result_dir = vm["detect_result_dir"].as<std::string>();  fs::create_directories(result_dir);
    ofs.open(result_dir + "/detect.txt", std::ios::out);
    while (dataloader(data)){

        // (4.1) Get data
        imageI = std::get<0>(data).to(device);  // {1,C,H,W} (image for input)
        imageD = std::get<1>(data);  // {1,3,H_D,W_D} (image for detection)

        // (4.2) Inference and Detection
        output = model->forward(imageI);  // {1,C,H,W} ===> {1,G,G,FF}
        detect_result = detector(output[0]);  // output[0]{G,G,FF} ===> detect_result{ (ids{BB_n}, coords{BB_n,4}, probs{BB_n}) }
        ids = std::get<0>(detect_result);  // ids{BB_n}
        coords = std::get<1>(detect_result);  // coords{BB_n,4}
        probs = std::get<2>(detect_result);  // probs{BB_n}
        imageO = visualizer::draw_detections_des(imageD[0].detach(), {ids, coords}, probs, class_names, label_palette, /*range=*/output_range);

        // (4.3) Save image
        fname = result_dir + '/' + std::get<2>(data).at(0);  // {1,C,H,W} ===> {1,G,G,FF}
        cv::imwrite(fname, imageO);

        // (4.4) Write detection result
        BB_n = ids.size(0);
        std::cout << '<' << std::get<2>(data).at(0) << "> " << BB_n << " { " << std::flush;
        ofs << '<' << std::get<2>(data).at(0) << "> " << BB_n << " { " << std::flush;
        for (size_t i = 0; i < BB_n; i++){
            class_name = class_names.at(ids[i].item<long int>());
            prob = probs[i].item<float>() * 100.0;
            ss.str(""); ss.clear(std::stringstream::goodbit);
            ss << std::fixed << std::setprecision(1) << prob;
            std::cout << class_name << ":" << ss.str() << "% " << std::flush;
            ofs << class_name << ":" << ss.str() << "% " << std::flush;
        }
        std::cout << "}" << std::endl;
        ofs << "}" << std::endl;

    }

    // Post Processing
    ofs.close();

    // End Processing
    return;

}