/**
 * @file methods/ann/layer/layer_types.hpp
 * @author Marcus Edel
 *
 * This provides a list of all modules that can be used to construct a model.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_ANN_LAYER_LAYER_TYPES_HPP
#define MLPACK_METHODS_ANN_LAYER_LAYER_TYPES_HPP

#include <boost/variant.hpp>

// Layer modules.
#include <mlpack/methods/ann/layer/add.hpp>
#include <mlpack/methods/ann/layer/alpha_dropout.hpp>
#include <mlpack/methods/ann/layer/base_layer.hpp>
#include <mlpack/methods/ann/layer/batch_norm.hpp>
#include <mlpack/methods/ann/layer/bilinear_interpolation.hpp>
#include <mlpack/methods/ann/layer/constant.hpp>
#include <mlpack/methods/ann/layer/concatenate.hpp>
#include <mlpack/methods/ann/layer/dropout.hpp>
#include <mlpack/methods/ann/layer/elu.hpp>
#include <mlpack/methods/ann/layer/hard_tanh.hpp>
#include <mlpack/methods/ann/layer/join.hpp>
#include <mlpack/methods/ann/layer/layer_norm.hpp>
#include <mlpack/methods/ann/layer/leaky_relu.hpp>
#include <mlpack/methods/ann/layer/c_relu.hpp>
#include <mlpack/methods/ann/layer/flexible_relu.hpp>
#include <mlpack/methods/ann/layer/linear_no_bias.hpp>
#include <mlpack/methods/ann/layer/log_softmax.hpp>
#include <mlpack/methods/ann/layer/lookup.hpp>
#include <mlpack/methods/ann/layer/multiply_constant.hpp>
#include <mlpack/methods/ann/layer/max_pooling.hpp>
#include <mlpack/methods/ann/layer/mean_pooling.hpp>
#include <mlpack/methods/ann/layer/noisylinear.hpp>
#include <mlpack/methods/ann/layer/adaptive_max_pooling.hpp>
#include <mlpack/methods/ann/layer/adaptive_mean_pooling.hpp>
#include <mlpack/methods/ann/layer/parametric_relu.hpp>
#include <mlpack/methods/ann/layer/reinforce_normal.hpp>
#include <mlpack/methods/ann/layer/reparametrization.hpp>
#include <mlpack/methods/ann/layer/select.hpp>
#include <mlpack/methods/ann/layer/softmax.hpp>
#include <mlpack/methods/ann/layer/subview.hpp>
#include <mlpack/methods/ann/layer/virtual_batch_norm.hpp>
#include <mlpack/methods/ann/layer/hardshrink.hpp>
#include <mlpack/methods/ann/layer/celu.hpp>
#include <mlpack/methods/ann/layer/softshrink.hpp>
#include <mlpack/methods/ann/layer/radial_basis_function.hpp>

// Convolution modules.
#include <mlpack/methods/ann/convolution_rules/border_modes.hpp>
#include <mlpack/methods/ann/convolution_rules/naive_convolution.hpp>
#include <mlpack/methods/ann/convolution_rules/fft_convolution.hpp>

// Regularizers.
#include <mlpack/methods/ann/regularizer/no_regularizer.hpp>

// Loss function modules.
#include <mlpack/methods/ann/loss_functions/negative_log_likelihood.hpp>

namespace mlpack {
namespace ann {

template<typename InputDataType, typename OutputDataType> class BatchNorm;
template<typename InputDataType, typename OutputDataType> class DropConnect;
template<typename InputDataType, typename OutputDataType> class Glimpse;
template<typename InputDataType, typename OutputDataType> class LayerNorm;
template<typename InputDataType, typename OutputDataType> class LSTM;
template<typename InputDataType, typename OutputDataType> class GRU;
template<typename InputDataType, typename OutputDataType> class FastLSTM;
template<typename InputDataType, typename OutputDataType> class VRClassReward;
template<typename InputDataType, typename OutputDataType> class Concatenate;
template<typename InputDataType, typename OutputDataType> class Padding;

template<typename InputDataType,
         typename OutputDataType,
         typename RegularizerType>
class Linear;

template<typename InputDataType,
         typename OutputDataType,
         typename Activation>
class RBF;

template<typename InputDataType,
         typename OutputDataType,
         typename RegularizerType>
class LinearNoBias;

template<typename InputDataType,
         typename OutputDataType>
class NoisyLinear;

template<typename InputDataType,
         typename OutputDataType
>
class VirtualBatchNorm;

template<typename InputDataType,
         typename OutputDataType
>
class MiniBatchDiscrimination;

template<typename InputDataType,
         typename OutputDataType
>
class Reparametrization;

template<typename InputDataType,
         typename OutputDataType,
         typename... CustomLayers
>
class AddMerge;

template<typename InputDataType,
         typename OutputDataType,
         bool residual,
         typename... CustomLayers
>
class Sequential;

template<typename InputDataType,
         typename OutputDataType,
         typename... CustomLayers
>
class Highway;

template<typename InputDataType,
         typename OutputDataType,
         typename... CustomLayers
>
class Recurrent;

template<typename InputDataType,
         typename OutputDataType,
         typename... CustomLayers
>
class Concat;

template<
    typename OutputLayerType,
    typename InputDataType,
    typename OutputDataType
>
class ConcatPerformance;

template<
    typename ForwardConvolutionRule,
    typename BackwardConvolutionRule,
    typename GradientConvolutionRule,
    typename InputDataType,
    typename OutputDataType
>
class Convolution;

template<
    typename ForwardConvolutionRule,
    typename BackwardConvolutionRule,
    typename GradientConvolutionRule,
    typename InputDataType,
    typename OutputDataType
>
class TransposedConvolution;

template<
    typename ForwardConvolutionRule,
    typename BackwardConvolutionRule,
    typename GradientConvolutionRule,
    typename InputDataType,
    typename OutputDataType
>
class AtrousConvolution;

template<
    typename InputDataType,
    typename OutputDataType
>
class RecurrentAttention;

template<typename InputDataType,
         typename OutputDataType,
         typename... CustomLayers
>
class MultiplyMerge;

template <typename InputDataType,
          typename OutputDataType,
          typename... CustomLayers
>
class WeightNorm;

template <typename InputDataType,
          typename OutputDataType
>
class AdaptiveMaxPooling;

template <typename InputDataType,
          typename OutputDataType
>
class AdaptiveMeanPooling;

using MoreTypes = boost::variant<
        Glimpse<arma::mat, arma::mat>*,
        Highway<arma::mat, arma::mat>*,
        Recurrent<arma::mat, arma::mat>*,
        RecurrentAttention<arma::mat, arma::mat>*,
        ReinforceNormal<arma::mat, arma::mat>*,
        Reparametrization<arma::mat, arma::mat>*,
        Select<arma::mat, arma::mat>*,
        Sequential<arma::mat, arma::mat, false>*,
        Sequential<arma::mat, arma::mat, true>*,
        Subview<arma::mat, arma::mat>*,
        VRClassReward<arma::mat, arma::mat>*,
        VirtualBatchNorm<arma::mat, arma::mat>*,
        RBF<arma::mat, arma::mat, GaussianFunction>*,
        BaseLayer<GaussianFunction, arma::mat, arma::mat>*
>;

template <typename... CustomLayers>
using LayerTypes = boost::variant<
    AdaptiveMaxPooling<arma::mat, arma::mat>*,
    AdaptiveMeanPooling<arma::mat, arma::mat>*,
    Add<arma::mat, arma::mat>*,
    AddMerge<arma::mat, arma::mat>*,
    AlphaDropout<arma::mat, arma::mat>*,
    AtrousConvolution<NaiveConvolution<ValidConvolution>,
                      NaiveConvolution<FullConvolution>,
                      NaiveConvolution<ValidConvolution>,
                      arma::mat, arma::mat>*,
    BaseLayer<LogisticFunction, arma::mat, arma::mat>*,
    BaseLayer<IdentityFunction, arma::mat, arma::mat>*,
    BaseLayer<TanhFunction, arma::mat, arma::mat>*,
    BaseLayer<SoftplusFunction, arma::mat, arma::mat>*,
    BaseLayer<RectifierFunction, arma::mat, arma::mat>*,
    BatchNorm<arma::mat, arma::mat>*,
    BilinearInterpolation<arma::mat, arma::mat>*,
    CELU<arma::mat, arma::mat>*,
    Concat<arma::mat, arma::mat>*,
    Concatenate<arma::mat, arma::mat>*,
    ConcatPerformance<NegativeLogLikelihood<arma::mat, arma::mat>,
                      arma::mat, arma::mat>*,
    Constant<arma::mat, arma::mat>*,
    Convolution<NaiveConvolution<ValidConvolution>,
                NaiveConvolution<FullConvolution>,
                NaiveConvolution<ValidConvolution>, arma::mat, arma::mat>*,
    CReLU<arma::mat, arma::mat>*,
    DropConnect<arma::mat, arma::mat>*,
    Dropout<arma::mat, arma::mat>*,
    ELU<arma::mat, arma::mat>*,
    FastLSTM<arma::mat, arma::mat>*,
    FlexibleReLU<arma::mat, arma::mat>*,
    GRU<arma::mat, arma::mat>*,
    HardTanH<arma::mat, arma::mat>*,
    Join<arma::mat, arma::mat>*,
    LayerNorm<arma::mat, arma::mat>*,
    LeakyReLU<arma::mat, arma::mat>*,
    Linear<arma::mat, arma::mat, NoRegularizer>*,
    LinearNoBias<arma::mat, arma::mat, NoRegularizer>*,
    LogSoftMax<arma::mat, arma::mat>*,
    Lookup<arma::mat, arma::mat>*,
    LSTM<arma::mat, arma::mat>*,
    MaxPooling<arma::mat, arma::mat>*,
    MeanPooling<arma::mat, arma::mat>*,
    MiniBatchDiscrimination<arma::mat, arma::mat>*,
    MultiplyConstant<arma::mat, arma::mat>*,
    MultiplyMerge<arma::mat, arma::mat>*,
    NegativeLogLikelihood<arma::mat, arma::mat>*,
    NoisyLinear<arma::mat, arma::mat>*,
    Padding<arma::mat, arma::mat>*,
    PReLU<arma::mat, arma::mat>*,
    Softmax<arma::mat, arma::mat>*,
    TransposedConvolution<NaiveConvolution<ValidConvolution>,
            NaiveConvolution<ValidConvolution>,
            NaiveConvolution<ValidConvolution>, arma::mat, arma::mat>*,
    WeightNorm<arma::mat, arma::mat>*,
    MoreTypes,
    CustomLayers*...
>;

} // namespace ann
} // namespace mlpack

#endif


/**
 * \file   verifier.hpp
 * \class  Verifier
 *
 * \author Lucas Brown
 * \date   April 9, 2019
 *
 * \brief  LMBTODO
 */

#include <ppl.hh>
#include <stack>
#include <vector>
#include <Eigen/Dense>

#include "annotation.hpp"
#include "rep-point.hpp"
#include "simulator.hpp"
#include "gstar-reach-tube.hpp"
#include "generalized-star.hpp"

using namespace std;

class Verifier
{
public:
  Verifier(Simulator simu, Annotation annotation);
  ~Verifier();

  int verify();
  void generateCoverStack();
  void rewriteSimulationOutput(ReachTube &simulation_tube);
  void bloatReachTube(ReachTube &simulation_tube,
                      std::vector<double> ref_delta_array, RepPoint curItrRepPoint);
  void checkInvariantsGuards(ReachTube &simulation_tube,
                             ReachTube &guard_set, std::vector<int> &index_in_unsafe_set);
  void refine(RepPoint rep_point, int dim_index);

  void setNumUnsafeEqns(int val);
  void setUnsafeMatrix(std::vector<double> vec);

  std::stack<RepPoint> getCoverStack();

  int ComputeLdf(vector<double> delta, int CT_step, int state);

  void setModeLinear(std::vector<int> vec); // LMBTODO: Band-aid
  void setKConsts(std::vector<double> vec);  // LMBTODO: Band-aid

  void setRefineStrat(int val);
  void setRefineOrder(std::vector<int> vec);

  void setLinearStrat(int val);

private:
  int num_unsafe_equations; // LMBTODO Replace this with a size function
                            //         Not sure if there's a reason we can't
  std::vector<double> unsafe_matrix;
  std::stack<RepPoint> cover_stack;
  Annotation annotation;

  void *lib;
  typedef std::vector<std::pair<std::NNC_Polyhedron, int>> (*guard_fn)(int, double *, double *);
  typedef bool (*inv_fn)(int, double *, double *);
  guard_fn guards;
  inv_fn invs;

  void *libJaThin;
  typedef std::vector<double> (*jcalc_fn)(vector<double>, int);
  typedef void (*thin_fn)(int, vector<double> &, vector<double> &, vector<double> &, vector<double> &);
  jcalc_fn jcalc;
  thin_fn thinHandle;

  Simulator simulator;
  Checker checker;
  int dimensions;
  LinearSet unsafe_set;
  LinearSet initial_set;
  int initial_mode_idx;
  std::vector<int> mode_linear;
  std::vector<double> k_consts;
  std::string visualize_filename;

  int refine_strat;
  vector<int> refine_order;

  int linear_strat; // linear strategy choosed by user, 0 for gstar, 1 for rectangle
};


/*******************************************************************************
 * reservoir/stats.hpp
 *
 * Statistics helpers
 *
 * Copyright (C) 2019 Lorenz Hübschle-Schneider <lorenz@4z2.de>
 *
 * All rights reserved. Published under the GNU General Public License 3
 ******************************************************************************/

#pragma once
#ifndef RESERVOIR_STATS_HEADER
#define RESERVOIR_STATS_HEADER

#include <reservoir/aggregate.hpp>

#include <tlx/die/core.hpp>

#include <boost/serialization/unordered_map.hpp>

#include <algorithm>
#include <ostream>
#include <string>
#include <unordered_map>
#include <vector>

namespace reservoir::_detail {

template <bool enabled>
struct res_stats;

template <>
struct res_stats<false> {
    void record(const std::string & /* key */, double /* value */) {}

    double get_throughput() const {
        return 0;
    }

    res_stats &operator+=(const res_stats & /* other */) {
        return *this;
    }

    template <typename Archive>
    void serialize(Archive & /* ar */, const unsigned int /* version */) {}

    friend std::ostream &operator<<(std::ostream &os, const res_stats &) {
        return os << "<no reservoir stats>";
    }
};

template <>
struct res_stats<true> {
    void record(const std::string &key, double value) {
        auto it = stats.find(key);
        if (it == stats.end()) {
            keyseq.push_back(key);
            maxlen = std::max(key.length(), maxlen);
            stats[key].add(value);
        } else {
            it->second.add(value);
        }
    }

    double get_throughput() const {
        auto it = stats.find("total");
        if (it != stats.end()) {
            return 1000.0 / it->second.avg();
        } else {
            return 0.0;
        }
    }

    res_stats &operator+=(const res_stats &other) {
        if (keyseq.empty()) {
            tlx_die_unless(stats.empty());
            stats = other.stats;
            keyseq = other.keyseq;
        } else {
            for (const auto &[key, agg] : other.stats) {
                stats[key] += agg;
            }
        }
        return *this;
    }

    bool has_key(const std::string &key) const {
        return stats.find(key) != stats.end();
    }

    const tlx::Aggregate<double>& operator[](const std::string &key) const {
        return stats.at(key);
    }

    template <typename Archive>
    void serialize(Archive &ar, const unsigned int /* version */) {
        // in our case, we don't need to send the keyseq, it's the same
        // everywhere and "stolen" on demand, see below
        ar &stats;
    }

    void steal_metadata(const res_stats &other) {
        keyseq = other.keyseq;
        maxlen = other.maxlen;
    }

    friend std::ostream &operator<<(std::ostream &os, const res_stats &s) {
        bool first = true;
        for (const auto &key : s.keyseq) {
            os << (first ? "\t" : "\n\t") << key << ": ";
            for (size_t i = key.length(); i < s.maxlen; i++) {
                os << ' ';
            }
            os << s.stats.at(key);
            first = false;
        }
        return os;
    }

    std::unordered_map<std::string, tlx::Aggregate<double>> stats;
    std::vector<std::string> keyseq; // to preserve order of keys
    size_t maxlen = 0; // length of longest key
};

} // namespace reservoir::_detail

#endif // RESERVOIR_STATS_HEADER


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the cosine of the input in radians.

    @par Header <boost/simd/function/cos.hpp>

    @par Notes
      The regular call to this functor is `cos(x)`,  but
    @c cos can also be called with two parameters as
      `cos(x, range_)` or with a decorator as `std_(cos)(x)` or `restricted_(cos)(x)`

    @c range_ is a tag that allows some control on the computation
      accuracy and speed.

      The control is on the reduction routine of the angle to the
      \f$[-\pi/4, \pi/4]\f$ interval.

      They actually are 3 reduction routines that respectly are
      sufficient for small_, medium_ and big_ angle values respecttively,
      to have (within
      cover test) one ulp of difference with the according crlibm
      (correctly rounded math library) result.

      Each tag covers respectively intervals \f$[-A, A]\f$ with :

        |             |   float  A       |  double  A     |
        |:-----------:|:----------------:|:--------------:|
        |    small_   |    \f$20\pi\f$   | \f$20\pi\f$    |
        |    medium_  |   \f$2^6\pi\f$   | \f$2^{18}\pi\f$|
        |     big_    |   \f$\infty\f$   | \f$\infty\f$   |

       In fact for each scalar singleton or simd vector of angles
       there are two possibilities :
       \arg one is to test if all vector element(s) are in the proper range
       for the consecutive increasing values of A until we reach a good
       one or the last :
       the corresponding template tags are @c small_, @c medium_ and @c big_
       \arg the second is to force directly a reduction method:
       the corresponding template tags are @c direct_small_, @c direct_medium_
       and @c direct_big_

       @par
       direct_small_ is NOT equivalent to small_ because there are also
       two other methods for \f$[0, \pi/4]\f$ (no reduction)
       and \f$[\pi/4, \pi/2]\f$
       (straight reduction) that are not considered in direct small_

       Note that for float the direct_big_ case is both early an hyper costly and
       shall be avoided whenever possible. To partially achieve this aim
       when double are available on the platform, this part of reduction
       is delegated to the double precision routines.

    @par Advices
       \arg If there is no restrictions ever on your angles and you care for precision
       use the default @c cos(x) or equivalently `cos(x, big_)`.
       \arg if you do not care for precision you can use
       `cos(x, medium_)` or `cos(x, small_)`
       that will be accurate for their proper range and degrade in accuracy
       with greater values.

       @par
       Now, the choice of direct or not relies on probabilities
       computations:
       assuming that a vector contains k elements and that testing all
       values that are in an interval takes c cycles and the probability of a value
       to be in interval \f$[a, b]\f$ is \f$p(a, b)\f$
       the number of cycles used by a
       direct\f${}_i\f$ method is simply the reduction time:
       \f$N(\f$direct\f${}_i)\f$
       On the other side the number of cycles for the non-direct methods will have a more
       complicated expression :

       \f$\hspace{5em}\sum_{i=1}^{m} p(A_{i-1}, A_i)^k N(\f$direct\f${}_i)\f$

       @par
       So the non direct methods will be interesting only if you want accurate
       results everywhere and have anyhow a big proportion of small angles.
       This is even more true (if possible) in simd and the more k is big, because
       of the kth power.

       @par
       For instance in the medium_ float case:

       \arg if angles are equidistributed  on \f$[0, 2^{16} \pi]\f$,
       the \f$p(0, 20\pi)\f$ will be
       less than \f$2^{-11}\f$ and thus (for example) if \f$k=4\f$,
       there will be 1 quadruple over 1.76e+13 falling in the small_ case...

       \arg Even sorting will do no good because the sort cost will be against
       the ratio of 1 successful quadruplet over 2048.
       \arg  Contrarily if your angles have a gaussian distribution with 0 mean and
       \f$10\pi\f$ standard deviation,  80% of the intervals will be in the "small_"
       case (95% of the values).
       \arg Finally for those that are sure of their angles taking place in a fixed
       range and want speed, three other template tags can be of choice as they use
       the chosen reduction, but return Nan for any outsider.

         `clipped_very_small_`, `clipped_small_` and `clipped_medium_`

    @par Decorators

       - std_ provides access to std::cos

       - restricted_ is equivalent to the clipped_very_small_ tag

    @see  sincos, cosd, cospi

    @par Example:

      @snippet cos.cpp cos

    @par Possible output:

      @snippet cos.txt cos

  **/
  IEEEValue cos(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/cos.hpp>
#include <boost/simd/function/simd/cos.hpp>

#endif


#include <iostream>

#include "ObjectTracker.h"

#include <opencv_workbench/wb/BlobProcess.h>
#include <opencv_workbench/wb/WB.h>
#include <opencv_workbench/wb/Parameters.h>
#include <opencv_workbench/utils/OpenCV_Helpers.h>
#include <opencv_workbench/syllo/syllo.h>

#include <Eigen/Dense>

using std::cout;
using std::endl;

ObjectTracker::ObjectTracker()
{
     next_id_ = 1;
     covar_tracker_.init(2,2);

     left_side_sign_ = 1;
}

int ObjectTracker::next_available_id()
{
     return next_id_++;
}

void ObjectTracker::process_frame(cv::Mat &src, cv::Mat &dst,
                                  std::vector<wb::Blob> &meas,
                                  Parameters *params)
{
     cv::Mat img = src.clone();
     cv::cvtColor(img,img,CV_GRAY2BGR);

     tracks_.clear();

     // Predict each KF
     for (std::vector<wb::Blob>::iterator it_prev = prev_tracks_.begin();
          it_prev != prev_tracks_.end(); it_prev++) {
          it_prev->predict_tracker();
     }

     std::vector<wb::Blob*> intermediate;

     // Do any of the new blobs match previous tracks. Only count blobs that
     // are of a certain age.
     // Key<int>     : ID of previous track
     // Value <list> : Measurement blob pointers that match track ID
     std::map<int, std::list<wb::Blob*> > track_matches;

     wb::Blob left_blob;
     wb::Blob right_blob;

     for (std::vector<wb::Blob>::iterator it_meas = meas.begin();
          it_meas != meas.end(); it_meas++) {

          // Don't use blobs unless they are of a certain age or not occluded
          //if (it_meas->age() < 5 || it_meas->occluded()) {
          //if (it_meas->age() < 2 || it_meas->occluded()) {
          if (it_meas->occluded()) {
               continue;
          }

          bool matched = false;
          for (std::vector<wb::Blob>::iterator it_prev = prev_tracks_.begin();
               it_prev != prev_tracks_.end(); it_prev++) {

               // Do any of the blob's points fall within 3-sigma region of the
               // track?
               for (std::vector<wb::Point>::iterator it_point = it_meas->points().begin();
                    it_point != it_meas->points().end(); it_point++) {

                    if (it_prev->pixel_tracker().is_within_region(cv::Point2d(it_point->x(),it_point->y()),0.9973)) {
                         track_matches[it_prev->id()].push_back(&(*it_meas));
                         matched = true;
                         break;
                    }
               }
          }

          // If the measurement doesn't fall within any previous track,
          // initiate a new one.
          if (!matched) {
               int id = next_available_id();
               it_meas->new_track(id);
               it_meas->pixel_tracker().set_R(1000,0,0,1000);
               it_meas->pixel_tracker().set_P(100);
               it_meas->pixel_tracker().set_Q(10);
               it_meas->set_dead_occluded_age(16);
               intermediate.push_back(&(*it_meas));
          }
     }

     // For each track ID with matches, use kalman filter to integrate position
     for (std::vector<wb::Blob>::iterator it_prev = prev_tracks_.begin();
          it_prev != prev_tracks_.end(); it_prev++) {

          if (track_matches.count(it_prev->id()) > 0) {

               //// If the left and right fin detectors are strong tracks, add
               //// them to the measurements.
               //double left_P_norm = left_tracker_.P().norm();
               //double right_P_norm = right_tracker_.P().norm();
               //
               //if (left_P_norm < params->covar_threshold &&
               //    right_P_norm < params->covar_threshold) {
               //
               //     // Transform left and right trackers to diver object's
               //     // frame of reference
               //     cv::Point v = it_prev->estimated_pixel_velocity();
               //     cv::Vec2d vel2d(v.x, v.y);
               //     cv::Vec2d vel_unit = vel2d / sqrt(pow(vel2d[0],2) + pow(vel2d[1],2));
               //
               //     {
               //          left_blob.set_age(1);
               //          left_blob.set_id(-1);
               //
               //          // Get relative position to diver's centroid:
               //          // Rotate point based on object's velocity
               //          cv::Point2d rel_p = left_tracker_.position();
               //          double theta = atan2(vel_unit[1], vel_unit[0]);
               //
               //          cv::Point2d rel_rot_p;
               //          rel_rot_p.x = cos(theta) * rel_p.x - sin(theta) * rel_p.y;
               //          rel_rot_p.y = sin(theta) * rel_p.x + cos(theta) * rel_p.y;
               //
               //          cv::Point2d pos;
               //          pos.x = rel_rot_p.x + it_prev->estimated_pixel_centroid().x;
               //          pos.y = rel_rot_p.y + it_prev->estimated_pixel_centroid().y;
               //
               //          left_blob.set_estimated_pixel_centroid(pos);
               //     }
               //
               //     {
               //          right_blob.set_age(1);
               //          right_blob.set_id(-1);
               //
               //          // Get relative position to diver's centroid:
               //          // Rotate point based on object's velocity
               //          cv::Point2d rel_p = right_tracker_.position();
               //          double theta = atan2(vel_unit[1], vel_unit[0]);
               //
               //          cv::Point2d rel_rot_p;
               //          rel_rot_p.x = cos(theta) * rel_p.x - sin(theta) * rel_p.y;
               //          rel_rot_p.y = sin(theta) * rel_p.x + cos(theta) * rel_p.y;
               //
               //          cv::Point2d pos;
               //          pos.x = rel_rot_p.x + it_prev->estimated_pixel_centroid().x;
               //          pos.y = rel_rot_p.y + it_prev->estimated_pixel_centroid().y;
               //
               //          right_blob.set_estimated_pixel_centroid(pos);
               //     }
               //
               //     cout << "Using right and left" << endl;
               //
               //     track_matches[it_prev->id()].push_back(&left_blob);
               //     track_matches[it_prev->id()].push_back(&right_blob);
               //}

               cv::Point2d sum_weighted(0,0);
               int count_weighted = 0;

               cv::Point2d sum(0,0);
               int count = 0;

               // Find "centroid" of all measurements
               for (std::list<wb::Blob*>::iterator it_match = track_matches[it_prev->id()].begin();
                    it_match != track_matches[it_prev->id()].end(); it_match++) {

                    sum_weighted.x += (*it_match)->estimated_pixel_centroid().x * (*it_match)->age();
                    sum_weighted.y += (*it_match)->estimated_pixel_centroid().y * (*it_match)->age();
                    count_weighted += (*it_match)->age();

                    sum.x += (*it_match)->estimated_pixel_centroid().x;
                    sum.y += (*it_match)->estimated_pixel_centroid().y;
                    count++;

                    wb::drawCross(img, (*it_match)->estimated_pixel_centroid(), cv::Scalar(0,255,0), 8);

                    std::ostringstream convert;
                    convert << (*it_match)->id();
                    const std::string& text = convert.str();
                    cv::putText(img, text, cv::Point((*it_match)->estimated_pixel_centroid().x-3,(*it_match)->estimated_pixel_centroid().y-3), cv::FONT_HERSHEY_DUPLEX, 0.75, cv::Scalar(255,255,255), 1, 8, false);
               }

               cv::Point2d avg_weighted(0,0);
               avg_weighted.x = sum_weighted.x / (double)count_weighted;
               avg_weighted.y = sum_weighted.y / (double)count_weighted;

               wb::Blob b;
               b.set_pixel_centroid(avg_weighted);
               b.set_centroid(avg_weighted);//todo, not right
               //wb::drawCross(img, avg_weighted, cv::Scalar(255,255,255), 5);

               cv::Point2d avg;
               avg.x = sum.x / (double)count;
               avg.y = sum.y / (double)count;
               //wb::drawCross(img, avg, cv::Scalar(0,0,255), 5);

               // Integrate the matched measurement
               it_prev->set_occluded(false);
               it_prev->inc_age();
               it_prev->copy_meas_info(b);
               it_prev->correct_tracker();

               // Adjust measurement noise matrix to accomodate diver's shape
               Eigen::MatrixXd covar_sum = Eigen::MatrixXd::Zero(2,2);
               count = 0;

               for (std::list<wb::Blob*>::iterator it_match = track_matches[it_prev->id()].begin();
                    it_match != track_matches[it_prev->id()].end(); it_match++) {

                    Eigen::MatrixXd diff(2,1);
                    diff << (*it_match)->estimated_pixel_centroid().x - avg.x,
                         (*it_match)->estimated_pixel_centroid().y - avg.y;

                    covar_sum += (diff * diff.transpose());

                    count++;
               }

               Eigen::MatrixXd covar(2,2);
               covar = covar_sum / ((double)count);
               //covar = covar.Eigen::sqrt();
               //covar *= 10;
               //covar *= 5;
               covar *= 4;

               double eig_scale = 0.1;
               if (count > 1) {
                    // Check to see if smaller eigenvalue is at least
                    // 0.001 times the larger eigenvalue
                    Eigen::VectorXcd eigvals = covar.eigenvalues();
                    double eig1 = eigvals(0).real();
                    double eig2 = eigvals(1).real();

                    if (eig1 < eig2) {
                         if (eig1 < eig2*eig_scale) {
                              eig1 = eig2*eig_scale;

                              Eigen::EigenSolver<Eigen::MatrixXd> es(covar);
                              Eigen::MatrixXcd ev = es.eigenvectors();
                              Eigen::MatrixXd ev_real = ev.real();

                              Eigen::MatrixXd eigs_corrected(2,2);
                              eigs_corrected << eig1, 0 , 0, eig2;
                              covar = ev_real * eigs_corrected * ev_real.inverse();
                         }

                    } else {
                         if (eig2 < eig1*eig_scale) {
                              eig2 = eig1*eig_scale;

                              Eigen::EigenSolver<Eigen::MatrixXd> es(covar);
                              Eigen::MatrixXcd ev = es.eigenvectors();
                              Eigen::MatrixXd ev_real = ev.real();

                              Eigen::MatrixXd eigs_corrected(2,2);
                              eigs_corrected << eig1, 0 , 0, eig2;
                              covar = ev_real * eigs_corrected * ev_real.inverse();
                         }
                    }

                    //covar_tracker_.predict();
                    //covar_tracker_.set_values(covar);
                    //covar = covar_tracker_.values();
                    //it_prev->pixel_tracker().set_R(covar(0,0), covar(0,1),
                    //                               covar(1,0), covar(1,1));
               } else {
                    // If only a single measurement, decrease size of
                    // measurement, generate measurement that is "circular"
                    //covar_tracker_.predict();
                    covar = it_prev->pixel_tracker().R().cast<double>();

                    Eigen::VectorXcd eigvals = covar.eigenvalues();
                    double eig1 = eigvals(0).real();
                    double eig2 = eigvals(1).real();

                    if (eig1 < eig2) {
                         eig1 *= 0.90;
                         eig2 = eig1;

                         Eigen::EigenSolver<Eigen::MatrixXd> es(covar);
                         Eigen::MatrixXcd ev = es.eigenvectors();
                         Eigen::MatrixXd ev_real = ev.real();

                         Eigen::MatrixXd eigs_corrected(2,2);
                         eigs_corrected << eig1, 0 , 0, eig2;
                         covar = ev_real * eigs_corrected * ev_real.inverse();

                    } else {
                         //eig2 *= 0.90;
                         eig2 *= 0.90;
                         eig1 = eig2;
                         Eigen::EigenSolver<Eigen::MatrixXd> es(covar);
                         Eigen::MatrixXcd ev = es.eigenvectors();
                         Eigen::MatrixXd ev_real = ev.real();

                         Eigen::MatrixXd eigs_corrected(2,2);
                         eigs_corrected << eig1, 0 , 0, eig2;
                         covar = ev_real * eigs_corrected * ev_real.inverse();
                    }

                    //covar_tracker_.set_values(covar);

                    //covar = covar_tracker_.values();
                    //it_prev->pixel_tracker().set_R(covar(0,0), covar(0,1),
                    //                               covar(1,0), covar(1,1));
               }

               //// Clamp minimum and maximum size for covar elements:
               //double max_covar = 2000;
               //for (int r = 0; r < 2; r++) {
               //     for (int c = 0; c < 2; c++) {
               //          if (covar(r,c) > max_covar) {
               //               covar(r,c) = max_covar;
               //          }
               //          //if (covar(r,c) < 50) {
               //          //     covar(r,c) = 50;
               //          //}
               //     }
               //}

               // TODO: create minimum / maximum covariance possibilities
               covar_tracker_.predict();
               covar_tracker_.set_values(covar);
               covar = covar_tracker_.values();
               it_prev->pixel_tracker().set_R(covar(0,0), covar(0,1),
                                              covar(1,0), covar(1,1));

               //cout << it_prev->id() << "-covar:" << endl << covar << endl;

               // Define maximum and minimum size for object in scene?

          } else {
               // Missed track measurement?
               it_prev->missed_track();
          }
          //tracks_.push_back(*it_prev);
          intermediate.push_back(&(*it_prev));
     }

     // Plot intermediate tracks
     cv::Mat intermediate_img;
     this->overlay(intermediate, src, intermediate_img, TRACKS | IDS | ERR_ELLIPSE);
     cv::imshow("Intermediate", intermediate_img);

     // Are any of the tracks very similar? If so, keep the oldest one
     // Determine if the centroids of any tracks are within 1 std of each
     // other.
     //Eigen::MatrixXf Zm1, Zm2; Zm1.resize(2,1); Zm2.resize(2,1);

     // TODO : We can't integrate the Kalman filters until we find the track
     // with the oldest age and smallest ID. First find tracks that "overlap"
     // and then integrate them.

     for (std::vector<wb::Blob*>::iterator it1 = intermediate.begin();
          it1 != intermediate.end(); it1++) {
          for (std::vector<wb::Blob*>::iterator it2 = intermediate.begin();
               it2 != intermediate.end(); it2++) {

               // If it's the same ID, move on to next track
               if ((*it1)->id() == (*it2)->id()) {
                    continue;
               }

               // If one of the tracks was marked "matched," it was already
               // integrated into a track
               if ((*it1)->matched() || (*it2)->matched()) {
                    continue;
               }

               //// Are the track's centroids within 3 std of each other?
               if ((*it1)->pixel_tracker().is_within_region((*it2)->estimated_pixel_centroid(),0.9973) &&
                   (*it2)->pixel_tracker().is_within_region((*it1)->estimated_pixel_centroid(),0.9973)) {

                    bool save_it1;
                    if ((*it1)->age() > (*it2)->age()) {
                         save_it1 = true;
                    } else if ((*it1)->age() < (*it2)->age()) {
                         save_it1 = false;
                    } else {
                         // equal ages...
                         if ((*it1)->id() < (*it2)->id()) {
                              save_it1 = true;
                         } else if ((*it1)->id() > (*it2)->id()) {
                              save_it1 = false;
                         } else {
                              save_it1 = true;
                              cout << "WARNING: unexpected object ID" << endl;
                         }
                    }

                    // Found similar tracks. Save the oldest track
                    if (save_it1) {
                         // Integrate younger track into older track
                         (*it1)->copy_meas_info(*(*it2));
                         (*it1)->set_occluded(false);
                         (*it1)->correct_tracker();

                         // Mark the younger track as matched, so it is overlooked later
                         (*it2)->set_matched(true);
                    } else {
                         // Integrate younger track into older track
                         (*it2)->copy_meas_info(*(*it1));
                         (*it2)->set_occluded(false);
                         (*it2)->correct_tracker();

                         // Mark the younger track as matched, so it is overlooked later
                         (*it1)->set_matched(true);
                    }
               }
          }
     }

     // Any track that wasn't marked as "matched" was either older or wasn't
     // similar to another track. Copy them to the final fused vector
     for (std::vector<wb::Blob*>::iterator it1 = intermediate.begin();
          it1 != intermediate.end(); it1++) {
          if (!(*it1)->matched()) {
               tracks_.push_back(*(*it1));
          }
     }

     // Cull dead tracks
     std::vector<wb::Blob>::iterator it = tracks_.begin();
     while(it != tracks_.end()) {
          it->set_matched(false);

          if (it->is_dead()) {
               it = tracks_.erase(it);
          } else {
               it++;
          }
     }

     //cv::imshow("Used", img);

     // Overlay object track data
     this->overlay(src, dst, TRACKS | IDS | ERR_ELLIPSE | VELOCITIES);

     this->diver_classification(src, dst, meas, params);

     prev_tracks_ = tracks_;
}

bool ObjectTracker::is_diver(std::vector<wb::Entity> &objects, int id)
{
     for (std::vector<wb::Entity>::iterator it_obj = objects.begin();
          it_obj != objects.end(); it_obj++) {
          if (id == it_obj->id()) {
               if (it_obj->type() == wb::Entity::Diver) {
                    return true;
               }
               return false;
          }
     }
     return false;
}

#define RAD_2_DEG (180.0/3.14159)

void ObjectTracker::diver_classification(cv::Mat &src, cv::Mat &dst,
                                         std::vector<wb::Blob> &meas,
                                         Parameters *params)
{
     cv::Mat obj_img = dst.clone();

     estimated_divers_.clear();
#if 1
     //////////////////////////////////////////////////////////////////////////
     // Leg Trackers:
     // Find blobs that are within the "back half" of the ellipse, where the
     // "back half" is the direction away from forward velocity.
     for (std::vector<wb::Blob>::iterator it_obj = tracks_.begin();
          it_obj != tracks_.end(); it_obj++) {

          //cout << "==================================================" << endl;

          // Predict left and right
          it_obj->left_tracker.predict();
          it_obj->right_tracker.predict();

          bool classified_as_diver = false;

          cv::Point track_centroid = it_obj->estimated_pixel_centroid();
          cv::Point v = it_obj->estimated_pixel_velocity();

          cv::Vec3d v3(v.x,v.y,0);

          cv::Vec2d vel2d(v.x, v.y);
          cv::Vec2d vel_unit = vel2d / sqrt(pow(vel2d[0],2) + pow(vel2d[1],2));

          // TODO: Velocity needed?
          // The velocity vector has to be above length threshold
          double v_norm = sqrt(pow(v3[0],2) + pow(v3[1],2));
          bool has_velocity = false;
          if (v_norm >= params->min_velocity_threshold_2) {
               //cout << "Too Slow: " << v_norm << endl;
               has_velocity = true;
          }

          if (has_velocity) {

               // Get major and minor axes of error ellipse
               Ellipse ell = it_obj->error_ellipse(0.9973); // 3 std

#if 1
               // Get rectangle around object
               double theta = atan2(vel_unit[1], vel_unit[0]);
               cv::RotatedRect rrect(cv::Point(track_centroid.x,track_centroid.y), cv::Size2f(2*ell.axes()(0),2*ell.axes()(1)), theta * RAD_2_DEG);

               cv::Point2f vertices[4];
               rrect.points(vertices);
               for (int i = 0; i < 4; i++) {
                    cv::line(obj_img, vertices[i], vertices[(i+1)%4], cv::Scalar(0,255,0));
               }
#endif

               // Line separating fins:
               cv::Vec2d sep = vel_unit * -1 * ell.axes()(0);// * 4;

               cv::line(dst,cv::Point(track_centroid.x,track_centroid.y),
                        cv::Point(track_centroid.x + sep[0],
                                  track_centroid.y + sep[1]),
                        cv::Scalar(255,77,77), 1, 8, 0);

               std::vector<wb::Blob> lefts; lefts.clear();
               std::vector<wb::Blob> rights; rights.clear();

               for (std::vector<wb::Blob>::iterator it_blob = meas.begin();
                    it_blob != meas.end(); it_blob++) {

                    if (it_blob->occluded()) {
                         continue;
                    }

                    //// TODO: age needed?
                    //if (it_blob->age() < 3) {
                    //     cout << "Too Young" << endl;
                    //     continue;
                    //}

                    cv::Point blob_centroid = it_blob->estimated_pixel_centroid();

                    // Is the blob within the error ellipse?
                    bool in_ellipse = false;
                    if (it_obj->pixel_tracker().is_within_region(blob_centroid,0.9973)) {
                         in_ellipse = true;
                    }

                    // Is the blob on the side of the ellipse, opposite the track's
                    // estimated velocity?

                    // Find blob centroid relative to the track's centroid
                    cv::Vec2d blob_relative(blob_centroid.x - track_centroid.x,
                                            blob_centroid.y - track_centroid.y);

                    // The blob relative norm distance can't be too small.
                    double blob_relative_norm = sqrt(pow(blob_relative[0],2) + pow(blob_relative[1],2));
                    //if (blob_relative_norm < 20) {
                    if (blob_relative_norm < params->min_relative_norm) {
                         continue;
                    }

                    cv::Vec2d blob_relative_unit = blob_relative / blob_relative_norm;
                    // The dot product of the velocity and the relative position of
                    // the blob's centroid is negative if they are in "opposite
                    // directions"
                    double dot = vel_unit.dot(blob_relative_unit);
                    if (dot >= 0) {
                         continue;
                    }

                    if (in_ellipse) {
                         // Plot the used points on the image
                         wb::drawCross(dst, it_blob->estimated_pixel_centroid(), cv::Scalar(0,0,255), 8);
                    }

                    cv::Vec3d sep_3d(sep[0],sep[1],0);
                    cv::Vec3d blob_relative_unit_3d(blob_relative_unit[0], blob_relative_unit[1], 0);
                    cv::Vec3d cross_value = sep_3d.cross(blob_relative_unit_3d);
                    int cross_value_sign = syllo::sign(cross_value[2]);

                    cv::Point2d rel_p = it_blob->estimated_pixel_centroid() - it_obj->estimated_pixel_centroid();
                    //cv::Point2d rel_p = it_obj->estimated_pixel_centroid() - it_blob->estimated_pixel_centroid();

                    //// Rotate point based on object's velocity
                    double theta = -atan2(vel_unit[1], vel_unit[0]);
                    cv::Point2d rel_rot_p;
                    rel_rot_p.x = cos(theta) * rel_p.x - sin(theta) * rel_p.y;
                    rel_rot_p.y = sin(theta) * rel_p.x + cos(theta) * rel_p.y;

                    it_blob->set_estimated_pixel_centroid(rel_rot_p);
                    it_blob->set_pixel_centroid(rel_rot_p);
                    it_blob->set_estimated_centroid(rel_rot_p);
                    it_blob->set_centroid(rel_rot_p);

                    // If the blob isn't inside of the ellipse, is it within
                    // rectangle behind the object, with bounds defined by
                    // 4xMajorAxis and 2xMinor Axis, rotated to velocity?
                    cv::Vec2d down_vel(0,1);
                    int dir_vel = syllo::sign((down_vel.dot(vel_unit)));

                    // Dot product is greater than 0 if the vectors lie in the same
                    // direction:
                    if (!in_ellipse && dir_vel > 0) {
                         // Make a copy of the current object track and offset it behind the object:
                         PositionTracker track_behind = it_obj->pixel_tracker();
                         cv::Point2d pt_behind = track_behind.position();
                         pt_behind.x = pt_behind.x + vel_unit[0] * ell.axes()(0) * -2;
                         pt_behind.y = pt_behind.y + vel_unit[1] * ell.axes()(0) * -2;
                         track_behind.set_position(pt_behind);

                         // Draw error ellipse
                         Ellipse ell_offset = track_behind.error_ellipse(0.9973); // 3 std
                         //cv::Point center = track_behind.position();
                         cv::Size axes(cvRound(ell_offset.axes()(0)), cvRound(ell_offset.axes()(1)));
                         //cv::ellipse(dst, center, axes, cvRound(ell_offset.angle()), 0, 360, cv::Scalar(255,0,0), 1, 8, 0);

                         if (track_behind.is_within_region(blob_centroid, 0.9973)) {
                              in_ellipse = true;
                         }
                    }

                    if (cross_value_sign == left_side_sign_) {
                         if (in_ellipse) {
                              lefts.push_back(*it_blob);
                         }
                    } else {
                         if (in_ellipse) {
                              rights.push_back(*it_blob);
                         }
                    }

               } // For each blob

               // Using the collected lefts and rights, determine which measurement
               // in each lefts and rights is "closest" to the left and right
               // tracker

               // Find tracks with smallest mahalanobis distance
               double min_dist = 1e9;
               std::vector<wb::Blob>::iterator it_left = lefts.begin();
               for (std::vector<wb::Blob>::iterator it = lefts.begin();
                    it != lefts.end(); it++) {

                    double mahal_dist = it_obj->left_tracker.mahalanobis(it->estimated_pixel_centroid());
                    if (mahal_dist < min_dist) {
                         mahal_dist = min_dist;
                         it_left = it;
                    }
               }

               min_dist = 1e9;
               std::vector<wb::Blob>::iterator it_right = rights.begin();
               for (std::vector<wb::Blob>::iterator it = rights.begin();
                    it != rights.end(); it++) {

                    double mahal_dist = it_obj->right_tracker.mahalanobis(it->estimated_pixel_centroid());
                    if (mahal_dist < min_dist) {
                         mahal_dist = min_dist;
                         it_right = it;
                    }
               }

               if (lefts.size() > 0) {
                    it_obj->left_tracker.set_measurement(it_left->estimated_pixel_centroid());
               }

               if (rights.size() > 0) {
                    it_obj->right_tracker.set_measurement(it_right->estimated_pixel_centroid());
               }

               double left_P_norm = it_obj->left_tracker.P().norm();
               double right_P_norm = it_obj->right_tracker.P().norm();

               //cout << "----------" << endl;
               //cout << "Left Norm: " << left_P_norm << endl;
               //cout << "Right Norm: " << right_P_norm << endl;

               double left_length = cv::norm(it_obj->left_tracker.position());
               double right_length = cv::norm(it_obj->right_tracker.position());

               //cout << "left: " << left_length << endl;
               //cout << "right: " << right_length << endl;

               if (left_P_norm < params->covar_threshold &&
                   right_P_norm < params->covar_threshold //) {
                   && std::abs(left_length - right_length) < params->max_leg_diff) {

                    it_obj->inc_class_age();
                    it_obj->set_type(wb::Entity::Diver);
                    estimated_divers_.push_back(*it_obj);
                    classified_as_diver = true;
               }
          }

          if (!classified_as_diver) {
               //if (is_diver(prev_estimated_divers_,it_obj->id()) && it_obj->class_age() > params->class_age_confirmed) {
               if (it_obj->class_age() > params->class_age_confirmed) {
                    //it_obj->dec_class_age();
                    it_obj->set_type(wb::Entity::Diver);
                    estimated_divers_.push_back(*it_obj);
               }
          }

          // Overlay left tracker...
          if (it_obj->left_tracker.initialized()) {
               // Get relative position to diver's centroid:
               // Rotate point based on object's velocity
               cv::Point2d rel_p = it_obj->left_tracker.position();
               double theta = atan2(vel_unit[1], vel_unit[0]);

               cv::Point2d rel_rot_p;
               rel_rot_p.x = cos(theta) * rel_p.x - sin(theta) * rel_p.y;
               rel_rot_p.y = sin(theta) * rel_p.x + cos(theta) * rel_p.y;

               cv::Point2d pos;
               pos.x = rel_rot_p.x + it_obj->estimated_pixel_centroid().x;
               pos.y = rel_rot_p.y + it_obj->estimated_pixel_centroid().y;

               // Draw text
               cv::putText(dst, "L", cv::Point(pos.x-3,pos.y-3), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255,255,255), 2, 8, false);

               // Draw centroid
               wb::drawCross(dst, pos, cv::Scalar(212,173,70), 3);

               // Draw error ellipse
               Ellipse ell = it_obj->left_tracker.error_ellipse(0.9973); // 3 std
               cv::Point center = pos;
               cv::Size axes(cvRound(ell.axes()(0)), cvRound(ell.axes()(1)));
               cv::ellipse(dst, center, axes, cvRound(ell.angle()), 0, 360, cv::Scalar(255,255,255), 1, 8, 0);

               // Draw line from tracker to centroid:
               cv::line(dst, center, it_obj->estimated_pixel_centroid(), cv::Scalar(0,255,0), 1, 8, 0);
          }

          // Overlay right tracker...
          if (it_obj->right_tracker.initialized()) {
               // Get relative position to diver's centroid:
               // Rotate point based on object's velocity
               cv::Point2d rel_p = it_obj->right_tracker.position();
               double theta = atan2(vel_unit[1], vel_unit[0]);

               cv::Point2d rel_rot_p;
               rel_rot_p.x = cos(theta) * rel_p.x - sin(theta) * rel_p.y;
               rel_rot_p.y = sin(theta) * rel_p.x + cos(theta) * rel_p.y;

               cv::Point2d pos;
               pos.x = rel_rot_p.x + it_obj->estimated_pixel_centroid().x;
               pos.y = rel_rot_p.y + it_obj->estimated_pixel_centroid().y;

               // Draw text
               cv::putText(dst, "R", cv::Point(pos.x-3,pos.y-3), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255,255,255), 2, 8, false);

               // Draw centroid
               wb::drawCross(dst, pos, cv::Scalar(255,173,70), 3);

               // Draw error ellipse
               Ellipse ell = it_obj->right_tracker.error_ellipse(0.9973); // 3 std
               cv::Point center = pos;
               cv::Size axes(cvRound(ell.axes()(0)), cvRound(ell.axes()(1)));
               cv::ellipse(dst, center, axes, cvRound(ell.angle()), 0, 360, cv::Scalar(255,255,255), 1, 8, 0);

               // Draw line from tracker to centroid:
               cv::line(dst, center, it_obj->estimated_pixel_centroid(), cv::Scalar(0,255,0), 1, 8, 0);
          }
     } // For each object
#else
     //////////////////////////////////////////////////////////////////////////
     // Velocity Based Detection:
     // Diver objects have a velocity within a threshold
     for (std::vector<wb::Blob>::iterator it_obj = tracks_.begin();
          it_obj != tracks_.end(); it_obj++) {

          bool classified_as_diver = false;

          cv::Point v = it_obj->estimated_pixel_velocity();

          // The velocity vector has to be above length threshold
          double v_norm = sqrt(pow(v.x,2) + pow(v.y,2));
          //cout << "ID: " << it_obj->id() << ", v: " << v_norm << endl;
          if (v_norm > params->min_velocity_threshold && v_norm < params->max_velocity_threshold) {
               it_obj->inc_class_age();
               it_obj->set_type(wb::Entity::Diver);
               estimated_divers_.push_back(*it_obj);
               classified_as_diver = true;
          }
          //else {
          // If it was a diver in the past and it's velocity is below a
          // threshold, it is still a diver:
          //if (v_norm < params->max_velocity_threshold &&
          //    is_diver(prev_estimated_divers_,it_obj->id())) {

          if (!classified_as_diver) {
               // If it wasn't classified as a diver
               if (it_obj->class_age() > params->class_age_confirmed) {
                    it_obj->set_type(wb::Entity::Diver);
                    estimated_divers_.push_back(*it_obj);
               }
          }
     }
#endif

     cv::imshow("Object-Vel", obj_img);
     prev_estimated_divers_ = estimated_divers_;
}

void ObjectTracker::overlay(std::vector<wb::Blob*> &tracks, cv::Mat &src, cv::Mat &dst, OverlayFlags_t flags)
{
     std::vector<wb::Blob> tracks_temp;
     for (std::vector<wb::Blob*>::iterator it = tracks.begin();
          it != tracks.end(); it++) {
          tracks_temp.push_back(*(*it));
     }
     overlay(tracks_temp,src,dst,flags);
}

void ObjectTracker::overlay(cv::Mat &src, cv::Mat &dst, OverlayFlags_t flags)
{
     overlay(tracks_,src,dst,flags);
}

void ObjectTracker::overlay(std::vector<wb::Blob> &tracks, cv::Mat &src, cv::Mat &dst, OverlayFlags_t flags)
{
     cv::Mat color;
     if (src.channels() == 1) {
          cv::cvtColor(src, color, CV_GRAY2BGR);
     } else {
          color = src;
     }
     dst = color;

     for(std::vector<wb::Blob>::iterator it = tracks.begin();
         it != tracks.end(); it++) {

          if ((flags & CONFIRMED_ONLY) && !it->is_confirmed()) {
               // If we are only plotting confirmed tracks and it's not
               // confirmed, ignore this track/blob.
               continue;
          }

          if (flags & BLOBS) {
               cv::Vec3b point_color = cv::Vec3b(20,255,57);
               if (it->occluded()) {
                    point_color = cv::Vec3b(0,0,0);
               }

               // Draw all blob points in the image
               std::vector<wb::Point> points = it->points();
               std::vector<wb::Point>::iterator it_points = points.begin();
               for(; it_points != points.end(); it_points++) {
                    dst.at<cv::Vec3b>(it_points->y(), it_points->x()) = point_color;
               }
          }

          cv::Rect rect = it->bbox().rectangle();

          if (flags & RECTS) {
               cv::rectangle(dst, rect, cv::Scalar(255,255,255), 1, 8, 0);
          }

          cv::Point est_centroid = it->estimated_pixel_centroid();
          if (flags & IDS) {
               std::ostringstream convert;
               convert << it->id();
               const std::string& text = convert.str();
               cv::putText(dst, text, cv::Point(est_centroid.x-3,est_centroid.y-3), cv::FONT_HERSHEY_DUPLEX, 0.75, cv::Scalar(255,255,255), 2, 8, false);
          }

          if (flags & TRACKS) {
               //cv::Point est_centroid = it->estimated_pixel_centroid();
               wb::drawCross(dst, est_centroid, cv::Scalar(255,255,255), 5);
          }

          if (flags & ERR_ELLIPSE) {
               Ellipse ell = it->error_ellipse(0.9973); // 3 std
               //Ellipse ell = it->error_ellipse(0.9545); // 2 std
               //Ellipse ell = it->error_ellipse(0.6827); // 1 std
               cv::Point center(cvRound(ell.center().x),cvRound(ell.center().y));
               cv::Size axes(cvRound(ell.axes()(0)), cvRound(ell.axes()(1)));
               //The -angle is used because OpenCV defines the angle clockwise instead of anti-clockwise
               //cv::ellipse(dst, center, axes, -cvRound(ell.angle()), 0, 360, cv::Scalar(255,255,255), 1, 8, 0);
               cv::ellipse(dst, center, axes, cvRound(ell.angle()), 0, 360, cv::Scalar(255,255,255), 1, 8, 0);
          }

          if (flags & VELOCITIES) {
               cv::Point pt1, pt2;
               pt1 = est_centroid;
               pt2 = est_centroid + it->estimated_pixel_velocity();

               int thickness = 1;
               double tip_length = 0.1;
               wb::arrowedLine(dst, pt1, pt2, cv::Scalar(0,255,0),
                               thickness, 8, 0, tip_length);
          }
     }
}


#include <boost/test/unit_test.hpp>

#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include "PeriodVal.h"
#include "PatternScannerEngine.h"
#include "SegmentConstraint.h"
#include "SegmentValsCloseToLinearEq.h"
#include "SegmentListConstraint.h"
#include "SlopeIncreasesConstraint.h"
#include "PatternMatchValidator.h"
#include "EndWithinPercentOfStart.h"
#include "PeriodValSegment.h"
#include "VScanner.h"
#include "TestHelper.h"
#include "RecoverPercentOfDepth.h"
#include "BreakoutAboveFirstHighValidatorFactory.h"

using namespace boost::posix_time;
using namespace boost::gregorian;
using namespace testHelper;

BOOST_AUTO_TEST_CASE( VScanner_QCOR_20130819_LHSofDoubleBottom )
{
	PeriodValSegmentPtr allPerSegment = PeriodValSegment::readFromFile("./patternScan/QCOR_DoubleBottom_Weekly.csv");

	PeriodValSegmentPair splitSeg = allPerSegment->split(11);
	PeriodValSegmentPtr vData = splitSeg.first;
	genPeriodValSegmentInfo("V segment data",*vData);

	VScanner scanner;
	scanner.upTrendValidatorFactory().addFactory(
			PatternMatchValidatorFactoryPtr(new RecoverPercentOfDepth(60.0)));

	PatternMatchListPtr patternMatches = scanner.scanPatternMatches(vData);

	verifyMatchList("VScanner_QCOR_20130819_LHSofDoubleBottom",patternMatches,1);

	PatternMatchPtr thePatternMatch = patternMatches->front();

	verifyPatternMatch("V Match on LHS",
			ptime(date(2013,8,26)),ptime(date(2013,10,21)),2,thePatternMatch);

}

BOOST_AUTO_TEST_CASE( VScanner_QCOR_20130819_RHSofDoubleBottom )
{
	PeriodValSegmentPtr chartData = PeriodValSegment::readFromFile("./patternScan/QCOR_DoubleBottom_Weekly.csv");

	BOOST_TEST_MESSAGE("Testing V sub-patterns for RHS of double bottom of QCOR");
	PeriodValSegmentPair splitQCORdata = chartData->split(8);
	PeriodValSegmentPtr rhsSegData = splitQCORdata.second;

	genPeriodValSegmentInfo("V RHS segment data",*rhsSegData);

	VScanner scanner;
    scanner.upTrendValidatorFactory().addFactory(
            PatternMatchValidatorFactoryPtr(new RecoverPercentOfDepth(50.0)));
    scanner.upTrendValidatorFactory().addFactory(PatternMatchValidatorFactoryPtr(new BreakoutAboveFirstHighValidatorFactory()));

	PatternMatchListPtr patternMatches = scanner.scanPatternMatches(rhsSegData);

	verifyMatchList("VScanner_QCOR_20130819_RHSofDoubleBottom",patternMatches,1);

	PatternMatchPtr thePatternMatch = patternMatches->front();

	verifyPatternMatch("V Match on RHS",
            ptime(date(2013,10,21)),ptime(date(2014,2,10)),2,thePatternMatch);

}


#include <boost/graph/astar_search.hpp>


//
//  Copyright (C) 2019 Greg Landrum
//
//   @@ All Rights Reserved @@
//  This file is part of the RDKit.
//  The contents are covered by the terms of the BSD license
//  which is included in the file license.txt, found at the root
//  of the RDKit source tree.
//
#define PY_ARRAY_UNIQUE_SYMBOL rdeht_array_API
#include <RDBoost/python.h>
#include <boost/python/list.hpp>
#include <RDGeneral/Exceptions.h>
#include <GraphMol/RDKitBase.h>
#include "EHTTools.h"

#include <RDBoost/boost_numpy.h>
#include <RDBoost/PySequenceHolder.h>
#include <RDBoost/Wrap.h>
#include <RDBoost/import_array.h>

namespace python = boost::python;

namespace RDKit {

namespace {

// from: https://stackoverflow.com/a/35960614
/// @brief Transfer ownership to a Python object.  If the transfer fails,
///        then object will be destroyed and an exception is thrown.
template <typename T>
boost::python::object transfer_to_python(T *t) {
  // Transfer ownership to a smart pointer, allowing for proper cleanup
  // incase Boost.Python throws.
  std::unique_ptr<T> ptr(t);

  // Use the manage_new_object generator to transfer ownership to Python.
  namespace python = boost::python;
  typename python::manage_new_object::apply<T *>::type converter;

  // Transfer ownership to the Python handler and release ownership
  // from C++.
  python::handle<> handle(converter(*ptr));
  ptr.release();

  return python::object(handle);
}

PyObject *getMatrixProp(const double *mat, unsigned int sz) {
  if (!mat) throw_value_error("matrix has not be initialized");
  npy_intp dims[2];
  dims[0] = sz;
  dims[1] = sz;

  PyArrayObject *res = (PyArrayObject *)PyArray_SimpleNew(2, dims, NPY_DOUBLE);

  memcpy(PyArray_DATA(res), static_cast<const void *>(mat),
         sz * sz * sizeof(double));

  return PyArray_Return(res);
}
PyObject *getSymmMatrixProp(const double *mat, unsigned int sz) {
  if (!mat) throw_value_error("matrix has not be initialized");
  npy_intp dims[1];
  dims[0] = sz * (sz + 1) / 2;

  PyArrayObject *res = (PyArrayObject *)PyArray_SimpleNew(1, dims, NPY_DOUBLE);

  memcpy(PyArray_DATA(res), static_cast<const void *>(mat),
         dims[0] * sizeof(double));

  return PyArray_Return(res);
}
PyObject *getVectorProp(const double *mat, unsigned int sz) {
  if (!mat) throw_value_error("vector has not be initialized");
  npy_intp dims[1];
  dims[0] = sz;

  PyArrayObject *res = (PyArrayObject *)PyArray_SimpleNew(1, dims, NPY_DOUBLE);

  memcpy(PyArray_DATA(res), static_cast<const void *>(mat),
         sz * sizeof(double));

  return PyArray_Return(res);
}
PyObject *getChargeMatrix(EHTTools::EHTResults &self) {
  return getMatrixProp(self.reducedChargeMatrix.get(), self.numAtoms);
}

PyObject *getOPMatrix(EHTTools::EHTResults &self) {
  return getSymmMatrixProp(self.reducedOverlapPopulationMatrix.get(),
                           self.numAtoms);
}

PyObject *getCharges(EHTTools::EHTResults &self) {
  return getVectorProp(self.atomicCharges.get(), self.numAtoms);
}
python::tuple runCalc(const RDKit::ROMol &mol, int confId) {
  auto eRes = new RDKit::EHTTools::EHTResults();
  bool ok = RDKit::EHTTools::runMol(mol, *eRes, confId);
  return python::make_tuple(ok, transfer_to_python(eRes));
}

}  // end of anonymous namespace

struct EHT_wrapper {
  static void wrap() {
    std::string docString = "";

    python::class_<RDKit::EHTTools::EHTResults, boost::noncopyable>(
        "EHTResults", docString.c_str(), python::no_init)
        .def_readonly("fermiEnergy", &RDKit::EHTTools::EHTResults::fermiEnergy)
        .def_readonly("totalEnergy", &RDKit::EHTTools::EHTResults::totalEnergy)
        .def("GetReducedChargeMatrix", getChargeMatrix,
             "returns the reduced charge matrix")
        .def("GetReducedOverlapPopulationMatrix", getOPMatrix,
             "returns the reduced overlap population matrix")
        .def("GetAtomicCharges", getCharges,
             "returns the calculated atomic charges");

    docString =
        R"DOC(Runs an extended Hueckel calculation for a molecule.
The molecule should have at least one conformation

ARGUMENTS:
   - mol: molecule to use
   - confId: (optional) conformation to use

RETURNS: a 2-tuple:
   - a boolean indicating whether or not the calculation succeeded
   - an EHTResults object with the results
)DOC";
    python::def("RunMol", runCalc,
                (python::arg("mol"), python::arg("confId") = -1),
                docString.c_str());
  }
};

}  // end of namespace RDKit
BOOST_PYTHON_MODULE(rdEHTTools) {
  python::scope().attr("__doc__") =
      R"DOC(Module containing interface to the YAeHMOP extended Hueckel library.
Please note that this interface should still be considered experimental and may
change from one release to the next.)DOC";
  rdkit_import_array();

  RDKit::EHT_wrapper::wrap();
}


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_SDK_MEMORY_SLICE_HPP_INCLUDED
#define NT2_SDK_MEMORY_SLICE_HPP_INCLUDED

////////////////////////////////////////////////////////////////////////////////
// Compute the number of slice between inner adn the Nth outer dimension
////////////////////////////////////////////////////////////////////////////////
#include <boost/mpl/size_t.hpp>
#include <nt2/sdk/meta/mpl.hpp>
#include <nt2/sdk/meta/fusion.hpp>
#include <nt2/sdk/memory/padding.hpp>
#include <nt2/sdk/functor/functor.hpp>
#include <boost/fusion/include/size.hpp>
#include <nt2/sdk/memory/details/category.hpp>
#include <nt2/sdk/functor/preprocessor/function.hpp>

namespace nt2
{
  namespace tag { struct slice_ {}; }

  //////////////////////////////////////////////////////////////////////////////
  // slice computes the potential padded product of all dimensions in a given
  // dimension set from the Nth.
  //////////////////////////////////////////////////////////////////////////////
  template<std::size_t N, class Seq,class Padding> inline
  typename boost::
  lazy_enable_if_c< (boost::fusion::result_of::size<Seq>::value >= N)
                  , nt2::meta
                    ::enable_call < tag::slice_ ( Seq const&
                                                , Padding const&
                                                , boost::mpl::size_t<N> const&
                                                )
                                  >
                  >::type
  slice(Seq const& s, Padding const& p)
  {
    functor<tag::slice_> callee;
    return callee(s,p,boost::mpl::size_t<N>() );
  }

  //////////////////////////////////////////////////////////////////////////////
  // slice computes the potential padded product of all dimensions in a given
  // dimension set from the Nth. As N is larger than the dimension set size, we
  // naturally return 1.
  //////////////////////////////////////////////////////////////////////////////
  template<std::size_t N, class Seq,class Padding> inline
  typename boost::
  lazy_enable_if_c< !(boost::fusion::result_of::size<Seq>::value >= N)
                  , boost::mpl::int_<1>
                  >::type
  slice(Seq const& , Padding const& )
  {
    return boost::mpl::int_<1>();
  }
}

////////////////////////////////////////////////////////////////////////////////
// slice dispatch on basic padding strategy
////////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH ( tag::slice_, tag::cpu_
                      , (A0)(A1)(A2)
                      , (fusion_sequence_<A0>)
                        (padding_<A1>)
                        (mpl_integral_< integer_<A2> >)
                      )
#endif


#define BOOST_TEST_MODULE webauthn_test_mod
#include <boost/test/included/unit_test.hpp>
#include <boost/algorithm/string.hpp>

#include <fc/crypto/public_key.hpp>
#include <fc/crypto/private_key.hpp>
#include <fc/crypto/signature.hpp>
#include <fc/utility.hpp>

using namespace fc::crypto;
using namespace fc;
using namespace std::literals;

static fc::crypto::webauthn::signature make_webauthn_sig(const fc::crypto::r1::private_key& priv_key,
                                                         std::vector<uint8_t>& auth_data,
                                                         const std::string& json) {

   //webauthn signature is sha256(auth_data || client_data_hash)
   fc::sha256 client_data_hash = fc::sha256::hash(json);
   fc::sha256::encoder e;
   e.write((char*)auth_data.data(), auth_data.size());
   e.write(client_data_hash.data(), client_data_hash.data_size());

   r1::compact_signature sig = priv_key.sign_compact(e.result());

   char buff[8192];
   datastream<char*> ds(buff, sizeof(buff));
   fc::raw::pack(ds, sig);
   fc::raw::pack(ds, auth_data);
   fc::raw::pack(ds, json);
   ds.seekp(0);

   fc::crypto::webauthn::signature ret;
   fc::raw::unpack(ds, ret);

   return ret;
}

//used for many below
static const r1::private_key priv = fc::crypto::r1::private_key::generate();
static const r1::public_key pub = priv.get_public_key();
static const fc::sha256 d = fc::sha256::hash("sup"s);
static const fc::sha256 origin_hash = fc::sha256::hash("fctesting.invalid"s);

BOOST_AUTO_TEST_SUITE(webauthn_suite)

//Good signature
BOOST_AUTO_TEST_CASE(good) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);

   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EQUAL(wa_pub, make_webauthn_sig(priv, auth_data, json).recover(d, true));
} FC_LOG_AND_RETHROW();

//A valid signature but shouldn't match public key due to presence difference
BOOST_AUTO_TEST_CASE(mismatch_presence) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_PRESENT, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));
   auth_data[32] |= 0x04; //User presence verified

   BOOST_CHECK_NE(wa_pub, make_webauthn_sig(priv, auth_data, json).recover(d, true));
} FC_LOG_AND_RETHROW();

//A valid signature but shouldn't match public key due to origin difference
BOOST_AUTO_TEST_CASE(mismatch_origin) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_PRESENT, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://mallory.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   fc::sha256 mallory_origin_hash = fc::sha256::hash("mallory.invalid"s);
   memcpy(auth_data.data(), mallory_origin_hash.data(), sizeof(mallory_origin_hash));

   BOOST_CHECK_NE(wa_pub, make_webauthn_sig(priv, auth_data, json).recover(d, true));
} FC_LOG_AND_RETHROW();

//A valid signature but shouldn't recover because http was in use instead of https
BOOST_AUTO_TEST_CASE(non_https) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"http://fctesting.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::assert_exception, [](const fc::assert_exception& e) {
      return e.to_detail_string().find("webauthn origin must begin with https://") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//A valid signature but shouldn't recover because there is no origin scheme
BOOST_AUTO_TEST_CASE(lacking_scheme) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"fctesting.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::assert_exception, [](const fc::assert_exception& e) {
      return e.to_detail_string().find("webauthn origin must begin with https://") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//A valid signature but shouldn't recover because empty origin
BOOST_AUTO_TEST_CASE(empty_origin) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::assert_exception, [](const fc::assert_exception& e) {
      return e.to_detail_string().find("webauthn origin must begin with https://") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//Good signature with a port
BOOST_AUTO_TEST_CASE(good_port) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid:123456\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EQUAL(wa_pub, make_webauthn_sig(priv, auth_data, json).recover(d, true));
} FC_LOG_AND_RETHROW();

//Good signature with an empty port
BOOST_AUTO_TEST_CASE(empty_port) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid:\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EQUAL(wa_pub, make_webauthn_sig(priv, auth_data, json).recover(d, true));
} FC_LOG_AND_RETHROW();

//valid signature but with misc junk in challenge
BOOST_AUTO_TEST_CASE(challenge_junk) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + "blahBLAH"s + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::exception, [](const fc::exception& e) {
      return e.to_detail_string().find("sha256: size mismatch") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//valid signature but with non base64 in challenge
BOOST_AUTO_TEST_CASE(challenge_non_base64) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + "hello@world$"s + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::exception, [](const fc::exception& e) {
      return e.to_detail_string().find("encountered non-base64 character") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//valid signature but with some other digest in the challenge that is not the one we are recovering from
BOOST_AUTO_TEST_CASE(challenge_wrong) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   fc::sha256 other_digest = fc::sha256::hash("yo"s);
   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(other_digest.data(), other_digest.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::assert_exception, [](const fc::assert_exception& e) {
      return e.to_detail_string().find("Wrong webauthn challenge") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//valid signature but wrong webauthn type
BOOST_AUTO_TEST_CASE(wrong_type) try {

   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"type\":\"webauthn.meh\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::assert_exception, [](const fc::assert_exception& e) {
      return e.to_detail_string().find("webauthn signature type not an assertion") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//signature if good but the auth_data rpid hash is not what is expected for origin
BOOST_AUTO_TEST_CASE(auth_data_rpid_hash_bad) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   fc::sha256 origin_hash_corrupt = fc::sha256::hash("fctesting.invalid"s);
   memcpy(auth_data.data(), origin_hash_corrupt.data(), sizeof(origin_hash_corrupt));
   auth_data[4]++;

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::assert_exception, [](const fc::assert_exception& e) {
      return e.to_detail_string().find("webauthn rpid hash doesn't match origin") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//signature if good but auth_data too short to store what needs to be stored
BOOST_AUTO_TEST_CASE(auth_data_too_short) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(1);

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::assert_exception, [](const fc::assert_exception& e) {
      return e.to_detail_string().find("auth_data not as large as required") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//Good signature but missing origin completely
BOOST_AUTO_TEST_CASE(missing_origin) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::assert_exception, [](const fc::assert_exception& e) {
      return e.to_detail_string().find("webauthn origin must begin with https://") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//Good signature but missing type completely
BOOST_AUTO_TEST_CASE(missing_type) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::assert_exception, [](const fc::assert_exception& e) {
      return e.to_detail_string().find("webauthn signature type not an assertion") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//Good signature but missing challenge completely
BOOST_AUTO_TEST_CASE(missing_challenge) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"type\":\"webauthn.get\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::exception, [](const fc::exception& e) {
      return e.to_detail_string().find("sha256: size mismatch") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//Good signature with some extra stuff sprinkled in the json
BOOST_AUTO_TEST_CASE(good_extrajunk) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"cool\":\"beans\",\"obj\":{\"array\":[4, 5, 6]},"
                      "\"type\":\"webauthn.get\",\"answer\": 42 ,\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EQUAL(wa_pub, make_webauthn_sig(priv, auth_data, json).recover(d, true));
} FC_LOG_AND_RETHROW();

//Good signature but it's not a JSON object!
BOOST_AUTO_TEST_CASE(not_json_object) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "hey man"s;

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::assert_exception, [](const fc::assert_exception& e) {
      return e.to_detail_string().find("Failed to parse client data JSON") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//damage the r/s portion of the signature
BOOST_AUTO_TEST_CASE(damage_sig) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   webauthn::signature sig = make_webauthn_sig(priv, auth_data, json);
   char buff[8192];
   datastream<char*> ds(buff, sizeof(buff));
   fc::raw::pack(ds, sig);
   buff[4]++;
   ds.seekp(0);
   fc::raw::unpack(ds, sig);

   bool failed_recovery = false;
   bool failed_compare = false;
   webauthn::public_key recovered_pub;

   try {
      recovered_pub = sig.recover(d, true);
      failed_compare = !(wa_pub == recovered_pub);
   }
   catch(fc::exception& e) {
      failed_recovery = e.to_detail_string().find("unable to reconstruct public key from signature") != std::string::npos;
   }

   //We can fail either by failing to recover any key, or recovering the wrong key. Check that at least one of these failed
   BOOST_CHECK_EQUAL(failed_recovery || failed_compare, true);
} FC_LOG_AND_RETHROW();

//damage the recovery index portion of the sig
BOOST_AUTO_TEST_CASE(damage_sig_idx) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   webauthn::signature sig = make_webauthn_sig(priv, auth_data, json);
   char buff[8192];
   datastream<char*> ds(buff, sizeof(buff));
   fc::raw::pack(ds, sig);
   buff[0]++;
   ds.seekp(0);
   fc::raw::unpack(ds, sig);

   bool failed_recovery = false;
   bool failed_compare = false;
   webauthn::public_key recovered_pub;

   try {
      recovered_pub = sig.recover(d, true);
      failed_compare = !(wa_pub == recovered_pub);
   }
   catch(fc::exception& e) {
      failed_recovery = e.to_detail_string().find("unable to reconstruct public key from signature") != std::string::npos;
   }

   //We can fail either by failing to recover any key, or recovering the wrong key. Check that at least one of these failed
   BOOST_CHECK_EQUAL(failed_recovery || failed_compare, true);
} FC_LOG_AND_RETHROW();

//Good signature but with a different private key than was expecting
BOOST_AUTO_TEST_CASE(different_priv_key) try {
   r1::private_key other_priv = fc::crypto::r1::private_key::generate();

   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_NE(wa_pub, make_webauthn_sig(other_priv, auth_data, json).recover(d, true));
} FC_LOG_AND_RETHROW();

//Good signature but has empty json
BOOST_AUTO_TEST_CASE(empty_json) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");
   std::string json;

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::assert_exception, [](const fc::assert_exception& e) {
      return e.to_detail_string().find("Failed to parse client data JSON") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//empty rpid not allowed for pub key
BOOST_AUTO_TEST_CASE(empty_rpid) try {
   char data[67] = {};
   datastream<char*> ds(data, sizeof(data));
   webauthn::public_key pubkey;

   BOOST_CHECK_EXCEPTION(fc::raw::unpack(ds, pubkey), fc::assert_exception, [](const fc::assert_exception& e) {
      return e.to_detail_string().find("webauthn pubkey must have non empty rpid") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

//good sig but remove the trailing =, should still be accepted
BOOST_AUTO_TEST_CASE(good_no_trailing_equal) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");

   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "\"}";
   boost::erase_all(json, "=");

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EQUAL(wa_pub, make_webauthn_sig(priv, auth_data, json).recover(d, true));
} FC_LOG_AND_RETHROW();

//Before the base64 decoder was adjusted to throw in error (pre-webauthn-merge), it would simply stop when encountering
//a non-base64 char. This means it was possible for the following JSON & challenge to validate completely correctly. Now it
//should not pass.
BOOST_AUTO_TEST_CASE(base64_wonky) try {
   webauthn::public_key wa_pub(pub.serialize(), webauthn::public_key::user_presence_t::USER_PRESENCE_NONE, "fctesting.invalid");

   std::string json = "{\"origin\":\"https://fctesting.invalid\",\"type\":\"webauthn.get\",\"challenge\":\"" + fc::base64url_encode(d.data(), d.data_size()) + "!@#$\"}";
   boost::erase_all(json, "=");

   std::vector<uint8_t> auth_data(37);
   memcpy(auth_data.data(), origin_hash.data(), sizeof(origin_hash));

   BOOST_CHECK_EXCEPTION(make_webauthn_sig(priv, auth_data, json).recover(d, true), fc::exception, [](const fc::exception& e) {
      return e.to_detail_string().find("encountered non-base64 character") != std::string::npos;
   });
} FC_LOG_AND_RETHROW();

BOOST_AUTO_TEST_SUITE_END()