#include <boost/asio.hpp>
#include "vdf.h"
using boost::asio::ip::tcp;

const int max_length = 2048;
const int kMaxProcessesAllowed = 3;
std::mutex socket_mutex;

int process_number;

void PrintInfo(std::string input) {
    std::cout << "VDF Client: " << input << "\n";
}

void CreateAndWriteProof(integer D, form x, int64_t num_iterations, WesolowskiCallback& weso, bool& stop_signal, tcp::socket& sock) {
    Proof result = CreateProofOfTimeNWesolowski(D, x, num_iterations, 0, weso, 2, 0, stop_signal);
    if (stop_signal == true) {
        PrintInfo("Got stop signal before completing the proof!");
        return ;
    }
    std::vector<unsigned char> bytes = ConvertIntegerToBytes(integer(num_iterations), 8);
    bytes.insert(bytes.end(), result.y.begin(), result.y.end());
    bytes.insert(bytes.end(), result.proof.begin(), result.proof.end());
    std::string str_result = BytesToStr(bytes);
    std::lock_guard<std::mutex> lock(socket_mutex);
    PrintInfo("Generated proof = " + str_result);;
    boost::asio::write(sock, boost::asio::buffer(str_result.c_str(), str_result.size()));
}

void session(tcp::socket& sock) {
    try {
        char disc[350];
        char disc_size[5];
        boost::system::error_code error;

        memset(disc,0x00,sizeof(disc)); // For null termination
        memset(disc_size,0x00,sizeof(disc_size)); // For null termination

        boost::asio::read(sock, boost::asio::buffer(disc_size, 3), error);
        int disc_int_size = atoi(disc_size);

        boost::asio::read(sock, boost::asio::buffer(disc, disc_int_size), error);

        integer D(disc);
        PrintInfo("Discriminant = " + to_string(D.impl));

        int space_needed = kSwitchIters / 10 + (kMaxItersAllowed - kSwitchIters) / 100;
        forms = (form*) calloc(space_needed, sizeof(form));

        PrintInfo("Calloc'd " + to_string(space_needed * sizeof(form)) + " bytes");

        // Init VDF the discriminant...

        if (error == boost::asio::error::eof)
            return ; // Connection closed cleanly by peer.
        else if (error)
            throw boost::system::system_error(error); // Some other error.

        if (getenv( "warn_on_corruption_in_production" )!=nullptr) {
            warn_on_corruption_in_production=true;
        }
        if (is_vdf_test) {
            PrintInfo( "=== Test mode ===" );
        }
        if (warn_on_corruption_in_production) {
            PrintInfo( "=== Warn on corruption enabled ===" );
        }
        assert(is_vdf_test); //assertions should be disabled in VDF_MODE==0
        init_gmp();
        allow_integer_constructor=true; //make sure the old gmp allocator isn't used
        set_rounding_mode();

        integer L=root(-D, 4);
        form f=form::generator(D);

        bool stop_signal = false;
        // (iteration, thread_id)
        std::set<std::pair<uint64_t, uint64_t> > seen_iterations;
        bool stop_vector[100];

        std::vector<std::thread> threads;
        WesolowskiCallback weso(1000000);

        //mpz_init(weso.forms[0].a.impl);
        //mpz_init(weso.forms[0].b.impl);
        //mpz_init(weso.forms[0].c.impl);

        forms[0]=f;
        weso.D = D;
        weso.L = L;
        weso.kl = 10;

        bool stopped = false;
        bool got_iters = false;
        std::thread vdf_worker(repeated_square, f, D, L, std::ref(weso), std::ref(stopped));

        // Tell client that I'm ready to get the challenges.
        boost::asio::write(sock, boost::asio::buffer("OK", 2));
        char data[20];

        while (!stopped) {
            memset(data, 0, sizeof(data));
            boost::asio::read(sock, boost::asio::buffer(data, 2), error);
            int size = (data[0] - '0') * 10 + (data[1] - '0');
            memset(data, 0, sizeof(data));
            boost::asio::read(sock, boost::asio::buffer(data, size), error);
            int iters = atoi(data);
            got_iters = true;

            if (iters == 0) {
                PrintInfo("Got stop signal!");
                stopped = true;
                for (int i = 0; i < threads.size(); i++)
                    stop_vector[i] = true;
                for (int t = 0; t < threads.size(); t++) {
                    threads[t].join();
                }
                vdf_worker.join();
                free(forms);
            } else {
                int max_iter = 0;
                int max_iter_thread_id = -1;
                int min_iter = std::numeric_limits<int> :: max();
                bool unique = true;
                for (auto active_iter: seen_iterations) {
                    if (active_iter.first > max_iter) {
                        max_iter = active_iter.first;
                        max_iter_thread_id = active_iter.second;
                    }
                    if (active_iter.first < min_iter) {
                        min_iter = active_iter.first;
                    }
                    if (active_iter.first == iters) {
                        unique = false;
                        break;
                    }
                }
                if (!unique) {
                    PrintInfo("Duplicate iteration " + to_string(iters) + "... Ignoring.");
                    continue;
                }
                if (threads.size() < kMaxProcessesAllowed || iters < min_iter) {
                    seen_iterations.insert({iters, threads.size()});
                    PrintInfo("Running proving for iter: " + to_string(iters));
                    stop_vector[threads.size()] = false;
                    threads.push_back(std::thread(CreateAndWriteProof, D, f, iters, std::ref(weso),
                                      std::ref(stop_vector[threads.size()]), std::ref(sock)));
                    if (threads.size() > kMaxProcessesAllowed) {
                        PrintInfo("Stopping proving for iter: " + to_string(max_iter));
                        stop_vector[max_iter_thread_id] = true;
                        seen_iterations.erase({max_iter, max_iter_thread_id});
                    }
                }
            }
        }
    } catch (std::exception& e) {
        PrintInfo("Exception in thread: " + to_string(e.what()));
    }

    try {
        // Tell client I've stopped everything, wait for ACK and close.
        boost::system::error_code error;

        PrintInfo("Stopped everything! Ready for the next challenge.");

        std::lock_guard<std::mutex> lock(socket_mutex);
        boost::asio::write(sock, boost::asio::buffer("STOP", 4));

        char ack[5];
        memset(ack,0x00,sizeof(ack));
        boost::asio::read(sock, boost::asio::buffer(ack, 3), error);
        assert (strncmp(ack, "ACK", 3) == 0);
    } catch (std::exception& e) {
        PrintInfo("Exception in thread: " + to_string(e.what()));
    }
}

int main(int argc, char* argv[])
{
  try {
    if (argc != 4)
    {
      std::cerr << "Usage: ./vdf_client <host> <port> <process_number>\n";
      return 1;
    }

    boost::asio::io_service io_service;

    tcp::resolver resolver(io_service);
    tcp::resolver::query query(tcp::v6(), argv[1], argv[2], boost::asio::ip::resolver_query_base::v4_mapped);
    tcp::resolver::iterator iterator = resolver.resolve(query);

    tcp::socket s(io_service);
    boost::asio::connect(s, iterator);
    process_number = atoi(argv[3]);
    session(s);
  } catch (std::exception& e) {
    std::cerr << "Exception: " << e.what() << "\n";
  }
  return 0;
}


#pragma once

// deal.II includes -------------------------------------------------------
#include <deal.II/base/tensor.h>
// system includes --------------------------------------------------------
#include <boost/multi_array.hpp>
#include <iostream>
// own includes -----------------------------------------------------------
#include "traits/tensor_type_traits.hpp"
#include "var_form.hpp"

namespace boltzmann {
/**
 * @brief common base class for Least squares and SUPG formulation
 *
 */
template <int dimX, int rank>
class PolarXVarFormRhs : public VarForm<dimX, dealii::QGauss>
{
 private:
  typedef VarForm<dimX, dealii::QGauss> base_class;

 public:
  static const int fe_dim;
  static const int n_quad_points;

  //@{
  /* Storage typedefs **/
  typedef typename tensor_traits<rank + 1, dimX>::template value_type<double> Tx_t;
  typedef typename tensor_traits<rank, dimX>::template value_type<double> Sx_t;
  // typedef dealii::Tensor<rank+1, dimX> Tx_t;
  // typedef dealii::Tensor<rank  , dimX> Sx_t;
  /// arrays of dimension dofs_per_cell
  typedef boost::multi_array<Tx_t, 1> Tx_vec_t;
  typedef boost::multi_array<Sx_t, 1> Sx_vec_t;
  //@}
  void clear_storage();

  const Tx_vec_t& T() const { return Tx_vec; }
  const Sx_vec_t& S() const { return Sx_vec; }

 public:
  template <typename FE>
  PolarXVarFormRhs(const FE& fe);

 protected:
  /// storage for stabilization term
  Tx_vec_t Tx_vec;
  /// storage for regular term
  Sx_vec_t Sx_vec;
};

template <int dimX, int rank>
const int PolarXVarFormRhs<dimX, rank>::fe_dim = 1;
/// hardcoded value n_quad_points!
template <int dimX, int rank>
const int PolarXVarFormRhs<dimX, rank>::n_quad_points = 3;

template <int dimX, int rank>
template <typename FE>
PolarXVarFormRhs<dimX, rank>::PolarXVarFormRhs(const FE& fe)
    : base_class(fe, n_quad_points)
{
  int n_dofs_per_cell = fe.dofs_per_cell;
  Tx_vec.resize(boost::extents[n_dofs_per_cell]);
  Sx_vec.resize(boost::extents[n_dofs_per_cell]);
}

template <int dimX, int rank>
void
PolarXVarFormRhs<dimX, rank>::clear_storage()
{
  const int n_dofs_per_cell = this->fe_values.dofs_per_cell;
  for (int i = 0; i < n_dofs_per_cell; ++i) {
    Tx_vec[i] = 0;
    Sx_vec[i] = 0;
  }
}
}  // end namespace boltzmann


#include <boost/math/special_functions/nonfinite_num_facets.hpp>


//
//  Copyright (C) 2021 Danny Havenith
//
//  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 PARSE_LOG_VALUE_ENCODING_HPP_
#define PARSE_LOG_VALUE_ENCODING_HPP_
#include "message.hpp"
#include <string>
#include <boost/lexical_cast.hpp>
#include <boost/endian/conversion.hpp>

/**
 * translate from string to a value of the requested type
 */
template< typename cooked_value_type>
cooked_value_type to_cooked_type( const std::string &value)
{
    // not the fastest way, but easiest to write
    try
    {
        return boost::lexical_cast< cooked_value_type>( value);
    }
    catch (std::exception &)
    {
        throw std::runtime_error( "could not convert this to a number:'" + value + "'");
    }
}

/**
 * Generally the raw type is the same as the cooked type,
 * so just return the cooked value.
 */
template< typename type, typename cooked_value_type>
cooked_value_type to_raw_type( type, const cooked_value_type &value)
{
    return value;
}

template< typename iterator, unsigned int size>
iterator write( rtlogs::signed_<size, rtlogs::big_endian>, long value, iterator destination)
{
    boost::endian::endian_store<long, size, boost::endian::order::big>( destination, value);
    return destination + size;
}

template< typename iterator, unsigned int size>
iterator write( rtlogs::unsigned_<size, rtlogs::big_endian>, unsigned long value, iterator destination)
{
    boost::endian::endian_store<unsigned long, size, boost::endian::order::big>( destination, value);
    return destination + size;
}

template< typename iterator, unsigned int size>
iterator write( rtlogs::signed_<size, rtlogs::little_endian>, long value, iterator destination)
{
    boost::endian::endian_store<long, size, boost::endian::order::little>( destination, value);
    return destination + size;
}

template< typename iterator, unsigned int size>
iterator write( rtlogs::unsigned_<size, rtlogs::little_endian>, unsigned long value, iterator destination)
{
    boost::endian::endian_store<unsigned long, size, boost::endian::order::little>( destination, value);
    return destination + size;
}

template< typename iterator>
iterator write( rtlogs::float32, float value, iterator destination)
{
    static_assert( sizeof( value) == rtlogs::float32::size, "This implementation expects the native float type to match IEEE 754");

    auto as_bytes = reinterpret_cast<unsigned char *>( &value);
    std::copy( as_bytes, as_bytes + rtlogs::float32::size, destination);

    return std::next( destination, rtlogs::float32::size);
}

template< typename iterator>
iterator write( rtlogs::nothing, long, iterator destination)
{
    return destination;
}

/**
 * for fixed point values, we can translate the floating point
 * value to a fixed point integer.
 */
template< typename int_type, unsigned int denom>
typename int_type::value_type to_raw_type( rtlogs::fixed_point< int_type, denom>, double value)
{
    return static_cast<typename int_type::value_type>( value * denom);
}

template< typename type_info, typename T>
T to_raw_type( const T& value)
{
    return value;
}

struct context
{
    int header;
    int channel;
    int column;
};

/**
 * pick a value from a vector of strings, convert it to some numerical value and write it to
 * a range of bytes.
 *
 * returns an iterator one past the last byte written.
 */
template< typename iterator, typename type>
iterator encode( type, const context &c, const std::vector<std::string> &values, iterator destination)
{
    const auto cooked = to_cooked_type<typename rtlogs::cooked_value_type<type>::type>( values[c.column]);
    const auto raw = to_raw_type( type{}, cooked);
    return write( typename type::raw_type_info{}, raw, destination);
}

template< typename iterator>
iterator encode( rtlogs::nothing, const context &c, const std::vector<std::string> &values, iterator destination)
{
    return destination;
}

template< typename iterator, unsigned int size>
iterator encode( rtlogs::ignore<size>, const context &c, const std::vector<std::string> &values, iterator destination)
{
    using std::next;
    std::fill( destination, next( destination, size), 0);
    return next(destination, size);
}

/**
 * Write the channel id to the range of bytes pointed to by the iterator 'destination'.
 */
template< typename iterator>
iterator encode( rtlogs::channel_id, const context &c, const std::vector<std::string> &, iterator destination)
{
    *destination = static_cast<unsigned char>( c.channel);
    return std::next( destination);
}

#endif /* PARSE_LOG_VALUE_ENCODING_HPP_ */


//
// drd-estimator.cpp
//
//  Created by Peter Gusev on 07 June 2016.
//  Copyright 2013-2016 Regents of the University of California
//

#include <boost/thread/lock_guard.hpp>
#include <boost/make_shared.hpp>

#include "drd-estimator.hpp"

using namespace ndnrtc;
using namespace estimators;

DrdEstimator::DrdEstimator(unsigned int initialEstimationMs, unsigned int windowMs):
initialEstimation_(initialEstimationMs),
windowSize_(windowMs),
cachedDrd_(Average(boost::make_shared<TimeWindow>(windowMs))),
originalDrd_(Average(boost::make_shared<TimeWindow>(windowMs))),
generationDelay_(Average(boost::make_shared<TimeWindow>(windowMs))),
latest_(&originalDrd_)
{}

void
DrdEstimator::newValue(double drd, bool isOriginal, double dGen)
{
	double oldValue = (isOriginal ? originalDrd_.value() : cachedDrd_.value());

	if (dGen > 0) generationDelay_.newValue(dGen);

	if (isOriginal)
		originalDrd_.newValue(drd);
	else
		cachedDrd_.newValue(drd);

	latest_ = (isOriginal ? &originalDrd_ : &cachedDrd_);

	double newValue = (isOriginal ? originalDrd_.value() : cachedDrd_.value());
    double deviation = (isOriginal ? originalDrd_.deviation() : cachedDrd_.deviation());

	if (oldValue != newValue)
	{
		boost::lock_guard<boost::mutex> scopedLock(mutex_);
		for (auto& o:observers_) {
			if (isOriginal)
				o->onOriginalDrdUpdate(newValue, deviation);
			else
				o->onCachedDrdUpdate(newValue, deviation);
			o->onDrdUpdate();
		}
	}
}

double
DrdEstimator::getCachedEstimation() const
{ return  cachedDrd_.count() ? cachedDrd_.value() : (double)initialEstimation_; }

double
DrdEstimator::getOriginalEstimation() const
{ return originalDrd_.count() ? originalDrd_.value() : (double)initialEstimation_; }

void
DrdEstimator::reset()
{
	cachedDrd_ = Average(boost::make_shared<TimeWindow>(windowSize_));
	originalDrd_ = Average(boost::make_shared<TimeWindow>(windowSize_));
}

void DrdEstimator::attach(IDrdEstimatorObserver* o)
{
    if (o)
    {
        boost::lock_guard<boost::mutex> scopedLock(mutex_);
        observers_.push_back(o);
    }
}

void DrdEstimator::detach(IDrdEstimatorObserver* o)
{
    std::vector<IDrdEstimatorObserver*>::iterator it = std::find(observers_.begin(), observers_.end(), o);
    if (it != observers_.end())
    {
        boost::lock_guard<boost::mutex> scopedLock(mutex_);
        observers_.erase(it);
    }
}


#ifndef INCLUDE_SWIFT_VIO_PROJ_PARAM_OPT_MODELS_HPP_
#define INCLUDE_SWIFT_VIO_PROJ_PARAM_OPT_MODELS_HPP_

#include <vector>
#include <Eigen/Core>
#include <okvis/ModelSwitch.hpp>

namespace swift_vio {
class ProjectionOptFXY_CXY {
 public:
  static const int kModelId = 1;
  static const size_t kNumParams = 4;
  static inline int getMinimalDim() { return kNumParams; }

  static void localToGlobal(const Eigen::VectorXd& local_opt_params,
                            Eigen::VectorXd* global_proj_params) {
    global_proj_params->head<4>() = local_opt_params;
  }
  static void globalToLocal(const Eigen::VectorXd& global_proj_params,
                            Eigen::VectorXd* local_opt_params) {
    (*local_opt_params) = global_proj_params.head<4>();
  }

  static void kneadIntrinsicJacobian(Eigen::Matrix2Xd* /*intrinsicJacobian*/) {}
  static Eigen::MatrixXd getInitCov(double sigma_focal_length,
                                    double sigma_principal_point) {
    Eigen::MatrixXd covProjIntrinsics = Eigen::Matrix<double, 4, 4>::Identity();
    covProjIntrinsics.topLeftCorner<2, 2>() *= std::pow(sigma_focal_length, 2);
    covProjIntrinsics.bottomRightCorner<2, 2>() *=
        std::pow(sigma_principal_point, 2);
    return covProjIntrinsics;
  }
  static void toDimensionLabels(std::vector<std::string>* dimensionLabels) {
    *dimensionLabels = {"fx[pixel]", "fy", "cx", "cy"};
  }

  static void toDesiredStdevs(Eigen::VectorXd* desiredStdevs) {
    desiredStdevs->resize(4);
    desiredStdevs->setConstant(1.0);
  }
};

class ProjectionOptFX_CXY {
 public:
  static const int kModelId = 2;
  static const size_t kNumParams = 3;
  static inline int getMinimalDim() { return kNumParams; }

  static void localToGlobal(const Eigen::VectorXd& local_opt_params,
                            Eigen::VectorXd* global_proj_params) {
    (*global_proj_params)(0) = local_opt_params[0];
    (*global_proj_params)(1) = local_opt_params[0];
    global_proj_params->segment<2>(2) = local_opt_params.segment<2>(1);
  }
  static void globalToLocal(const Eigen::VectorXd& global_proj_params,
                            Eigen::VectorXd* local_opt_params) {
    local_opt_params->resize(3, 1);
    (*local_opt_params)[0] = global_proj_params[0];
    local_opt_params->segment<2>(1) = global_proj_params.segment<2>(2);
  }
  static void kneadIntrinsicJacobian(Eigen::Matrix2Xd* intrinsicJac) {
    const int resultCols = intrinsicJac->cols() - 1;
    intrinsicJac->col(0) += intrinsicJac->col(1);
    intrinsicJac->block(0, 1, 2, resultCols - 1) =
        intrinsicJac->block(0, 2, 2, resultCols - 1);
    intrinsicJac->conservativeResize(Eigen::NoChange, resultCols);
  }
  static Eigen::MatrixXd getInitCov(double sigma_focal_length,
                                    double sigma_principal_point) {
    Eigen::MatrixXd covProjIntrinsics = Eigen::Matrix<double, 3, 3>::Identity();
    covProjIntrinsics(0, 0) *= std::pow(sigma_focal_length, 2);
    covProjIntrinsics.bottomRightCorner<2, 2>() *=
        std::pow(sigma_principal_point, 2);
    return covProjIntrinsics;
  }
  static void toDimensionLabels(std::vector<std::string>* dimensionLabels) {
    *dimensionLabels = {"fx[pixel]", "cx", "cy"};
  }

  static void toDesiredStdevs(Eigen::VectorXd* desiredStdevs) {
    desiredStdevs->resize(3);
    desiredStdevs->setConstant(1.0);
  }
};

class ProjectionOptFX {
 public:
  static const int kModelId = 3;
  static const size_t kNumParams = 1;
  static inline int getMinimalDim() { return kNumParams; }

  static void localToGlobal(const Eigen::VectorXd& local_opt_params,
                            Eigen::VectorXd* global_proj_params) {
    (*global_proj_params)[0] = local_opt_params[0];
    (*global_proj_params)[1] = local_opt_params[0];
  }
  static void globalToLocal(const Eigen::VectorXd& global_proj_params,
                            Eigen::VectorXd* local_opt_params) {
    local_opt_params->resize(1, 1);
    (*local_opt_params)[0] = global_proj_params[0];
  }
  static void kneadIntrinsicJacobian(Eigen::Matrix2Xd* intrinsicJac) {
    const int resultCols = intrinsicJac->cols() - 3;
    intrinsicJac->col(0) += intrinsicJac->col(1);
    intrinsicJac->block(0, 1, 2, resultCols - 1) =
        intrinsicJac->block(0, 4, 2, resultCols - 1);
    intrinsicJac->conservativeResize(Eigen::NoChange, resultCols);
  }
  static Eigen::MatrixXd getInitCov(double sigma_focal_length,
                                    double /*sigma_principal_point*/) {
    Eigen::MatrixXd covProjIntrinsics =
        Eigen::Matrix<double, 1, 1>::Identity() *
        std::pow(sigma_focal_length, 2);
    return covProjIntrinsics;
  }
  static void toDimensionLabels(std::vector<std::string>* dimensionLabels) {
    *dimensionLabels = {"fx[pixel]"};
  }
  static void toDesiredStdevs(Eigen::VectorXd* desiredStdevs) {
    desiredStdevs->resize(1);
    desiredStdevs->setConstant(1.0);
  }
};

#ifndef PROJ_OPT_MODEL_CASES
#define PROJ_OPT_MODEL_CASES                \
  PROJ_OPT_MODEL_CASE(ProjectionOptFXY_CXY) \
  PROJ_OPT_MODEL_CASE(ProjectionOptFX_CXY)  \
  PROJ_OPT_MODEL_CASE(ProjectionOptFX)
#endif

inline int ProjectionOptGetMinimalDim(int model_id) {
  switch (model_id) {
#define MODEL_CASES PROJ_OPT_MODEL_CASES
#define PROJ_OPT_MODEL_CASE(ProjOptModel) \
  case ProjOptModel::kModelId:            \
    return ProjOptModel::getMinimalDim();

    MODEL_SWITCH_CASES

#undef PROJ_OPT_MODEL_CASE
#undef MODEL_CASES
  }
  return 0;
}

inline int ProjectionOptNameToId(std::string pinhole_opt_rep, bool* isFixed=nullptr) {
  std::transform(pinhole_opt_rep.begin(), pinhole_opt_rep.end(),
                 pinhole_opt_rep.begin(),
                 [](unsigned char c) { return std::toupper(c); });
  if (isFixed) {
      *isFixed = false;
  }
  if (pinhole_opt_rep.compare("FXY_CXY") == 0) {
    return ProjectionOptFXY_CXY::kModelId;
  } else if (pinhole_opt_rep.compare("FX_CXY") == 0) {
    return ProjectionOptFX_CXY::kModelId;
  } else if (pinhole_opt_rep.compare("FX") == 0) {
    return ProjectionOptFX::kModelId;
  } else {
    if (isFixed) {
        *isFixed = true;
    }
    return ProjectionOptFXY_CXY::kModelId;
  }
}

inline void ProjectionOptKneadIntrinsicJacobian(
    int model_id, Eigen::Matrix2Xd* intrinsicJac) {
  switch (model_id) {
#define MODEL_CASES PROJ_OPT_MODEL_CASES
#define PROJ_OPT_MODEL_CASE(ProjOptModel) \
  case ProjOptModel::kModelId:            \
    return ProjOptModel::kneadIntrinsicJacobian(intrinsicJac);

    MODEL_SWITCH_CASES

#undef PROJ_OPT_MODEL_CASE
#undef MODEL_CASES
  }
}

// apply estimated projection intrinsic params to the full param vector
// be careful global_proj_params may contain trailing distortion params
inline void ProjectionOptLocalToGlobal(int model_id,
                                       const Eigen::VectorXd& local_opt_params,
                                       Eigen::VectorXd* global_proj_params) {
  switch (model_id) {
#define MODEL_CASES PROJ_OPT_MODEL_CASES
#define PROJ_OPT_MODEL_CASE(ProjOptModel) \
  case ProjOptModel::kModelId:            \
    return ProjOptModel::localToGlobal(local_opt_params, global_proj_params);

    MODEL_SWITCH_CASES

#undef PROJ_OPT_MODEL_CASE
#undef MODEL_CASES
  }
}

inline void ProjectionOptGlobalToLocal(
    int model_id, const Eigen::VectorXd& global_proj_params,
    Eigen::VectorXd* local_opt_params) {
  switch (model_id) {
#define MODEL_CASES PROJ_OPT_MODEL_CASES
#define PROJ_OPT_MODEL_CASE(ProjOptModel) \
  case ProjOptModel::kModelId:            \
    return ProjOptModel::globalToLocal(global_proj_params, local_opt_params);

    MODEL_SWITCH_CASES

#undef PROJ_OPT_MODEL_CASE
#undef MODEL_CASES
  }
}

inline Eigen::MatrixXd ProjectionModelGetInitCov(int model_id,
                                                 double sigma_focal_length,
                                                 double sigma_principal_point) {
  switch (model_id) {
#define MODEL_CASES PROJ_OPT_MODEL_CASES
#define PROJ_OPT_MODEL_CASE(ProjOptModel) \
  case ProjOptModel::kModelId:            \
    return ProjOptModel::getInitCov(sigma_focal_length, sigma_principal_point);

    MODEL_SWITCH_CASES

#undef PROJ_OPT_MODEL_CASE
#undef MODEL_CASES
  }
  return Eigen::MatrixXd();
}

inline void ProjectionOptToDimensionLabels(int model_id, std::vector<std::string>* dimensionLabels) {
    switch (model_id) {
  #define MODEL_CASES PROJ_OPT_MODEL_CASES
  #define PROJ_OPT_MODEL_CASE(ProjOptModel) \
    case ProjOptModel::kModelId:            \
      return ProjOptModel::toDimensionLabels(dimensionLabels);

      MODEL_SWITCH_CASES

  #undef PROJ_OPT_MODEL_CASE
  #undef MODEL_CASES
    }
}

inline void ProjectionOptToDesiredStdevs(int model_id,
                                         Eigen::VectorXd *desiredStdevs) {
  switch (model_id) {
#define MODEL_CASES PROJ_OPT_MODEL_CASES
#define PROJ_OPT_MODEL_CASE(ProjOptModel)                                      \
  case ProjOptModel::kModelId:                                                 \
    return ProjOptModel::toDesiredStdevs(desiredStdevs);

    MODEL_SWITCH_CASES

#undef PROJ_OPT_MODEL_CASE
#undef MODEL_CASES
  }
}

}  // namespace swift_vio
#endif  // INCLUDE_SWIFT_VIO_PROJ_PARAM_OPT_MODELS_HPP_


/*
 * This file will be generated as an executable and wrapper for apc_features
 * Takes a ***folder*** within ***resource folder***  pcd file as input and calls apc_features
 * The PCD is located in the packagepath/resources/pcd and file name is given as command line argument
 * Hv to think abt multiple file interface, a dir iterator is more meaningful
 */

#include <ros/package.h>
#include <apc_capture/apc_features.h>
#include <apc_capture/box_registration.h>
#include <apc_objects/apc_objects.h>
#include <boost/filesystem.hpp>
#include <camera_calibration_parsers/parse_ini.h>
#include <sstream>
#include <Eigen/Core>
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <tf_conversions/tf_eigen.h>
#include <boost/algorithm/string/split.hpp>
#include <string>
#include <pcl/filters/conditional_removal.h>

namespace fs = boost::filesystem;

int main(int argc, char** argv)
{
	if(argc != 2)
	{
		fprintf(stderr, "Usage: apc_features_exe <path to dataset>\n");
		return 1;
	}

	std::string rosPackagePath = ros::package::getPath("apc_capture");
	std::string dataPath = argv[1];

	fs::path dataDir(dataPath);
	if(!fs::is_directory(dataDir))
	{
		fprintf(stderr, "Could not open the data directory.\n");
		return 0;
	}

	// Gather list of images
	std::vector<fs::path> jobs;
	{
		auto it = fs::recursive_directory_iterator(dataDir);
		auto end = fs::recursive_directory_iterator();

		for(; it != end; ++it)
		{
			fs::path path = *it;
			if(fs::is_directory(path) && fs::exists(path / "polygons.yaml"))
				jobs.push_back(fs::canonical(path));
		}
	}

	std::string iniFileName, pcdFileName, boxIndexFileName, rgbFileName, shelfMaskFileName;

	apc_object_perception::ApcFeatures features;

	// Load shelf mesh
	pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloudfromMesh (new pcl::PointCloud<pcl::PointXYZ>);
	std::string MeshPath = rosPackagePath+"/resource/pcd/shelf_withbackface.pcd";
	if(pcl::io::loadPCDFile(MeshPath, *m_cloudfromMesh) < 0)
	{
		fprintf(stderr, "Could not load shelf mesh\n");
		return 1;
	}

	// Translate to distRobot2shelf  (FIXME: This is a magic number)
	Eigen::Affine3f translation(Eigen::Translation3f(apc_objects::distRobot2Shelf,0,0));
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTemp (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*m_cloudfromMesh,*cloudTemp,translation.matrix());
	m_cloudfromMesh=cloudTemp;

	features.setMeshCloud(m_cloudfromMesh);

	for(const fs::path& path : jobs)
	{
		printf("\n\nProcessing frame %s\n", path.c_str());

		iniFileName = (path / "camera.ini").string();
		pcdFileName = (path / "frame.pcd").string();
		rgbFileName = (path / "rgb.png").string();
		boxIndexFileName = (path / "box_index.txt").string();
		shelfMaskFileName = (path / "mask_container.png").string();

		std::ifstream File(boxIndexFileName);
		std::string line;
		Eigen::Vector2i box_indices;
		std::vector<std::string> indices;

		while (std::getline(File, line)) {
			boost::algorithm::split(indices, line, boost::algorithm::is_any_of(" "));
			std::string::size_type sz;

			box_indices(0) = std::stoi(indices[0], &sz);
			box_indices(1) = std::stoi(indices[1], &sz);

			std::cout << "Box indices: " << box_indices(0)<< " " << box_indices(1) << std::endl;
		}

		features.setBoxIndices(box_indices);
		apc_capture::BoxCoordinates boxMsg;
		boxMsg = features.getBoxCoordinates();

		Eigen::Vector3f cameraGuess;
		cameraGuess.x() = apc_objects::distRobot2Shelf + boxMsg.boxCoordinates[0] - 0.4f; // 20 cms infont of the box front face
		cameraGuess.y() = ( boxMsg.boxCoordinates[3] + boxMsg.boxCoordinates[2] ) / 2;
		cameraGuess.z() = ( boxMsg.boxCoordinates[4] + boxMsg.boxCoordinates[5] ) / 2 + 0.04f;

		std::cout << "camera guess: " << cameraGuess.transpose() << std::endl;

		// Camera
		pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloudfromSensorXYZ(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr m_cloudfromSensorXYZRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
		if(pcl::io::loadPCDFile(pcdFileName, *m_cloudfromSensorXYZRGB) < 0)
		{
			fprintf(stderr, "Could not load PCD file 'frame.pcd' for image '%s'\n", path.c_str());
			return 1;
		}
		pcl::copyPointCloud(*m_cloudfromSensorXYZRGB, *m_cloudfromSensorXYZ);

		cv::Mat rgbImage = cv::imread(rgbFileName);
		cv::Mat shelfMask = cv::imread(shelfMaskFileName,CV_LOAD_IMAGE_GRAYSCALE);

		// Erode the shelf mask (we want to discard points rather than use them for ICP)
		{
			constexpr int erosion_size = 10;
			cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE,
				cv::Size(2*erosion_size + 1, 2*erosion_size+1),
				cv::Point(erosion_size, erosion_size)
			);
			cv::erode(shelfMask, shelfMask, element);
		}

		Eigen::Affine3f translationCaminWorld;
		translationCaminWorld = Eigen::Translation3f(cameraGuess);
		Eigen::Affine3f transformCaminWorld = translationCaminWorld *
				Eigen::AngleAxisf(10 *(M_PI/180) ,Eigen::Vector3f::UnitY()) *
				Eigen::AngleAxisf(M_PI/2 ,Eigen::Vector3f::UnitY()) *
				Eigen::AngleAxisf(M_PI/2 ,Eigen::Vector3f::UnitZ()) ;

		printf("Running ICP...\n");
		Eigen::Affine3f camPoints2Shelf = features.doRegistration(m_cloudfromSensorXYZ, transformCaminWorld, shelfMask);
		printf("done.\n");

		// Transform sensor cloud into shelf frame
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudInShelfFrame (new pcl::PointCloud<pcl::PointXYZRGB>);
		pcl::transformPointCloud(*m_cloudfromSensorXYZRGB,*cloudInShelfFrame,camPoints2Shelf);

		// Filter out some outliers
		float error_limit = 0.05f;// *** check this***
		pcl::ConditionAnd<pcl::PointXYZRGB>::Ptr range_cond (new pcl::ConditionAnd<pcl::PointXYZRGB> ());
		range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZRGB> ("x", pcl::ComparisonOps::GT, boxMsg.boxCoordinates[0] - error_limit)));
		range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZRGB> ("x", pcl::ComparisonOps::LT, boxMsg.boxCoordinates[1] + error_limit)));
		range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZRGB> ("y", pcl::ComparisonOps::GT, boxMsg.boxCoordinates[2] - error_limit)));
		range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZRGB> ("y", pcl::ComparisonOps::LT, boxMsg.boxCoordinates[3] + error_limit)));
		range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZRGB> ("z", pcl::ComparisonOps::GT, boxMsg.boxCoordinates[4] - error_limit))); //0.08
		range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZRGB> ("z", pcl::ComparisonOps::LT, boxMsg.boxCoordinates[5] + error_limit))); //0.05

		pcl::ConditionalRemoval<pcl::PointXYZRGB> removal (range_cond);
		removal.setInputCloud (cloudInShelfFrame);
		removal.setKeepOrganized(true);
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudT (new pcl::PointCloud<pcl::PointXYZRGB>);
		removal.filter (*cloudT);
		cloudInShelfFrame = cloudT;

		std::string cameraName;
		sensor_msgs::CameraInfoPtr camInfo(new sensor_msgs::CameraInfo);
		camera_calibration_parsers::readCalibrationIni(iniFileName,cameraName,*camInfo);
		features.setPointCloud(cloudInShelfFrame);
		features.setCameraModel(camInfo);
		features.setRGBImage(rgbImage);

		cv::Mat_<float> height3D,height2D,dist2shelf,depth;
		cv::Mat isValidHeight;
		cv::Mat edgeFeature;

		Eigen::Affine3f shelfToCameraFrame = camPoints2Shelf.inverse();

		cv::Mat binMask;
		features.createBinMask(shelfToCameraFrame,binMask);
		features.computeDist2ShelfFeature(dist2shelf);
		features.computeHeightFeatures(camPoints2Shelf,height3D,height2D,isValidHeight,depth);
		features.computeEdge(edgeFeature);

		cv::Mat hha = features.computeHHA(camPoints2Shelf, m_cloudfromSensorXYZRGB);

		// New HVA, HHV encodings
		{
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr tgvCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
			pcl::io::loadPCDFile((path / "frame_tgv.pcd").string(), *tgvCloud);

			if(tgvCloud->size() > 0)
			{
				cv::Mat_<uint8_t> validMask = cv::imread((path / "frame_tgv_weights.png").string(), CV_LOAD_IMAGE_GRAYSCALE);

				for(int y = 0; y < validMask.rows; ++y)
				{
					for(int x = 0; x < validMask.cols; ++x)
					{
						if(validMask(y,x) != 0)
							validMask(y,x) = 100;
					}
				}

				cv::Mat hva = features.computeHVA(camPoints2Shelf, tgvCloud, validMask);
				cv::Mat hhv = features.computeHHV(camPoints2Shelf, tgvCloud, validMask);
				cv::Mat hhav = features.computeHHAV(camPoints2Shelf, tgvCloud, validMask);

				cv::imwrite((path / "feature_hva.png").string(), hva);
				cv::imwrite((path / "feature_hhv.png").string(), hhv);
				cv::imwrite((path / "feature_hhav.png").string(), hhav);
			}
		}

		{
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr tgvCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
			pcl::io::loadPCDFile((path / "frame_upper_filled.pcd").string(), *tgvCloud);

			if(tgvCloud->size() == 0) // can happen on run frames
				pcl::io::loadPCDFile((path / "frame_tgv.pcd").string(), *tgvCloud);

			if(tgvCloud->size() > 0)
			{
				cv::Mat_<uint8_t> validMask = cv::imread((path / "frame_tgv_weights.png").string(), CV_LOAD_IMAGE_GRAYSCALE);

				for(int y = 0; y < validMask.rows; ++y)
				{
					for(int x = 0; x < validMask.cols; ++x)
					{
						if(validMask(y,x) != 0)
							validMask(y,x) = 100;
					}
				}

				cv::Mat hha = features.computeHHA(camPoints2Shelf, tgvCloud);
				cv::Mat hva = features.computeHVA(camPoints2Shelf, tgvCloud, validMask);
				cv::Mat hhv = features.computeHHV(camPoints2Shelf, tgvCloud, validMask);
				cv::Mat hhav = features.computeHHAV(camPoints2Shelf, tgvCloud, validMask);

				cv::imwrite((path / "feature_upper_hha.png").string(), hha);
				cv::imwrite((path / "feature_upper_hva.png").string(), hva);
				cv::imwrite((path / "feature_upper_hhv.png").string(), hhv);
				cv::imwrite((path / "feature_upper_hhav.png").string(), hhav);
			}
		}

		// For visualizing
		cv::Mat height2DViz (height2D.rows,height2D.cols,CV_8UC1);
		cv::Mat height3DViz (height2D.rows,height2D.cols,CV_8UC1);
		cv::Mat dist2shelfViz(dist2shelf.rows,dist2shelf.cols,CV_8UC1);
		double min, max;
		double distMin,distMax;
		double height3DMin, height3DMax;

		cv::minMaxLoc(dist2shelf,&distMin,&distMax);
		cv::minMaxLoc(height2D,&min,&max);
		cv::minMaxLoc(height3D, &height3DMin, &height3DMax);
		std::cout<<" Dist2Shelf Min/Max : "<<distMin<<" "<<distMax<<std::endl;

		std::cout << "Height 3D Min/Max: " << height3DMin <<"  "<< height3DMax << "\n";
		height3DMin = -0.02;

		distMin = 0, distMax = 0.1;
		for (int y = 0;y < height2D.rows;y++)
		{
			for (int x = 0; x < height2D.cols;x++)
			{
				height2DViz.at<uchar>(y,x) =cv::saturate_cast<uint8_t>(( ( height2D(y,x) -min ) / ( max - min ) )*255);
				dist2shelfViz.at<uchar>(y,x) =cv::saturate_cast<uint8_t>(( ( dist2shelf(y,x) -distMin ) / ( distMax - distMin ) )*255);
				height3DViz.at<uchar>(y,x) = cv::saturate_cast<uint8_t>( (height3D(y,x) - height3DMin) / (height3DMax - height3DMin) * 255 );
			}
		}

		cv::flip(height2DViz, height2DViz, -1);
		cv::flip(dist2shelfViz, dist2shelfViz, -1);
		cv::flip(height3DViz, height3DViz, -1);

		isValidHeight = 255*isValidHeight ;
		cv::imwrite((path / "feature_height2D.png").string(), height2DViz);
		cv::imwrite((path / "feature_height3D.png").string(), height3DViz);
		cv::imwrite((path / "feature_isValidHeight.png").string(), isValidHeight);
		cv::imwrite((path / "feature_edge.png").string(), edgeFeature);
		cv::imwrite((path / "feature_dist2shelf.png").string(), dist2shelfViz);
		cv::imwrite((path / "feature_hha.png").string(), hha);
		cv::imwrite((path / "bin_mask_estimated.png").string(), binMask);

		FILE* file = fopen((path / "feature_height2D.raw").c_str(), "w");
		fwrite(height2D.data, sizeof(float), height2D.rows*height2D.cols, file);
		fclose(file);

		file = fopen((path / "feature_dist2shelf.raw").c_str(), "w");
		fwrite(dist2shelf.data, sizeof(float), dist2shelf.rows*dist2shelf.cols, file);
		fclose(file);

		file = fopen((path / "feature_height3D.raw").c_str(), "w");
		fwrite(height3D.data, sizeof(float), height3D.rows*height3D.cols, file);
		fclose(file);

		file = fopen((path / "feature_depth.raw").c_str(), "w");
		fwrite(depth.data, sizeof(float), depth.rows*depth.cols, file);
		fclose(file);
	}

	return 0;
}


#include "LevelSet.h"

#include <array>

#include <Eigen/Dense>
#include <Eigen/SVD>

#include "tbb/blocked_range3d.h"
#include "tbb/tbb.h"

namespace FluidSim3D
{

LevelSet::LevelSet()
    : myNarrowBand(0)
    , myPhiGrid(Transform(0, Vec3d::Zero()), Vec3i::Zero())
    , myIsBackgroundNegative(false)
{
    exactinit();
}

LevelSet::LevelSet(const Transform& xform, const Vec3i& size)
    : LevelSet(xform, size, size[0] * size[1] * size[2])
{}

LevelSet::LevelSet(const Transform& xform, const Vec3i& size, double bandwidth, bool isBoundaryNegative)
    : myNarrowBand(bandwidth * xform.dx())
    , myIsBackgroundNegative(isBoundaryNegative)
    , myPhiGrid(xform, size, myIsBackgroundNegative ? -myNarrowBand : myNarrowBand)
{
    for (int axis : {0, 1, 2}) assert(size[axis] >= 0);

    // In order to deal with triangle meshes, we need to initialize
    // the geometric predicate library.
    exactinit();
}

void LevelSet::initFromMesh(const TriMesh& initialMesh, bool doResizeGrid)
{
	// The internal code can't handle a mesh that falls outside of the bounds.
	// If the mesh does, we need to create a new copy and clamp it into the bounds of the grid.

    if (!doResizeGrid)
    {
		bool outOfBounds = false;
		for (const Vec3d& vertex : initialMesh.vertices())
		{
			Vec3d indexPoint = worldToIndex(vertex);

            for (int axis : {0, 1, 2})
            {
			    if (indexPoint[axis] <= 0 || indexPoint[axis] >= size()[axis])
				    outOfBounds = true;
            }
		}

		if (outOfBounds)
		{
			TriMesh clampedMesh = initialMesh;

			// Clamp to be inside grid
			for (int vertIndex = 0; vertIndex < clampedMesh.vertexCount(); ++vertIndex)
			{
				const Vec3d& vertex = clampedMesh.vertex(vertIndex);
				Vec3d indexPoint = worldToIndex(vertex);

				double offset = 1e-5 * dx();

                for (int axis : {0, 1, 2})
                {
                    indexPoint[axis] = std::clamp(indexPoint[axis], offset, size()[axis] - 1. - offset);
                }

				clampedMesh.setVertex(vertIndex, indexToWorld(indexPoint));
			}

			initFromMeshImpl(clampedMesh, doResizeGrid);
			return;
		}
    }

    initFromMeshImpl(initialMesh, doResizeGrid);
}

void LevelSet::reinit()
{
    TriMesh tempMesh = buildMesh();
    initFromMeshImpl(tempMesh, false);
}

bool LevelSet::isGridMatched(const LevelSet& grid) const
{
    if (size() != grid.size()) return false;
    if (xform() != grid.xform()) return false;
    return true;
}

bool LevelSet::isGridMatched(const ScalarGrid<double>& grid) const
{
    if (grid.sampleType() != ScalarGridSettings::SampleType::CENTER) return false;
    if (size() != grid.size()) return false;
    if (xform() != grid.xform()) return false;
    return true;
}

void LevelSet::unionSurface(const LevelSet& unionPhi)
{
    assert(isGridMatched(unionPhi));

    tbb::parallel_for(tbb::blocked_range<int>(0, voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
    {
        for (int index = range.begin(); index != range.end(); ++index)
        {
            Vec3i cell = myPhiGrid.unflatten(index);
            myPhiGrid(cell) = std::min(myPhiGrid(cell), unionPhi(cell));
        }
    });
}

bool LevelSet::isBackgroundNegative() const
{
    return myIsBackgroundNegative;
}

void LevelSet::setBackgroundNegative()
{
    myIsBackgroundNegative = true;
}

TriMesh LevelSet::buildMesh() const
{
    // Create grid to store index to dual contouring point. Note that phi is
    // center sampled so the DC grid must be node sampled and one cell shorter
    // in each dimension
    UniformGrid<int> dcPointIndices(size() - Vec3i::Ones(), -1);
    std::vector<std::pair<Vec3i, Vec3d>> dcPointPair;

    // Build list of dual contouring points
    {
        tbb::enumerable_thread_specific<std::vector<std::pair<Vec3i, Vec3d>>> parallelDCPoints;

        tbb::parallel_for(tbb::blocked_range<int>(0, dcPointIndices.voxelCount(), tbbHeavyGrainSize), [&](const tbb::blocked_range<int>& range)
        {
            auto& localDCPoints = parallelDCPoints.local();

            for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
            {
                Vec3i cell = dcPointIndices.unflatten(cellIndex);

                VecVec3d points;
                VecVec3d normals;

                Vec3d averagePoint = Vec3d::Zero();

                for (int edgeAxis : {0, 1, 2})
                    for (int edgeIndex = 0; edgeIndex < 4; ++edgeIndex)
                    {
                        Vec3i edge = cellToEdge(cell, edgeAxis, edgeIndex);

                        Vec3i backwardNode = edgeToNode(edge, edgeAxis, 0);
                        Vec3i forwardNode = edgeToNode(edge, edgeAxis, 1);

                        // Look for zero crossings.
                        // Note that nodes for the DC grid fall exactly on the cell centers
                        // of the level set grid.
                        if ((myPhiGrid(backwardNode) <= 0 && myPhiGrid(forwardNode) > 0) ||
                            (myPhiGrid(backwardNode) > 0 && myPhiGrid(forwardNode) <= 0))
                        {
                            // Find interface point
                            Vec3d point = interpolateInterface(backwardNode, forwardNode);

                            for (int axis : {0, 1, 2})
                                assert(point[axis] >= backwardNode[axis] && point[axis] <= forwardNode[axis]);

                            points.push_back(point);

                            averagePoint += point;

                            // Find associated surface normal
                            Vec3d localNormal = normal(indexToWorld(point));

                            normals.push_back(localNormal);
                        }
                    }

                if (points.size() > 0)
                {
                    averagePoint.array() /= double(points.size());

                    Matrix3x3d AtA = Matrix3x3d::Zero();

                    Vec3d rhs = Vec3d::Zero();

                    for (int pointIndex = 0; pointIndex < points.size(); ++pointIndex)
                    {
                        AtA += normals[pointIndex] * normals[pointIndex].transpose();
                        rhs += normals[pointIndex] * normals[pointIndex].dot(points[pointIndex] - averagePoint);
                    }

                    Eigen::SelfAdjointEigenSolver<Matrix3x3d> eigenSolver(AtA);
                    const Vec3d& eigenvalues = eigenSolver.eigenvalues();

                    // Clamp eigenvalues
                    double tolerance = 0.01 * eigenvalues.cwiseAbs().maxCoeff();

                    Vec3d invEigenvalues;
                    int clamped = 0;
                    for (int index : {0, 1, 2})
                    {
                        if (std::fabs(eigenvalues[index] < tolerance))
                        {
                            invEigenvalues[index] = 0;
                            ++clamped;
                        }
                        else
                        {
                            invEigenvalues[index] = 1. / eigenvalues[index];
                        }
                    }

                    Vec3d qefPoint;
                    if (clamped < 3)
                    {
                        qefPoint = averagePoint + eigenSolver.eigenvectors() * invEigenvalues.asDiagonal() * eigenSolver.eigenvectors().transpose() * rhs;
                    }
                    else
                    {
                        qefPoint = averagePoint;
                    }

                    // Clamp to cell
                    if (qefPoint[0] < cell[0] || qefPoint[0] > cell[0] + 1 ||
                        qefPoint[1] < cell[1] || qefPoint[1] > cell[1] + 1 ||
                        qefPoint[2] < cell[2] || qefPoint[2] > cell[2] + 1)
                    {
                        AlignedBox3d cellBbox(cell.cast<double>());
                        cellBbox.extend((cell + Vec3i::Ones()).cast<double>());
                        Vec3d rayDirection = averagePoint - qefPoint;
                        double alpha = computeRayBBoxIntersection(cellBbox, qefPoint, rayDirection);

                        if (alpha <= 1)
                        {
                            qefPoint += alpha * rayDirection;
                        }
                        else
                        {
                            qefPoint = averagePoint;
                        }
                    }

                    localDCPoints.emplace_back(cell, qefPoint);
                }
            }
        });

        mergeLocalThreadVectors(dcPointPair, parallelDCPoints);
    }

    VecVec3d vertices(dcPointPair.size());

    // Set DC point index for direct look up when building mesh and
    // convert points to world space.
    tbb::parallel_for(tbb::blocked_range<size_t>(0, dcPointPair.size(), tbbLightGrainSize), [&](const tbb::blocked_range<size_t>& range)
    {
        for (size_t index = range.begin(); index != range.end(); ++index)
        {
            const Vec3i& cell = dcPointPair[index].first;

            assert(dcPointIndices(cell) == -1);
            dcPointIndices(cell) = int(index);

            const Vec3d& point = dcPointPair[index].second;
            vertices[index] = indexToWorld(point);
        }
    });

    // Build triangle mesh using dual contouring points

    VecVec3i triangles;

    for (int edgeAxis : {0, 1, 2})
    {
        Vec3i start = Vec3i::Zero();
        ++start[(edgeAxis + 1) % 3];
        ++start[(edgeAxis + 2) % 3];

        Vec3i end = dcPointIndices.size();

        tbb::enumerable_thread_specific<VecVec3i> parallelTriangles;

        auto loopRange3d = tbb::blocked_range3d<int>(start[0], end[0], int(std::cbrt(tbbLightGrainSize)), start[1], end[1],
                                      int(std::cbrt(tbbLightGrainSize)), start[2], end[2], int(std::cbrt(tbbLightGrainSize)));

        tbb::parallel_for(loopRange3d, [&](const tbb::blocked_range3d<int>& range)
        {
            auto& localTriFaces = parallelTriangles.local();

            Vec3i edge;
            for (edge[0] = range.pages().begin(); edge[0] != range.pages().end(); ++edge[0])
                for (edge[1] = range.rows().begin(); edge[1] != range.rows().end(); ++edge[1])
                    for (edge[2] = range.cols().begin(); edge[2] != range.cols().end(); ++edge[2])
                    {
                        Vec3i backwardNode = edgeToNode(edge, edgeAxis, 0);
                        Vec3i forwardNode = edgeToNode(edge, edgeAxis, 1);

                        if ((myPhiGrid(backwardNode) <= 0 && myPhiGrid(forwardNode) > 0) ||
                            (myPhiGrid(backwardNode) > 0 && myPhiGrid(forwardNode) <= 0))
                        {
                            std::array<Vec3i, 4> dcCells;
                            std::array<int, 4> vertexIndices;

                            for (int cellIndex = 0; cellIndex < 4; ++cellIndex)
                            {
                                dcCells[cellIndex] = edgeToCellCCW(edge, edgeAxis, cellIndex);
                                vertexIndices[cellIndex] = dcPointIndices(dcCells[cellIndex]);
                                assert(vertexIndices[cellIndex] >= 0);
                            }

                            if (myPhiGrid(backwardNode) <= 0.)
                            {
                                localTriFaces.emplace_back(vertexIndices[0], vertexIndices[1], vertexIndices[2]);
                                localTriFaces.emplace_back(vertexIndices[0], vertexIndices[2], vertexIndices[3]);
                            }
                            else
                            {
                                localTriFaces.emplace_back(vertexIndices[0], vertexIndices[2], vertexIndices[1]);
                                localTriFaces.emplace_back(vertexIndices[0], vertexIndices[3], vertexIndices[2]);
                            }
                        }
                    }
        });

        mergeLocalThreadVectors(triangles, parallelTriangles);
    }

    return TriMesh(triangles, vertices);
}

void LevelSet::clear()
{
    myPhiGrid.clear();
}

void LevelSet::resize(const Vec3i& size)
{
    myPhiGrid.resize(size);
}

// Find the nearest point on the interface starting from the index position.
// If the position falls outside of the narrow band, there isn't a defined gradient
// to use. In this case, the original position will be returned.
Vec3d LevelSet::findSurface(const Vec3d& worldPoint, int iterationLimit, double tolerance) const
{
    assert(iterationLimit >= 0);

    double phi = myPhiGrid.triLerp(worldPoint);

    double epsilon = tolerance * dx();
    Vec3d tempPoint = worldPoint;

    int iterationCount = 0;
    if (std::fabs(phi) < myNarrowBand)
    {
        while (std::fabs(phi) > epsilon && iterationCount < iterationLimit)
        {
            tempPoint -= phi * .8 * normal(tempPoint);
            phi = myPhiGrid.triLerp(tempPoint);
            ++iterationCount;
        }
    }

    return tempPoint;
}

Vec3d LevelSet::interpolateInterface(const Vec3i& startPoint, const Vec3i& endPoint) const
{
    assert((myPhiGrid(startPoint) <= 0 && myPhiGrid(endPoint) > 0) ||
           (myPhiGrid(startPoint) > 0 && myPhiGrid(endPoint) <= 0));

    // Find weight to zero isosurface
    double theta = lengthFraction(myPhiGrid(startPoint), myPhiGrid(endPoint));

    assert(theta >= 0 && theta <= 1);

    if (myPhiGrid(startPoint) > 0)
        theta = 1. - theta;

    return startPoint.cast<double>() + theta * (endPoint - startPoint).cast<double>();
}

void LevelSet::drawGrid(Renderer& renderer, bool doOnlyNarrowBand) const
{
    if (doOnlyNarrowBand)
    {
        forEachVoxelRange(Vec3i::Zero().eval(), size(), [&](const Vec3i& cell)
        {
            if (std::fabs(myPhiGrid(cell)) < myNarrowBand) myPhiGrid.drawGridCell(renderer, cell);
        });
    }
    else
        myPhiGrid.drawGrid(renderer);
}

void LevelSet::drawGridPlane(Renderer& renderer, Axis planeAxis, double position, bool doOnlyNarrowBand) const
{
    position = std::clamp(position, double(0), double(1));

    Vec3i start = Vec3i::Zero();
    Vec3i end(myPhiGrid.size() - Vec3i::Ones());

    if (planeAxis == Axis::XAXIS)
    {
        start[0] = int(std::floor(position * double(myPhiGrid.size()[0] - 1)));
        end[0] = start[0] + 1;
    }
    else if (planeAxis == Axis::YAXIS)
    {
        start[1] = int(std::floor(position * double(myPhiGrid.size()[1] - 1)));
        end[1] = start[1] + 1;
    }
    else if (planeAxis == Axis::ZAXIS)
    {
        start[2] = int(std::floor(position * double(myPhiGrid.size()[2] - 1)));
        end[2] = start[2] + 1;
    }

    forEachVoxelRange(start, end, [&](const Vec3i& cell)
    {
        if (doOnlyNarrowBand)
        {
            if (std::fabs(myPhiGrid(cell)) < myNarrowBand) myPhiGrid.drawGridCell(renderer, cell);
        }
        else
            myPhiGrid.drawGridCell(renderer, cell);
    });
}

// Display a supersampled slice of the grid. The plane will have a normal in the plane_axis direction.
// The position is from [0,1] where 0 is at the grid origin and 1 is at the origin + size * dx.
void LevelSet::drawSupersampledValuesPlane(Renderer& renderer, Axis planeAxis, double position, double radius,
                                           int samples, double sampleSize) const
{
    myPhiGrid.drawSupersampledValuesPlane(renderer, planeAxis, position, radius, samples, sampleSize);
}
void LevelSet::drawSampleNormalsPlane(Renderer& renderer, Axis planeAxis, double position, const Vec3d& colour,
                                      double length) const
{
    myPhiGrid.drawSampleGradientsPlane(renderer, planeAxis, position, colour, length);
}

void LevelSet::drawSurface(Renderer& renderer, const Vec3d& colour, double lineWidth) const
{
    TriMesh tempMesh = buildMesh();
    tempMesh.drawMesh(renderer, true, colour, lineWidth);
}

//
// Private methods
//

void LevelSet::initFromMeshImpl(const TriMesh& initialMesh, bool doResizeGrid)
{
    if (doResizeGrid)
    {
        // Determine the bounding box of the mesh to build the underlying grids
		AlignedBox3d bbox = initialMesh.boundingBox();

		// Expand grid beyond the narrow band of the mesh
		double maxPadding = 50. * dx();
		maxPadding = std::min(2. * myNarrowBand, maxPadding);

		bbox.extend(bbox.min() - Vec3d::Constant(maxPadding));
		bbox.extend(bbox.max() + Vec3d::Constant(maxPadding));

		Vec3d origin = indexToWorld(floor(worldToIndex(bbox.min())).eval());
		Transform xform(dx(), origin);
		Vec3d topRight = indexToWorld(ceil(worldToIndex(bbox.max())).eval());

		// TODO: add the ability to reset grid so we don't have ot re-allocate memory
		myPhiGrid = ScalarGrid<double>(xform, ((topRight - origin) / dx()).cast<int>(), myIsBackgroundNegative ? -myNarrowBand : myNarrowBand);
    }

    // We want to track which cells in the level set contain valid distance information.
    // The first pass will set cells close to the mesh as FINISHED.
    UniformGrid<VisitedCellLabels> reinitializedCells(size(), VisitedCellLabels::UNVISITED_CELL);
    UniformGrid<int> meshCellParities(size(), 0);

    for (const Vec3i& tri : initialMesh.triangles())
    {
        // It's easier to work in our index space and just scale the distance later.
        std::array<Vec3d, 3> triVertices;
        for (int localVertexIndex : {0, 1, 2})
            triVertices[localVertexIndex] = worldToIndex(initialMesh.vertex(tri[localVertexIndex]));

		AlignedBox3d triBbox(triVertices[0]);
		triBbox.extend(triVertices[1]);
		triBbox.extend(triVertices[2]);

        Vec3i triCeilMin = ceil(triBbox.min()).cast<int>();
        Vec3i triFloorMin = floor(triBbox.min()).cast<int>() - Vec3i::Ones();
        Vec3i triFloorMax = floor(triBbox.max()).cast<int>();

        // Z-axis intersection tests. Iterate along an aligned set of grid edges
        // in decsending order, checking for intersections at each edge.
        // If an intersection is found then we can stop searching along the set.
        for (int i = triCeilMin[0]; i <= triFloorMax[0]; ++i)
            for (int j = triFloorMin[1]; j <= triFloorMax[1]; ++j)
                for (int k = triFloorMax[2]; k >= triFloorMin[2]; --k)
                {
                    Vec3d gridPoint(i, j, k);
                    IntersectionLabels intersectionResult = exactTriIntersect(gridPoint, triVertices[0], triVertices[1], triVertices[2], Axis::ZAXIS);

                    for (int axis : {0, 1, 2})
				        assert(gridPoint[axis] >= 0 && gridPoint[axis] < myPhiGrid.size()[axis] - 1);

                    if (intersectionResult == IntersectionLabels::NO) continue;

                    int parityChange = -1;
                    double qrs = orient2d(triVertices[0].data(), triVertices[1].data(), triVertices[2].data());
                    assert(qrs != 0);
                    if (qrs < 0)
                        parityChange = 1;

                    if (intersectionResult == IntersectionLabels::YES)
                        meshCellParities(i, j, k + 1) += parityChange;
                    else
                    {
                        assert(intersectionResult == IntersectionLabels::ON);

                        if (parityChange == 1)
                            meshCellParities(i, j, k) += parityChange;
                        else
                        {
                            meshCellParities(i, j, k + 1) += parityChange;
                        }
                    }

                    break;
                }
    }

    // Now that all the z-axis edge crossings have been found, we can compile the parity changes
    // and label grid nodes that are at the interface
    for (int i = 0; i < size()[0]; ++i)
        for (int j = 0; j < size()[1]; ++j)
        {
            int parity = myIsBackgroundNegative ? 1 : 0;

            for (int k = 0; k < size()[2]; ++k)
            {
                Vec3i cell(i, j, k);

                parity += meshCellParities(cell);
                meshCellParities(cell) = parity;

                if (parity > 0)
                    myPhiGrid(cell) = -myNarrowBand;
                else
                    myPhiGrid(cell) = myNarrowBand;
            }

            assert(myIsBackgroundNegative ? parity == 1 : parity == 0);
        }

    // With the parity assigned, loop over the grid once more and label nodes that have a sign change
    // with neighbouring nodes (this means parity goes from -'ve (and zero) to +'ve or vice versa).
    forEachVoxelRange(Vec3i::Ones(), size() - Vec3i::Ones(), [&](const Vec3i& cell)
    {
        bool isCellInside = meshCellParities(cell) > 0;

        for (int axis : {0, 1, 2})
            for (int direction : {0, 1})
            {
                Vec3i adjacentCell = cellToCell(cell, axis, direction);

                bool isAdjacentCellInside = meshCellParities(adjacentCell) > 0;

                if (isCellInside != isAdjacentCellInside)
                    reinitializedCells(cell) = VisitedCellLabels::FINISHED_CELL;
            }
    });

    // Loop over all the triangles in the mesh. Level set grid cells labelled as FINISHED will be
    // updated with the distance to the surface if it happens to be shorter than the current
    // distance to the surface.
    for (const Vec3i& tri : initialMesh.triangles())
    {
        std::array<Vec3d, 3> vertices;
        for (int localVertexIndex : {0, 1, 2})
            vertices[localVertexIndex] = worldToIndex(initialMesh.vertex(tri[localVertexIndex]));

        AlignedBox3d triBbox(vertices[0]);
        triBbox.extend(vertices[1]);
        triBbox.extend(vertices[2]);

        // Expand outward by 2-voxels in each direction
        triBbox.extend(triBbox.min() - Vec3d::Constant(2));
        triBbox.extend(triBbox.max() + Vec3d::Constant(2));

        AlignedBox3d clampBbox;
        clampBbox.extend(Vec3d::Zero());
        clampBbox.extend(size().cast<double>() - Vec3d::Ones());

        triBbox.clamp(clampBbox);

        for (int axis : {0, 1, 2})
            assert(triBbox.min()[axis] >= 0 && triBbox.max()[axis] < size()[axis]);

        forEachVoxelRange(triBbox.min().cast<int>(), triBbox.max().cast<int>() + Vec3i::Ones(), [&](const Vec3i& cell)
        {
            if (reinitializedCells(cell) != VisitedCellLabels::UNVISITED_CELL)
            {
                Vec3d cellPoint = cell.cast<double>();

                Vec3d triProjectionPoint = pointToTriangleProjection(cellPoint, vertices[0], vertices[1], vertices[2]);

                double surfaceDistance = (cellPoint - triProjectionPoint).norm() * dx();

                // If the new distance is closer than existing distance values, update cell
                if (std::fabs(myPhiGrid(cell)) > surfaceDistance)
                {
                    // If the parity says the node is inside, set it to be negative
                    myPhiGrid(cell) = (meshCellParities(cell) > 0) ? -surfaceDistance : surfaceDistance;
                }
            }
        });
    }

    reinitFastMarching(reinitializedCells);
}

void LevelSet::reinitFastMarching(UniformGrid<VisitedCellLabels>& reinitializedCells)
{
    assert(reinitializedCells.size() == size());

    auto solveEikonal2D = [&](double Ux, double Uy) -> double
    {
        if (std::fabs(Ux - Uy) >= dx())
            return std::min(Ux, Uy) + dx();
        else
        {
            // Quadratic equation from the Eikonal
            double rootEntry = std::pow(Ux + Uy, 2) - 2. * (std::pow(Ux, 2) + std::pow(Uy, 2) - std::pow(dx(), 2));
            assert(rootEntry >= 0);
            return .5 * (Ux + Uy + std::sqrt(rootEntry));
        }
    };

    auto solveEikonal = [&](const Vec3i& cell) -> double
    {
        double max = std::numeric_limits<double>::max();

        Vec3d Uaxis = Vec3d::Constant(max);
		for (int axis : {0, 1, 2})
			for (int direction : {0, 1})
			{
				Vec3i adjacentCell = cellToCell(cell, axis, direction);

				if (adjacentCell[axis] < 0 || adjacentCell[axis] >= size()[axis])
				{
					assert(myPhiGrid(cell) > 0 && !myIsBackgroundNegative || myPhiGrid(cell) < 0 && myIsBackgroundNegative);
					Uaxis[axis] = std::min(max, Uaxis[axis]);
				}
				else
				{
					Uaxis[axis] = std::min(std::fabs(myPhiGrid(adjacentCell)), Uaxis[axis]);
				}
			}

        double discrim = std::pow(Uaxis.sum(), 2) - 3. * (Uaxis.squaredNorm() - std::pow(dx(), 2));
        if (discrim < 0.)
        {
            double dist = std::min(std::min(solveEikonal2D(Uaxis[0], Uaxis[1]), solveEikonal2D(Uaxis[1], Uaxis[2])), solveEikonal2D(Uaxis[0], Uaxis[2]));

            assert(std::isfinite(dist));

            return dist;
        }
        else
        {
            double dist = (Uaxis.sum() + std::sqrt(discrim)) / 3.;
            assert(std::isfinite(dist));
            return dist;
        }
    };

    // Load up the BFS queue with the unvisited cells next to the finished ones
    using Node = std::pair<Vec3i, double>;
    auto cmp = [](const Node& a, const Node& b) -> bool { return std::fabs(a.second) > std::fabs(b.second); };
    std::priority_queue<Node, std::vector<Node>, decltype(cmp)> marchingQ(cmp);

    forEachVoxelRange(Vec3i::Zero(), reinitializedCells.size(), [&](const Vec3i& cell)
    {
        if (reinitializedCells(cell) == VisitedCellLabels::FINISHED_CELL)
        {
            for (int axis : {0, 1, 2})
                for (int direction : {0, 1})
                {
                    Vec3i adjacentCell = cellToCell(cell, axis, direction);

                    if (adjacentCell[axis] < 0 || adjacentCell[axis] >= reinitializedCells.size()[axis]) continue;

                    if (reinitializedCells(adjacentCell) == VisitedCellLabels::UNVISITED_CELL)
                    {
                        double dist = solveEikonal(adjacentCell);

                        if (!std::isfinite(dist))
                            int a = 0;
                        assert(dist >= 0);

                        myPhiGrid(adjacentCell) = myPhiGrid(adjacentCell) < 0 ? -dist : dist;

                        Node node(adjacentCell, dist);

                        marchingQ.push(node);
                        reinitializedCells(adjacentCell) = VisitedCellLabels::VISITED_CELL;
                    }
                }
        }
    });

    while (!marchingQ.empty())
    {
        Node localNode = marchingQ.top();
        Vec3i localCell = localNode.first;
        marchingQ.pop();

        // Since you can't just update parts of the priority queue,
        // it's possible that a cell has been solidified at a smaller distance
        // and an older insert if doubleing around.
        if (reinitializedCells(localCell) == VisitedCellLabels::FINISHED_CELL)
        {
            // Make sure that the distance assigned to the cell is smaller than
            // what is doubleing around
            assert(std::fabs(myPhiGrid(localCell)) <= std::fabs(localNode.second));
            continue;
        }
        assert(reinitializedCells(localCell) == VisitedCellLabels::VISITED_CELL);

        if (std::fabs(myPhiGrid(localCell)) < myNarrowBand)
        {
            // Debug check that there is indeed a FINISHED cell next to it
            bool foundFinishedCell = false;

            for (int axis : {0, 1, 2})
                for (int direction : {0, 1})
                {
                    Vec3i adjacentCell = cellToCell(localCell, axis, direction);

                    if (adjacentCell[axis] < 0 || adjacentCell[axis] >= reinitializedCells.size()[axis]) continue;

                    if (reinitializedCells(adjacentCell) == VisitedCellLabels::FINISHED_CELL)
                        foundFinishedCell = true;
                    else
                    {
                        double dist = solveEikonal(adjacentCell);
                        assert(dist >= 0);

                        if (dist > myNarrowBand) dist = myNarrowBand;

                        if (reinitializedCells(adjacentCell) == VisitedCellLabels::VISITED_CELL &&
                            dist > std::fabs(myPhiGrid(adjacentCell)))
                            continue;

                        myPhiGrid(adjacentCell) = myPhiGrid(adjacentCell) < 0 ? -dist : dist;

                        Node node(adjacentCell, dist);

                        marchingQ.push(node);
                        reinitializedCells(adjacentCell) = VisitedCellLabels::VISITED_CELL;
                    }
                }
            assert(foundFinishedCell);
        }
        else
            myPhiGrid(localCell) = myPhiGrid(localCell) < 0 ? -myNarrowBand : myNarrowBand;

        reinitializedCells(localCell) = VisitedCellLabels::FINISHED_CELL;
    }
}

Vec3d LevelSet::findSurfaceIndex(const Vec3d& indexPoint, int iterationLimit, double tolerance) const
{
    Vec3d worldPoint = indexToWorld(indexPoint);
    worldPoint = findSurface(worldPoint, iterationLimit, tolerance);
    return worldToIndex(worldPoint);
}

}


// Copyright (C) 2006  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <dlib/rand.h>
#include <dlib/compress_stream.h>
#include <dlib/hash.h>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.rand");

    void check_bpp (
        const std::string str
    )
    {
        istringstream rdata;
        ostringstream sout;
        rdata.str(str);
        double compressed_size;
        compress_stream::kernel_1a cs1;
        compress_stream::kernel_2a cs2;

        compress_stream_kernel_1<
            entropy_encoder_model_kernel_5<257,entropy_encoder::kernel_1a,4000000,4>,
            entropy_decoder_model_kernel_5<257,entropy_decoder::kernel_1a,4000000,4>,
            crc32::kernel_1a
            > cs3;

        print_spinner();

        rdata.clear();
        rdata.seekg(0);
        sout.clear();
        sout.str("");
        cs1.compress(rdata,sout);
        compressed_size = sout.str().size();
        compressed_size *= 8;
        compressed_size /= str.size();
        DLIB_TEST_MSG(compressed_size >= 8, "order 0 bps: " << compressed_size);
        dlog << LINFO << "order 0: " << compressed_size;

        print_spinner();

        rdata.clear();
        rdata.seekg(0);
        sout.clear();
        sout.str("");
        cs2.compress(rdata,sout);
        compressed_size = sout.str().size();
        compressed_size *= 8;
        compressed_size /= str.size();
        DLIB_TEST_MSG(compressed_size >= 8, "order 1 bps: " << compressed_size);
        dlog << LINFO << "order 1: " << compressed_size;

        print_spinner();

        rdata.clear();
        rdata.seekg(0);
        sout.clear();
        sout.str("");
        cs3.compress(rdata,sout);
        compressed_size = sout.str().size();
        compressed_size *= 8;
        compressed_size /= str.size();
        DLIB_TEST_MSG(compressed_size >= 8, "order 4 bps: " << compressed_size);
        dlog << LINFO << "order 4: " << compressed_size;

    }

    template <
        typename rand
        >
    void rand_test (
    )
    /*!
        requires
            - rand is an implementation of rand/rand_kernel_abstract.h
              is instantiated with int
        ensures
            - runs tests on rand for compliance with the specs
    !*/
    {

        ostringstream seed;
        seed << (unsigned int)time(0);

        ostringstream sout;

        rand r, r2;
        DLIB_TEST(r.get_seed() == "");
        r.set_seed(seed.str());

        DLIB_TEST(r.get_seed() == seed.str());
        r.clear();
        DLIB_TEST(r.get_seed() == "");
        swap(r,r2);
        DLIB_TEST(r.get_seed() == "");
        r.set_seed(seed.str());
        DLIB_TEST(r.get_seed() == seed.str());
        swap(r,r2);
        DLIB_TEST(r2.get_seed() == seed.str());
        DLIB_TEST(r.get_seed() == "");
        swap(r,r2);
        DLIB_TEST(r.get_seed() == seed.str());
        DLIB_TEST(r2.get_seed() == "");

        print_spinner();
        unsigned long size = 100000;
        for (unsigned long i = 0; i < size; ++i)
        {
            uint32 ch = r.get_random_32bit_number();
            sout.write((char*)&ch,4);
        }

        check_bpp(sout.str());
        sout.clear();
        sout.str("");

        print_spinner();
        for (unsigned long i = 0; i < size; ++i)
        {
            uint16 ch = r.get_random_16bit_number();
            sout.write((char*)&ch,2);
        }

        check_bpp(sout.str());
        sout.clear();
        sout.str("");

        print_spinner();
        for (unsigned long i = 0; i < size; ++i)
        {
            unsigned char ch = r.get_random_8bit_number();
            sout.write((char*)&ch,1);
        }

        check_bpp(sout.str());
        sout.clear();
        sout.str("");

        // make sure the things can serialize right
        {
            r.clear();
            r2.clear();

            for (int i =0; i < 1000; ++i)
            {
                r.get_random_32bit_number();
                r.get_random_gaussian();
            }

            ostringstream sout;
            serialize(r, sout);

            istringstream sin(sout.str());
            deserialize(r2, sin);

            for (int i =0; i < 1000; ++i)
            {
                DLIB_TEST(r.get_random_32bit_number() == r2.get_random_32bit_number());
                DLIB_TEST(std::abs(r.get_random_gaussian() - r2.get_random_gaussian()) < 1e-14);
            }
        }

        // make sure calling clear() and set_seed("") do the same thing
        {
            r.clear();
            r2.set_seed("");
            rand r3;

            DLIB_TEST(r.get_seed() == r2.get_seed());
            DLIB_TEST(r.get_seed() == r3.get_seed());

            for (int i =0; i < 1000; ++i)
            {
                const uint32 num1 = r.get_random_32bit_number();
                const uint32 num2 = r2.get_random_32bit_number();
                const uint32 num3 = r3.get_random_32bit_number();
                DLIB_TEST( num1 == num2);
                DLIB_TEST( num1 == num3);
            }
        }

    }

    template <typename rand_type>
    void test_normal_numbers(
        rand_type& rnd
    )
    {
        print_spinner();
        dlog << LINFO << "test normality";
        double cnt1 = 0; // num <= -1.2
        double cnt2 = 0; // num <= -0.5
        double cnt3 = 0; // num <= 0
        double cnt4 = 0; // num <= 0.5
        double cnt5 = 0; // num <= 1.2

        const unsigned long total = 1000000;
        for (unsigned long i = 0; i < total; ++i)
        {
            const double r = rnd.get_random_gaussian();
            if (r <= -1.2) cnt1 += 1;
            if (r <= -0.5) cnt2 += 1;
            if (r <=  0)   cnt3 += 1;
            if (r <=  0.5) cnt4 += 1;
            if (r <=  1.2) cnt5 += 1;
        }

        cnt1 /= total;
        cnt2 /= total;
        cnt3 /= total;
        cnt4 /= total;
        cnt5 /= total;

        dlog << LINFO << "cnt1: "<< cnt1;
        dlog << LINFO << "cnt2: "<< cnt2;
        dlog << LINFO << "cnt3: "<< cnt3;
        dlog << LINFO << "cnt4: "<< cnt4;
        dlog << LINFO << "cnt5: "<< cnt5;

        DLIB_TEST(std::abs(cnt1 - 0.11507) < 0.001);
        DLIB_TEST(std::abs(cnt2 - 0.30854) < 0.001);
        DLIB_TEST(std::abs(cnt3 - 0.5)     < 0.001);
        DLIB_TEST(std::abs(cnt4 - 0.69146) < 0.001);
        DLIB_TEST(std::abs(cnt5 - 0.88493) < 0.001);

    }

    void test_gaussian_random_hash()
    {
        print_spinner();
        dlog << LINFO << "test_gaussian_random_hash()";
        double cnt1 = 0; // num <= -1.2
        double cnt2 = 0; // num <= -0.5
        double cnt3 = 0; // num <= 0
        double cnt4 = 0; // num <= 0.5
        double cnt5 = 0; // num <= 1.2

        const unsigned long total = 1000000;
        for (unsigned long i = 0; i < total; ++i)
        {
            const double r = gaussian_random_hash(i,0,0);
            if (r <= -1.2) cnt1 += 1;
            if (r <= -0.5) cnt2 += 1;
            if (r <=  0)   cnt3 += 1;
            if (r <=  0.5) cnt4 += 1;
            if (r <=  1.2) cnt5 += 1;
        }
        for (unsigned long i = 0; i < total; ++i)
        {
            const double r = gaussian_random_hash(0,i,0);
            if (r <= -1.2) cnt1 += 1;
            if (r <= -0.5) cnt2 += 1;
            if (r <=  0)   cnt3 += 1;
            if (r <=  0.5) cnt4 += 1;
            if (r <=  1.2) cnt5 += 1;
        }
        for (unsigned long i = 0; i < total; ++i)
        {
            const double r = gaussian_random_hash(0,0,i);
            if (r <= -1.2) cnt1 += 1;
            if (r <= -0.5) cnt2 += 1;
            if (r <=  0)   cnt3 += 1;
            if (r <=  0.5) cnt4 += 1;
            if (r <=  1.2) cnt5 += 1;
        }

        cnt1 /= total*3;
        cnt2 /= total*3;
        cnt3 /= total*3;
        cnt4 /= total*3;
        cnt5 /= total*3;

        dlog << LINFO << "cnt1: "<< cnt1;
        dlog << LINFO << "cnt2: "<< cnt2;
        dlog << LINFO << "cnt3: "<< cnt3;
        dlog << LINFO << "cnt4: "<< cnt4;
        dlog << LINFO << "cnt5: "<< cnt5;

        DLIB_TEST(std::abs(cnt1 - 0.11507) < 0.001);
        DLIB_TEST(std::abs(cnt2 - 0.30854) < 0.001);
        DLIB_TEST(std::abs(cnt3 - 0.5)     < 0.001);
        DLIB_TEST(std::abs(cnt4 - 0.69146) < 0.001);
        DLIB_TEST(std::abs(cnt5 - 0.88493) < 0.001);
    }

    void test_uniform_random_hash()
    {
        print_spinner();
        dlog << LINFO << "test_uniform_random_hash()";
        double cnt1 = 0; // num <= 0.2
        double cnt2 = 0; // num <= 0.4
        double cnt3 = 0; // num <= 0.6
        double cnt4 = 0; // num <= 0.8
        double cnt5 = 0; // num <= 1.0

        double min_val = 10;
        double max_val = 0;

        const unsigned long total = 1000000;
        for (unsigned long i = 0; i < total; ++i)
        {
            const double r = uniform_random_hash(i,0,0);
            min_val = min(r,min_val);
            max_val = max(r,max_val);

            if (r <=  0.2) cnt1 += 1;
            if (r <=  0.4) cnt2 += 1;
            if (r <=  0.6) cnt3 += 1;
            if (r <=  0.8) cnt4 += 1;
            if (r <=  1.0) cnt5 += 1;
        }
        for (unsigned long i = 0; i < total; ++i)
        {
            const double r = uniform_random_hash(0,i,0);
            min_val = min(r,min_val);
            max_val = max(r,max_val);

            if (r <=  0.2) cnt1 += 1;
            if (r <=  0.4) cnt2 += 1;
            if (r <=  0.6) cnt3 += 1;
            if (r <=  0.8) cnt4 += 1;
            if (r <=  1.0) cnt5 += 1;
        }
        for (unsigned long i = 0; i < total; ++i)
        {
            const double r = uniform_random_hash(0,0,i);
            min_val = min(r,min_val);
            max_val = max(r,max_val);

            if (r <=  0.2) cnt1 += 1;
            if (r <=  0.4) cnt2 += 1;
            if (r <=  0.6) cnt3 += 1;
            if (r <=  0.8) cnt4 += 1;
            if (r <=  1.0) cnt5 += 1;
        }

        cnt1 /= total*3;
        cnt2 /= total*3;
        cnt3 /= total*3;
        cnt4 /= total*3;
        cnt5 /= total*3;

        dlog << LINFO << "cnt1: "<< cnt1;
        dlog << LINFO << "cnt2: "<< cnt2;
        dlog << LINFO << "cnt3: "<< cnt3;
        dlog << LINFO << "cnt4: "<< cnt4;
        dlog << LINFO << "cnt5: "<< cnt5;
        dlog << LINFO << "min_val: "<< min_val;
        dlog << LINFO << "max_val: "<< max_val;

        DLIB_TEST(std::abs(cnt1 - 0.2) < 0.001);
        DLIB_TEST(std::abs(cnt2 - 0.4) < 0.001);
        DLIB_TEST(std::abs(cnt3 - 0.6) < 0.001);
        DLIB_TEST(std::abs(cnt4 - 0.8) < 0.001);
        DLIB_TEST(std::abs(cnt5 - 1.0) < 0.001);
        DLIB_TEST(std::abs(min_val - 0.0) < 0.001);
        DLIB_TEST(std::abs(max_val - 1.0) < 0.001);
    }

    void test_get_integer()
    {

        print_spinner();
        dlib::rand rnd;

        int big_val = 0;
        int small_val = 0;

        const long long maxval = (((unsigned long long)1)<<62) + (((unsigned long long)1)<<61);
        for (int i = 0; i < 10000000; ++i)
        {
            if (rnd.get_integer(maxval) > maxval/2)
                ++big_val;
            else
                ++small_val;
        }

        // make sure there isn't any funny bias
        DLIB_TEST(std::abs(big_val/(double)small_val - 1) < 0.001);

        //cout << big_val/(double)small_val << endl;

    }

    class rand_tester : public tester
    {
    public:
        rand_tester (
        ) :
            tester ("test_rand",
                    "Runs tests on the rand component.")
        {}

        void perform_test (
        )
        {
            dlog << LINFO << "testing kernel_1a";
            rand_test<dlib::rand>();
            rand_test<dlib::rand>();

            dlib::rand rnd;
            test_normal_numbers(rnd);
            test_gaussian_random_hash();
            test_uniform_random_hash();
            test_get_integer();
        }
    } a;

}


#include <iostream>
#include <fstream>
#include <sstream>
#include <stdlib.h>
#include <Structs/Graphs/dynamicGraph.h>
#include <Structs/Graphs/adjacencyListImpl.h>
#include <Algorithms/multicriteriaDijkstra.h>
#include <Algorithms/multicriteriaGraph.h>
#include <Algorithms/namoaStar.h>
#include <Algorithms/namoaStar2.h>
#include <Algorithms/multicriteriaArc.h>
#include <Heuristics/blind.h>
#include <Heuristics/ideal.h>
#include <Heuristics/boundedIdeal.h>
#include <Checkers/multiCriteriaChecker.h>
#include <Checkers/multiCriteriaResults.h>
#include <Checkers/Grids/multiCriteriaGridChecker.h>
#include <Utilities/timer.h>
#include <Utilities/colormod.h>
#include <boost/program_options.hpp>

namespace po = boost::program_options;

template< class algorithmVariant, typename GraphType>
void runQueries( GraphType& G, std::vector< std::pair<unsigned int,unsigned int> >& queries,
                 std::vector<typename GraphType::NodeDescriptor>& ids, GridChecker& gChecker, const unsigned int showOnScreen)
{
    typedef typename GraphType::NodeIterator NodeIterator;
    NodeIterator s,t;
    unsigned int sourceId, targetId;
    unsigned int timestamp = 0;
    //create algorithm
    algorithmVariant algorithm( G, NUM_CRITERIA, &timestamp);
    //run queries
    unsigned int query_n = 0;
    double numLabels = 0;
    double totalTime = 0;
    for( std::vector< std::pair<unsigned int,unsigned int> >::iterator it = queries.begin();
         it != queries.end(); ++it)
    {
        //clear nodes
        NodeIterator u, lastnode;
        for( u = G.beginNodes(), lastnode = G.endNodes(); u != lastnode; ++u)
        {
            u->g_op.clear();
            u->g_cl.clear();
        }
        sourceId = it->first;
        targetId = it->second;
        s = G.getNodeIterator( ids[sourceId]);
        t = G.getNodeIterator( ids[targetId]);
        Timer timer;
        timer.start();
        algorithm.init( s, t, NUM_CRITERIA);
        auto heuristicTime = 1000 * timer.getElapsedTime();
        timer.start();
        algorithm.runQuery( s, t);
        auto runtime = 1000 * timer.getElapsedTime();
        totalTime += runtime;
        // add Pareto efficient solutions to the results class
        MulticriteriaResults mResults( queries);
        mResults.addResults( t->g_cl);
        numLabels += algorithm.getGeneratedLabels();
        // Two things are checked: First, the set of solutions is the same; second, the solutions have been found in the same order.
        bool order = true;
        bool correctness = mResults.checkParetoCosts( gChecker, query_n, order);
        Color::Modifier def(Color::FG_DEFAULT);
        Color::Modifier red(Color::FG_RED);
        Color::Modifier green(Color::FG_GREEN);
        Color::Modifier yellow(Color::FG_YELLOW);
        Color::Modifier b_blue(Color::BG_BLUE);
        Color::Modifier b_def(Color::BG_DEFAULT);

        std::cout << b_blue << "(" << sourceId << "->" << targetId << ")" << b_def << "\t\t";

        if ( showOnScreen)
        {
            Color::Modifier blue(Color::FG_BLUE);
            Color::Modifier lblue(Color::FG_LIGHT_BLUE);
            Color::Modifier cyan(Color::FG_CYAN);
            Color::Modifier lcyan(Color::FG_LIGHT_CYAN);
            std::cout << blue << t->g_cl.size() << def << " efficient paths were found.\n";
            std::cout << "\t\t\t" << lblue << algorithm.getGeneratedLabels() << def << " labels were scanned.\n";
            std::cout << "\t\t\t" << cyan << heuristicTime << def << " msec. - calculation of heuristic.\n";
            std::cout << "\t\t\t" << lcyan << runtime << def << " msec. - algorithm runtime.\n";
        }
        std::cout << "Solutions ... ->\t";
        if ( !( correctness))
        {
            std::cout << red << "Different!!!\n" << def;
            t->printLabels( std::cout, G);
            exit (EXIT_FAILURE);
        }
        else
        {
            std::cout << green << "OK" << def << "\n";
            if (! (order)) std::cout << yellow << "Warning!" << def << " The same set of solutions was found, but in different order.\n";
        }
        std::cout << "---------------------------------------------------------------------\n";
        ++query_n;
    }

    if ( showOnScreen)
    {
        std::cout << "\tAlgorithm Runtime:\t" << totalTime << " msec. ( " << (totalTime / queries.size()) << " msec. per query)\n";
        std::cout << "\tGenerated labels:\t " << numLabels << std::endl;
    }
}

template< typename GraphType>
void runBenchmarks( GraphType& G, std::vector< std::pair<unsigned int,unsigned int> >& queries,
                    std::vector<typename GraphType::NodeDescriptor>& ids, GridChecker& gChecker,
                    const std::string& name, const unsigned int& algorithmVariant, const unsigned int showOnScreen)
{
    switch( algorithmVariant)
    {
    case 1:
        // output message for all queries
        std::cout << "with NAMOA* (blind) ...\n\n";
        runQueries<NamoaStar2<GraphType,BlindHeuristic> >( G, queries, ids, gChecker, showOnScreen);
        break;
    case 2:
        std::cout << "with NAMOA*_tc ...\n\n";
        runQueries<NamoaStar2<GraphType,TCHeuristic> >( G, queries, ids, gChecker, showOnScreen);
        break;
    case 3:
        std::cout << "with NAMOA*_bound_tc ...\n\n";
        runQueries<NamoaStar2<GraphType,BoundedTCHeuristic> >( G, queries, ids, gChecker, showOnScreen);
        //std::cout << "with NAMOA* Arc Flags (" << name << ") ...\n\n";
        //runQueries<NamoaStarArc<GraphType,GreatCircleHeuristic> >( G, queries, ids, gChecker, showOnScreen);
        break;
    case 0: // default
        std::cout << "with NAMOA* (blind) ...\n\n";
        runQueries<NamoaStar2<GraphType,BlindHeuristic> >( G, queries, ids, gChecker, showOnScreen);
        std::cout << "with NAMOA*_tc ...\n\n";
        runQueries<NamoaStar2<GraphType,TCHeuristic> >( G, queries, ids, gChecker, showOnScreen);
        std::cout << "with NAMOA*_bound_tc ...\n\n";
        runQueries<NamoaStar2<GraphType,BoundedTCHeuristic> >( G, queries, ids, gChecker, showOnScreen);
        break;
    }
}

typedef DynamicGraph< AdjacencyListImpl, Node, Edge>       Graph;
//typedef DynamicGraph< AdjacencyListImpl, LWNode, Edge>     LWGraph;
typedef Graph::NodeIterator                                NodeIterator;
typedef Graph::EdgeIterator                                EdgeIterator;
typedef Graph::NodeDescriptor                              NodeDescriptor;

int main( int argc, char* argv[])
{
    std::string gridBenchmarksPath = "/home/francis/Projects/Benchmarks/";
    std::string gridsPath = gridBenchmarksPath + "grids/";
    std::string gridQueriesPath = gridBenchmarksPath + "queries/";
    std::string gridSolutionsPath = gridBenchmarksPath + "solutions/";

    unsigned int benchmarkVariant = 2;
    unsigned int algorithmVariant = 0;
    unsigned int showOnScreen = 1;

    // Declare the supported options.
    po::options_description desc("Allowed options");
    desc.add_options()
        ("benchmark,b", po::value< unsigned int>(), "Benchmark to run. Bicriteria[2],  Tri-criterion[3]. Default:2")
        ("algorithm,a", po::value< unsigned int>(), "Multicriteria Heuristic. All[0], Blind[1], Ideal Point[2], Bounded Ideal point[3]. Default:0")
        ("showOnScreen,s", po::value< unsigned int>(), "Display stats on screen. Yes[1], No[0]. Default:1");

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.empty()) {
        std::cout << desc << "\n";
        return 0;
    }
    if (vm.count("benchmark"))
    {
        benchmarkVariant = vm["benchmark"].as<unsigned int>();
    }
    if (vm.count("algorithm"))
    {
        algorithmVariant = vm["algorithm"].as<unsigned int>();
    }
    if (vm.count("showOnScreen"))
    {
        showOnScreen = vm["showOnScreen"].as<unsigned int>();
    }
    unsigned int nqueries;
    if ( benchmarkVariant == 2)
    {
        NUM_CRITERIA = 2;
        nqueries = NQUERIES_2;
    }
    else if ( benchmarkVariant == 3)
    {
        NUM_CRITERIA = 3;
        nqueries = NQUERIES_3;
    }
    else
    {
        std::cerr << "BenchmarkVariant provided not implemented yet\n";
    }

    for ( unsigned int grid_n = 0; grid_n < NGRIDS; ++grid_n)
    {
        GridChecker gChecker( nqueries, benchmarkVariant,
                              gridQueriesPath + "queries" + std::to_string( benchmarkVariant) + ".txt",
                              gridSolutionsPath + "p" + std::to_string( grid_n) + "/" +
                              std::to_string( benchmarkVariant) + ".txt",
                              GRIDDIMSIZE);
        std::vector< std::pair< unsigned int ,unsigned int> > queries;
        queries = gChecker.getQueries (benchmarkVariant);
        Graph G;
        std::string gridProblemPath = gridsPath + "Grid" + std::to_string( grid_n)+ ".txt";
        std::cout << "\nBenchmark at " << gridProblemPath;
        GridReader<Graph> reader( gridProblemPath, benchmarkVariant);
        Timer timer;
        timer.start();
        G.read(&reader);
        std::cout << "Graph has " << (double)G.memUsage()/1048576 << " Mbytes. Time spent to read:\t" << timer.getElapsedTime() << "sec" << std::endl;
        std::cout << "Checking correctness of " << queries.size() << " queries \n";
        timer.start();
        runBenchmarks( G, queries, reader.getIds(), gChecker, "ADJ", algorithmVariant, showOnScreen);
        std::cout << "\tBenchmark runtime: \t" << timer.getElapsedTime() << " sec" << std::endl;
    }
    return 0;
}