// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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 BOOST_UNITS_CMATH_IMPL_HPP
#define BOOST_UNITS_CMATH_IMPL_HPP

#include <boost/config.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

namespace boost {
namespace units {
namespace detail {

template<class Y>
inline bool isgreater BOOST_PREVENT_MACRO_SUBSTITUTION(const Y& v1,const Y& v2)
{
    if((boost::math::isnan)(v1) || (boost::math::isnan)(v2)) return false;
    else return v1 > v2;
}

template<class Y>
inline bool isgreaterequal BOOST_PREVENT_MACRO_SUBSTITUTION(const Y& v1,const Y& v2)
{
    if((boost::math::isnan)(v1) || (boost::math::isnan)(v2)) return false;
    else return v1 >= v2;
}

template<class Y>
inline bool isless BOOST_PREVENT_MACRO_SUBSTITUTION(const Y& v1,const Y& v2)
{
    if((boost::math::isnan)(v1) || (boost::math::isnan)(v2)) return false;
    else return v1 < v2;
}

template<class Y>
inline bool islessequal BOOST_PREVENT_MACRO_SUBSTITUTION(const Y& v1,const Y& v2)
{
    if((boost::math::isnan)(v1) || (boost::math::isnan)(v2)) return false;
    else return v1 <= v2;
}

template<class Y>
inline bool islessgreater BOOST_PREVENT_MACRO_SUBSTITUTION(const Y& v1,const Y& v2)
{
    if((boost::math::isnan)(v1) || (boost::math::isnan)(v2)) return false;
    else return v1 < v2 || v1 > v2;
}

template<class Y>
inline bool isunordered BOOST_PREVENT_MACRO_SUBSTITUTION(const Y& v1,const Y& v2)
{
    return (boost::math::isnan)(v1) || (boost::math::isnan)(v2);
}

template<class Y>
inline Y fdim BOOST_PREVENT_MACRO_SUBSTITUTION(const Y& v1,const Y& v2)
{
    if((boost::math::isnan)(v1)) return v1;
    else if((boost::math::isnan)(v2)) return v2;
    else if(v1 > v2) return(v1 - v2);
    else return(Y(0));
}

#if 0

template<class T>
struct fma_issue_warning {
    enum { value = false };
};

template<class Y>
inline Y fma(const Y& v1,const Y& v2,const Y& v3)
{
    //this implementation does *not* meet the
    //requirement of infinite intermediate precision
    BOOST_STATIC_WARNING((fma_issue_warning<Y>::value));

    return v1 * v2 + v3;
}

#endif

template<class Y>
inline Y fmax BOOST_PREVENT_MACRO_SUBSTITUTION(const Y& v1,const Y& v2)
{
    if((boost::math::isnan)(v1)) return(v2);
    else if((boost::math::isnan)(v2)) return(v1);
    else if(v1 > v2) return(v1);
    else return(v2);
}

template<class Y>
inline Y fmin BOOST_PREVENT_MACRO_SUBSTITUTION(const Y& v1,const Y& v2)
{
    if((boost::math::isnan)(v1)) return(v2);
    else if((boost::math::isnan)(v2)) return(v1);
    else if(v1 < v2) return(v1);
    else return(v2);
}

//template<class Y>
//inline long long llrint(const Y& val)
//{
//    return static_cast<long long>(rint(val));
//}
//
//template<class Y>
//inline long long llround(const Y& val)
//{
//    return static_cast<long long>(round(val));
//}

#if 0

template<class Y>
inline Y nearbyint(const Y& val)
{
    //this is not really correct.
    //the result should be according to the
    //current rounding mode.
    using boost::math::round;
    return round(val);
}

template<class Y>
inline Y rint(const Y& val)
{
    //I don't feel like trying to figure out
    //how to raise a floating pointer exception
    return nearbyint(val);
}

#endif

template<class Y>
inline Y trunc BOOST_PREVENT_MACRO_SUBSTITUTION(const Y& val)
{
    if(val > 0) return std::floor(val);
    else if(val < 0) return std::ceil(val);
    else return val;
}

}
}
}

#endif // BOOST_UNITS_CMATH_IMPL_HPP


#ifndef VIENNACL_LINALG_DETAIL_AMG_AMG_INTERPOL_HPP
#define VIENNACL_LINALG_DETAIL_AMG_AMG_INTERPOL_HPP

/* =========================================================================
   Copyright (c) 2010-2012, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** @file amg_interpol.hpp
    @brief Implementations of several variants of the AMG interpolation operators (setup phase). Experimental.
*/

#include <boost/numeric/ublas/vector.hpp>
#include <cmath>
#include "viennacl/linalg/amg.hpp"

#include <map>
#ifdef VIENNACL_WITH_OPENMP
#include <omp.h>
#endif

#include "amg_debug.hpp"

namespace viennacl
{
  namespace linalg
  {
    namespace detail
    {
      namespace amg
      {

    /** @brief Calls the right function to build interpolation matrix
     * @param level    Coarse level identifier
     * @param A      Operator matrix on all levels
     * @param P      Prolongation matrices. P[level] is constructed
     * @param Pointvector  Vector of points on all levels
     * @param tag    AMG preconditioner tag
    */
    template <typename InternalType1, typename InternalType2>
    void amg_interpol(unsigned int level, InternalType1 & A, InternalType1 & P, InternalType2 & Pointvector, amg_tag & tag)
    {
      switch (tag.get_interpol())
      {
        case VIENNACL_AMG_INTERPOL_DIRECT: amg_interpol_direct (level, A, P, Pointvector, tag); break;
        case VIENNACL_AMG_INTERPOL_CLASSIC: amg_interpol_classic (level, A, P, Pointvector, tag); break;
        case VIENNACL_AMG_INTERPOL_AG: amg_interpol_ag (level, A, P, Pointvector, tag); break;
        case VIENNACL_AMG_INTERPOL_SA: amg_interpol_sa (level, A, P, Pointvector, tag); break;
      }
    }
    /** @brief Direct interpolation. Multi-threaded! (VIENNACL_AMG_INTERPOL_DIRECT)
     * @param level    Coarse level identifier
     * @param A      Operator matrix on all levels
     * @param P      Prolongation matrices. P[level] is constructed
     * @param Pointvector  Vector of points on all levels
     * @param tag    AMG preconditioner tag
    */
    template <typename InternalType1, typename InternalType2>
    void amg_interpol_direct(unsigned int level, InternalType1 & A, InternalType1 & P, InternalType2 & Pointvector, amg_tag & tag)
    {
      typedef typename InternalType1::value_type SparseMatrixType;
      typedef typename InternalType2::value_type PointVectorType;
      typedef typename SparseMatrixType::value_type ScalarType;
      typedef typename SparseMatrixType::iterator1 InternalRowIterator;
      typedef typename SparseMatrixType::iterator2 InternalColIterator;

      ScalarType temp_res;
      ScalarType row_sum, c_sum, diag;
      //int diag_sign;
      unsigned int x, y;
      amg_point *pointx, *pointy;
      unsigned int c_points = Pointvector[level].get_cpoints();

      // Setup Prolongation/Interpolation matrix
      P[level] = SparseMatrixType(A[level].size1(),c_points);
      P[level].clear();

      // Assign indices to C points
      Pointvector[level].build_index();

      // Direct Interpolation (Yang, p.14)
#ifdef VIENNACL_WITH_OPENMP
      #pragma omp parallel for private (pointx,pointy,row_sum,c_sum,temp_res,y,x,diag) shared (P,A,Pointvector,tag)
#endif
      for (x=0; x < Pointvector[level].size(); ++x)
      {
        pointx = Pointvector[level][x];
        /*if (A[level](x,x) > 0)
          diag_sign = 1;
        else
          diag_sign = -1;*/

        // When the current line corresponds to a C point then the diagonal coefficient is 1 and the rest 0
        if (pointx->is_cpoint())
          P[level](x,pointx->get_coarse_index()) = 1;

        // When the current line corresponds to a F point then the diagonal is 0 and the rest has to be computed (Yang, p.14)
        if (pointx->is_fpoint())
        {
          // Jump to row x
          InternalRowIterator row_iter = A[level].begin1();
          row_iter += x;

          // Row sum of coefficients (without diagonal) and sum of influencing C point coefficients has to be computed
          row_sum = c_sum = diag = 0;
          for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
          {
            y = col_iter.index2();
            if (x == y)// || *col_iter * diag_sign > 0)
            {
              diag += *col_iter;
              continue;
            }

            // Sum all other coefficients in line x
            row_sum += *col_iter;

            pointy = Pointvector[level][y];
            // Sum all coefficients that correspond to a strongly influencing C point
            if (pointy->is_cpoint())
              if (pointx->is_influencing(pointy))
                c_sum += *col_iter;
          }
          temp_res = -row_sum/(c_sum*diag);

          // Iterate over all strongly influencing points of point x
          for (amg_point::iterator iter = pointx->begin_influencing(); iter != pointx->end_influencing(); ++iter)
          {
            pointy = *iter;
            // The value is only non-zero for columns that correspond to a C point
            if (pointy->is_cpoint())
            {
              if (temp_res != 0)
                P[level](x, pointy->get_coarse_index()) = temp_res * A[level](x,pointy->get_index());
            }
          }

          //Truncate interpolation if chosen
          if (tag.get_interpolweight() != 0)
            amg_truncate_row(P[level], x, tag);
        }
      }

      // P test
      //test_interpolation(A[level], P[level], Pointvector[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Prolongation Matrix:" << std::endl;
      printmatrix (P[level]);
      #endif
    }

    /** @brief Classical interpolation. Don't use with onepass classical coarsening or RS0 (Yang, p.14)! Multi-threaded! (VIENNACL_AMG_INTERPOL_CLASSIC)
     * @param level    Coarse level identifier
     * @param A      Operator matrix on all levels
     * @param P      Prolongation matrices. P[level] is constructed
     * @param Pointvector  Vector of points on all levels
     * @param tag    AMG preconditioner tag
    */
    template <typename InternalType1, typename InternalType2>
    void amg_interpol_classic(unsigned int level, InternalType1 & A, InternalType1 & P, InternalType2 & Pointvector, amg_tag & tag)
    {
      typedef typename InternalType1::value_type SparseMatrixType;
      typedef typename InternalType2::value_type PointVectorType;
      typedef typename SparseMatrixType::value_type ScalarType;
      typedef typename SparseMatrixType::iterator1 InternalRowIterator;
      typedef typename SparseMatrixType::iterator2 InternalColIterator;

      ScalarType temp_res;
      ScalarType weak_sum, strong_sum;
      int diag_sign;
      amg_sparsevector<ScalarType> c_sum_row;
      amg_point *pointx, *pointy, *pointk, *pointm;
      unsigned int x, y, k, m;

      unsigned int c_points = Pointvector[level].get_cpoints();

      // Setup Prolongation/Interpolation matrix
      P[level] = SparseMatrixType(A[level].size1(), c_points);
      P[level].clear();

      // Assign indices to C points
      Pointvector[level].build_index();

      // Classical Interpolation (Yang, p.13-14)
#ifdef VIENNACL_WITH_OPENMP
      #pragma omp parallel for private (pointx,pointy,pointk,pointm,weak_sum,strong_sum,c_sum_row,temp_res,x,y,k,m,diag_sign) shared (A,P,Pointvector)
#endif
      for (x=0; x < Pointvector[level].size(); ++x)
      {
        pointx = Pointvector[level][x];
        if (A[level](x,x) > 0)
          diag_sign = 1;
        else
          diag_sign = -1;

        // When the current line corresponds to a C point then the diagonal coefficient is 1 and the rest 0
        if (pointx->is_cpoint())
          P[level](x,pointx->get_coarse_index()) = 1;

        // When the current line corresponds to a F point then the diagonal is 0 and the rest has to be computed (Yang, p.14)
        if (pointx->is_fpoint())
        {
          // Jump to row x
          InternalRowIterator row_iter = A[level].begin1();
          row_iter += x;

          weak_sum = 0;
          c_sum_row = amg_sparsevector<ScalarType>(A[level].size1());
          c_sum_row.clear();
          for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
          {
            k = col_iter.index2();
            pointk = Pointvector[level][k];

            // Sum of weakly influencing neighbors + diagonal coefficient
            if (x == k || !pointx->is_influencing(pointk))// || *col_iter * diag_sign > 0)
            {
              weak_sum += *col_iter;
              continue;
            }

            // Sums of coefficients in row k (strongly influening F neighbors) of C point neighbors of x are calculated
            if (pointk->is_fpoint() && pointx->is_influencing(pointk))
            {
              for (amg_point::iterator iter = pointx->begin_influencing(); iter != pointx->end_influencing(); ++iter)
              {
                pointm = *iter;
                m = pointm->get_index();

                if (pointm->is_cpoint())
                  // Only use coefficients that have opposite sign of diagonal.
                  if (A[level](k,m) * diag_sign < 0)
                    c_sum_row[k] += A[level](k,m);
              }
              continue;
            }
          }

          // Iterate over all strongly influencing points of point x
          for (amg_point::iterator iter = pointx->begin_influencing(); iter != pointx->end_influencing(); ++iter)
          {
            pointy = *iter;
            y = pointy->get_index();

            // The value is only non-zero for columns that correspond to a C point
            if (pointy->is_cpoint())
            {
              strong_sum = 0;
              // Calculate term for strongly influencing F neighbors
              for (typename amg_sparsevector<ScalarType>::iterator iter2 = c_sum_row.begin(); iter2 != c_sum_row.end(); ++iter2)
              {
                k = iter2.index();
                // Only use coefficients that have opposite sign of diagonal.
                if (A[level](k,y) * diag_sign < 0)
                  strong_sum += (A[level](x,k) * A[level](k,y)) / (*iter2);
              }

              // Calculate coefficient
              temp_res = - (A[level](x,y) + strong_sum) / (weak_sum);
              if (temp_res != 0)
                P[level](x,pointy->get_coarse_index()) = temp_res;
            }
          }

          //Truncate iteration if chosen
          if (tag.get_interpolweight() != 0)
            amg_truncate_row(P[level], x, tag);
        }
      }

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Prolongation Matrix:" << std::endl;
      printmatrix (P[level]);
      #endif
    }

    /** @brief Interpolation truncation (for VIENNACL_AMG_INTERPOL_DIRECT and VIENNACL_AMG_INTERPOL_CLASSIC)
    *
    * @param P    Interpolation matrix
    * @param row  Row which has to be truncated
    * @param tag  AMG preconditioner tag
    */
    template <typename SparseMatrixType>
    void amg_truncate_row(SparseMatrixType & P, unsigned int row, amg_tag & tag)
    {
      typedef typename SparseMatrixType::value_type ScalarType;
      typedef typename SparseMatrixType::iterator1 InternalRowIterator;
      typedef typename SparseMatrixType::iterator2 InternalColIterator;

      ScalarType row_max, row_min, row_sum_pos, row_sum_neg, row_sum_pos_scale, row_sum_neg_scale;

      InternalRowIterator row_iter = P.begin1();
      row_iter += row;

      row_max = 0;
      row_min = 0;
      row_sum_pos = 0;
      row_sum_neg = 0;

      // Truncate interpolation by making values to zero that are a lot smaller than the biggest value in a row
      // Determine max entry and sum of row (seperately for negative and positive entries)
      for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
      {
        if (*col_iter > row_max)
          row_max = *col_iter;
        if (*col_iter < row_min)
          row_min = *col_iter;
        if (*col_iter > 0)
          row_sum_pos += *col_iter;
        if (*col_iter < 0)
          row_sum_neg += *col_iter;
      }

      row_sum_pos_scale = row_sum_pos;
      row_sum_neg_scale = row_sum_neg;

      // Make certain values to zero (seperately for negative and positive entries)
      for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
      {
        if (*col_iter > 0 && *col_iter < tag.get_interpolweight() * row_max)
        {
          row_sum_pos_scale -= *col_iter;
          *col_iter = 0;
        }
        if (*col_iter < 0 && *col_iter > tag.get_interpolweight() * row_min)
        {
          row_sum_pos_scale -= *col_iter;
          *col_iter = 0;
        }
      }

      // Scale remaining values such that row sum is unchanged
      for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
      {
        if (*col_iter > 0)
          *col_iter = *col_iter *(row_sum_pos/row_sum_pos_scale);
        if (*col_iter < 0)
          *col_iter = *col_iter *(row_sum_neg/row_sum_neg_scale);
      }
    }

    /** @brief AG (aggregation based) interpolation. Multi-Threaded! (VIENNACL_INTERPOL_SA)
     * @param level    Coarse level identifier
     * @param A      Operator matrix on all levels
     * @param P      Prolongation matrices. P[level] is constructed
     * @param Pointvector  Vector of points on all levels
    */
    template <typename InternalType1, typename InternalType2>
    void amg_interpol_ag(unsigned int level, InternalType1 & A, InternalType1 & P, InternalType2 & Pointvector, amg_tag)
    {
      typedef typename InternalType1::value_type SparseMatrixType;
      typedef typename InternalType2::value_type PointVectorType;
      typedef typename SparseMatrixType::value_type ScalarType;
      typedef typename SparseMatrixType::iterator1 InternalRowIterator;
      typedef typename SparseMatrixType::iterator2 InternalColIterator;

      unsigned int x;
      amg_point *pointx, *pointy;
      unsigned int c_points = Pointvector[level].get_cpoints();

      P[level] = SparseMatrixType(A[level].size1(), c_points);
      P[level].clear();

      // Assign indices to C points
      Pointvector[level].build_index();

      // Set prolongation such that F point is interpolated (weight=1) by the aggregate it belongs to (Vanek et al p.6)
#ifdef VIENNACL_WITH_OPENMP
      #pragma omp parallel for private (x,pointx) shared (P)
#endif
      for (x=0; x<Pointvector[level].size(); ++x)
      {
        pointx = Pointvector[level][x];
        pointy = Pointvector[level][pointx->get_aggregate()];
        // Point x belongs to aggregate y.
        P[level](x,pointy->get_coarse_index()) = 1;
      }

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Aggregation based Prolongation:" << std::endl;
      printmatrix(P[level]);
      #endif
    }

    /** @brief SA (smoothed aggregate) interpolation. Multi-Threaded! (VIENNACL_INTERPOL_SA)
     * @param level    Coarse level identifier
     * @param A      Operator matrix on all levels
     * @param P      Prolongation matrices. P[level] is constructed
     * @param Pointvector  Vector of points on all levels
     * @param tag    AMG preconditioner tag
    */
    template <typename InternalType1, typename InternalType2>
    void amg_interpol_sa(unsigned int level, InternalType1 & A, InternalType1 & P, InternalType2 & Pointvector, amg_tag & tag)
    {
      typedef typename InternalType1::value_type SparseMatrixType;
      typedef typename InternalType2::value_type PointVectorType;
      typedef typename SparseMatrixType::value_type ScalarType;
      typedef typename SparseMatrixType::iterator1 InternalRowIterator;
      typedef typename SparseMatrixType::iterator2 InternalColIterator;

      unsigned int x,y;
      ScalarType diag = 0;
      unsigned int c_points = Pointvector[level].get_cpoints();

      InternalType1 P_tentative = InternalType1(P.size());
      SparseMatrixType Jacobi = SparseMatrixType(A[level].size1(), A[level].size2());
      Jacobi.clear();
      P[level] = SparseMatrixType(A[level].size1(), c_points);
      P[level].clear();

      // Build Jacobi Matrix via filtered A matrix (Vanek et al. p.6)
#ifdef VIENNACL_WITH_OPENMP
      #pragma omp parallel for private (x,y,diag) shared (A,Pointvector)
#endif
      for (x=0; x<A[level].size1(); ++x)
      {
        diag = 0;
        InternalRowIterator row_iter = A[level].begin1();
        row_iter += x;
        for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
        {
          y = col_iter.index2();
          // Determine the structure of the Jacobi matrix by using a filtered matrix of A:
          // The diagonal consists of the diagonal coefficient minus all coefficients of points not in the neighborhood of x.
          // All other coefficients are the same as in A.
          // Already use Jacobi matrix to save filtered A matrix to speed up computation.
          if (x == y)
            diag += *col_iter;
          else if (!Pointvector[level][x]->is_influencing(Pointvector[level][y]))
            diag += -*col_iter;
          else
            Jacobi (x,y) = *col_iter;
        }
        InternalRowIterator row_iter2 = Jacobi.begin1();
        row_iter2 += x;
        // Traverse through filtered A matrix and compute the Jacobi filtering
        for (InternalColIterator col_iter2 = row_iter2.begin(); col_iter2 != row_iter2.end(); ++col_iter2)
        {
            *col_iter2 = - static_cast<ScalarType>(tag.get_interpolweight())/diag * *col_iter2;
        }
        // Diagonal can be computed seperately.
        Jacobi (x,x) = 1 - static_cast<ScalarType>(tag.get_interpolweight());
      }

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Jacobi Matrix:" << std::endl;
      printmatrix(Jacobi);
      #endif

      // Use AG interpolation as tentative prolongation
      amg_interpol_ag(level, A, P_tentative, Pointvector, tag);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Tentative Prolongation:" << std::endl;
      printmatrix(P_tentative[level]);
      #endif

      // Multiply Jacobi matrix with tentative prolongation to get actual prolongation
      amg_mat_prod(Jacobi,P_tentative[level],P[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Prolongation Matrix:" << std::endl;
      printmatrix (P[level]);
      #endif
    }
      } //namespace amg
    }
  }
}

#endif


#include <vector>
#include <cmath>
#include <random>
#include <limits>

#include <exception>
#include <iostream>
#include <fstream>
#include <string>
#include <utility>
#include <cstdlib>
#include <boost/program_options.hpp>
#include "../include/common.hpp"
#include "../include/configurations.hpp"
#include "../include/instance.hpp"
#include "../include/instance_creator.hpp"

// declare the random number generator
std::mt19937_64 rng;
std::string configuration_file = DEFAULT_CONFIG_FILE;

void read_parameters(int argc, char** argv, Configurations& con);

int main(int argc, char** argv){
	Configurations con;
	read_parameters(argc, argv, con);
	printf("Seed: %ld\n", con.seed);
	rng = std::mt19937_64(con.seed);

   Instance inst;
   auto hhc_params = GeneratorParameters(con);
   if(con.type == InstanceType::HHCP) {
      // if (con.nclusters != 1) {
      //    printf("ERROR: Number of clusters must be 1 in HHCP.\n");
      //    return EXIT_FAILURE;
      // }
      if (con.verbose >= 1) printf("Generating initial data for the HHCP.\n");
      inst.hhcp_data.reset(new WeeklyData(hhc_params));
      size_t size = inst.hhcp_data->generateInitialData();
      if (con.verbose >= 1) printf("Patients pool size is %d, daily patients is %d.\n", inst.hhcp_data->patientsPoolSize(), con.ndaily);
      if (con.verbose >= 1 && long(con.num_points) != long(size+1)) printf("Changing the number of points from %lu to %lu.\n", con.num_points, size+1);
      con.num_points = size+1;
   }

	if(con.verbose >= 1) printf("Creating instance: getting locations, routing paths, and generating remaining data...\n");
	int ret = InstanceCreator::create(con, inst);
	if(ret == EXIT_FAILURE) return ret;
	else if(con.verbose >= 1) printf("DONE: Instance created.\n");

   if (con.type != InstanceType::HHCP) {
      if(con.verbose >= 1) printf("Writing instance to file %s...\n", con.inst_file.c_str());
	   FileCode rwrite = FileCode::WriteError;
      rwrite = Instance::write_instance_file(con.inst_file, inst);
      ret = rwrite == FileCode::WriteError ? EXIT_FAILURE : EXIT_SUCCESS;
	   if(ret != EXIT_FAILURE && con.verbose >= 1) printf("DONE: Instance written to file.\n");
   } else {
      inst.hhcp_data->generateDailyData();
      for (int d = 0; d < hhc_params.numDays(); ++d) {
         FileCode rwrite = inst.hhcp_data->writeDailyData(d);
         ret = rwrite == FileCode::WriteError ? EXIT_FAILURE : EXIT_SUCCESS;
	      if(ret != EXIT_FAILURE && con.verbose >= 1) printf("DONE: Daily instance [%d] written to file.\n", d);
         else break;
      }
   }

    return ret;
}

void read_parameters(int argc, char** argv, Configurations& con){
	size_t temp_seed = std::numeric_limits<size_t>::max();
	con.seed = 0;
	con.verbose = 100;
	con.inst_file = DEFAULT_INST_FILE;

	namespace po = boost::program_options;
	po::options_description desc("parameters");
	desc.add_options()
		("help,h", "print this help info")
		("configuration,c", po::value<std::string>(&configuration_file),
		 ("Configuration file to read the parameters. Default: " + DEFAULT_CONFIG_FILE).c_str())
		("output,o", po::value<std::string>(&con.inst_file),
		 ("output file where the generated instance will be recorded. Default: " + DEFAULT_INST_FILE + ". Does nothing when generating HHCP instances").c_str())
		("seed,s", po::value<size_t>(&temp_seed),
		 ("define the RNG seed. Default: 0"))
		("verbose,v", po::value<int>(&con.verbose),
		 ("verbosity level to print intermediate information. Values: 0 (none), 1 (some), 2 (full). Default: 1"))
		;

	po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm); // seems to be what put the read values in the variables

	if (vm.count("help")) {
		std::cout << desc << std::endl;
		exit(EXIT_SUCCESS);
	}

	if(con.verbose >= 1) printf("Opening configuration file %s...\n", configuration_file.c_str());
	std::ifstream cfile(configuration_file);
	if(not cfile.is_open()){
		printf("Could not open file \"%s\"\n", configuration_file.c_str());
		exit(EXIT_FAILURE);
	}

	if(con.verbose >= 1){
		printf("DONE: configuration file opened.\n");
		printf("Reading configuration...\n");
	}
	if(Configurations::read_configurations(con, cfile) == FileCode::ReadError){
		printf("Error reading configuration from file %s\n", configuration_file.c_str());
		cfile.close();
		exit(EXIT_FAILURE);
	}
	if(con.verbose >= 1) printf("DONE: Configuration read.\n");

	cfile.close();
	if(temp_seed != std::numeric_limits<size_t>::max()){
		con.seed = temp_seed;
	}
}


//
//  Slip_Weakening_lumpM.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/8/18.
//
//

#ifndef Slip_Weakening_lumpM_hpp
#define Slip_Weakening_lumpM_hpp

#include <stdio.h>
#include <iostream>
#include <Eigen/Eigen>
#include "maplocal.hpp"

void Slip_Weakening_lumpM(VectorXd &M_global_vec, ArrayXi &top_surf_index, ArrayXi &bot_surf_index, VectorXd &fe_global, double dt, double dx,double dy, int nx, ArrayXd &delt_v_n, ArrayXd &delt_u_n, ArrayXd &T_0, ArrayXd &tau_s, double mu_s, double mu_d, double Dc, int Ndofn, VectorXd &M_pos, VectorXd &M_neg, VectorXd &F_fault, ArrayXd &T_c, double fault_angle);
#endif /* Slip_Weakening_lumpM_hpp */


#include <iostream>
#include <nt2/sdk/meta/type_id.hpp>

  template<class T>
  struct print
  {
      unsigned : 80;
      typedef T type;
  };

#include <nt2/sdk/timing/ctic.hpp>
#include <nt2/include/functions/cos.hpp>
#include <nt2/include/functions/fast_cos.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/operator/functions.hpp>
#include <nt2/include/functions/function.hpp>

#include <boost/simd/sdk/simd/meta/vector_of.hpp>
#include <boost/simd/sdk/simd/meta/native_cardinal.hpp>
#include <boost/simd/sdk/simd/meta/is_vectorizable.hpp>

/*
 for(int y=1;y<=d1;++y)
  {
    for(int x=1;x<=d0;++x)
    {
      boost::array<int,2> p = {{x,y}};
      std::cout << (boost::proto::value(b).data())(p) << " ";
    }
    std::cout << "\n";
  }
*/

#include <vector>

template< class T
        , bool Enable = boost::simd::
                        meta::is_vectorizable<T,BOOST_SIMD_DEFAULT_EXTENSION>::value
        >
struct evaluation_type
{
  typedef nt2::meta::as_<T> type;
};

template<class T>
struct  evaluation_type<T, true>
{
  typedef typename boost::simd::meta::
          vector_of<T, boost::simd::meta::native_cardinal<T>::value>::type base;
  typedef nt2::meta::as_<base>                                             type;
};

struct foo { int i; int j; };

int main()
{
  using nt2::container::table;

#if 0
  int iter, d0,d1;
  std::cin >> iter >> d0 >> d1;

  std::cout << "std::cos applied on linear std::vector:\n";
  {
    std::vector<double> cc;
    std::vector<double> a(d0*d1),b(d0*d1);
    for(int x=0;x<d0*d1;++x) b[x] = a[x] = 0.;

    for(int i=0;i<iter;++i)
    {
      nt2::ctic();
      for(int x=0;x<d0*d1;++x)
          b[x] = std::cos(a[x]);
      cc.push_back(nt2::ctoc(false)/(1.*d0*d1));
    }

    std::cout << b[0] << " " << b[d0*d1/2-1] << " " << b[b.size()-1] << "\n";
    std::sort(cc.begin(),cc.end());
    std::cout << cc[cc.size()/2 - 1] << " c/elements\n";
  }

  {
    std::vector<double> cc;
    std::vector<double> a(d0*d1),b(d0*d1);
    for(int x=0;x<d0*d1;++x) b[x] = a[x] = 0.;
    std::cout << "nt2::cos applied on linear std::vector:\n";

    for(int i=0;i<iter;++i)
    {
      nt2::ctic();
      for(int x=0;x<d0*d1;++x)
          b[x] = nt2::cos(a[x]);
      cc.push_back(nt2::ctoc(false)/(1.*d0*d1));
    }

    std::cout << b[0] << " " << b[d0*d1/2-1] << " " << b[b.size()-1] << "\n";
    std::sort(cc.begin(),cc.end());
    std::cout << cc[cc.size()/2 - 1] << " c/elements\n";
  }

  std::cout << "nt2::cos applied on nt2::table:\n";
  {
    std::vector<double> cc;

    table<float,nt2::_2D> b( nt2::of_size(d0,d1) ), a( nt2::of_size(d0,d1) );

    for(int y=1;y<=d1;++y)
    {
      for(int x=1;x<=d0;++x)
      {
        boost::array<int,2> p = {{x,y}};
        (boost::proto::value(a).data())(p) = 0;
      }
    }

    for(int i=0;i<iter;++i)
    {
      nt2::ctic();
      b = nt2::cos(a);
      cc.push_back(nt2::ctoc(false)/(1.*d0*d1));
    }

    std::sort(cc.begin(),cc.end());
    std::cout << cc[cc.size()/2 - 1] << " c/elements\n";
  }

  a = -(b + c*(a+(c * -c))) + b;
  a = b + c;
  a = -b;
  b += c;
  c += -a;
  //c(a) += a;                // function undefined
  //c(a) = b(c) + c(a)*b(a);
#endif

  table<double> t(nt2::of_size(10, 10));
  nt2::run(nt2::assign(t(1), 42.));
  double d = t(1);
  std::cout << d << std::endl;

}


#ifndef OPENGM_PYTHON_INTERFACE
#define OPENGM_PYTHON_INTERFACE 1
#endif

#include <stdexcept>
#include <stddef.h>
#include <string>
#include <boost/python.hpp>
#include <opengm/graphicalmodel/graphicalmodel.hxx>
#include <opengm/inference/inference.hxx>
#include <opengm/inference/graphcut.hxx>
#include <opengm/inference/alphaexpansion.hxx>
#ifdef WITH_BOOST
#  include <opengm/inference/auxiliary/minstcutboost.hxx>
#endif
#ifdef WITH_MAXFLOW
#  include <opengm/inference/auxiliary/minstcutkolmogorov.hxx>
#endif

#include "nifty_iterator.hxx"
#include "inferencehelpers.hxx"
#include "../export_typedes.hxx"

#define AE_EXPORT_HELPER(AE_CLASS,GC_STRING,P_STRING,V_STRING)\
class_<typename AE_CLASS::Parameter > (P_STRING, init<>() ) \
      .def(init<const size_t ,const typename AE_CLASS::Parameter::InferenceParameter & >())\
      .def(init<const size_t>())\
      .def_readwrite("steps", & AE_CLASS::Parameter::maxNumberOfSteps_)\
      .def_readwrite("graphCutParameter", & AE_CLASS::Parameter::parameter_)\
      .def ("set", &pyae::set<AE_CLASS,typename AE_CLASS::Parameter>, \
            ( \
            arg("steps")=1000,\
            arg("graphCutParameter")=typename AE_CLASS::Parameter::InferenceParameter()\
            ) \
      ) \
      ; \
   OPENGM_PYTHON_VERBOSE_VISITOR_EXPORTER(typename AE_CLASS::VerboseVisitorType,V_STRING );\
   OPENGM_PYTHON_INFERENCE_NO_RESET_EXPORTER(AE_CLASS,GC_STRING)

using namespace boost::python;

namespace  pyae{
   template<class INF,class PARAM>
   void set
   (
      PARAM & p,
      const size_t steps,
      const typename INF::Parameter::InferenceParameter & parameter
   ){
      p.maxNumberOfSteps_=steps;
      p.parameter_=parameter;
   }
}

template<class GM,class ACC>
void export_ae(){
   import_array();
   typedef GM PyGm;
   typedef typename PyGm::ValueType ValueType;
   // Boost Graphcut  swap and expansion Types

   typedef opengm::MinSTCutBoost<size_t, ValueType, opengm::KOLMOGOROV> MinStCutBoostKolmogorov;
   typedef opengm::GraphCut<PyGm, ACC, MinStCutBoostKolmogorov> PyGraphCutBoostKolmogorov;
   typedef opengm::AlphaExpansion<PyGm, PyGraphCutBoostKolmogorov> PyAlphaExpansionBoostKolmogorov;

   AE_EXPORT_HELPER(PyAlphaExpansionBoostKolmogorov,
      "AlphaExpansionBoostKolmogorov",
      "AlphaExpansionBoostKolmogorovParameter",
      "AlphaExpansionBoostKolmogorovVerboseVisitor");

}

template void export_ae<GmAdder,opengm::Minimizer>();


#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include "ros/ros.h"
#include "std_msgs/Float32.h"
#include "videoray/Throttle.h"
#include "geometry_msgs/Twist.h"
#include "nav_msgs/Odometry.h"
#include <iostream>
#include <fstream>
#include <signal.h>
#include <sstream>
#include "videoray/Status.h"
#include "videoray/UHRIComm.h"

double depth_ref = 0;
double speed_ref = 0;
double heading_ref = 0;
int VideoRay_Speed = 0;
double sharpturn_speed = 0;
double roll_ = 0;
double pitch_ = 0;
double yaw_ = 0;
double depth_err = 0;
double speed_err = 0;
double heading_err = 0;
double heading = 0;
double heading_weight = 0.5;
double speed_weight = 0.5;
double K_heading = 1;
double K_speed = 10;
double K_depth = 50;
videoray::Throttle throttle_;
//VideoRayComm comm;
float err;
float erry;
float previous_err = 0;
float integral = 0;
float derivative = 0;
float output;
float Kp=0.01;
float Ki=0;
float Kd=0.01;
float des_speed=0;

static const std::string OPENCV_WINDOW = "Image window";
int lowH_low = 0; // 13
int highH_low = 10; // 65 Set Hue
int lowH_high = 160; // 13
int highH_high = 180;

int lowS = 70; //72
int highS = 255; // 192Set Saturation

int lowV = 50; // 193
int highV = 255; //255 Set Value
int imageWidth;
int imageHeight;
int search_top;
int search_bottom;

int cx;
int cy;
  cv::Mat diffPts;
  cv::Mat ptsx, ptsy;
  cv::Mat canny_output;
  cv::Mat imgOriginal; //Input image
  cv::Mat hsvImg; //HSV Image
  cv::Mat mask1;
  cv::Mat mask2;
  cv::Mat threshImg; //Threshimage
  cv::Mat M;
 cv::Mat drawing;
 std::vector< cv::Vec4i > linesP;
  std::vector< std::vector<cv::Point> > contours;
std::vector< std::vector<cv::Point> > biggest_contours;
cv::Scalar color;
cv::RNG rng(12345);
 double dist;
 double dist_new;
size_t largest_contours = 0;
float heading_port, heading_star, speed_port, speed_star, speed_vert; //speed_vert is a new addition
ros::Publisher throttle_pub;
class ImageConverter
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  image_transport::Publisher image_pub_;

     ros::Publisher videoray_status_pub_;
public:
  ImageConverter()
    : it_(nh_)
  {

    // Subscribe to input video feed and publish output video feed
    image_sub_ = it_.subscribe("/videoray/camera/image_raw", 1,
      &ImageConverter::imageCb, this);
    image_pub_ = it_.advertise("/image_converter/output_video", 1);
    throttle_pub = nh_.advertise<videoray::Throttle>("/videoray/throttle", 1);
    //throttle_pub = nh_.advertise<videoray::Throttle>("/rqt_thrust_monitor/throttle_cmd", 1);
  }

  ~ImageConverter()
  {
     throttle_.PortInput =  0;
     throttle_.StarInput =  0;
     throttle_.VertInput = 0;
     throttle_pub.publish(throttle_);

    cv::destroyWindow(OPENCV_WINDOW);
  }

  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

imgOriginal = cv_ptr->image;

cv::cvtColor(imgOriginal, hsvImg, CV_BGR2HSV);

cv::inRange( hsvImg, cv::Scalar(lowH_low, lowS, lowV), cv::Scalar(highH_low, highS, highV), mask1);
cv::inRange( hsvImg, cv::Scalar(lowH_high, lowS, lowV), cv::Scalar(highH_high, highS, highV), mask2);
threshImg = mask1 | mask2;
 imageWidth = imgOriginal.size().width;
 imageHeight = imgOriginal.size().height;
std::cout << "width" << imageWidth << std::endl;
std::cout << "height" << imageHeight << std::endl;
 search_top = 3 * imageHeight / 4;
 search_bottom = search_top + 20;
for (int i; i < imageHeight; i++){
for(int j; j < imageWidth; j++){
if ( i < search_top && j < imageWidth){
	hsvImg.at<uchar>(i, j, 0) = 0 ;

}
if (   search_top < i && j > imageWidth){
	hsvImg.at<uchar>(i, j, 0) = 0 ;
}}
}
cv::GaussianBlur(threshImg, threshImg, cv::Size(3,3), 0); //reduce details
cv::dilate(threshImg, threshImg, 0); //removes noise
cv::erode(threshImg, threshImg, 0); //trims image

    cv::Canny( threshImg, canny_output, 100, 100*2, 3 ); //edge detection

    cv::findContours( canny_output, contours, cv::RETR_TREE,cv::CHAIN_APPROX_SIMPLE ); //finds edge
    std::vector<cv::Moments> mu(contours.size() ); //calculates moments
    for( int i = 0; i < contours.size(); i++ )
    {
        mu[i] = cv::moments( contours[i] );
    }
    std::vector<cv::Point2f> mc( contours.size() );
    for( int i = 0; i < contours.size(); i++ )
    {
        //add 1e-5 to avoid division by zero
        mc[i] = cv::Point2f( static_cast<float>(mu[i].m10 / (mu[i].m00 + 1e-5)),
                         static_cast<float>(mu[i].m01 / (mu[i].m00 + 1e-5)) ); //x center, y center
        //std::cout << "mc[" << i << "]=" << mc[i] << std::endl;
    }

    cv::Mat drawing = cv::Mat::zeros( canny_output.size(), CV_8UC3 );
	if (contours.size() > 1 ){
	dist =	cv::arcLength(contours[0], true);
		largest_contours = 0;
	for( size_t i = 0; i < contours.size(); i++ ){
		dist_new = cv::arcLength(contours[i], true);
		//std::cout << "old dist: " << dist << " new dist: " << dist_new << std::endl;
		if ( dist_new > dist) {
			dist = dist_new;
			largest_contours = i;
			//std::cout<< "largest countour: " << i << std::endl;

}
}

        color = cv::Scalar( rng.uniform(0, 256), rng.uniform(0,256), rng.uniform(0,256) );
        cv::drawContours( imgOriginal, contours, (int)largest_contours, color, 2 );//highlight the color
        cv::circle( imgOriginal, mc[largest_contours], 4, color, -1 );

	err = mc[largest_contours].x - imageWidth/2;
	erry = mc[largest_contours].y - imageHeight/2; //added
	speed_port=(err)/imageWidth*30+15;
	speed_star=-(err)/imageWidth*30+15;
	speed_vert=(erry)/imageHeight*40+15; //added

	std::cout<< "speed star " << speed_star << std::endl;
	std::cout<< "speed port " << speed_port << std::endl;
	std::cout<< "speed vert " << speed_vert << std::endl; //added
	//port = left, star = right, vert = up & down

	//if(abs(err)<20){
	  //speed_port = (double)VideoRay_Speed;
	  //speed_star = (double)VideoRay_Speed;
	//}
	//else if(err < -290){
	//sharpturn left
      	  //speed_port = -1 * (double)VideoRay_Speed;
	  //speed_star = (double)VideoRay_Speed;
	//}
	//else if(err<0){
	  //slight turn left
	  //speed_port = 0;
	  //speed_star = (double)VideoRay_Speed;
	//}
	//else if ( err > 290){
	  //sharp turn right
          //speed_port = (double)VideoRay_Speed;
	  //speed_star = -1 * (double)VideoRay_Speed;
	//}
	//else if(err>0){
	  //slight turn right
          //speed_port = (double)VideoRay_Speed;
	  //speed_star = 0;
	//}

  }

cv::namedWindow("imgOriginal", CV_WINDOW_NORMAL);
cv::namedWindow("threshImg", CV_WINDOW_NORMAL);

     /* Create trackbars in "threshImg" window to adjust according to object and environment.*/

  cv::createTrackbar("LowH", "threshImg", &lowH_low, 180); //Hue (0 - 179)
  cv::createTrackbar("HighH", "threshImg", &highH_low, 180);

  cv::createTrackbar("LowS", "threshImg", &lowS, 255); //Saturation (0 - 255)
  cv::createTrackbar("HighS", "threshImg", &highS, 255);

  cv::createTrackbar("LowV", "threshImg", &lowV, 255); //Value (0 - 255)
  cv::createTrackbar("HighV", "threshImg", &highV, 255);

  cv::createTrackbar("Speed", "threshImg", &VideoRay_Speed, 100);

  cv::imshow("imgOriginal", imgOriginal);     // show windows
  cv::imshow("threshImg", threshImg);

    // Update GUI Window
    //cv::imshow(OPENCV_WINDOW, cv_ptr->image);
    cv::waitKey(3);

    // Output modified video stream
     image_pub_.publish(cv_ptr->toImageMsg());
     //VideoRayComm comm;
     //comm.set_vertical_thruster(speed_vert);
     throttle_.PortInput=speed_port;
     throttle_.StarInput=speed_star;
     throttle_.VertInput= speed_vert; //was originally set to 0
     throttle_pub.publish(throttle_);

  }
};
void mySigintHandler(int sig){
 throttle_.PortInput =  0;
 throttle_.StarInput =  0;
 throttle_.VertInput = 0;
 throttle_pub.publish(throttle_);
 ros::shutdown();
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_converter");

 ImageConverter ic;
  signal(SIGINT, mySigintHandler);
  ros::spin();

return 0;
}


#pragma once
#include "Core/Common.hpp"
#include "BoundingBox.hpp"
#include "Vulkan/VulkanFactory.hpp"
#include "Vulkan/Camera.hpp"
#include "Color.hpp"
#include <Eigen/Dense>
#include <ostream>

struct HTConstants
{
	static constexpr uint32_t PAGE_SIZE = 512;
	// TODO: Probably should not be constant (should be computed from the actual input voxel grid - as its
	// size is not going to change). This change involves also removing other related values from constants.
	static constexpr uint32_t MAX_LEVEL_COUNT = 12; // To fit 4096^3
	static constexpr uint32_t TREE_SPAN = 4096; // NOTE: Needs to be changed with the tree size (if ever modified).
	static constexpr uint32_t LEAF_LEVEL = MAX_LEVEL_COUNT - 2;
	// TODO: Potentially change these.
	// in pages
	static constexpr uint32_t TOP_LEVEL_BUCKET_SIZE = 1024;
	static constexpr uint32_t BOTTOM_LEVEL_BUCKET_SIZE = 4096;
	static constexpr uint32_t TOP_LEVEL_BUCKET_COUNT = 1024;
	static constexpr uint32_t BOTTOM_LEVEL_BUCKET_COUNT = 65536;
	static constexpr uint32_t TOP_LEVEL_RANK = 6;
	static constexpr uint32_t BOTTOM_LEVEL_RANK = MAX_LEVEL_COUNT - TOP_LEVEL_RANK;
	static constexpr uint32_t TOTAL_TOP_BUCKET_COUNT = TOP_LEVEL_RANK * TOP_LEVEL_BUCKET_COUNT;
	static constexpr uint32_t TOTAL_BOTTOM_BUCKET_COUNT = BOTTOM_LEVEL_RANK * BOTTOM_LEVEL_BUCKET_COUNT;
	static constexpr uint32_t TOTAL_BUCKET_COUNT = TOTAL_TOP_BUCKET_COUNT + TOTAL_BOTTOM_BUCKET_COUNT;
	static constexpr uint32_t TOTAL_PAGE_COUNT =
		(TOTAL_TOP_BUCKET_COUNT * TOP_LEVEL_BUCKET_SIZE +
			TOTAL_BOTTOM_BUCKET_COUNT * BOTTOM_LEVEL_BUCKET_SIZE)
		/ PAGE_SIZE;
	static constexpr uint32_t INVALID_POINTER = 0xFFFFFFFF;
};

struct HTStats
{
	uint32_t memoryAllocatedBytes;
	uint32_t emptyBuckets;
	float emptyToTotalRatio;
	float emptyTopToTotalRatio;
	float emptyBottomToTotalRatio;
	float avgTopBucketFullness;
	float avgBottomBucketFullness;
	float percentageOfMemoryUsed;
	uint32_t levelNodeCount[HTConstants::LEAF_LEVEL + 1];

	void Print() const;
};

// TODO: We are only interested in color modifications, the infrastructure could be simplified when colors are in place.

struct HashDAGPushConstants
{
	uint32_t MaxLevels = HTConstants::MAX_LEVEL_COUNT;
	uint32_t LeafLevel = HTConstants::LEAF_LEVEL;
	uint32_t PageSize = HTConstants::PAGE_SIZE;
	uint32_t PageCount;
	uint32_t TreeCount;
};

struct HashDAGGPUInfo
{
	VulkanFactory::Buffer::BufferInfo PagesStorageBuffer;
	VulkanFactory::Buffer::BufferInfo PageTableStorageBuffer;
	VulkanFactory::Buffer::BufferInfo TreeRootsStorageBuffer;
	VulkanFactory::Buffer::BufferInfo SortedTreesStorageBuffer;
	uint32_t PageCount;
	uint32_t TreeCount;
};

class HashTable
{
public:
	/// Physical Pointer into the hash table.
	typedef uint32_t* ptr_t;
	/// Virtual memory index that can be translated into the physical pointer.
	typedef uint32_t vptr_t;
	/// Global bucket number that the 'bucketsSizes' can be indexed by.
	typedef uint32_t bucket_t;

	HashTable() = default;
	~HashTable();
	/// Pre-allocates the memory necessary for operating this HashDAG.
	void Init(uint32_t pagePoolSize);
	void Destroy();

	/// Returns a physical memory pointer to the node represented by the virtual pointer.
	ptr_t Translate(vptr_t ptr) const;

	// TODO: Check if this can be private.
	/// Remembers given leaf node under given hash.
	vptr_t AddLeaf(const uint64_t leaf, const uint32_t hash);
	/// Remembers given non-leaf node under given hash.
	vptr_t AddNode(const uint32_t level, const uint32_t nodeSize, const uint32_t* node, const uint32_t hash);

	// TODO: Check if this can be private.
	/// Finds this leaf node in the given bucket and returns a virtual pointer to it.
	vptr_t FindLeafInBucket(bucket_t bucket, uint64_t leaf) const;
	/// Finds this non-leaf node in the given bucket and returns a virtual pointer to it.
	vptr_t FindNodeInBucket(bucket_t bucket, uint32_t nodeSize, uint32_t* const node) const;

	/// Finds this leaf node in the structure or creates a new entry and returns a virtual pointer to it.
	vptr_t FindOrAddLeaf(uint64_t leaf);
	/// Finds this non-leaf node in the structure or creates a new entry and returns a virtual pointer to it.
	vptr_t FindOrAddNode(uint32_t level, uint32_t nodeSize, uint32_t* const node);

	/// Returns the number of MB that is allocated by this class.
	uint32_t GetMemAllocatedBytes() const;
	/// How many nodes there is in the level.
	uint32_t CountLevelNodes(uint32_t level) const;
	// TODO: Add void AddFullNode(uint32_t) function which lets us populate the hash dag if we know the nodes in advance.

	/// Returns some useful statistics.
	HTStats GetStats() const;

	void UploadToGPU(const VulkanFactory::Device::DeviceInfo& deviceInfo, VkCommandPool commandPool,
		VkQueue queue, HashDAGGPUInfo& uploadInfo);

#ifdef MEASURE_MEMORY_CONSUMPTION
	uint32_t GetMemoryDadoAttributes() const;
	uint32_t GetMemoryDoloniusAttributes() const;
	uint32_t GetMemoryUsed() const;
	uint32_t GetMemoryNoDAGDadoAttributes() const;
	uint32_t GetMemoryNoDAGDoloniusAttributes() const;
	uint32_t GetSVOInternalNodes() const;
	uint32_t GetSVOLeafNodes() const;
#endif

private:
	/// Allocates a page from the pre-allocated page pool.
	void AllocatePage(uint32_t page);
	/// Check if the page has already been allocated from the page pool.
	bool IsPageAllocated(uint32_t page);

	/// Creates a 32-bit hash for the leaf node.
	static uint32_t HashLeaf(uint64_t leaf);
	/// Creates a 32-bit hash for the non-leaf node.
	static uint32_t HashNode(const uint32_t nodeSize, const uint32_t* node);

	/// Finds a bucket that corresponds to the given hash in the given level.
	static bucket_t GetBucket(uint32_t level, uint32_t hash);
	/// Retrieves a virtual pointer to the bucket.
	static uint32_t GetBucketPtr(bucket_t bucket);

	/// Determines the size of a non-leaf node.
	static uint32_t GetNodeSize(uint32_t* const ptr);

#ifdef MEASURE_MEMORY_CONSUMPTION
	uint8_t GetFirstLeafMask(uint64_t leaf) const;
#endif

	/// The top of the already allocated pool pages. Starts at 1, 0 is reserved of un-allocated entries.
	uint32_t poolTop_ = 1;
	uint32_t poolSize_;
	/// Physical page memory with 32 bit pointers.
	uint32_t* pagePool_;
	/// The virtual page table (virtual -> physical address). The physical address is an index of the pagePool.
	uint32_t* pageTable_;
	/// The array holding the sizes of all buckets present in the hash table.
	uint32_t* bucketsSizes_;
#ifdef MEASURE_MEMORY_CONSUMPTION
	uint32_t memoryDadoAttributes_ = 0;
	uint32_t memoryDoloniusAttributes_ = 0;
	uint32_t memoryNoDAGCompressionDado_ = 0;
	uint32_t memoryNoDAGCompressionDolonius_ = 0;
	uint32_t SVOInternalNodeCount_ = 0;
	uint32_t SVOLeafNodeCount_ = 0;
#endif
};

struct HashTree
{
	Eigen::Vector3i rootOffset;
	HashTable::vptr_t rootNode = HTConstants::INVALID_POINTER;
};

/// This class determines the path that a traversal algorithm should take to get to a certain voxel (used in tracing).
/// Starting from the HTConstants::MAX_LEVEL_COUNT-th bit from the right for each component going to the right-most bit,
/// we are presented with the children we should pick for our descent.
/// In the ray intersection algorithm itself, the right-most bit represents the current tree level (not necessarily the
/// last one).
class TraversalPath
{
public:
	TraversalPath();
	TraversalPath(uint32_t x, uint32_t y, uint32_t z);

	/// Ascends in the tree path by the specified number of levels.
	void Ascend(uint32_t levels = 1);
	/// Descends one level into the child specified by the argument.
	void Descend(uint8_t child);

	uint8_t ChildAtLevel(uint32_t level);

	Eigen::Vector3f AsPosition(uint32_t levelRank) const;

	bool IsNull() const;
private:
	/// x component of the tree path.
	uint32_t x_;
	/// y component of the tree path.
	uint32_t y_;
	/// z component of the tree path.
	uint32_t z_;
};

/// Node information holding structure that allows for caching already computed nodes and reusing the data
/// when ascending back up to them.
struct NodeInfo
{
	/// A virtual pointer to the node that is inside the
	HashTable::vptr_t nodePtr;
	/// The child mask of the node.
	uint8_t childMask;
	/// A mask of the children that are to be visited. This is computed based on the traversal ray.
	uint8_t visitMask;
	uint64_t voxelIndex;
};

class HashDAG
{
public:
	HashDAG();
	~HashDAG() = default;

	void Init(uint32_t pagePoolSize);

	/// Finds this leaf node in the structure or creates a new entry and returns a virtual pointer to it.
	HashTable::vptr_t FindOrAddLeaf(uint64_t leaf);
	/// Finds this non-leaf node in the structure or creates a new entry and returns a virtual pointer to it.
	HashTable::vptr_t FindOrAddNode(uint32_t level, uint32_t nodeSize, uint32_t* const node);
	/// Makes a node the root of a tree and assigns it an ofset from the origin.
	void AddRoot(HashTable::vptr_t node, const Eigen::Vector3i& offset);

	Color& AddColorArray(int voxelCount);

	/// Querries the trees stored in this structure. Returns true if the voxels is on, returns false otherwise.
	bool IsActive(const Eigen::Vector3i& voxel) const;

	/// Simulates a ray traversal through the structure. Returns true, if the ray hits a voxel. If returns true,
	/// voxel will contain the position of the voxel that has been hit first on the path of the ray.
	bool CastRay(const Eigen::Vector3f& position, const Eigen::Vector3f& direction, Eigen::Vector3i& voxel,
		const Eigen::Vector3f& perturbationEpsilon = { 1e-5f, 1e-5f, 1e-5f }) const;

	void UploadToGPU(const VulkanFactory::Device::DeviceInfo& deviceInfo, VkCommandPool commandPool,
		VkQueue queue, HashDAGGPUInfo& uploadInfo, ColorGPUInfo& colorInfo, float colorCompressionMargin = 0.f);

	void UploadColorRangeToGPU(const VulkanFactory::Device::DeviceInfo& deviceInfo, VkCommandPool commandPool,
		VkQueue queue, ColorGPUInfo& colorInfo, uint32_t tree, uint64_t offset, uint64_t size, float colorCompressionMargin = 0.f);

	void SetBoundingBox(const InternalBoundingBox& boundingBox);

	int Bottom() const;
	int Top() const;

	int Left() const;
	int Right() const;

	int Back() const;
	int Front() const;

	uint64_t ComputeVoxelIndex(uint32_t tree, uint32_t x, uint32_t y, uint32_t z) const;

	void SetVoxelColor(uint32_t tree, uint64_t voxelIndex, const openvdb::Vec3s& color);
	openvdb::Vec3s GetVoxelColor(uint32_t tree, uint64_t voxelIndex) const;

	void StartColorOperation();
	void EndColorOperation();

	int GetTreeCount() const;

	bool Undo(int tree, uint64_t& rangeStart, uint64_t& rangeEnd);
	bool Redo(int tree, uint64_t& rangeStart, uint64_t& rangeEnd);

	void SortAndUploadTreeIndices(VulkanFactory::Device::DeviceInfo& deviceInfo, VkCommandPool commandPool,
		VkQueue queue, const Eigen::Vector3f& cameraPosition, VulkanFactory::Buffer::BufferInfo& sortedTreesBuffer);

	const Eigen::Vector3i& GetTreeOffset(int tree) const;
	uint32_t GetCoordsTree(const Eigen::Vector3i& coords) const;

#ifdef MEASURE_MEMORY_CONSUMPTION
	uint32_t GetMemoryDadoAttributes() const;
	uint32_t GetMemoryDoloniusAttributes() const;
	uint32_t GetMemoryUsed() const;
	uint32_t GetMemoryNoDAGDadoAttributes() const;
	uint32_t GetMemoryNoDAGDoloniusAttributes() const;
	uint32_t GetSVOInternalNodes() const;
	uint32_t GetSVOLeafNodes() const;
#endif

	HTStats GetHashTableStats() const;

	uint32_t GetColorMemorySize() const;

private:
	/// Recurses through the tree and finds out if the specified voxel is on or off (internal implementation of IsActive).
	bool Traverse(const Eigen::Vector3i& voxel, uint32_t node, uint32_t level, const InternalBoundingBox& bbox) const;

	/// Returns the child mask of the specified node.
	uint8_t GetNodeChildMask(HashTable::vptr_t node) const;
	/// Returns the node as a leaf (using it anywhere besides the leaf level will return undefined numbers).
	uint64_t GetLeaf(HashTable::vptr_t node) const;
	/// Returns the child node of the specified node. Selected 'child' must be included in the child mask of the
	/// specified node. The 'childMask' argument is included to avoid having to query it from the hash table.
	HashTable::vptr_t GetChildNode(HashTable::vptr_t node, uint8_t child, uint8_t childMask) const;
	uint32_t GetChildOffset(HashTable::vptr_t node, uint8_t child, uint8_t childMask) const;
	/// Computes the mask of all children of a node that would be intersected by the ray.
	uint8_t ComputeChildIntersectionMask(uint32_t level, const TraversalPath& path,const Eigen::Vector3f& rayOrigin,
		const Eigen::Vector3f& rayDirection, const Eigen::Vector3f& rayDirectionInverted, float treeScale, bool isRoot = false) const;

	/// Separates the first layer of the child mask of the 4x4x4 leaf and returns it.
	uint8_t GetFirstLeafMask(uint64_t leaf) const;
	uint32_t GetFirstVoxelCount(uint64_t leaf, uint32_t nextChild) const;
	/// Separates the second layer of the child mask of the 4x4x4 leaf using the mask of the first layer and returns it.
	uint8_t GetSecondLeafMask(uint64_t leaf, uint8_t firstMask) const;
	uint32_t GetSecondVoxelCount(uint32_t mask, uint32_t nextChild) const;

	HashTable ht_;
	std::vector<HashTree> trees_;
	std::vector<std::unique_ptr<Color>> treeColorArrays_;
	InternalBoundingBox boundingBox_;
	openvdb::Int32Grid::Ptr treeGrid_;
	openvdb::Int32Grid::Accessor treeGridAccessor_;
};


#include <boost/graph/edmonds_karp_max_flow.hpp>


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

#include <sirikata/core/util/Platform.hpp>
#include "DistributedCoordinateSegmentation.hpp"
#include "WorldPopulationBSPTree.hpp"
#include <sirikata/core/options/CommonOptions.hpp>
#include <boost/tokenizer.hpp>

namespace Sirikata {

BoundingBox3f intersect(const BoundingBox3f& b1, const BoundingBox3f& b2) {

  float x1 = (b1.min().x > b2.min().x) ? b1.min().x : b2.min().x;
  float y1 = (b1.min().y > b2.min().y) ? b1.min().y : b2.min().y;
  float z1 = (b1.min().z > b2.min().z) ? b1.min().z : b2.min().z;

  float x2 = (b1.max().x < b2.max().x) ? b1.max().x : b2.max().x;
  float y2 = (b1.max().y < b2.max().y) ? b1.max().y : b2.max().y;
  float z2 = (b1.max().z < b2.max().z) ? b1.max().z : b2.max().z;

  return BoundingBox3f( Vector3f(x1,y1,z1), Vector3f(x2,y2,z2) );
}

bool intersects(const BoundingBox3f& bbox1, const BoundingBox3f& bbox2) {
  return bbox1.intersects(bbox2);
}

void WorldPopulationBSPTree::setupRegionBoundaries(WorldRegion* regionList) {
  for (int i=0; i<mWorldHeight; i++) {
    for (int j=0; j<mWorldWidth; j++) {
      regionList[i*mWorldWidth+j].mBoundingBox =
	BoundingBox3f( Vector3f(j*mCellEdgeWidth, i*mCellEdgeWidth,0),
		       Vector3f(j*mCellEdgeWidth + mCellEdgeWidth, i*mCellEdgeWidth + mCellEdgeWidth,0) );
      }
    }
}

void WorldPopulationBSPTree::constructBSPTree(SegmentedRegion& bspTree, WorldRegion* regionList, int listLength, bool makeHorizontalCut, int depth)
{
  SegmentedRegion* bspTree1 = new SegmentedRegion(bspTree);
  SegmentedRegion* bspTree2 = new SegmentedRegion(bspTree);

  if (depth > mBiggestDepth) {
    mBiggestDepth = depth;
    std::cout << "mBiggestDepth=" << mBiggestDepth << "\n";
  }

  if (makeHorizontalCut) {
    bspTree1->mBoundingBox =
                  BoundingBox3f(Vector3f(bspTree.mBoundingBox.min().x, bspTree.mBoundingBox.min().y, 0),
		  Vector3f(bspTree.mBoundingBox.max().x, (bspTree.mBoundingBox.min().y+bspTree.mBoundingBox.max().y)/2.0, 0));

    bspTree2->mBoundingBox =
      BoundingBox3f(Vector3f(bspTree.mBoundingBox.min().x, (bspTree.mBoundingBox.min().y+bspTree.mBoundingBox.max().y)/2.0, 0),
		    Vector3f(bspTree.mBoundingBox.max().x, bspTree.mBoundingBox.max().y, 0));

    bspTree1->mSplitAxis = bspTree2->mSplitAxis = SegmentedRegion::Y;
  }
  else {
    bspTree1->mBoundingBox =
      BoundingBox3f(Vector3f(bspTree.mBoundingBox.min().x, bspTree.mBoundingBox.min().y, 0),
		    Vector3f((bspTree.mBoundingBox.min().x+bspTree.mBoundingBox.max().x)/2.0, bspTree.mBoundingBox.max().y, 0));

    bspTree2->mBoundingBox =
      BoundingBox3f(Vector3f((bspTree.mBoundingBox.min().x+bspTree.mBoundingBox.max().x)/2.0, bspTree.mBoundingBox.min().y, 0),
		    Vector3f(bspTree.mBoundingBox.max().x, bspTree.mBoundingBox.max().y, 0));

    bspTree1->mSplitAxis = bspTree2->mSplitAxis = SegmentedRegion::X;
  }

  bool split1 =false, split2 = false;
  double sum1 = 0;
  double sum2 = 0;

  int  i =0;
  int idx1=0, idx2=0;

  WorldRegion* tempRegionList1 = new WorldRegion[listLength];
  WorldRegion* tempRegionList2 = new WorldRegion[listLength];

  for (i=0; i<  listLength; i++) {
    if (regionList[i].density == 0) continue;

    if (intersects(bspTree1->mBoundingBox, regionList[i].mBoundingBox) ) {

      mIntersect1 = intersect(bspTree1->mBoundingBox, regionList[i].mBoundingBox);
      sum1 += regionList[i].density * mIntersect1.across().x * mIntersect1.across().y;
      tempRegionList1[idx1++] = regionList[i];
    }
    if (intersects(bspTree2->mBoundingBox, regionList[i].mBoundingBox) ) {

      mIntersect2 = intersect(bspTree2->mBoundingBox, regionList[i].mBoundingBox);
      sum2 += regionList[i].density * mIntersect2.across().x * mIntersect2.across().y;
      tempRegionList2[idx2++] = regionList[i];
    }

    if (sum1 > mMaxPeopleInLeaf) {
      split1 = true;
    }
    if (sum2 > mMaxPeopleInLeaf) {
      split2 = true;
    }
  }

  WorldRegion* regionList1 = new WorldRegion[idx1];
  WorldRegion* regionList2 = new WorldRegion[idx2];

  for (i=0; i<idx1; i++) {
    regionList1[i] = tempRegionList1[i];
  }
  for (i=0; i<idx2; i++) {
    regionList2[i] = tempRegionList2[i];
  }

  delete tempRegionList1;
  delete tempRegionList2;

  if (split1 || split2) {
    bspTree.mLeftChild = bspTree1;
    constructBSPTree(*bspTree1, regionList1, idx1, !makeHorizontalCut, depth+1);

    bspTree.mRightChild = bspTree2;
    constructBSPTree(*bspTree2, regionList2, idx2, !makeHorizontalCut, depth+1);
  }

  if (!split1 && !split2) {
    bspTree.mServer = (mTotalLeaves % mInitialSpaceServerCount) + 1;
    ++mTotalLeaves;
    mHistogram[depth]++;
  }

  delete regionList1;
  delete regionList2;
}

WorldPopulationBSPTree::WorldPopulationBSPTree()
 : mFileName(GetOptionValue<String>("cseg-population-density-file")),
   mMaxPeopleInLeaf(GetOptionValue<uint32>("cseg-max-leaf-population"))
{
    mWorldWidth = GetOptionValue<uint32>("cseg-world-width");
    mWorldHeight = GetOptionValue<uint32>("cseg-world-height");
  mBiggestDepth = 0;
  mNumRegions = mWorldWidth * mWorldHeight;

  Vector3ui32 layout = GetOptionValue<Vector3ui32>("layout");
  mInitialSpaceServerCount = layout.x * layout.y * layout.z;

  mCellEdgeWidth = sqrt(WORLD_SURFACE_AREA/(mWorldWidth*mWorldHeight));
}

void WorldPopulationBSPTree::constructBSPTree(SegmentedRegion& topLevelRegion) {
  mBiggestDepth = 0;
  mTotalLeaves = 0;

  double i =0, sum=0, largestdensity = 0;
  int j=0, k=0;

  WorldRegion* regionList = new WorldRegion[mNumRegions];

  std::ifstream filestream(mFileName.c_str());

  while(!filestream.bad() && !filestream.fail() && !filestream.eof()) {
    char line[1048576];
    filestream.getline(line,1048576);

    boost::char_separator<char> sep(",: ");
    boost::tokenizer<boost::char_separator<char> > tokens(String(line), sep);
    for(boost::tokenizer<boost::char_separator<char> >::iterator beg=tokens.begin();
	beg!=tokens.end();++beg)
      {
	String token = *beg;

	if (token != "-9999") {
	  double val = atof(token.c_str());
	  regionList[j].density = val;
	  if (val > largestdensity) {
	    largestdensity = val;
	  }
	}
	else {
	  regionList[j].density = 0;
	}

	j++;
      }
  }

  mHistogram = new int[NUM_HISTOGRAM_BINS];
  memset(mHistogram, 0 , NUM_HISTOGRAM_BINS * sizeof(int));

  setupRegionBoundaries(regionList);

  int mindensity = MIN_REGION_DENSITY_CUTOFF;
  j=0;
  std::vector<WorldRegion> regionVector;
  for (k=0; k<mNumRegions; k++) {
    if (regionList[k].density > mindensity) {
      regionVector.push_back(regionList[k]);
      j++;
    }
  }

  WorldRegion* regionList2 = new WorldRegion[regionVector.size()];
  for (k=0; k < (int)regionVector.size(); k++) {
    regionList2[k] = regionVector[k];
  }

  regionVector.clear();

  delete regionList;

  topLevelRegion.mBoundingBox = BoundingBox3f(
				    Vector3f(0,0,0),
 			            Vector3f(mWorldWidth*mCellEdgeWidth, mWorldHeight*mCellEdgeWidth, 0));

  constructBSPTree(topLevelRegion, regionList2, j, false, 1);

  printf("total_leaves=%d\n", mTotalLeaves);
  for (j = 0 ; j < NUM_HISTOGRAM_BINS; j++) {
    std::cout << j << "," << mHistogram[j] << "\n";
  }

  delete regionList2;

  delete mHistogram;

  fflush(stdout);
}

}


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

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <string>
#include <cstdio>

#include "../features.h"
#include "../volume.h"

using namespace std;

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void sampleSphere(PointCloud::Ptr& cloud, const Eigen::Vector3d& origin, double radius, int pointCount)
{
    boost::mt19937 rng;
    boost::normal_distribution<> nd(0.0, 1.0);
    boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(rng, nd);

    for(int i = 0; i < pointCount; i++) {
        Eigen::Vector3d v(var_nor(), var_nor(), var_nor());
        Eigen::Vector3d pointOnSphere = radius / v.norm() * v;
        cloud->points.push_back( PointType(pointOnSphere(0), pointOnSphere(1), pointOnSphere(2)) );
    }
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void samplePlane(PointCloud::Ptr& cloud, const Eigen::Vector3d& normalVector, double distanceToOrigin, int pointCount, double scale1 = 1.0, double scale2 = 1.0)
{
    boost::mt19937 rng;
    boost::variate_generator<boost::mt19937&, boost::uniform_real<> > uni1(rng, boost::uniform_real<>(-0.2 * scale1, +0.2 * scale1));
    boost::variate_generator<boost::mt19937&, boost::uniform_real<> > uni2(rng, boost::uniform_real<>(-0.2 * scale2, +0.2 * scale2));

    for(int i = 0; i < pointCount; i++) {
        double x0 = uni1();
        double x1 = uni2();
        double x2 = (-distanceToOrigin - normalVector(0)*x0 - normalVector(1)*x1) / normalVector(2);

        Eigen::Vector3d pointOnPlane(x0, x1, x2);
        cloud->points.push_back( PointType(pointOnPlane(0), pointOnPlane(1), pointOnPlane(2)) );
    }
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int main(int argc, char **argv)
{
    ros::init(argc, argv, "test_features");
    ros::NodeHandle nodeHandle("");
    ros::NodeHandle privateHandle("~");

    // Create feature calculator
    FeatureCalculator featureCalculator;
    const std::vector<string>& featureNames = featureCalculator.getFeatureNames();

    pcl::PointXYZ minCoords(-0.3, -0.3, 0), maxCoords(0.3, 0.3, 1.8);
    Volume parentVolume(minCoords, maxCoords);

    // Create point cloud publisher
    ros::Publisher pointCloudPublisher = nodeHandle.advertise<sensor_msgs::PointCloud2>("cloud", 1, true);

    for(int testCase = 0; testCase < 7 && ros::ok(); testCase++) {
        PointCloud::Ptr testCloud(new PointCloud);
        testCloud->header.stamp = ros::Time::now().toNSec() / 1000;
        testCloud->header.frame_id = "extracted_cloud_frame";

        switch(testCase)
        {
            case 0:
            {
                ROS_INFO("=== SMALL SPHERE ===");
                Eigen::Vector3d origin(1, 2, 3);
                sampleSphere(testCloud, origin, 0.2, 100);
                break;
            }
            case 1:
            {
                ROS_INFO("=== LARGE SPHERE ===");
                Eigen::Vector3d origin(0, 0, 0);
                sampleSphere(testCloud, origin, 0.4, 500);
                break;
            }
            case 2:
            {
                ROS_INFO("=== EXTRA-LARGE SPHERE ===");
                Eigen::Vector3d origin(1, 1, 0);
                sampleSphere(testCloud, origin, 0.8, 500);
                break;
            }

            case 3:
            {
                ROS_INFO("=== FIRST PLANE ===");
                Eigen::Vector3d normalVector(0.9, 0, 0.3);
                samplePlane(testCloud, normalVector.normalized(), 0.1, 100);
                break;
            }
            case 4:
            {
                ROS_INFO("=== SECOND PLANE ===");
                Eigen::Vector3d normalVector(0.2, 0, 0.5);
                samplePlane(testCloud, normalVector.normalized(), -1.0, 500);
                break;
            }
            case 5:
            {
                ROS_INFO("=== THIRD PLANE ===");
                Eigen::Vector3d normalVector(0, 0, 1);
                samplePlane(testCloud, normalVector.normalized(), 0.0, 200);
                break;
            }
            case 6:
            {
                ROS_INFO("=== FOURTH PLANE ===");
                Eigen::Vector3d normalVector(0, 0, 1);
                samplePlane(testCloud, normalVector.normalized(), 0.0, 200, 0.1, 10.0);
                break;
            }
        }

        std::vector<int> indices;
        for(int i = 0; i < testCloud->points.size(); i++) indices.push_back(i);

        std::vector<double> featureVector;
        featureCalculator.calculateFeatures(parentVolume, *testCloud, indices, featureCalculator.maskAllFeaturesActive(), featureVector);

        for(int f = 0; f < featureVector.size(); f++) {
            ROS_INFO("Feature %s:  %g", featureNames[f].c_str(), featureVector[f]);
        }

        pointCloudPublisher.publish(testCloud);
        ros::spinOnce();

        ROS_INFO("Press ENTER to show next test case...");
        getchar();
    }

    return 0;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


//  Copyright (c) 2007 John Maddock
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_COS_PI_HPP
#define BOOST_MATH_COS_PI_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/config/no_tr1/cmath.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/tools/promotion.hpp>
#include <boost/math/constants/constants.hpp>

namespace boost{ namespace math{ namespace detail{

template <class T, class Policy>
T cos_pi_imp(T x, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names
   // cos of pi*x:
   bool invert = false;
   if(fabs(x) < 0.5)
      return cos(constants::pi<T>() * x);

   if(x < 1)
   {
      x = -x;
   }
   T rem = floor(x);
   if(itrunc(rem, pol) & 1)
      invert = !invert;
   rem = x - rem;
   if(rem > 0.5f)
   {
      rem = 1 - rem;
      invert = !invert;
   }
   if(rem == 0.5f)
      return 0;

   rem = cos(constants::pi<T>() * rem);
   return invert ? T(-rem) : rem;
}

} // namespace detail

template <class T, class Policy>
inline typename tools::promote_args<T>::type cos_pi(T x, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   return boost::math::detail::cos_pi_imp<result_type>(x, pol);
}

template <class T>
inline typename tools::promote_args<T>::type cos_pi(T x)
{
   return boost::math::cos_pi(x, policies::policy<>());
}

} // namespace math
} // namespace boost
#endif