//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE2_SORT_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE2_SORT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/swar/functions/sort.hpp>
#include <boost/simd/include/functions/simd/min.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/include/functions/simd/minimum.hpp>
#include <boost/simd/include/functions/simd/maximum.hpp>
#include <boost/simd/include/functions/simd/make.hpp>
#include <boost/simd/swar/functions/details/shuffle.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::sort_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_ < type32_<A0>
                                              , boost::simd::tag::sse_
                                              >
                                      ))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      A0 p0  = details::shuffle<2,3,0,1>(a0);
      A0 mn = min(a0,p0);
      A0 mx = max(a0,p0);

      A0 minmax = details::shuffle<0,1,0,1>(mn,mx);
      A0 maxmin = details::shuffle<1,0,1,0>(mn,mx);
      mn = min(minmax,maxmin);
      mx = max(minmax,maxmin);

      return  details::shuffle<0,1,0,1>
              ( A0(details::shuffle<0,2,0,2>(mn))
              , A0(details::shuffle<0,2,0,2>(mx))
              );
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::sort_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_ < type64_<A0>
                                              , boost::simd::tag::sse_
                                              >
                                      ))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return boost::simd::make<A0>(minimum(a0), maximum(a0));
    }
  };
} } }

#endif
#endif


#include <cmath>
#include <iostream>

#include <boost/python/list.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/extract.hpp>

#include <mmtbx/tls/utils.h>
#include <mmtbx/tls/optimise_amplitudes.h>

namespace mmtbx { namespace tls { namespace optimise {

template <typename T>
T find_max(const af::shared<T> &array)
{
  T max_val = array[0];
  for (size_t i=1; i<array.size(); i++)
  {
    if (max_val < array[i]) { max_val = array[i]; }
  }
  return max_val;
};

bool is_zero(const sym s, double tol=1e-12)
{
  if (
    (std::abs(s[0])<tol) &&
    (std::abs(s[1])<tol) &&
    (std::abs(s[2])<tol) &&
    (std::abs(s[3])<tol) &&
    (std::abs(s[4])<tol) &&
    (std::abs(s[5])<tol)
    )
  {
    return true;
  }
  return false;
}

//! Main constructor
MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator(
    const symArrNd &target_uijs,
    const dblArrNd &target_weights,
    const dblArr1d &base_amplitudes,
    const bp::list &base_uijs,
    const bp::list &base_atom_indices,
    const selArr1d &base_dataset_hash,
    const symArr1d &atomic_uijs,
    double weight_sum_of_amplitudes,
    double weight_sum_of_squared_amplitudes,
    double weight_sum_of_amplitudes_squared ) :
  target_uijs(target_uijs),
  target_weights(target_weights),
  base_dataset_hash(base_dataset_hash),
  atomic_uijs(atomic_uijs),
  weight_sum_amp(weight_sum_of_amplitudes),
  weight_sum_sqr_amp(weight_sum_of_squared_amplitudes),
  weight_sum_amp_sqr(weight_sum_of_amplitudes_squared),
  n_dst(target_uijs.accessor().all()[0]),
  n_atm(target_uijs.accessor().all()[1]),
  n_base(base_amplitudes.size()),
  n_total(base_amplitudes.size() + atomic_uijs.size()),
  atomic_mask_total(0),
  n_call(0)
{
  std::ostringstream errMsg;

  // ==========================
  // Check target uijs
  // ==========================
  if (target_uijs.accessor().nd() != 2) {
    errMsg << "invalid target_uijs: must be 2-dimensional flex array (currently " << target_uijs.accessor().nd() << ")";
    throw std::invalid_argument( errMsg.str() );
  }

  // ==========================
  // Check weights
  // ==========================
  if (target_uijs.accessor().nd() != target_weights.accessor().nd())
  {
    errMsg << "invalid dimension of target_weights (dimension " << target_weights.accessor().nd() << "): must be same dimension as target_uijs (dimension " << target_uijs.accessor().nd() << ")";
    throw std::invalid_argument( errMsg.str() );
  }
  for (size_t i=0; i<target_uijs.accessor().nd(); i++)
  {
    if (target_uijs.accessor().all()[i] != target_weights.accessor().all()[i])
    {
      errMsg << "incompatible dimension of target_weights (axis " << i << "): must be same size as target_uijs (" << target_weights.accessor().all()[i] << " != " << target_uijs.accessor().all()[i] <<")";
      throw std::invalid_argument( errMsg.str() );
    }
  }

  // ==========================
  // Check base_amplitudes, base_uijs and base_atom_indices (common error message)
  // ==========================
  errMsg << "invalid input base components. "
    << "base_amplitudes (length " << base_amplitudes.size()
    << "), base_uijs (length " << bp::len(base_uijs)
    << ") and base_atom_indices (length " << bp::len(base_atom_indices)
    << ") must all be the same length";
  if (base_amplitudes.size() != n_base) { throw std::invalid_argument( errMsg.str() ); }
  if (bp::len(base_uijs) != n_base) { throw std::invalid_argument( errMsg.str() ); }
  if (bp::len(base_atom_indices) != n_base) { throw std::invalid_argument( errMsg.str() ); }
  errMsg.str(""); // clear the previous error message

  // ==========================
  // Check weights
  // ==========================
  if (weight_sum_amp < 0.0)     { throw std::invalid_argument( "weight_sum_of_amplitudes must be positive" ); }
  if (weight_sum_sqr_amp < 0.0) { throw std::invalid_argument( "weight_sum_of_squared_amplitudes must be positive" ); }
  if (weight_sum_amp_sqr < 0.0) { throw std::invalid_argument( "weight_sum_of_amplitudes_squared must be positive" ); }

  // ==========================
  // Unpack base_uijs and base_atom_indices
  // ==========================
  base_u.reserve(bp::len(base_uijs));
  base_i.reserve(bp::len(base_atom_indices));
  for (std::size_t i = 0; i < n_base; ++i)
  {
    symArr1d* atm_u = new symArr1d(bp::extract<symArr1d>(base_uijs[i]));
    selArr1d* atm_i = new selArr1d(bp::extract<selArr1d>(base_atom_indices[i]));

    if (atm_u->size() != atm_i->size())
    {
      errMsg << "incompatible pair (element " << i << ") in base_uijs/base_atom_indices: pairwise elements must be the same length (" << atm_u->size() << " and " << atm_i->size() << ")";
      throw std::invalid_argument( errMsg.str() );
    }
    if (find_max(*atm_i) >= n_atm)
    {
      errMsg << "invalid selection in base_atom_indices (" << find_max(*atm_i) << "): attempting to select atom outside of array (size " << n_atm << ")";
      throw std::invalid_argument( errMsg.str() );
    }

    base_u.push_back(atm_u);
    base_i.push_back(atm_i);
  }

  // ==========================
  // Check dataset hash
  // ==========================
  if (base_dataset_hash.size() != n_base) {
    errMsg << "invalid base_dataset_hash (length " << base_dataset_hash.size() << "): must be same length as base_amplitudes, base_uijs & base_atom_indices (length " << base_amplitudes.size() << ")";
    throw std::invalid_argument( errMsg.str() );
  }
  if (find_max(base_dataset_hash) >= n_dst)
  {
    errMsg << "invalid value in base_dataset_hash (" << find_max(base_dataset_hash) << "): attempts to select element outside range of target_uijs (size " << n_dst << ")";
    throw std::invalid_argument( errMsg.str() );
  }
  for (size_t i_dst=0; i_dst < n_dst; i_dst++)
  {
    bool found = false;
    for (size_t i_base=0; i_base < n_base; i_base++)
    {
      if (base_dataset_hash[i_base] == i_dst)
      {
        found = true;
        break;
      }
    }
    if (found == false)
    {
      errMsg << "Dataset index " << i_dst << " is not present in base_dataset_hash -- this dataset has no base elements associated with it.";
      throw std::invalid_argument( errMsg.str() );
    }
  }

  // ==========================
  // Check atomic uijs
  // ==========================
  if (atomic_uijs.size() != n_atm)
  {
    errMsg << "invalid size of atomic_uijs (" << atomic_uijs.size() << "): must match 2nd dimension of target_uijs (" << n_atm << ")";
    throw std::invalid_argument( errMsg.str() );
  }

  // ==========================
  // Assign class members
  // ==========================

  // Initialise atomic amplitude array
  dblArr1d res_amplitudes(n_atm, 1.0);

  // Concatenate amplitude arrays
  initial_amplitudes.reserve(n_total);
  std::copy(base_amplitudes.begin(), base_amplitudes.end(), std::back_inserter(initial_amplitudes));
  std::copy(res_amplitudes.begin(), res_amplitudes.end(), std::back_inserter(initial_amplitudes));

  // Copy to current values
  current_amplitudes = dblArr1d(initial_amplitudes);

  // Total Uijs (summed over levels) (datasets * atoms)
  total_uijs = symArrNd(af::flex_grid<>(n_dst, n_atm), sym(0.,0.,0.,0.,0.,0.));

  // Calculate dataset weights (average target weight over each dataset)
  calculateDatasetWeights();

  // Initialise blank atomic mask
  setAtomicOptimisationMask(blnArr1d(n_dst, true));

  // Initialise output
  functional = 0.0;
  gradients = dblArr1d(n_total, 0.0);

}

dblArr1d MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::getCurrentAmplitudes()
{
  return current_amplitudes;
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::setCurrentAmplitudes(const dblArr1d &values)
{
  if (values.size() != current_amplitudes.size()) {
    std::ostringstream errMsg;
    errMsg << "Input array (size " << values.size() << ") must be the same length as current_amplitudes (size " << current_amplitudes.size() << ")";
    throw std::invalid_argument( errMsg.str() );
  }
  for (size_t i=0; i<current_amplitudes.size(); i++)
  {
    current_amplitudes[i] = values[i];
  }
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::printCurrentAmplitudes()
{
  for (size_t i=0; i<current_amplitudes.size(); i++)
  {
    std::cout << i << " - " << current_amplitudes[i] << std::endl;
  }
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::setAtomicOptimisationMask(const blnArr1d &mask)
{
  if (mask.size() != n_dst) {
    std::ostringstream errMsg;
    errMsg << "Input array (size " << mask.size() << ") must be the same length as number of datasets (" << n_dst << ")";
    throw std::invalid_argument( errMsg.str() );
  }
  atomic_mask = blnArr1d(mask);
  atomic_mask_total = 0;
  for (size_t i=0; i<atomic_mask.size(); i++)
  {
    atomic_mask_total += (int)atomic_mask[i];
  }
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::calculateDatasetWeights()
{
  // Overall weights for each dataset
  dataset_weights = dblArr1d(n_dst, 0.0);
  average_dataset_weight = 0.0;
  for (size_t i_dst=0; i_dst<n_dst; i_dst++)
  {
    for (size_t i_atm=0; i_atm<n_atm; i_atm++)
    {
      dataset_weights[i_dst] += target_weights(i_dst,i_atm) / (double)n_atm;
    }
    average_dataset_weight += dataset_weights[i_dst] / (double)n_dst;
  }
}

bp::tuple MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::computeFunctionalAndGradients()
{
  // Reset everything
  zero();

  n_call++;

  // Reset negative amplitudes
  //
  sanitiseCurrentAmplitudes();

  // Apply amplitudes to base uijs
  //
  calculateTotalUijs();

  // Calculate least-squares component of target function
  //
  calculateFGLeastSquares();

  // Penalty: Sum(amplitudes)
  if (weight_sum_amp > 0.0)
  {
    calculateFGSumAmp();
  }

  // Penalty: Sum(amplitudes^2)
  if (weight_sum_sqr_amp > 0.0)
  {
    calculateFGSumSqrAmp();
  }

  // Penalty: (Sum(amplitudes))^2
  if (weight_sum_amp_sqr > 0.0)
  {
    calculateFGSumAmpSqr();
  }

  // Return as python tuple for optimiser
  //
  return bp::make_tuple(functional, gradients);
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::zero()
{
  // Reset functional and gradients
  functional = 0.0;
  memset(&gradients[0], 0.0, sizeof(double) * gradients.size());

  // Zero-out the level uijs
  memset(&total_uijs[0], 0.0, sizeof(sym) * total_uijs.size());
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::sanitiseCurrentAmplitudes()
{
  // Set negative amplitudes to zero
  for (size_t i_opt=0; i_opt<current_amplitudes.size(); i_opt++)
  {
    if (current_amplitudes[i_opt] < 0.0)
    {
      // Constrain the value to be zero (and thereby negate any benefit to the functional that could have been gained)
      current_amplitudes[i_opt] = 0.0;
    }
  }
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::calculateTotalUijs()
{
  // ==========================================================
  // Sum over amplitudes to generate totals - !!! BASE TERMS !!!
  //
  for (size_t i_base=0; i_base<n_base; i_base++)
  {
    symArr1d &base_u_atom = *(base_u[i_base]);
    selArr1d &base_i_atom = *(base_i[i_base]);
    size_t i_dst = base_dataset_hash[i_base];
    size_t i_opt = i_base;

    // Iterate through atoms associated with this base element
    for (size_t i_atm_x=0; i_atm_x<base_u_atom.size(); i_atm_x++)
    {
      // apply multipliers
      sym m = current_amplitudes[i_opt] * base_u_atom[i_atm_x];
      // Skip if null
      if (is_zero(m))
      {
        continue;
      }
      // Add to total uijs
      total_uijs(i_dst, base_i_atom[i_atm_x]) += m;
    }
  }
  //
  // Sum over amplitudes to generate totals - !!! ATOMIC TERMS !!!
  //
  for (size_t i_atm=0; i_atm<n_atm; i_atm++)
  {
    size_t i_opt = n_base + i_atm;
    sym m = current_amplitudes[i_opt] * atomic_uijs[i_atm];
    for (size_t i_dst=0; i_dst<n_dst; i_dst++)
    {
      total_uijs(i_dst, i_atm) += m;
    }
  }
  // ==========================================================
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::calculateFGLeastSquares()
{
  // Normalisation term (none)
  double norm_all = 1.;

  // Normalisation term for atomic level
  double norm_res = 0.0; // if unchanged, atomic level will not be optimised
  if (atomic_mask_total > 0)
  {
    // Upweights by term n_all/n_calc to immitate being calculated over all datasets
    norm_res = norm_all * (double)(n_dst) / (double)(atomic_mask_total);
  }

  // ==========================================================
  // Calculate functional and gradients
  for (size_t i_dst=0; i_dst<n_dst; i_dst++)
  {
    // Extract weights for this dataset
    dblArr1d d_wgts(&target_weights(i_dst,0), &target_weights(i_dst, n_atm));
    // Extract total uijs for this dataset
    symArr1d d_total(&total_uijs(i_dst,0), &total_uijs(i_dst, n_atm));

    // Calculate difference to target
    symArr1d d_diffs(n_atm); // all elements will be populated
    for (size_t i_atm=0; i_atm<n_atm; i_atm++)
    {
      sym d_diff_i = target_uijs(i_dst,i_atm) - total_uijs(i_dst,i_atm);
      d_diffs[i_atm] = d_diff_i;

      // Calculate functional -- least-squares component
      for (size_t i_elem=0; i_elem<6; i_elem++)
      {
        functional += (
          d_diff_i[i_elem] *
          d_diff_i[i_elem] *
          d_wgts[i_atm] *
          norm_all
          );
      }
    }

    // Calculate gradients -- BASE TERMS
    for (size_t i_base=0; i_base<n_base; i_base++)
    {
      // Only calculate if this base term is related to this dataset
      if (i_dst != base_dataset_hash[i_base])
      {
        continue;
      }

      // i_ relative to full length of amplitudes
      size_t i_opt = i_base;

      // Extract the base elements
      symArr1d &base_u_atom = *(base_u[i_base]);
      selArr1d &base_i_atom = *(base_i[i_base]);

      // Iterate through the atoms in the base element
      for (size_t i_atm_x=0; i_atm_x<base_i_atom.size(); i_atm_x++)
      {
        // i_atm relative to n_atm
        size_t i_atm = base_i_atom[i_atm_x];

        // Extract the uij for this base element
        sym base_sym = base_u_atom[i_atm_x];

        // Skip this atom if the base_uij is zero at this position
        if (is_zero(base_sym))
        {
          continue;
        }

        // Corresponding u_diff
        sym diff_sym = d_diffs[i_atm];

        // Gradient from least-squares
        // -2*base*diffs*wgts
        for (size_t i_elem=0; i_elem<6; i_elem++)
        {
          gradients[i_opt] += (
            -2.0 *
            base_sym[i_elem] *
            diff_sym[i_elem] *
            d_wgts[i_atm] *
            norm_all
            );
        }
      }
    }

    // Calculate gradients -- ATOMIC TERMS
    if (atomic_mask[i_dst])
    {
      for (size_t i_atm=0; i_atm<n_atm; i_atm++)
      {
        size_t i_opt = n_base + i_atm;

        // Extract the uij for this base element
        sym base_sym = atomic_uijs[i_atm];

        // Skip this atom if the base_uij is zero at this position
        if (is_zero(base_sym))
        {
          continue;
        }

        // Corresponding u_diff
        sym diff_sym = d_diffs[i_atm];

        // Gradient from least-squares
        // -2*base*diffs*wgts
        for (size_t i_elem=0; i_elem<6; i_elem++)
        {
          gradients[i_opt] += (
            -2.0 *
            base_sym[i_elem] *
            diff_sym[i_elem] *
            d_wgts[i_atm] *
            norm_res
            );
        }
      }
    }
  }
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::calculateFGSumAmp()
{

  // common multiplier for all terms
  double multiplier_all = weight_sum_amp;
  double multiplier_res = multiplier_all * (double)n_dst;

  //
  // calculate functional and gradients
  //
  // base terms
  //
  for (size_t i_base=0; i_base<n_base; i_base++)
  {
    // Extract dataset
    size_t i_dst = base_dataset_hash[i_base];
    // weight by dataset weight
    functional += multiplier_all * dataset_weights[i_dst] * current_amplitudes[i_base];
    gradients[i_base] += multiplier_all * dataset_weights[i_dst];
  }
  //
  // atomic terms
  //
  double res_amp;
  for (size_t i_atm=0; i_atm<n_atm; i_atm++)
  {
    size_t i_opt = n_base + i_atm;
    sym m = current_amplitudes[i_opt] * atomic_uijs[i_atm];
    res_amp = (m[0] + m[1] + m[2]) / 3.0;
    // weight by average_dataset_weight
    functional += multiplier_res * average_dataset_weight * res_amp;
    gradients[i_opt] += multiplier_res * average_dataset_weight;
  }
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::calculateFGSumSqrAmp()
{

  // common multiplier for all terms
  double multiplier_all = weight_sum_sqr_amp;
  double multiplier_res = multiplier_all * (double)n_dst;

  //
  // calculate functional and gradients
  //
  // base terms
  //
  for (size_t i_base=0; i_base<n_base; i_base++)
  {
    // Extract dataset
    size_t i_dst = base_dataset_hash[i_base];
    // weight by dataset weight
    functional += multiplier_all * dataset_weights[i_dst] * current_amplitudes[i_base] * current_amplitudes[i_base];
    gradients[i_base] += 2.0 * multiplier_all * dataset_weights[i_dst] * current_amplitudes[i_base];
  }
  //
  // atomic terms
  //
  double res_amp;
  for (size_t i_atm=0; i_atm<n_atm; i_atm++)
  {
    size_t i_opt = n_base + i_atm;
    sym m = current_amplitudes[i_opt] * atomic_uijs[i_atm];
    res_amp = (m[0] + m[1] + m[2]) / 3.0;
    // weight by average_dataset_weight
    functional += multiplier_res * average_dataset_weight * res_amp * res_amp;
    gradients[i_opt] += 2.0 * multiplier_res * average_dataset_weight * res_amp;
  }
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::calculateFGSumAmpSqr()
{

  // common multiplier for all terms
  double multiplier_all = weight_sum_amp_sqr;

  //
  // calculate amplitude sums
  //
  // base terms
  //
  dblArr1d dst_amp_sum(n_dst, 0.0); // Sum of amplitudes
  for (size_t i_base=0; i_base<n_base; i_base++)
  {
    size_t i_dst = base_dataset_hash[i_base];
    dst_amp_sum[i_dst] += current_amplitudes[i_base];
  }
  //
  // atomic terms
  //
  double res_amp_sum = 0.0;
  for (size_t i_atm=0; i_atm<n_atm; i_atm++)
  {
    size_t i_opt = n_base + i_atm;
    sym m = current_amplitudes[i_opt] * atomic_uijs[i_atm];
    res_amp_sum += (m[0] + m[1] + m[2]) / 3.0;
  }

  //
  // Calculate functionals
  //
  for (size_t i_dst=0; i_dst<n_dst; i_dst++)
  {
    functional += (multiplier_all * dataset_weights[i_dst]) * ( dst_amp_sum[i_dst] + res_amp_sum ) * ( dst_amp_sum[i_dst] + res_amp_sum );
  }

  // Calculate gradients
  //
  // base terms
  //
  for (size_t i_base=0; i_base<n_base; i_base++)
  {
    size_t i_dst = base_dataset_hash[i_base];
    gradients[i_base] += 2.0 * (multiplier_all * dataset_weights[i_dst]) * ( dst_amp_sum[i_dst] + res_amp_sum );
  }
  //
  // atomic terms
  //
  for (size_t i_atm=0; i_atm<n_atm; i_atm++)
  {
    size_t i_opt = n_base + i_atm;
    for (size_t i_dst=0; i_dst<n_dst; i_dst++)
    {
      gradients[i_opt] += 2.0 * (multiplier_all * dataset_weights[i_dst]) * ( dst_amp_sum[i_dst] + res_amp_sum );
    }
  }
}

} } } // close namepsace mmtbx/tls/optimise


//
// Created by riku on 5/5/15.
//

#ifndef CCS2015_FHEUTILS_HPP
#define CCS2015_FHEUTILS_HPP
#include <NTL/ZZX.h>
#include "fhe/Ctxt.h"
#include "fhe/FHEContext.h"
#include "fhe/FHE.h"
#include <cstring>
#include <vector>
#include <deque>
#include <strstream>
typedef void (*functor)(Ctxt      & op1,
                        const Ctxt& op2);

void add_with_log_noise(Ctxt                   & res,
                        const std::vector<Ctxt>& input);

void mul_with_log_noise(Ctxt                   & res,
                        const std::vector<Ctxt>& input);

/*!
 * @param plain: packing into this polynomial
 * @param n : phi(m)
 */
void pack_into_coeff(NTL::ZZX               & plain,
                     const std::vector<long>& input,
                     long                     n,
                     bool                     reverse = false);

/*!
 * @param n: phi(m)
 * @param p: plaintext range
 * @param except: no noise of this coefficient
 */
void add_noise_to_coeff(Ctxt& res,
                        long  n,
                        long  p,
                        long  except);

template<class T>
std::vector<unsigned char>fhe_convert(const T& v)
{
    std::stringstream sstream;

    sstream << v;
    const auto& str = sstream.str();
    const unsigned char *p = (const unsigned char *)str.c_str();
    std::vector<unsigned char> cpy(str.size());
    std::memcpy(cpy.data(), p, str.size());
    return cpy;
}

template<class T>
void fhe_convert(T& v, const std::vector<unsigned char>& bytes)
{
    const char *p = (const char *)bytes.data();
    std::string str(p, bytes.size());
    std::stringstream sstream(str);
    sstream >> v;
}

void dump_FHE_setting_to_file(const std::string& file,
                              long               k,
                              long               m,
                              long               p,
                              long               r,
                              long               L);

void load_FHE_setting(const std::string& file,
                      FHEcontext       & context,
                      FHESecKey        & sk);

/// Sum the first r-slots. 1 <= r <= ea.size()
/// @param ea: EncryptedArray
/// @param r: the fitst r slots to Sum
/// @param ctxt: ctxt that to be sum
void totalSums(const EncryptedArray &ea, const long r, Ctxt &ctxt);
#endif // CCS2015_FHEUTILS_HPP


#include <Engine/MeshEdit/ASAP.h>

#include <Engine/MeshEdit/MinSurf.h>

#include <Engine/Primitive/TriMesh.h>

#include <Eigen/Sparse>

#include <Engine/MeshEdit/Paramaterize.h>

using namespace Ubpa;
using namespace std;
using namespace Eigen;

ASAP::ASAP(Ptr<TriMesh> triMesh)
	: heMesh(make_shared<HEMesh<V>>())
{
	Init(triMesh);
}

void ASAP::Clear() {
	heMesh->Clear();
	triMesh = nullptr;
}

bool ASAP::Init(Ptr<TriMesh> triMesh) {
	Clear();

	if (triMesh == nullptr)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::ASAP::Init:\n"
			"\t""trimesh is invalid\n");
		return false;
	}

	// init half-edge structure
	size_t nV = triMesh->GetPositions().size();
	vector<vector<size_t>> triangles;
	triangles.reserve(triMesh->GetTriangles().size());
	for (auto triangle : triMesh->GetTriangles())
		triangles.push_back({ triangle->idx[0], triangle->idx[1], triangle->idx[2] });
	heMesh->Reserve(nV);
	heMesh->Init(triangles);

	if (!heMesh->IsTriMesh() || !heMesh->HaveBoundary()) {
		printf("ERROR::ASAP::Init:\n"
			"\t""trimesh is not a triangle mesh or hasn't a boundaries\n");
		heMesh->Clear();
		return false;
	}

	// triangle mesh's texcoords and positions ->  half-edge structure's texcoords and positions
	for (int i = 0; i < nV; i++) {
		auto v = heMesh->Vertices().at(i);
		v->coord = triMesh->GetTexcoords()[i].cast_to<vecf2>();
		v->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
	}

	// flatten the triangles to the plane
	flatten();

	this->triMesh = triMesh;
	return true;
}

bool ASAP::Run() {
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::MinSurf::Run\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}
	kernelASAP();

	// half-edge structure -> triangle mesh
	size_t nV = heMesh->NumVertices();
	size_t nF = heMesh->NumPolygons();

	vector<pointf2> texcoords;
	for (auto v : heMesh->Vertices())
		texcoords.push_back(v->coord.cast_to<pointf2>());
	triMesh->Update(texcoords);

	cout << "ASAP done" << endl;
	return true;
}

bool ASAP::Show()
{
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::ASAP::Show\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}

	kernelASAP();

	// half-edge structure -> triangle mesh
	size_t nV = heMesh->NumVertices();
	size_t nF = heMesh->NumPolygons();
	vector<pointf3> positions;
	vector<unsigned> indice;
	positions.reserve(nV);
	indice.reserve(3 * nF);

	// Set positions
	for (auto v : heMesh->Vertices()) {
		positions.push_back(v->coord.cast_to<pointf3>());
	}

	for (auto f : heMesh->Polygons()) { // f is triangle
		for (auto v : f->BoundaryVertice()) { // vertices of the triangle
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
		}
	}

	triMesh->Init(indice, positions);

	// Set texcoords
	vector<pointf2> texcoords;
	for (auto v : heMesh->Vertices())
		texcoords.push_back(v->coord.cast_to<pointf2>());
	triMesh->Update(texcoords);

	cout << "ASAP done" << endl;
	return true;
}

void ASAP::kernelASAP()
{
	int nT = static_cast<int>(heMesh->NumPolygons());
	int nV = static_cast<int>(heMesh->NumVertices());

	if (nV < 2)
		return;
	int index1 = static_cast<int>(heMesh->Index(heMesh->Boundaries()[0][0]->Origin()));
	size_t a = heMesh->Boundaries()[0].size() / 2;
	int index2 = static_cast<int>(heMesh->Index(heMesh->Boundaries()[0][a]->End()));

	vector<double> two_points = { 0, 0, 1, 1 };

	// construct the equations
	std::vector<Triplet<double>> A_Triplet;
	VectorXd b = VectorXd::Zero(2 * (size_t)nV + 2 * (size_t)nT);
	auto triangles = heMesh->Polygons();

	for (int t = 0; t < nT; t++)
	{
		double tmp = 0;

		for (int i = 0; i < 3; i++)
		{
			int v_index = static_cast<int>(heMesh->Index(triangles[t]->BoundaryVertice()[i]));
			tmp += cot[t][i] * (diff_x[t][i] * diff_x[t][i] + diff_y[t][i] * diff_y[t][i]);

			if (v_index != index1 && v_index != index2)
			{
				A_Triplet.push_back(Triplet<double>(2 * nV + 2 * t, 2 * v_index,
					-cot[t][i] * diff_x[t][i] + cot[t][i == 0 ? 2 : i - 1] * diff_x[t][i == 0 ? 2 : i - 1]));
				A_Triplet.push_back(Triplet<double>(2 * nV + 2 * t, 2 * v_index + 1,
					-cot[t][i] * diff_y[t][i] + cot[t][i == 0 ? 2 : i - 1] * diff_y[t][i == 0 ? 2 : i - 1]));
				A_Triplet.push_back(Triplet<double>(2 * nV + 2 * t + 1, 2 * v_index,
					-cot[t][i] * diff_y[t][i] + cot[t][i == 0 ? 2 : i - 1] * diff_y[t][i == 0 ? 2 : i - 1]));
				A_Triplet.push_back(Triplet<double>(2 * nV + 2 * t + 1, 2 * v_index + 1,
					cot[t][i] * diff_x[t][i] - cot[t][i == 0 ? 2 : i - 1] * diff_x[t][i == 0 ? 2 : i - 1]));

				A_Triplet.push_back(Triplet<double>(2 * v_index, 2 * nV + 2 * t,
					-cot[t][i] * diff_x[t][i] + cot[t][i == 0 ? 2 : i - 1] * diff_x[t][i == 0 ? 2 : i - 1]));
				A_Triplet.push_back(Triplet<double>(2 * v_index + 1, 2 * nV + 2 * t,
					-cot[t][i] * diff_y[t][i] + cot[t][i == 0 ? 2 : i - 1] * diff_y[t][i == 0 ? 2 : i - 1]));
				A_Triplet.push_back(Triplet<double>(2 * v_index, 2 * nV + 2 * t + 1,
					-cot[t][i] * diff_y[t][i] + cot[t][i == 0 ? 2 : i - 1] * diff_y[t][i == 0 ? 2 : i - 1]));
				A_Triplet.push_back(Triplet<double>(2 * v_index + 1, 2 * nV + 2 * t + 1,
					cot[t][i] * diff_x[t][i] - cot[t][i == 0 ? 2 : i - 1] * diff_x[t][i == 0 ? 2 : i - 1]));
			}

			else if (v_index == index2)
			{
				b(2 * (size_t)nV + 2 * (size_t)t) -= (-cot[t][i] * diff_x[t][i] + cot[t][i == 0 ? 2 : i - 1] * diff_x[t][i == 0 ? 2 : i - 1]) * two_points[2];
				b(2 * (size_t)nV + 2 * (size_t)t) -= (-cot[t][i] * diff_y[t][i] + cot[t][i == 0 ? 2 : i - 1] * diff_y[t][i == 0 ? 2 : i - 1]) * two_points[3];
				b(2 * (size_t)nV + 2 * (size_t)t + 1) -= (-cot[t][i] * diff_y[t][i] + cot[t][i == 0 ? 2 : i - 1] * diff_y[t][i == 0 ? 2 : i - 1]) * two_points[2];
				b(2 * (size_t)nV + 2 * (size_t)t + 1) -= (cot[t][i] * diff_x[t][i] - cot[t][i == 0 ? 2 : i - 1] * diff_x[t][i == 0 ? 2 : i - 1]) * two_points[3];
			}
		}

		A_Triplet.push_back(Triplet<double>(2 * nV + 2 * t, 2 * nV + 2 * t, tmp));
		A_Triplet.push_back(Triplet<double>(2 * nV + 2 * t + 1, 2 * nV + 2 * t + 1, tmp));
	}

	for (int i = 0; i < nV; i++)
	{
		auto adjedges = heMesh->Vertices()[i]->OutHEs();
		size_t size = adjedges.size();
		double tmp = 0;

		if (i == index1 || i == index2)
		{
			A_Triplet.push_back(Triplet<double>(2 * i, 2 * i, 1));
			A_Triplet.push_back(Triplet<double>(2 * i + 1, 2 * i + 1, 1));
			b(2 * (size_t)i) = i == index1 ? two_points[0] : two_points[2];
			b(2 * (size_t)i + 1) = i == index1 ? two_points[1] : two_points[3];
			continue;
		}

		for (int j = 0; j < size; j++)
		{
			double coe_ij = 0;
			// Left Triangle
			auto triangle = adjedges[j]->Polygon();
			size_t t = 0;
			int index = 0;
			if (triangle != nullptr)
			{
				t = heMesh->Index(triangle);
				for (index = 0; heMesh->Index(triangle->BoundaryVertice()[index]) != i; index++);
				coe_ij += cot[t][index];
			}
			// Right Triangle
			triangle = adjedges[j]->Pair()->Polygon();
			if (triangle != nullptr)
			{
				t = heMesh->Index(triangle);
				for (index = 0; heMesh->Index(triangle->BoundaryVertice()[index]) != i; index++);
				coe_ij += cot[t][index == 0 ? 2 : index - 1];
			}
			// Set
			tmp += coe_ij;
			int v_index = static_cast<int>(heMesh->Index(adjedges[j]->End()));
			if (v_index != index1 && v_index != index2)
			{
				A_Triplet.push_back(Triplet<double>(i * 2, 2 * v_index, -coe_ij));
				A_Triplet.push_back(Triplet<double>(i * 2 + 1, 2 * v_index + 1, -coe_ij));
			}
			else if (v_index == index2)
			{
				b((size_t)i * 2) -= (-coe_ij) * two_points[2];
				b((size_t)i * 2 + 1) -= (-coe_ij) * two_points[3];
			}
		}

		A_Triplet.push_back(Triplet<double>(i * 2, i * 2, tmp));
		A_Triplet.push_back(Triplet<double>(i * 2 + 1, i * 2 + 1, tmp));
	}

	//cout << b << endl;

	// solve the equations
	SparseMatrix<double> A(2 * (size_t)nV + 2 * (size_t)nT, 2 * (size_t)nV + 2 * (size_t)nT);
	SimplicialLLT<SparseMatrix<double>> solver;
	A.setFromTriplets(A_Triplet.begin(), A_Triplet.end());
	//cout << A << endl;
	solver.compute(A);

	VectorXd V = solver.solve(b);
	//cout << V << endl;

	// Update the solutions
	for (auto v : heMesh->Vertices()) {
		size_t i = heMesh->Index(v);
		if (i != index1 && i != index2) {
			pointf2 tmp = { V(static_cast<Index>(2 * i)), V(static_cast<Index>(2 * i + 1)) };
			v->coord = tmp.cast_to<vecf2>();
		}
		else if (i == index1) {
			pointf2 tmp = { two_points[0], two_points[1] };
			v->coord = tmp.cast_to<vecf2>();
		}
		else {
			pointf2 tmp = { two_points[2], two_points[3] };
			v->coord = tmp.cast_to<vecf2>();
		}
	}
}

void ASAP::flatten()
{
	size_t nT = heMesh->NumPolygons();
	std::vector<std::vector<vecf2>> x_plane(nT);
	cot.resize(nT);
	auto triangles = heMesh->Polygons();

	for (int i = 0; i < nT; i++) {
		auto x_tmp = triangles[i]->BoundaryVertice();
		x_plane[i].push_back(vecf2(0, 0));
		x_plane[i].push_back(vecf2((x_tmp[1]->pos - x_tmp[0]->pos).norm(), 0));
		float x = (x_tmp[2]->pos - x_tmp[0]->pos).dot(x_tmp[1]->pos - x_tmp[0]->pos) / (x_tmp[1]->pos - x_tmp[0]->pos).norm();
		x_plane[i].push_back(vecf2(x, pow(pow((x_tmp[2]->pos - x_tmp[0]->pos).norm(), 2) - pow(x, 2), 0.5)));

		if (Paramaterize::EqualVector(x_tmp[0]->pos, x_tmp[1]->pos) || Paramaterize::EqualVector(x_tmp[1]->pos, x_tmp[2]->pos) || Paramaterize::EqualVector(x_tmp[2]->pos, x_tmp[0]->pos))
		{
			cot[i] = { 1, 1, 1 };
			diff_x.push_back({ 1, 1, 1 });
			diff_y.push_back({ 1, 1, 1 });
			continue;
		}

		diff_x.push_back({ (double)x_plane[i][0][0] - x_plane[i][1][0], (double)x_plane[i][1][0] - x_plane[i][2][0], (double)x_plane[i][2][0] - x_plane[i][0][0] });
		diff_y.push_back({ (double)x_plane[i][0][1] - x_plane[i][1][1], (double)x_plane[i][1][1] - x_plane[i][2][1], (double)x_plane[i][2][1] - x_plane[i][0][1] });

		for (int j = 0; j < 3; j++) {
			double cos_theta = (x_plane[i][j] - x_plane[i][j == 0 ? 2 : j - 1]).cos_theta(x_plane[i][j == 2 ? 0 : j + 1] - x_plane[i][j == 0 ? 2 : j - 1]);
			double abs_cot = pow(cos_theta * cos_theta / (1 - cos_theta * cos_theta), 0.5);

			cot[i].push_back(cos_theta > 0 ? abs_cot : -abs_cot);
		}
	}
}


// Copyright 2020 Peter Dimov
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt)

#include <boost/bind/protect.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/config.hpp>
#include <boost/config/workaround.hpp>
#include <functional>

//

int main()
{
    BOOST_TEST_EQ( boost::protect( std::plus<int>() )( 1, 2 ), 3 );
    BOOST_TEST_EQ( boost::protect( std::minus<int>() )( 1, 2 ), -1 );
    BOOST_TEST_EQ( boost::protect( std::multiplies<int>() )( 1, 2 ), 2 );
    BOOST_TEST_EQ( boost::protect( std::divides<int>() )( 1, 2 ), 0 );
    BOOST_TEST_EQ( boost::protect( std::modulus<int>() )( 1, 2 ), 1 );
    BOOST_TEST_EQ( boost::protect( std::negate<int>() )( 1 ), -1 );

    BOOST_TEST_EQ( boost::protect( std::equal_to<int>() )( 1, 2 ), false );
    BOOST_TEST_EQ( boost::protect( std::not_equal_to<int>() )( 1, 2 ), true );
    BOOST_TEST_EQ( boost::protect( std::greater<int>() )( 1, 2 ), false );
    BOOST_TEST_EQ( boost::protect( std::less<int>() )( 1, 2 ), true );
    BOOST_TEST_EQ( boost::protect( std::greater_equal<int>() )( 1, 2 ), false );
    BOOST_TEST_EQ( boost::protect( std::less_equal<int>() )( 1, 2 ), true );

    BOOST_TEST_EQ( boost::protect( std::logical_and<int>() )( 1, 2 ), true );
    BOOST_TEST_EQ( boost::protect( std::logical_or<int>() )( 1, 2 ), true );
    BOOST_TEST_EQ( boost::protect( std::logical_not<int>() )( 1 ), false );

#if !BOOST_WORKAROUND(BOOST_MSVC, < 1600)

    BOOST_TEST_EQ( boost::protect( std::bit_and<int>() )( 1, 2 ), 0 );
    BOOST_TEST_EQ( boost::protect( std::bit_or<int>() )( 1, 2 ), 3 );
    BOOST_TEST_EQ( boost::protect( std::bit_xor<int>() )( 1, 2 ), 3 );

#endif

    return boost::report_errors();
}


#include <iostream>
#include "conditional_moment_ineq.h"
#include <Eigen/Dense>
using namespace std;
using namespace conditional_moment_inequality;

using Mat = Eigen::Matrix<FLOAT, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
using Vec = Eigen::Matrix<FLOAT, Eigen::Dynamic, 1>;

template<typename T>
void
apply(Mat& M, T& f)
{
    FLOAT* M_ptr = M.data();
    UInt   n_M   = M.size();

    for (UInt i = 0; i < n_M; ++i)
    {
        M_ptr[i] = f(M_ptr[i]);
    }
}

// randn
inline void
randn(Mat& M, std::mt19937& rng)
{
    std::normal_distribution<double> normal_dist(0, 1);

    auto f = [&rng, &normal_dist](FLOAT x) { return normal_dist(rng); };
    apply(M, f);
}

int main()
{
    mt19937 rng(1000);
    if (sizeof(FLOAT) == 4)
    {
        cout << "runing with 32-bit float" << endl;
    }
    if (sizeof(FLOAT) == 8) cout << "runing with 64-bit FLOAT" << endl;

    int sample_size = 1000;
    Mat M(sample_size, 4);
    randn(M, rng);

    vector<FLOAT *> X(3);
    for (auto & x : X)
    {
        x = new FLOAT[sample_size];
        std::normal_distribution<double> normal_dist(0, 1);
        for (UInt i = 0; i < sample_size; ++i) x[i] = normal_dist(rng);
    }

    // Test true parameter
    cout << "Test true parameter: " << endl;
    Conditional_Moment_Ineq ineq(sample_size, 0.05, Conditional_Moment_Ineq::KS_S1);
    vector<int> sign(M.cols(), 0);
    ineq.set_cond_M(M.data(), M.cols(), sign.data());
    ineq.set_instruments(X, M.cols());

    bool test_result = ineq.inference();
    if (test_result)
    {
        cout << "the moment model is accepted." << endl;
    }
    else
    {
        cout << "the moment model is rejected." << endl;
    }

    // Test wrong parameter
    cout << "Test wrong parameter: " << endl;
    Mat M_wrong = M;
    M_wrong.col(0).array() -= 0.2;
    sign[0] = 1;
    ineq.set_cond_M(M_wrong.data(), M_wrong.cols(), sign.data());
    test_result = ineq.inference();
    if (test_result)
    {
        cout << "the moment model is accepted." << endl;
    }
    else
    {
        cout << "the moment model is rejected." << endl;
    }

    for (auto & x : X)
    {
        delete [] x;
    }
    return 0;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE InfinityWrapperTests
#include <boost/test/unit_test.hpp>

#include "math/InfinityWrapper.hpp"

namespace pbqppapa {

BOOST_AUTO_TEST_CASE(infinityLimitTest) {

	InfinityWrapper<unsigned int> inful = InfinityWrapper<unsigned int>::getInfinite();
	InfinityWrapper<unsigned int> inful2 (4294967295);
	BOOST_CHECK(inful == inful2);
	BOOST_CHECK(inful.isInfinite());
	BOOST_CHECK(inful2.isInfinite());
	BOOST_TEST_MESSAGE(std::to_string(inful.getValue()));
	BOOST_TEST_MESSAGE(std::to_string(inful2.getValue()));
}

}


#include "resreq.h"
#include "Absence.h"
#include <boost/date_time/gregorian/greg_duration.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/gregorian/parsers.hpp>
#include <iostream>
#include <map>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>

int main(int argc, char **argv)
{
	using boost::gregorian::date_period;
	using boost::gregorian::days;
	using boost::gregorian::from_string;
	using boost::gregorian::date;

	auto [filename, end_date, period_years, inclusive] =
		parseArguments(argc, argv);

	std::vector<std::string> dates_input;
	try {
		dates_input = loadFile(filename);
	} catch (std::runtime_error &e) {
		std::cout << e.what() << '\n';
		return EXIT_FAILURE;
	}
	const auto main_period = setPeriod(end_date, period_years);
	auto absences = std::map<date_period, days>();
	for (const auto &line : dates_input) {
		try {
			std::stringstream sstream(line);
			std::string s1, s2;
			sstream >> s1 >> s2;
			auto dates = date_period(from_string(s1), from_string(s2));
			auto duration = dates_difference(main_period, dates, inclusive);
			if (duration.days() != 0) {
				absences[dates] = duration;
			}
		} catch (std::exception &e) {
			die({ "Error: bad date format entry:", "\nDetail:", e.what() });
		}
	}
	print_dates(std::cout, main_period, absences);
	std::exit(EXIT_SUCCESS);
}


#include "dataset.h"
#include <sstream>
#include <fstream>
#include <iostream>
#include <Eigen/Core>

using namespace Eigen;

Dataset::Dataset(void) {
    this->features = 0;
    this->records = 0;
    this->x = nullptr;
    this->y = nullptr;
}

Dataset::~Dataset(void){
    if (this->x != nullptr)
        delete[] this->x;
    if (this->y != nullptr)
        delete[] this->y;
}

void Dataset::normalize(void){
    MatrixXd m = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(this->x,this->records,this->features);
    RowVectorXd mean = m.colwise().mean();
    RowVectorXd var = (m.rowwise() - mean).array().square().colwise().mean().sqrt();
    m = (m.rowwise() - mean).array().rowwise() / var.array();
}

void Dataset::read_csv(std::string filename){
    std::ifstream ifs(filename);
    std::string row;
    while(std::getline(ifs, row)){
        this->records++;
        if (this->records == 1) {
            std::stringstream ss(row);
            std::string value;
            while (std::getline(ss, value, ',')) {
                this->features++;
            }
            this->features--; // Remove the target label
        }
    }
    ifs.clear();
    ifs.seekg(0, std::ios::beg);
    this->x = new double[this->records*this->features];
    this->y = new double[this->records];
    int index = 0;
    while(std::getline(ifs, row)){
        std::stringstream ss(row);
        std::string value;
        int feature_index = 0;
        while(std::getline(ss, value, ',')){
            if (feature_index < this->features)
                this->x[index * this->features + feature_index] = std::stod(value);
            else
                this->y[index] = std::stod(value);
            feature_index++;
        }
        index++;
    }
}

void Dataset::random_swap(unsigned int how_many){
    double swap_holder_x, swap_holder_y;
    for(int i=0; i<how_many; i++){
        int index_a = rand() % this->records;
        int index_b = rand() % this->records;
        swap_holder_y = this->y[index_a];
        for(int j=0; j<this->features; j++){
            swap_holder_x = this->x[index_b * this->features + j];
            this->x[index_b * this->features + j] = this->x[index_a * this->features + j];
            this->x[index_a * this->features + j] = swap_holder_x;
        }
        this->y[index_a] = this->y[index_b];
        this->y[index_b] = swap_holder_y;
    }
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_ARITHMETIC_FUNCTION_SIMD_COMMON_TWO_SPLIT_HPP_INCLUDED
#define NT2_TOOLBOX_ARITHMETIC_FUNCTION_SIMD_COMMON_TWO_SPLIT_HPP_INCLUDED
#include <nt2/sdk/constant/real.hpp>
#include <nt2/sdk/meta/adapted_traits.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/sdk/meta/strip.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::two_split_, tag::cpu_,
                            (A0)(X),
                            ((simd_<arithmetic_<A0>,X>))
                           );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::two_split_(tag::simd_<tag::arithmetic_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::strip<A0>::type                    str_t;
      typedef typename boost::fusion::tuple<str_t, str_t>        type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      typename NT2_RETURN_TYPE(1)::type res;
      eval(a0,boost::fusion::at_c<0>(res),boost::fusion::at_c<1>(res));
      return res;
    }
  private :
    template<class A0,class R0,class R1> inline void
    eval(A0 const& a, R0& r0, R1& r1)const
    {
      typedef typename meta::scalar_of<A0>::type s_type;
      A0 c = Splitfactor<A0>()*a  ;
      r0 =  c-(c-a);
      r1 = a-r0;
    }
  };
} }

#endif
// modified by jt the 04/01/2011


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

#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/simd/include/constants/mindenormal.hpp>
#include <boost/simd/include/constants/smallestposval.hpp>
#include <boost/simd/toolbox/ieee/functions/simd/common/details/ulp.hpp>
#include <boost/simd/include/functions/simd/is_eqz.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/ldexp.hpp>
#include <boost/simd/include/functions/simd/abs.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::ulp_, simd::tag::altivec_
                                   , (A0)
                                   , ((simd_<floating_<A0>, simd::tag::altivec_>))
                                   )
  {
    typedef A0 result_type;
    typedef typename dispatch::meta::as_integer<A0>::type in_t;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      const A0 x   = simd::abs(a0);
      const in_t n = simd::Nbmantissabits<A0>();
      const A0 res = simd::if_else( simd::is_eqz(x)
                                  , simd::Mindenormal<A0>()
                                  , simd::if_else( simd::is_less(x,simd::Smallestposval<A0>())
                                                 , simd::ldexp(details::ulp(simd::ldexp(x,n)),-n)
                                                 , details::ulp(x)
                                                 )
                                  );
      return res;
    }
  };

} } }

#endif
#endif


#ifndef JHMI_FILL_LIVER_VOLUME_HPP_NRC_20141022
#define JHMI_FILL_LIVER_VOLUME_HPP_NRC_20141022

#include "utility/cube.hpp"
#include "utility/pt3.hpp"
#include "utility/units.hpp"
#include "utility/views.hpp"
#include "shape/voxelized_shape.hpp"
#include <boost/optional.hpp>
#include <range/v3/algorithm.hpp>
#include <cmath>
#include <cstddef>

namespace jhmi {
  namespace lobule {
    //Source: Heterogeneity of microsphere distribution in resected liver and tumor tissue
    // following selective intrahepatic radiotherapy by J. Hogberg et al.
    //A lobule weights 2 mg, 1 portal tract per mg of liver tissue.

    //Source: Targeted radionuclide therapy has the potential to selectively deliver
    // radiation to diseased cells with minimal toxicity to surrounding tissues.
    // M Sathekge et al.
    //SIRT injects radioembolizers via the hepatic artery.
    // Liver metastases are primarily fed via the hepatic artery, whereas normal liver
    // tissue is fed primarily via the portal vein

    //Source: Hepatic Structural Dosimetry in 90Y Microsphere Treatment: A Monte Carlo
    // Modeling Approach Based on Lobular Microanatomy Seza A. Gulec et al.
    constexpr static m const side_length = 0.693_mm;
    constexpr static m const min_radius = 0.6_mm;
    constexpr static m const cell_thickness = 1.5_mm;

    //Source: Portal Hypertension: Pathobiology, Evaluation, and Treatment
    // By Arun J. Sanyal, Vijay H. Shah.
    //static m const centrilobular_vein_radius = 32.5_um;
    //static m const portal_tract_vein_radius = 20_um;//Leads to sinusiods
    //static m const hepatic_arteriole_radius = 7.5_um;//Leads to sinusiods

    //Probably better: Crawford, Lin and Crawford
    // Normal Adult Human Liver Biopsy: A Quantitative Reference Standard
    constexpr static m const centrilobular_vein_radius = 33.45_um;//std 20.15
    constexpr static m const portal_tract_vein_radius = 17.5_um;//std 12.25
    constexpr static m const hepatic_arteriole_radius = 5.9_um;//std 2.35

    //   /\/\/\       y
    //  | | | |       ^
    //   \/\/\/       |-->x
    // z is perpindicular to the x-y plane shown above.
    constexpr static auto const xstep = lobule::min_radius;//Distance between centers of adjacent rows
    constexpr static auto const ystep = lobule::side_length*3.;//Distance between centers in same row
    constexpr static auto const zstep = lobule::cell_thickness;//Thickness of each row of lobules

    constexpr static auto const xoff = xstep / 2.;
    constexpr static auto const yoff0 = 5. * lobule::side_length / 2.;
    constexpr static auto const yoff1 = lobule::side_length;
    constexpr static auto const zoff = zstep / 2.;

    constexpr static auto const dloff = m3{lobule::min_radius, lobule::side_length / 2., 0_mm};
    constexpr static auto const doff = m3{0_mm, lobule::side_length, 0_mm};
  }//end lobule

  //This calls a functor f for each lobule center of a given volume.
  //  Volume must be callable with an m3, returning true if that point is
  //    contained, false otherwise.
  //  F is required to accept an m3 and int3 to its function call operator.
  template <typename Shape, typename F>
  void for_lobule(Shape const& shape, F f) {
    using namespace lobule;
    auto cube = extents(shape);
    int zidx = 0;
    for (auto z = cube.ul().z + zoff; z < cube.lr().z; z += zstep, ++zidx) {
      int xidx = 0;
      for (auto x = cube.ul().x + xoff; x < cube.lr().x; x += xstep, ++xidx) {
        int yidx = 0;
        auto yoff = xidx % 2 == 0 ? yoff0 : yoff1;
        for (auto y = cube.ul().y + yoff; y < cube.lr().y; y += ystep, ++yidx) {
          auto pt = m3{x,y,z};
          if (shape(pt))
            f(pt, int3{xidx,yidx,zidx});
        }//y
      }//x
    }//z
  }
  template <typename Shape>
  auto lobules_in(Shape const& shape) {
    namespace rv = ranges::view;
    auto ex = extents(shape);
    using namespace lobule;
    return rv::for_each(view::step(ex.ul().z + zoff, ex.lr().z, zstep),
      [=](m z) { return rv::for_each(view::step(ex.ul().x + xoff, ex.lr().x, xstep),
      [=, yoff = yoff1](m x) mutable {
        yoff = yoff < yoff0 ? yoff0 : yoff1;
        return rv::for_each(view::step(ex.ul().y + yoff, ex.lr().y, ystep),
          [=](m y) { return ranges::yield(m3{x,y,z}); }); }); })
      | rv::filter([&](m3 const& pt) { return shape(pt); });
  }

  namespace jhmi_detail {
    int to_idx(m p, m step, m offset) {
      return int(std::round((p - offset) / step));
    }
    int3 to_idx(m3 const& pt) {
      using namespace lobule;
      int x = to_idx(pt.x, xstep, xoff);
      int z = to_idx(pt.z, zstep, zoff);
      int y = to_idx(pt.y, ystep, x % 2 == 0 ? yoff0 : yoff1);
      return int3{x,y,z};
    }
    m3 from_idx(int3 const& ipt) {
      using namespace lobule;
      return element_multiply(dbl3{ipt}, m3{xstep,ystep,zstep}) +
        m3{xoff, ipt.x % 2 == 0 ? yoff0 : yoff1, zoff};
    }
    int3 adjust_index(int3 const& pt, int xoffset) {
      return int3{pt.x + xoffset,
                  pt.y + (xoffset % 2 != 0 && pt.x % 2 == 0 ? 1 : 0),
                  pt.z};
    }
  }

  using opt_pts = boost::optional<std::pair<m3, int3>>;
  template <typename Shape>
  opt_pts find_near_lobule(Shape const& shape, m3 const& pt) {
    auto ext = extents(shape);
    auto ipt = jhmi_detail::to_idx(pt - ext.ul());
    auto mpt = jhmi_detail::from_idx(ipt) + ext.ul();
    if (!shape(mpt))
      return boost::none;
    return std::make_pair(mpt, ipt);
  }
  template <typename Shape>
  opt_pts find_near_tract(Shape const& shape, m3 const& pt) {
    using namespace lobule;
    auto ext = extents(shape);
    auto lpt = find_near_lobule(shape, pt);
    if (!lpt)
      return boost::none;

    auto get_closer_pt = [&](int xoff0, int xoff1) -> opt_pts {
      using namespace jhmi_detail;
      auto idx0 = adjust_index(lpt->second, xoff0);
      auto idx1 = adjust_index(lpt->second, xoff1);

      auto pt0 = from_idx(idx0) - doff + ext.ul();
      auto pt1 = from_idx(idx1) - dloff + ext.ul();
      if (distance_squared(pt - pt0) < distance_squared(pt - pt1))
        return shape(pt0) ? std::make_pair(pt0, int3{idx0.x,2*idx0.y,idx0.z}) : opt_pts{boost::none};
      else
        return shape(pt1) ? std::make_pair(pt1, int3{idx1.x,2*idx1.y+1,idx1.z}) : opt_pts{boost::none};
    };
    if (pt.x < lpt->first.x && pt.y < lpt->first.y)
      return get_closer_pt(0,0);
    else if (pt.x >= lpt->first.x && pt.y < lpt->first.y)
      return get_closer_pt(0,2);
    else if (pt.x < lpt->first.x && pt.y >= lpt->first.y)
      return get_closer_pt(-1,1);
    else
      return get_closer_pt(1,1);
  }

  template <typename Shape, typename F>
  void for_portal_tract(Shape const& shape, F f) {
    for_lobule(shape, [&](m3 const& mpt, int3 spt) {
      auto mpt1 = mpt - lobule::doff;
      spt.y *= 2;
      if (shape(mpt1))
        f(mpt1, spt);

      auto mpt2 = mpt - lobule::dloff;
      spt.y += 1;
      if (shape(mpt2))
        f(mpt2, spt);
    });
  }
#if 1
  template <typename Shape>
  auto tracts_in(Shape const& shape) {
    namespace rv = ranges::view;
    using type = decltype(lobules_in(shape));
    return lobules_in(shape) | rv::for_each([](m3 const& pt) {
      return rv::concat(rv::single(pt - lobule::doff),
                        rv::single(pt - lobule::dloff));
    }) | rv::filter([&](m3 const& pt) { return shape(pt); });
  }
#endif

  auto lobule_mask(m step) {
    auto sides = m3{lobule::min_radius, lobule::side_length, lobule::cell_thickness/2.};
    //Center the mask at the origin
    auto mask_ext = cube<m3>{-sides, sides};

    auto lobule_dims = int3{floor(2.*(mask_ext.lr()+dbl3{1,1,0}*step) / step)};
    auto mask = volume_image<bool>{lobule_dims, mask_ext};
    auto mid_y = lobule::side_length / 2.;
    RANGES_FOR(auto pix, mask | view::equal_step(step)) {
      //Operate only in one octant
      auto p = abs(pix.physical_loc);
      if (p.z > lobule::cell_thickness/2.)
        pix.value = false;
      else if (p.x > lobule::min_radius)
        pix.value = false;
      else {
        p.y - mid_y;
        pix.value = (p.y - mid_y < mid_y - p.x * mid_y / lobule::min_radius);
      }
    }
    return mask;
  }
}//end jhmi

#endif


#pragma once

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <functional>
#ifndef _MSC_VER
extern "C"{
#include "type.h"
}
#else
#include "type.h"
#endif

DllExport void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const CTYPE* matrix, CTYPE* state, ITYPE dim);
DllExport void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const Eigen::MatrixXcd& eigen_matrix, CTYPE* state, ITYPE dim);
DllExport void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const Eigen::Matrix<std::complex<double>,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>& eigen_matrix, CTYPE* state, ITYPE dim);

DllExport void multi_qubit_sparse_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const Eigen::SparseMatrix<std::complex<double>>& eigen_matrix, CTYPE* state, ITYPE dim);

/**
 * \~english
 * Apply reversible boolean function as a unitary gate.
 *
 * Apply reversible boolean function as a unitary gate. Boolean function is given as a pointer of int -> int function.
 *
 * @param[in] target_qubit_index_list ターゲット量子ビットのリスト
 * @param[in] target_qubit_index_count ターゲット量子ビットの数
 * @param[in] matrix 添え字および対象ビットの次元を受け取ると添え字を返す関数
 * @param[in,out] state 量子状態
 * @param[in] dim 次元
 *
 *
 * \~japanese-en
 * 可逆回路関数をユニタリゲートとして作用する
 *
 *  可逆回路関数をユニタリゲートとして作用する。可逆回路関数は添え字を与えると結果の添え字を返す関数。
 *
 * @param[in] target_qubit_index_list ターゲット量子ビットのリスト
 * @param[in] target_qubit_index_count ターゲット量子ビットの数
 * @param[in] matrix 添え字および対象ビットの次元を受け取ると添え字を返す関数
 * @param[in,out] state 量子状態
 * @param[in] dim 次元
 *
 */
DllExport void reversible_boolean_gate(const UINT* target_qubit_index_list, UINT target_qubit_index_count, std::function<ITYPE(ITYPE,ITYPE)> function_ptr, CTYPE* state, ITYPE dim);


#include <chrono>
#include <iostream>

#include <Eigen/Core>

#include "ldaplusplus/e_step_utils.hpp"

using namespace Eigen;
using namespace ldaplusplus;

int main(int argc, char **argv) {
    VectorXd X = VectorXd::Random(1000);
    X.array() -= X.minCoeff();
    X.array() /= X.sum();
    VectorXi X_counts = (X*1000).cast<int>();

    MatrixXd beta = MatrixXd::Random(600, 1000);
    beta.array() -= beta.minCoeff();
    beta.array().rowwise() /= beta.array().colwise().sum();

    MatrixXd eta = MatrixXd::Random(600, 10);

    VectorXd gamma = VectorXd::Constant(600, 0.1 + static_cast<double>(X_counts.sum()) / 600.0);

    MatrixXd phi_old(600, 1000);
    MatrixXd phi = MatrixXd::Random(600, 1000);
    phi.array() -= phi.minCoeff();
    phi.array().rowwise() /= phi.array().colwise().sum();

    VectorXd h(phi.rows());

    // Warm up the cache
    for (int i=0; i<10; i++) {
        e_step_utils::compute_supervised_phi_gamma<double>(
            X_counts,
            X,
            0,
            beta,
            eta,
            1,
            phi,
            gamma,
            h
        );
    }

    std::chrono::high_resolution_clock clock;
    std::chrono::high_resolution_clock::duration s(0);
    for (int i=0; i<100; i++) {
        auto start = clock.now();
        e_step_utils::compute_supervised_phi_gamma<double>(
            X_counts,
            X,
            0,
            beta,
            eta,
            1,
            phi,
            gamma,
            h
        );
        s += clock.now() - start;
    }

    std::cout << std::chrono::duration_cast<std::chrono::duration<double, std::ratio<1> > >(s).count() << "s" << std::endl;

    return 0;
}


#include "POSQStateSpace.h"
#include <ompl/base/SpaceInformation.h>
#include <ompl/util/Exception.h>
#include <queue>
#include <boost/math/constants/constants.hpp>

void POSQStateSpace::initialize()
{
    state_space_ = new POSQ();
}

double POSQStateSpace::distance(const ompl::base::State *state1, const ompl::base::State *state2) const
{
    double distance = state_space_->distance(state1, state2);
//    OMPL_DEBUG("distance %f \n", distance);
    return distance;
}

void POSQStateSpace::interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t,
                                 ompl::base::State *state) const
{
    bool firstTime = true;
    vector<UnicycleState> states;
    states.clear();
    interpolate(from, to, t, firstTime, states, state);
}

void POSQStateSpace::interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t,
                                 bool &firstTime, vector<UnicycleState> &states, ompl::base::State *state) const
{
    if (firstTime)
    {
        if (t >= 1.)
        {
            if (to != state)
                copyState(state, to);
            return;
        }
        if (t <= 0.)
        {
            if (from != state)
                copyState(state, from);
            return;
        }

//	    OMPL_DEBUG("Generating states.. ");
      double distance = 0;
	    states = state_space_->steer(from, to, distance);
//		  OMPL_DEBUG("state number %d ", (int)states.size());

      firstTime = false;
    }

//    OMPL_DEBUG("Interpolating again.. ");
    interpolate(from, states, t, state);
}

void POSQStateSpace::interpolate(const ompl::base::State *from, const vector<UnicycleState> states, double t,
                                 ompl::base::State *state) const
{
    std::vector<UnicycleState> v(states);
	UnicycleState result = state_space_->interpolate(from, v, t);
    state->as<StateType>()->setX(result.x_);
    state->as<StateType>()->setY(result.y_);
    state->as<StateType>()->setYaw(result.yaw_);
//    OMPL_DEBUG("Time: %f, x: %f  y:%f  z: %f ", t, result.x_, result.y_, result.yaw_);
}

void POSQMotionValidator::defaultSettings()
{
    stateSpace_ = dynamic_cast<POSQStateSpace *>(si_->getStateSpace().get());
    if (stateSpace_ == nullptr)
        throw ompl::Exception("No state space for motion validator");
}

bool POSQMotionValidator::checkMotion(const ompl::base::State *s1, const ompl::base::State *s2,
                                          std::pair<ompl::base::State *, double> &lastValid) const
{
    /* assume motion starts in a valid configuration so s1 is valid */

    bool result = true, firstTime = true;
    vector<UnicycleState> states;
    int nd = stateSpace_->validSegmentCount(s1, s2);

    if (nd > 1)
    {
         /* temporary storage for the checked state */
        ompl::base::State *test = si_->allocState();

        for (int j = 1; j < nd; ++j)
        {
            stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, states, test);
            if (!si_->isValid(test))
            {
                lastValid.second = (double)(j - 1) / (double)nd;
                if (lastValid.first != nullptr)
                    stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, states, lastValid.first);
                result = false;
                break;
            }
        }
        si_->freeState(test);
    }

    if (result)
        if (!si_->isValid(s2))
        {
            lastValid.second = (double)(nd - 1) / (double)nd;
            if (lastValid.first != nullptr)
                stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, states, lastValid.first);
            result = false;
        }

    if (result)
        valid_++;
    else
        invalid_++;

    return result;
}

bool POSQMotionValidator::checkMotion(const ompl::base::State *s1, const ompl::base::State *s2) const
{
    /* assume motion starts in a valid configuration so s1 is valid */
    if (!si_->isValid(s2))
        return false;

    bool result = true, firstTime = true;

    std::vector<UnicycleState> states;
    states.clear();

    int nd = stateSpace_->validSegmentCount(s1, s2);

    // /* initialize the queue of test positions */
    std::queue<std::pair<int, int>> pos;
    if (nd >= 2)
    {
        pos.push(std::make_pair(1, nd - 1));

         /* temporary storage for the checked state */
        ompl::base::State *test = si_->allocState();

        /* repeatedly subdivide the path segment in the middle (and check the middle) */
        while (!pos.empty())
        {
            std::pair<int, int> x = pos.front();

            int mid = (x.first + x.second) / 2;
            stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, states, test);

            if (!si_->isValid(test))
            {
                result = false;
                break;
            }

            pos.pop();

            if (x.first < mid)
                pos.push(std::make_pair(x.first, mid - 1));
            if (x.second > mid)
                pos.push(std::make_pair(mid + 1, x.second));
        }

        si_->freeState(test);
    }

    if (result)
        valid_++;
    else
        invalid_++;

    return result;
}


///////////////////////////////////////////////////////////////////////////////
// ars::function::adaptor.hpp                                                //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_DETAIL_ARS_FUNCTION_ADAPTOR_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_FUNCTION_ADAPTOR_HPP_ER_2009
#include <boost/type_traits.hpp>
#include <boost/call_traits.hpp>

//  Make sure to precede this file by
// #include <boost/statistics/detail/distribution_toolkit/distributions/D.hpp>
// #include <boost/statistics/detail/distribution_toolkit/fwd_math/cdf.hpp>
// if D is defined in boost::math

namespace boost{
namespace statistics{
namespace detail{
namespace ars{
namespace function{

// Adapts a distribution in sandbox/distribution_toolkit/distributions to the
// required signature.
template<typename D>
class adaptor{

    typedef typename remove_const<
        typename remove_reference<
            D
        >::type
    >::type dist_t;

    public:
    typedef typename dist_t::value_type      value_type;
    typedef typename dist_t::policy_type     policy_type;
    typedef adaptor<dist_t>    base_type;

    adaptor(typename call_traits<D>::param_type d):d_(d){}
    adaptor(const adaptor& that):d_(that.d_){}

    adaptor&
    operator=(const adaptor& that){
        if(&that!=this){
            d_ = that.d_;
        }
        return *this;
    }

    void operator()(
        const value_type& x,
        value_type& log_pdf,
        value_type& dlog_pdf
    )const{
        log_pdf = log_unnormalized_pdf(
            this->distribution(),
            x
        );

        dlog_pdf = derivative_log_unnormalized_pdf(
            this->distribution(),
            x
        );
    }

    typename call_traits<D>::const_reference distribution()const{
        return d_;
    }

    private:
        typename call_traits<D>::value_type d_;

};

}
}// ars
}// detail
}// statistics
}// boost

#endif


#define BOOST_TEST_MODULE pcraster fieldapi readonly_non_spatial
#include <boost/test/unit_test.hpp>
#include "csftypes.h"
#include "fieldapi_readonlynonspatial.h"

template<typename T>
void testType(T initVal)
{
  using namespace fieldapi;

  ReadOnlyNonSpatial<T> t(initVal,2,3);
  T v;
  BOOST_CHECK(!t.spatial());
  BOOST_CHECK(t.nrRows()==2);
  BOOST_CHECK(t.nrCols()==3);
  BOOST_CHECK(t.nrCols()==3);
  BOOST_CHECK(t.get(v,0,0));
  BOOST_CHECK(v==initVal);
  BOOST_CHECK(!t.get(v,2,3));
  BOOST_CHECK(!t.get(v,-1,0));
  BOOST_CHECK(t.value(0,0)==initVal);
}

BOOST_AUTO_TEST_CASE(all)
{
 testType<UINT1>(34);
 testType<INT4>(-4);
 testType<INT4>(1058);
 testType<REAL8>(0);
 testType<REAL8>(3.45);
}


/** @file config_webots.hpp
 *  @brief The configuration file for IMACS framework using WEBOTS simulator
 */
#ifndef CONFIG_WEBOTS_H_
#define CONFIG_WEBOTS_H_

#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <iostream>

#include "paths.hpp"

#include "webots_api.hpp"
#include "Halide.h"
#include "lateral_Control_WEBOTS.hpp"
#include "lane_detection_webots.hpp"
#include "image_signal_processing.hpp"
using namespace std;

// ------------ defs -------------//
#define TOP_SPEED 60 		//!< maximum speed
#define VEH_SPEED 50 		//!< cruising speed
#define DRAW_SLIDING_WINDOWS 1  //!< select whether to draw the sliding window while tracking
#define WEBOTS_CAM 1            //!< select when VREP camera frames are used
#define RE_DRAW_IMAGE 1        	//!< run re-draw image function
#define DEBUG 1		//!< print debug statements to check intermediary outputs. Results in slow simulation sometimes
#define WEBOTS_YPIXEL_PER_METRE 112.72 //!< The number of ypixels in the image per metre
// ------------ If the settling value is not zero, WEBOTS_REF_TUNE needs to be modified ---------//
//#define WEBOTS_REF_TUNE 3.355 // for 50kmph (city_straight_night_nostreet.wbt)
//#define WEBOTS_REF_TUNE 3.255 // for 50kmph (city_straight_night_nostreet.wbt) // VER:3
//#define WEBOTS_REF_TUNE 3.615 // for 50kmph (city_straight_night_nostreet.wbt) // VER:0
#define WEBOTS_REF_TUNE_NIGHT 3.555 //!< for 50kmph (city_straight_night.wbt, city_straight_dawn.wbt)
#define WEBOTS_REF_TUNE_STRAIGHT 3.615 //!< for 50kmph (city_straight.wbt, city_straight_dusk.wbt, city_straight_foggy.wbt)
//#define WEBOTS_REF_TUNE 15.615 // for 50kmph (city_straight_snow.wbt)
//#define WEBOTS_REF_TUNE 30.815 // for 50kmph (city_straight_lqg.wbt)
// ------------ for pipeline VER:8 ---------//
//#define WEBOTS_REF_TUNE 5.415 // for 50kmph (city_straight_night_nostreet.wbt)
//#define WEBOTS_REF_TUNE 3.005 // for 50 kmph (city_straight_night.wbt, city_straight_dawn.wbt, city_straight_dusk.wbt)
//#define WEBOTS_REF_TUNE 2.985 // for 50 kmph (city_straight.wbt)
/// To differentiate between the straight road and left road (e.g.)
#define WEBOTS_LEFT_THRESOLD 100 //!< the left lane x-axis threshold in the image
#define WEBOTS_RIGHT_THRESOLD 400 //!< the right lane x-axis threshold in the image

#define FINE_TUNE 0.010652587064676618 //!< Fine tuning parameter to compute actual_yL
#define ROAD_WIDTH 3.25 //!< The road width of the webots scene

#endif


#pragma once
#include <ros/ros.h>
#include "kalman-cpp/kalman.hpp"
#include "l_shape_tracker.hpp"
#include <Eigen/Dense>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include "datmo/Track.h"
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <chrono>

using namespace std;
using namespace Eigen;

typedef std::pair<double, double> Point;
typedef std::vector<Point> pointList;
const double pi = 3.141592653589793238463;

class Cluster {
public:

  Cluster(unsigned long int id, const pointList&, const double&, const string&, const tf::Transform& ego_pose);

  string frame_name;
  Point ego_coordinates;

  datmo::Track msg_track_mean;
  datmo::Track msg_track_mean_kf;
  datmo::Track msg_track_box;

  unsigned long int id; //identifier for the cluster
  unsigned long int age; //age of the cluster
  float r, g, b, a; //color of the cluster

  visualization_msgs::Marker getBoundingBoxCenterVisualisationMessage();
  visualization_msgs::Marker getClosestCornerPointVisualisationMessage();
  visualization_msgs::Marker getClusterVisualisationMessage();
  visualization_msgs::Marker getLineVisualisationMessage();
  visualization_msgs::Marker getArrowVisualisationMessage();
  visualization_msgs::Marker getThetaL2VisualisationMessage();
  visualization_msgs::Marker getThetaL1VisualisationMessage();
  visualization_msgs::Marker getThetaBoxVisualisationMessage();
  visualization_msgs::Marker getBoundingBoxVisualisationMessage();
  visualization_msgs::Marker getBoxModelVisualisationMessage();
  visualization_msgs::Marker getLShapeVisualisationMessage();
  visualization_msgs::Marker getPoseCovariance();

  pair<int, int> getRectangleFittingExecutionTime(){return dur_size_rectangle_fitting;};
  pair<int, int> dur_size_rectangle_fitting;

  void update(const pointList&, const double dt, const tf::Transform& ego_pose);
  void populateTrackingMsgs();
  void detectCornerPointSwitch();
  void detectCornerPointSwitch(double& from, double& to);
  bool red_flag, green_flag, blue_flag;

  std::pair<double, double> mean() { return mean_values; }; //Return mean of cluster.

  double meanX() { return mean_values.first; };
  double meanY() { return mean_values.second;};

  LShapeTracker l_shape;
  KalmanFilter kf_mean;
  double old_thetaL1, old_thetaL2;
  double L1, L2, thetaL1, thetaL2;
  double cx, cy, L1_box, L2_box, th;

private:

  pointList new_cluster;
  vector<Point> corner_list;

  vector<Point> l1l2; //save coordinates of the three points that define the lines

  // mean value of the cluster
  std::pair<double, double> mean_values;
  std::pair<double, double> previous_mean_values;

  Point closest_corner_point;

  visualization_msgs::Marker boxcenter_marker_;
  void calcMean(const pointList& ); //Find the mean value of the cluster
  void rectangleFitting(const pointList& ); //Search-Based Rectangle Fitting
  double areaCriterion(const VectorXd&, const VectorXd& );
  double closenessCriterion(const VectorXd& ,const VectorXd&, const float& );
  Point lineIntersection(double& , double& , double& , double& , double& , double& );
  double perpendicularDistance(const Point&, const Point&, const Point&);
  void ramerDouglasPeucker(const vector<Point>&, double, vector<Point>&);
};


/*
 * This file belongs to the Galois project, a C++ library for exploiting parallelism.
 * The code is being released under the terms of the 3-Clause BSD License (a
 * copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

#include "galois/Galois.h"
#include "galois/Timer.h"
#include "galois/Bag.h"
#include "galois/Reduction.h"
#include "Lonestar/BoilerPlate.h"
#include "galois/runtime/Profile.h"

#include <boost/math/constants/constants.hpp>
#include <boost/iterator/transform_iterator.hpp>

#include <array>
#include <limits>
#include <iostream>
#include <fstream>
#include <random>
#include <deque>

#include <strings.h>

#include "Point.h"

const char* name = "Barnes-Hut N-Body Simulator";
const char* desc =
    "Simulates gravitational forces in a galactic cluster using the "
    "Barnes-Hut n-body algorithm";
const char* url = "barneshut";

static llvm::cl::opt<int>
    nbodies("n", llvm::cl::desc("Number of bodies (default value 10000)"),
            llvm::cl::init(10000));
static llvm::cl::opt<int>
    ntimesteps("steps", llvm::cl::desc("Number of steps (default value 1)"),
               llvm::cl::init(1));
static llvm::cl::opt<int> seed("seed",
                               llvm::cl::desc("Random seed (default value 7)"),
                               llvm::cl::init(7));

struct Node {
  Point pos;
  double mass;
  bool Leaf;
};

struct Body : public Node {
  Point vel;
  Point acc;
};

/**
 * A node in an octree is either an internal node or a leaf.
 */
struct Octree : public Node {
  std::array<galois::substrate::PtrLock<Node>, 8> child;
  char cLeafs;
  char nChildren;

  Octree(const Point& p) {
    Node::pos  = p;
    Node::Leaf = false;
    cLeafs     = 0;
    nChildren  = 0;
  }
};

std::ostream& operator<<(std::ostream& os, const Body& b) {
  os << "(pos:" << b.pos << " vel:" << b.vel << " acc:" << b.acc
     << " mass:" << b.mass << ")";
  return os;
}

struct BoundingBox {
  Point min;
  Point max;
  explicit BoundingBox(const Point& p) : min(p), max(p) {}
  BoundingBox()
      : min(std::numeric_limits<double>::max()),
        max(std::numeric_limits<double>::min()) {}

  void merge(const BoundingBox& other) {
    min.pairMin(other.min);
    max.pairMax(other.max);
  }

  void merge(const Point& other) {
    min.pairMin(other);
    max.pairMax(other);
  }

  double diameter() const { return (max - min).minDim(); }
  double radius() const { return diameter() * 0.5; }
  Point center() const { return (min + max) * 0.5; }
};

std::ostream& operator<<(std::ostream& os, const BoundingBox& b) {
  os << "(min:" << b.min << " max:" << b.max << ")";
  return os;
}

struct Config {
  const double dtime; // length of one time step
  const double eps;   // potential softening parameter
  const double tol;   // tolerance for stopping recursion, <0.57 to bound error
  const double dthf, epssq, itolsq;
  Config()
      : dtime(0.5), eps(0.05), tol(0.05), // 0.025),
        dthf(dtime * 0.5), epssq(eps * eps), itolsq(1.0 / (tol * tol)) {}
};

std::ostream& operator<<(std::ostream& os, const Config& c) {
  os << "Barnes-Hut configuration:"
     << " dtime: " << c.dtime << " eps: " << c.eps << " tol: " << c.tol;
  return os;
}

Config config;

inline int getIndex(const Point& a, const Point& b) {
  int index = 0;
  for (int i = 0; i < 3; ++i)
    if (a[i] < b[i])
      index += (1 << i);
  return index;
}

inline Point updateCenter(Point v, int index, double radius) {
  for (int i = 0; i < 3; i++)
    v[i] += (index & (1 << i)) > 0 ? radius : -radius;
  return v;
}

typedef galois::InsertBag<Body> Bodies;
typedef galois::InsertBag<Body*> BodyPtrs;
// FIXME: reclaim memory for multiple steps
typedef galois::InsertBag<Octree> Tree;

struct BuildOctree {

  Tree& T;

  void insert(Body* b, Octree* node, double radius) const {
    int index   = getIndex(node->pos, b->pos);
    Node* child = node->child[index].getValue();

    // go through the tree lock-free while we can
    if (child && !child->Leaf) {
      insert(b, static_cast<Octree*>(child), radius);
      return;
    }

    node->child[index].lock();
    child = node->child[index].getValue();

    if (child == NULL) {
      node->child[index].unlock_and_set(b);
      return;
    }

    radius *= 0.5;
    if (child->Leaf) {
      // Expand leaf

      Octree* new_node = &T.emplace(updateCenter(node->pos, index, radius));
      if (b->pos == child->pos) {
        // Jitter point to gaurantee uniqueness.
        double jitter = config.tol / 2;
        assert(jitter < radius);
        b->pos += (new_node->pos - b->pos) * jitter;
      }

      // assert(node->pos != b->pos);
      // node->child[index].unlock_and_set(new_node);
      insert(b, new_node, radius);
      insert(static_cast<Body*>(child), new_node, radius);
      node->child[index].unlock_and_set(new_node);
    } else {
      node->child[index].unlock();
      insert(b, static_cast<Octree*>(child), radius);
    }
  }
};

unsigned computeCenterOfMass(Octree* node) {
  double mass = 0.0;
  Point accum;
  unsigned num = 1;

  // Reorganize leaves to be dense
  // remove copies values
  int index = 0;
  for (int i = 0; i < 8; ++i)
    if (node->child[i].getValue())
      node->child[index++].setValue(node->child[i].getValue());
  for (int i = index; i < 8; ++i)
    node->child[i].setValue(NULL);
  node->nChildren = index;

  for (int i = 0; i < index; i++) {
    Node* child = node->child[i].getValue();
    if (!child->Leaf) {
      num += computeCenterOfMass(static_cast<Octree*>(child));
    } else {
      node->cLeafs |= (1 << i);
      ++num;
    }
    mass += child->mass;
    accum += child->pos * child->mass;
  }

  node->mass = mass;

  if (mass > 0.0)
    node->pos = accum / mass;
  return num;
}

/*
void printRec(std::ofstream& file, Node* node, unsigned level) {
  static const char* ct[] = {
    "blue", "cyan", "aquamarine", "chartreuse",
    "darkorchid", "darkorange",
    "deeppink", "gold", "chocolate"
  };
  if (!node) return;
  file << "\"" << node << "\" [color=" << ct[node->owner / 4] << (node->owner %
4 + 1) << (level ? "" : " style=filled") << " label = \"" << (node->Leaf ? "L" :
"N") << "\"];\n"; if (!node->Leaf) { Octree* node2 = static_cast<Octree*>(node);
    for (int i = 0; i < 8 && node2->child[i]; ++i) {
      if (level == 3 || level == 6)
        file << "subgraph cluster_" << level << "_" << i << " {\n";
      file << "\"" << node << "\" -> \"" << node2->child[i] << "\"
[weight=0.01]\n"; printRec(file, node2->child[i], level + 1); if (level == 3 ||
level == 6) file << "}\n";
    }
  }
}

void printTree(Octree* node) {
  std::ofstream file("out.txt");
  file << "digraph octree {\n";
  file << "ranksep = 2\n";
  file << "root = \"" << node << "\"\n";
  //  file << "overlap = scale\n";
  printRec(file, node, 0);
  file << "}\n";
}
*/

Point updateForce(Point delta, double psq, double mass) {
  // Computing force += delta * mass * (|delta|^2 + eps^2)^{-3/2}
  double idr   = 1 / sqrt((float)(psq + config.epssq));
  double scale = mass * idr * idr * idr;
  return delta * scale;
}

struct ComputeForces {
  // Optimize runtime for no conflict case

  Octree* top;
  double root_dsq;

  ComputeForces(Octree* _top, double diameter) : top(_top) {
    assert(diameter > 0.0 && "non positive diameter of bb");
    root_dsq = diameter * diameter * config.itolsq;
  }

  template <typename Context>
  void computeForce(Body* b, Context& cnx) {
    Point p = b->acc;
    b->acc  = Point(0.0, 0.0, 0.0);
    iterate(*b, cnx);
    b->vel += (b->acc - p) * config.dthf;
  }

  struct Frame {
    double dsq;
    Octree* node;
    Frame(Octree* _node, double _dsq) : dsq(_dsq), node(_node) {}
  };

  template <typename Context>
  void iterate(Body& b, Context& cnx) {
    std::deque<Frame, galois::PerIterAllocTy::rebind<Frame>::other> stack(
        cnx.getPerIterAlloc());
    stack.push_back(Frame(top, root_dsq));

    while (!stack.empty()) {
      const Frame f = stack.back();
      stack.pop_back();

      Point p    = b.pos - f.node->pos;
      double psq = p.dist2();

      // Node is far enough away, summarize contribution
      if (psq >= f.dsq) {
        b.acc += updateForce(p, psq, f.node->mass);
        continue;
      }

      double dsq = f.dsq * 0.25;
      for (int i = 0; i < f.node->nChildren; i++) {
        Node* n = f.node->child[i].getValue();
        assert(n);
        if (f.node->cLeafs & (1 << i)) {
          assert(n->Leaf);
          if (static_cast<const Node*>(&b) != n) {
            Point p = b.pos - n->pos;
            b.acc += updateForce(p, p.dist2(), n->mass);
          }
        } else {
#ifndef GALOIS_CXX11_DEQUE_HAS_NO_EMPLACE
          stack.emplace_back(static_cast<Octree*>(n), dsq);
#else
          stack.push_back(Frame(static_cast<Octree*>(n), dsq));
#endif
          __builtin_prefetch(n);
        }
      }
    }
  }
};

struct centerXCmp {
  template <typename T>
  bool operator()(const T& lhs, const T& rhs) const {
    return lhs.pos[0] < rhs.pos[0];
  }
};

struct centerYCmp {
  template <typename T>
  bool operator()(const T& lhs, const T& rhs) const {
    return lhs.pos[1] < rhs.pos[1];
  }
};

struct centerYCmpInv {
  template <typename T>
  bool operator()(const T& lhs, const T& rhs) const {
    return rhs.pos[1] < lhs.pos[1];
  }
};

template <typename Iter, typename Gen>
void divide(const Iter& b, const Iter& e, Gen& gen) {
  if (std::distance(b, e) > 32) {
    std::sort(b, e, centerXCmp());
    Iter m = galois::split_range(b, e);
    std::sort(b, m, centerYCmpInv());
    std::sort(m, e, centerYCmp());
    divide(b, galois::split_range(b, m), gen);
    divide(galois::split_range(b, m), m, gen);
    divide(m, galois::split_range(m, e), gen);
    divide(galois::split_range(m, e), e, gen);
  } else {
    std::shuffle(b, e, gen);
  }
}

/**
 * Generates random input according to the Plummer model, which is more
 * realistic but perhaps not so much so according to astrophysicists
 */
void generateInput(Bodies& bodies, BodyPtrs& pBodies, int nbodies, int seed) {
  double v, sq, scale;
  Point p;
  double PI = boost::math::constants::pi<double>();

  std::mt19937 gen(seed);
#if __cplusplus >= 201103L || defined(HAVE_CXX11_UNIFORM_INT_DISTRIBUTION)
  std::uniform_real_distribution<double> dist(0, 1);
#else
  std::uniform_real<double> dist(0, 1);
#endif

  double rsc = (3 * PI) / 16;
  double vsc = sqrt(1.0 / rsc);

  std::vector<Body> tmp;

  for (int body = 0; body < nbodies; body++) {
    double r = 1.0 / sqrt(pow(dist(gen) * 0.999, -2.0 / 3.0) - 1);
    do {
      for (int i = 0; i < 3; i++)
        p[i] = dist(gen) * 2.0 - 1.0;
      sq = p.dist2();
    } while (sq > 1.0);
    scale = rsc * r / sqrt(sq);

    Body b;
    b.mass = 1.0 / nbodies;
    b.pos  = p * scale;
    do {
      p[0] = dist(gen);
      p[1] = dist(gen) * 0.1;
    } while (p[1] > p[0] * p[0] * pow(1 - p[0] * p[0], 3.5));
    v = p[0] * sqrt(2.0 / sqrt(1 + r * r));
    do {
      for (int i = 0; i < 3; i++)
        p[i] = dist(gen) * 2.0 - 1.0;
      sq = p.dist2();
    } while (sq > 1.0);
    scale  = vsc * v / sqrt(sq);
    b.vel  = p * scale;
    b.Leaf = true;
    tmp.push_back(b);
    // pBodies.push_back(&bodies.push_back(b));
  }

  // sort and copy out
  divide(tmp.begin(), tmp.end(), gen);

  galois::do_all(galois::iterate(tmp),
                 [&pBodies, &bodies](const Body& b) {
                   pBodies.push_back(&(bodies.push_back(b)));
                 },
                 galois::loopname("InsertBody"));
}

struct CheckAllPairs {
  Bodies& bodies;

  CheckAllPairs(Bodies& b) : bodies(b) {}

  double operator()(const Body& body) const {
    const Body* me = &body;
    Point acc;
    for (Bodies::iterator ii = bodies.begin(), ei = bodies.end(); ii != ei;
         ++ii) {
      Body* b = &*ii;
      if (me == b)
        continue;
      Point delta = me->pos - b->pos;
      double psq  = delta.dist2();
      acc += updateForce(delta, psq, b->mass);
    }

    double dist2 = acc.dist2();
    acc -= me->acc;
    double retval = acc.dist2() / dist2;
    return retval;
  }
};

double checkAllPairs(Bodies& bodies, int N) {
  Bodies::iterator end(bodies.begin());
  std::advance(end, N);

  return galois::ParallelSTL::map_reduce(bodies.begin(), end,
                                         CheckAllPairs(bodies),
                                         std::plus<double>(), 0.0) /
         N;
}

void run(Bodies& bodies, BodyPtrs& pBodies, size_t nbodies) {
  typedef galois::worklists::StableIterator<true> WLL;

  galois::preAlloc(galois::getActiveThreads() +
                   (3 * sizeof(Octree) + 2 * sizeof(Body)) * nbodies /
                       galois::runtime::pagePoolSize());
  galois::reportPageAlloc("MeminfoPre");

  for (int step = 0; step < ntimesteps; step++) {

    auto MB = [](BoundingBox& lhs, const Point& rhs) { lhs.merge(rhs); };

    // Do tree building sequentially
    galois::GBigReducible<decltype(MB), BoundingBox> boxes(MB);

    galois::do_all(galois::iterate(pBodies),
                   [&boxes](const Body* b) { boxes.update(b->pos); },
                   galois::loopname("reduceBoxes"));

    BoundingBox box = boxes.reduce(
        [](BoundingBox& lhs, BoundingBox& rhs) { lhs.merge(rhs); });

    Tree t;
    BuildOctree treeBuilder{t};
    Octree& top = t.emplace(box.center());

    galois::StatTimer T_build("BuildTime");
    T_build.start();
    galois::do_all(
        galois::iterate(pBodies),
        [&](Body* body) { treeBuilder.insert(body, &top, box.radius()); },
        galois::loopname("BuildTree"));
    T_build.stop();

    // update centers of mass in tree
    galois::timeThis(
        [&](void) {
          unsigned size = computeCenterOfMass(&top);
          // printTree(&top);
          std::cout << "Tree Size: " << size << "\n";
        },
        "summarize-Serial");

    ComputeForces cf(&top, box.diameter());

    galois::StatTimer T_compute("ComputeTime");
    T_compute.start();
    galois::for_each(galois::iterate(pBodies),
                     [&](Body* b, auto& cnx) { cf.computeForce(b, cnx); },
                     galois::loopname("compute"), galois::wl<WLL>(),
                     galois::no_conflicts(), galois::no_pushes(),
                     galois::per_iter_alloc());
    T_compute.stop();

    if (!skipVerify) {
      galois::timeThis(
          [&](void) {
            std::cout << "MSE (sampled) "
                      << checkAllPairs(bodies, std::min((int)nbodies, 100))
                      << "\n";
          },
          "checkAllPairs");
    }
    // Done in compute forces
    galois::do_all(galois::iterate(pBodies),
                   [](Body* b) {
                     Point dvel(b->acc);
                     dvel *= config.dthf;
                     Point velh(b->vel);
                     velh += dvel;
                     b->pos += velh * config.dtime;
                     b->vel = velh + dvel;
                   },
                   galois::loopname("advance"));

    std::cout << "Timestep " << step << " Center of Mass = ";
    std::ios::fmtflags flags =
        std::cout.setf(std::ios::showpos | std::ios::right |
                       std::ios::scientific | std::ios::showpoint);
    std::cout << top.pos;
    std::cout.flags(flags);
    std::cout << "\n";
  }

  galois::reportPageAlloc("MeminfoPost");
}

int main(int argc, char** argv) {
  galois::SharedMemSys G;
  LonestarStart(argc, argv, name, desc, url);

  std::cout << config << "\n";
  std::cout << nbodies << " bodies, " << ntimesteps << " time steps\n";

  Bodies bodies;
  BodyPtrs pBodies;
  generateInput(bodies, pBodies, nbodies, seed);

  galois::StatTimer T;
  T.start();
  run(bodies, pBodies, nbodies);
  T.stop();
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/rem_pio2.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/pio_2.hpp>

STF_CASE_TPL (" rem_pio2",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::rem_pio2;
  using r_t = decltype(rem_pio2(T()));

  {
    r_t res = rem_pio2(bs::Zero<T>());
    STF_ULP_EQUAL( res.first, bs::Zero<T>(), 1);
    STF_ULP_EQUAL( res.second, bs::Zero<T>(), 1);
  }
} // end of test for floating_


#include <boost/test/unit_test.hpp>
#include <iostream>

#include "objects/core/char.hpp"

#include "objects/invokable/generic_function.hpp"

#include "objects/numerics/integer.hpp"
#include "objects/numerics/float.hpp"

#include "globals/globals.hpp"

using obj::Object;
using namespace sct;

BOOST_AUTO_TEST_SUITE( add_suite )

BOOST_AUTO_TEST_CASE ( mod_cmp_genfn_specs ) {
  using obj::GenericFunction;

  GenericFunction* cmp = dynamic_cast<GenericFunction*>(ord::cmp);

  BOOST_TEST(cmp != nullptr);

  BOOST_TEST(cmp->functions.size() == 4);
}

BOOST_AUTO_TEST_CASE( ord_cmp_chr ) {
  using obj::Char;
  Char c1('a');
  Char c2('b');

  Object* res = std::get<Object*>((*ord::cmp)({&c1, &c2}, *context::empty));

  BOOST_TEST(res == ord::le);

  c1 = Char('a');
  c2 = Char('a');

  res = std::get<Object*>((*ord::cmp)({&c1, &c2}, *context::empty));

  BOOST_TEST(res == ord::eq);

  c1 = Char('b');
  c2 = Char('a');

  res = std::get<Object*>((*ord::cmp)({&c1, &c2}, *context::empty));

  BOOST_TEST(res == ord::gr);
}

BOOST_AUTO_TEST_CASE( ord_cmp_int ) {
  using obj::Integer;
  Integer i1(-1);
  Integer i2(2);

  Object* res = std::get<Object*>((*ord::cmp)({&i1, &i2}, *context::empty));

  BOOST_TEST(res == ord::le);

  i1 = Integer(10);
  i2 = Integer(10);

  res = std::get<Object*>((*ord::cmp)({&i1, &i2}, *context::empty));

  BOOST_TEST(res == ord::eq);

  i1 = Integer(2);
  i2 = Integer(-1);

  res = std::get<Object*>((*ord::cmp)({&i1, &i2}, *context::empty));

  BOOST_TEST(res == ord::gr);
}

BOOST_AUTO_TEST_CASE( ord_cmp_float ) {
  using obj::Float;
  Float f1(-1.2);
  Float f2(3.44);

  Object* res = std::get<Object*>((*ord::cmp)({&f1, &f2}, *context::empty));

  BOOST_TEST(res == ord::le);

  f1 = Float(1.0);
  f2 = Float(1.0);

  res = std::get<Object*>((*ord::cmp)({&f1, &f2}, *context::empty));

  BOOST_TEST(res == ord::eq);

  f1 = Float(34.2);
  f2 = Float(-10.0);

  res = std::get<Object*>((*ord::cmp)({&f1, &f2}, *context::empty));

  BOOST_TEST(res == ord::gr);
}

BOOST_AUTO_TEST_SUITE_END()


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"
#include "graph.hh"
#include "graph_properties.hh"

#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct get_kruskal_min_span_tree
{
    template <class TreeMap>
    class tree_inserter
    {
    public:
        tree_inserter(TreeMap tree_map): _tree_map(tree_map) {}

        tree_inserter& operator++() { return *this; }
        tree_inserter& operator++(int) { return *this; }
        tree_inserter& operator*() { return *this; }

        tree_inserter& operator=
        (const typename property_traits<TreeMap>::key_type& e)
        {
            _tree_map[e] = 1;
            return *this;
        }

    private:
        TreeMap _tree_map;
    };

    template <class Graph, class IndexMap, class WeightMap, class TreeMap>
    void operator()(const Graph& g, IndexMap vertex_index, WeightMap weights,
                    TreeMap tree_map) const
    {
        kruskal_minimum_spanning_tree(g, tree_inserter<TreeMap>(tree_map),
                                      weight_map(weights).
                                      vertex_index_map(vertex_index));
    }
};

struct get_prim_min_span_tree
{
    template <class Graph, class IndexMap, class WeightMap, class TreeMap>
    void operator()(const Graph& g, size_t root, IndexMap vertex_index,
                    WeightMap weights, TreeMap tree_map) const
    {
        typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
        typedef typename graph_traits<Graph>::edge_descriptor edge_t;

        unchecked_vector_property_map<vertex_t,IndexMap>
            pred_map(vertex_index, num_vertices(g));
        prim_minimum_spanning_tree(g, pred_map,
                                   root_vertex(vertex(root, g)).
                                   weight_map(weights).
                                   vertex_index_map(vertex_index));

        // convert the predecessor map to a tree map, and avoid trouble with
        // parallel edges
        parallel_vertex_loop
                (g,
                 [&](auto v)
                 {
                     vector<edge_t> edges;
                     vector<typename property_traits<WeightMap>::value_type> ws;
                     for (auto e : out_edges_range(v, g))
                     {
                         if (target(e,g) == pred_map[v])
                         {
                             edges.push_back(e);
                             ws.push_back(weights[e]);
                         }
                     }
                     if (!edges.empty())
                     {
                         edge_t e = *(edges.begin() +
                                      size_t(min_element(ws.begin(),
                                                         ws.end()) - ws.begin()));
                         tree_map[e] = 1;
                     }
                 });
    }
};

typedef property_map_types::apply<mpl::vector<uint8_t>,
                                  GraphInterface::edge_index_map_t,
                                  mpl::bool_<false> >::type
    tree_properties;

void get_kruskal_spanning_tree(GraphInterface& gi, boost::any weight_map,
                               boost::any tree_map)
{

    typedef UnityPropertyMap<size_t,GraphInterface::edge_t> cweight_t;

    if (weight_map.empty())
        weight_map = cweight_t();

    typedef mpl::push_back<writable_edge_scalar_properties, cweight_t>::type
        weight_maps;

    run_action<graph_tool::detail::never_directed>()
        (gi, std::bind(get_kruskal_min_span_tree(), std::placeholders::_1, gi.get_vertex_index(),
                       std::placeholders::_2, std::placeholders::_3),
         weight_maps(), writable_edge_scalar_properties())(weight_map, tree_map);
}

void get_prim_spanning_tree(GraphInterface& gi, size_t root,
                            boost::any weight_map, boost::any tree_map)
{
    typedef UnityPropertyMap<size_t,GraphInterface::edge_t> cweight_t;

    if (weight_map.empty())
        weight_map = cweight_t();

    typedef mpl::push_back<writable_edge_scalar_properties, cweight_t>::type
        weight_maps;

    run_action<graph_tool::detail::never_directed>()
        (gi, std::bind(get_prim_min_span_tree(), std::placeholders::_1, root,
                       gi.get_vertex_index(), std::placeholders::_2, std::placeholders::_3),
         weight_maps(), tree_properties())(weight_map, tree_map);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/ieee/include/functions/frexp.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <boost/fusion/include/vector_tie.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL( frexp, NT2_SIMD_REAL_TYPES)
{
  using nt2::frexp;
  using nt2::tag::frexp_;
  using boost::simd::native;
  typedef native<T,NT2_SIMD_DEFAULT_EXTENSION>            vT;
  typedef typename nt2::meta::as_integer<vT,signed>::type viT;

  NT2_TEST_TYPE_IS( (typename nt2::meta::call<frexp_(vT)>::type)
                  , (std::pair<vT,viT>)
                  );

  {
    viT e;
    vT  m;

    frexp(nt2::One<vT>(), m, e);
    NT2_TEST_EQUAL(m, nt2::Half<vT>());
    NT2_TEST_EQUAL(e, nt2::One<viT>());
  }

  {
    viT e;
    vT  m;

    m = frexp(nt2::One<vT>(), e);
    NT2_TEST_EQUAL(m, nt2::Half<vT>());
    NT2_TEST_EQUAL(e, nt2::One<viT>());
  }

  {
    viT e;
    vT  m;

    boost::fusion::vector_tie(m,e) = frexp(nt2::One<vT>());
    NT2_TEST_EQUAL(m, nt2::Half<vT>());
    NT2_TEST_EQUAL(e, nt2::One<viT>());
  }

  {
    std::pair<vT,viT> p;

    p = frexp(nt2::One<vT>());
    NT2_TEST_EQUAL(p.first  , nt2::Half<vT>());
    NT2_TEST_EQUAL(p.second , nt2::One<viT>());
  }
}