/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          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 __OPTIMISATION2_BUNDLE_COST_AND_SAVE_HPP__
#define __OPTIMISATION2_BUNDLE_COST_AND_SAVE_HPP__

#include <cmath>
#include "../function/function.hpp"
#include <libv/lma/ttt/traits/wrap.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/fusion/include/for_each.hpp>
#include <boost/fusion/include/has_key.hpp>
#include "../ba/nan_error.hpp"
#include "../ba/mat.hpp"
#include "../omp/omp.hpp"

#include <libv/lma/ttt/traits/unroll1.hpp>
#include <libv/lma/ttt/traits/rm_all.hpp>
#include <libv/lma/lm/trait/use_estimator.hpp>

namespace std
{
  template<size_t I> const double& get(const double& d)
  {
    static_assert(I==0,"get<I>(double) : I!=0");
    return d;
  }
}

namespace lma
{

  template<class Obs, class Bundle, class VErreur, class Mad> std::pair<double,int> cost_and_save_(const Bundle& bundle, VErreur& errors, const Mad& mad)
  {
    const auto nb_obs = bundle.template at_obs<Obs>().size();
    if (nb_obs==0) return {0,0};
    errors.resize(nb_obs());
    double error = 0;
    size_t cpt = 0;
// #pragma omp parallel for reduction(+:error) if(use_omp())
    for(auto iobs = bundle.template at_obs<Obs>().first() ; iobs < nb_obs ; ++iobs)
    {

      if ((errors[iobs()].second = make_function(bundle.obs(iobs))(bundle.map(iobs),errors[iobs()].first)))
      {
        auto tmp = errors[iobs()].first;
        detail::apply_mestimator_erreur<Obs>(bundle.obs(iobs),tmp,mad);
        error += squared_norm(tmp);
        cpt ++;
      }
    }

    if (is_invalid(error))
    {
      std::cout << " Erreur : " << error << std::endl;
      std::string msg = std::string() + " NAN : cost_and_save in functor " + ttt::name<Obs>() + ".";
      throw NAN_ERROR(msg);
    }

    return std::make_pair(error / 2.0, cpt);
  }

  template<class Bundle, class MapErreur, class Meds> struct CosterSave
  {
    const Bundle& bundle;
    MapErreur& map_erreur;
    const Meds& meds;
    std::pair<double,int> sumcpt;

    CosterSave(const Bundle& bundle_, MapErreur& map_erreur_, const Meds& meds_):bundle(bundle_),map_erreur(map_erreur_),meds(meds_),sumcpt({0,0}){}

    template<class Obs> void operator()(ttt::wrap<Obs>)
    {
      auto pair = cost_and_save_<Obs,Bundle>(bundle,map_erreur.template at_key<Obs>(),meds);
      sumcpt.first += pair.first;
      sumcpt.second += pair.second;
    }
  };

  template<class Bundle, class MapErreur, class Meds> std::pair<double,int> cost_and_save(const Bundle& bundle, MapErreur& map_erreur, const Meds& meds)
  {
    CosterSave<Bundle,MapErreur,Meds> coster(bundle,map_erreur,meds);
    mpl::for_each<typename Bundle::ListFunction, ttt::wrap<mpl::placeholders::_1>>(boost::ref(coster));
    return coster.sumcpt;
  }

  //////////////
  // MEDIANE  //
  //////////////

  template<class Error> struct _ADDNORM
  {
    Error error;
    std::vector<double>& norms;
    _ADDNORM(Error error_, std::vector<double>& norms_):error(error_),norms(norms_){}

    template<size_t I> void operator()(Int<I> const &)
    {
      norms.push_back(std::abs(std::get<I>(error)));
    }
  };

  template<class Obs, class Bundle> void cost_and_save_mad_(const Bundle& bundle, std::vector<double>& norms)
  {
    const auto nb_obs = bundle.template at_obs<Obs>().size();
    if (nb_obs!=0) norms.reserve(nb_obs());
// #pragma omp parallel for if(use_omp())
    for(auto iobs = bundle.template at_obs<Obs>().first() ; iobs < nb_obs ; ++iobs)
    {
      typename Function<Obs>::ErreurType error;
      if (make_function(bundle.obs(iobs))(bundle.map(iobs),error))
      {
#ifndef NDEBUG
        if (std::isnan(squared_norm(error)))
        {
          std::cout << " Nan found in " << ttt::name<Obs>() << std::endl;
          throw NAN_ERROR(" NAN : cost_and_save_mad_");
        }
#endif
        ttt::unroll<0,Size<typename ttt::rm_all<decltype(error)>::type>::value>(_ADDNORM<decltype(error)>(error,norms));
      }
    }
  }

  template<class Bundle, class Meds> struct CosterSaveMad
  {
    const Bundle& bundle;
    Meds& meds;//une mediane par foncteur robuste

    CosterSaveMad(const Bundle& bundle_, Meds& meds_):bundle(bundle_),meds(meds_){}

    template<class Obs, class Value> void operator()(ttt::wrap<bf::pair<Obs,Value>>)
    {
      if (bundle.template at_obs<Obs>().size()==0) return;
      std::vector<double> norms;
      cost_and_save_mad_<Obs,Bundle>(bundle,norms);

      //std::cout << color.bold() << "\n Mad " << ttt::name<Obs>() << " : ";
      bf::at_key<Obs>(meds) = bundle.template at_obs<Obs>()(0).compute(norms);
      //std::cout << color.reset();
    }
  };

  //! pour chaque foncteur
  template<class Meds, class Bundle>
  void cost_and_save_mad(const Bundle& bundle, Meds& meds)
  {
    CosterSaveMad<Bundle,Meds> coster(bundle,meds);
    mpl::for_each<Meds, ttt::wrap<mpl::placeholders::_1>>(boost::ref(coster));
  }

}//eon

#endif


#include "IntervalTreeVisitor.h"
#include "Logger.h"
#include <exception>
#include <boost/lexical_cast.hpp>
#include "HydLaError.h"

namespace hydla
{
namespace interval
{

using namespace hydla::symbolic_expression;

itvd IntervalTreeVisitor::pi = kv::constants<itvd>::pi();
itvd IntervalTreeVisitor::e = kv::constants<itvd>::e();

IntervalTreeVisitor::IntervalTreeVisitor()
{
}

itvd IntervalTreeVisitor::get_interval_value(const node_sptr& node, itvd *t, parameter_map_t *map)
{
  time_interval = t;
  parameter_map = map;
  HYDLA_LOGGER_DEBUG_VAR(get_infix_string(node));
  accept(node);
  if(current_value.is_integer)
    return itvd(current_value.integer);
  else
    return current_value.interval_value;
}

void IntervalTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Plus> node)
{
  accept(node->get_lhs());
  IntervalOrInteger lhs = current_value;
  accept(node->get_rhs());
  IntervalOrInteger rhs = current_value;
  current_value = lhs + rhs;
  // HYDLA_LOGGER_DEBUG("Plus : ", current_value.interval_value);
  return;
}

void IntervalTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Subtract> node)
{
  accept(node->get_lhs());
  IntervalOrInteger lhs = current_value;
  accept(node->get_rhs());
  IntervalOrInteger rhs = current_value;
  current_value = lhs - rhs;
  // HYDLA_LOGGER_DEBUG("Subtract : ", current_value.interval_value);
  return;
}

void IntervalTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Times> node)
{
  accept(node->get_lhs());
  IntervalOrInteger lhs = current_value;
  accept(node->get_rhs());
  IntervalOrInteger rhs = current_value;
  current_value = lhs * rhs;
  // HYDLA_LOGGER_DEBUG("Times : ", current_value.interval_value);
  return;
}

void IntervalTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Divide> node)
{
  accept(node->get_lhs());
  IntervalOrInteger lhs = current_value;
  accept(node->get_rhs());
  IntervalOrInteger rhs = current_value;
  current_value = lhs / rhs;
  // HYDLA_LOGGER_DEBUG("Divide : ", current_value.interval_value);
  return;
}

void IntervalTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Power> node)
{
  accept(node->get_lhs());
  IntervalOrInteger lhs = current_value;
  accept(node->get_rhs());
  IntervalOrInteger rhs = current_value;

  itvd base;
  if(lhs.is_integer)
    base = itvd(lhs.integer);
  else
    base = lhs.interval_value;
  // TODO: avoid string comparison
  if(rhs.is_integer)
  {
    current_value.interval_value = pow(base, rhs.integer);
  }
  else if(get_infix_string(node->get_rhs()) == "1/2")
  {
    current_value.interval_value = sqrt(base);
  }
  else if(get_infix_string(node->get_rhs()) == "-1/2")
  {
    HYDLA_LOGGER_DEBUG_VAR(base);
    current_value.interval_value = 1/sqrt(base);
  }
  else
  {
    current_value.interval_value = pow(base, rhs.interval_value);
  }

  current_value.is_integer = false;
  // HYDLA_LOGGER_DEBUG("Power : ", current_value.interval_value);
  return;
}

void IntervalTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Negative> node)
{
  accept(node->get_child());
  current_value = -current_value;
}

void IntervalTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Positive> node)
{
  accept(node->get_child());
}

void IntervalTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Pi> node)
{
  current_value.interval_value = pi;
  current_value.is_integer = false;
  // HYDLA_LOGGER_DEBUG("Pi : ", current_value.interval_value);
  return;
}

void IntervalTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::E> node)
{
  current_value.interval_value = e;
  current_value.is_integer = false;
  // HYDLA_LOGGER_DEBUG("E : ", current_value.interval_value);
  return;
}

void IntervalTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Number> node)
{
  std::string number_str = node->get_number();

  // try translation to int
  try
  {
    int integer = boost::lexical_cast<int>(number_str);
    current_value.is_integer = true;
    current_value.integer = integer;
    // HYDLA_LOGGER_DEBUG("Number : ", current_value.integer);
    // HYDLA_LOGGER_NODE_VALUE;
    return;
  }
  catch(const boost::bad_lexical_cast &e)
  {
    // do nothing
  }

  itvd itv = itvd(number_str);
  current_value.interval_value = itv;
  current_value.is_integer = false;
  // HYDLA_LOGGER_DEBUG("Number : ", current_value.interval_value);
  return;
}

void IntervalTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Float> node)
{
  current_value.interval_value = itvd(node->get_number());
  current_value.is_integer = false;
  // HYDLA_LOGGER_DEBUG("Float : ", current_value.interval_value);
  return;
}

void IntervalTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Function> node)
{
  std::string name = node->get_name();
  itvd arg;
  if(name == "sin")
  {
    if(node->get_arguments_size() != 1)
    {
      invalid_node(*node);
    }
    accept(node->get_argument(0));

    if(current_value.is_integer)
    {
      arg = itvd(current_value.integer);
    }
    else
    {
      arg = current_value.interval_value;
    }

    current_value.interval_value = sin(arg);
    current_value.is_integer = false;
    // HYDLA_LOGGER_DEBUG("Sin : ", current_value.interval_value);
  }
  else if(name == "cos")
  {
    if(node->get_arguments_size() != 1)
    {
      invalid_node(*node);
    }
    accept(node->get_argument(0));

    if(current_value.is_integer)
    {
      arg = itvd(current_value.integer);
    }
    else
    {
      arg = current_value.interval_value;
    }

    current_value.interval_value = cos(arg);
    current_value.is_integer = false;
    // HYDLA_LOGGER_DEBUG("Cos : ", current_value.interval_value);
  }
  else if(name == "tan")
  {
    if(node->get_arguments_size() != 1)
    {
      invalid_node(*node);
    }
    accept(node->get_argument(0));

    if(current_value.is_integer)
    {
      arg = itvd(current_value.integer);
    }
    else
    {
      arg = current_value.interval_value;
    }

    current_value.interval_value = tan(arg);
    current_value.is_integer = false;
  }
  else if(name == "log")
  {
    HYDLA_LOGGER_DEBUG_VAR(get_infix_string(node));
    if(node->get_arguments_size() != 1)
    {
      invalid_node(*node);
    }
    accept(node->get_argument(0));

    if(current_value.is_integer)
    {
      arg = itvd(current_value.integer);
    }
    else
    {
      arg = current_value.interval_value;
    }

    current_value.interval_value = log(arg);
    current_value.is_integer = false;
    // HYDLA_LOGGER_DEBUG("Log : ", current_value.interval_value);
  }
  else if(name == "sinh")
  {
    if(node->get_arguments_size() != 1)
    {
      invalid_node(*node);
    }
    accept(node->get_argument(0));

    if(current_value.is_integer)
    {
      arg = itvd(current_value.integer);
    }
    else
    {
      arg = current_value.interval_value;
    }

    current_value.interval_value = sinh(arg);
    current_value.is_integer = false;
  }
  else if(name == "cosh")
  {
    if(node->get_arguments_size() != 1)
    {
      invalid_node(*node);
    }
    accept(node->get_argument(0));

    if(current_value.is_integer)
    {
      arg = itvd(current_value.integer);
    }
    else
    {
      arg = current_value.interval_value;
    }

    current_value.interval_value = cosh(arg);
    current_value.is_integer = false;
  }
  else if(name == "tanh")
  {
    if(node->get_arguments_size() != 1)
    {
      invalid_node(*node);
    }
    accept(node->get_argument(0));

    if(current_value.is_integer)
    {
      arg = itvd(current_value.integer);
    }
    else
    {
      arg = current_value.interval_value;
    }

    current_value.interval_value = tanh(arg);
    current_value.is_integer = false;
  }
  else
  {
    invalid_node(*node);
  }
  return;
}

void IntervalTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::SymbolicT> node)
{
  if(time_interval == nullptr)invalid_node(*node);
  current_value.interval_value = *time_interval;
  current_value.is_integer = false;
  // HYDLA_LOGGER_DEBUG("SymbolicT : ", current_value.interval_value);
  return;
}

void IntervalTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Parameter> node)
{
  if(parameter_map == nullptr)invalid_node(*node);
  parameter_t param(node->get_name(),
                    node->get_differential_count(),
                    node->get_phase_id());
  auto param_it = parameter_map->find(param);
  if(param_it == parameter_map->end())invalid_node(*node);

  range_t range = param_it->second;;

  if(range.unique())
  {
    accept(range.get_unique_value().get_node());
  }
  else
  {
    value_t lower_value = (range.get_lower_bound()).value;
    accept(lower_value.get_node());
    itvd lower_itvd;
    if(current_value.is_integer)
      lower_itvd = itvd(current_value.integer);
    else
      lower_itvd = current_value.interval_value;

    value_t uppper_value = (range.get_upper_bound()).value;
    accept(uppper_value.get_node());
    itvd upper_itvd;
    if(current_value.is_integer)
      upper_itvd = itvd(current_value.integer);
    else
      upper_itvd = current_value.interval_value;

    current_value.interval_value = itvd(lower_itvd.lower(), upper_itvd.upper());
    current_value.is_integer = false;
  }
}

void IntervalTreeVisitor::invalid_node(symbolic_expression::Node &node)
{
  throw HYDLA_ERROR("invalid node: " + node.get_string());
}

void IntervalTreeVisitor::debug_print(std::string str, itvd x)
{
  std::cout << str << x << "\n";
}

#define DEFINE_INVALID_NODE(NODE_NAME)                                \
  void IntervalTreeVisitor::visit(boost::shared_ptr<NODE_NAME> node)  \
{                                                                \
  HYDLA_LOGGER_DEBUG("");                                        \
  invalid_node(*node);                                           \
}

DEFINE_INVALID_NODE(Variable)
DEFINE_INVALID_NODE(Differential)

DEFINE_INVALID_NODE(ConstraintDefinition)
DEFINE_INVALID_NODE(ProgramDefinition)
DEFINE_INVALID_NODE(ConstraintCaller)
DEFINE_INVALID_NODE(ProgramCaller)
DEFINE_INVALID_NODE(Constraint)
DEFINE_INVALID_NODE(Ask)
DEFINE_INVALID_NODE(Tell)

DEFINE_INVALID_NODE(Equal)
DEFINE_INVALID_NODE(UnEqual)

DEFINE_INVALID_NODE(Less)
DEFINE_INVALID_NODE(LessEqual)

DEFINE_INVALID_NODE(Greater)
DEFINE_INVALID_NODE(GreaterEqual)

DEFINE_INVALID_NODE(LogicalAnd)
DEFINE_INVALID_NODE(LogicalOr)

DEFINE_INVALID_NODE(Weaker)
DEFINE_INVALID_NODE(Parallel)

DEFINE_INVALID_NODE(Always)

DEFINE_INVALID_NODE(Previous)

DEFINE_INVALID_NODE(Print)
DEFINE_INVALID_NODE(PrintPP)
DEFINE_INVALID_NODE(PrintIP)
DEFINE_INVALID_NODE(Scan)
DEFINE_INVALID_NODE(Exit)
DEFINE_INVALID_NODE(Abort)
DEFINE_INVALID_NODE(SVtimer)

DEFINE_INVALID_NODE(Not)

DEFINE_INVALID_NODE(UnsupportedFunction)

DEFINE_INVALID_NODE(ImaginaryUnit)
DEFINE_INVALID_NODE(Infinity)
DEFINE_INVALID_NODE(True)
DEFINE_INVALID_NODE(False)

DEFINE_INVALID_NODE(ProgramList)
DEFINE_INVALID_NODE(ConditionalProgramList)
DEFINE_INVALID_NODE(ExpressionList)
DEFINE_INVALID_NODE(ConditionalExpressionList)
DEFINE_INVALID_NODE(EachElement)
DEFINE_INVALID_NODE(DifferentVariable)
DEFINE_INVALID_NODE(ExpressionListElement)
DEFINE_INVALID_NODE(ExpressionListCaller)
DEFINE_INVALID_NODE(ExpressionListDefinition)
DEFINE_INVALID_NODE(ProgramListElement)
DEFINE_INVALID_NODE(ProgramListCaller)
DEFINE_INVALID_NODE(ProgramListDefinition)
DEFINE_INVALID_NODE(Range)
DEFINE_INVALID_NODE(Union)
DEFINE_INVALID_NODE(Intersection)
DEFINE_INVALID_NODE(SumOfList)
DEFINE_INVALID_NODE(MulOfList)
DEFINE_INVALID_NODE(SizeOfList)

} // namespace interval
} // namespace hydla


#include <boost/accumulators/statistics/weighted_tail_quantile.hpp>


#ifndef GENERATER_H
#define GENERATER_H
#include <array>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <string>
#include <unordered_map>

#include "geoutil.hpp"
const double PI = 3.1415926535897932384626;
bool computeSweep(const Polygon_2& in, const FT offset, const Direction_2& dir,
                  bool counter_clockwise, std::vector<Point_2>* waypoints) {
  waypoints->clear();
  if (in.is_clockwise_oriented()) return false;
  Line_2 sweep(Point_2(0.0, 0.0), dir);
  std::vector<Point_2> sorted_pts = sortVerticesToLine(in, sweep);
  sweep = Line_2(sorted_pts.front(), dir);
  Vector_2 offset_vector = sweep.perpendicular(sorted_pts.front()).to_vector();
  offset_vector = offset * offset_vector /
                  std::sqrt(CGAL::to_double(offset_vector.squared_length()));
  const CGAL::Aff_transformation_2<K> kOffset(CGAL::TRANSLATION, offset_vector);
  Segment_2 sweep_segment;
  bool intersect = false;
  bool has_sweep_segment =
      findSweepSegment(in, sweep, &sweep_segment, intersect);
  if (intersect == true) {
    waypoints->clear();
    return false;
  }
  while (has_sweep_segment == true) {
    // cout << "inter: " << intersect << endl;
    if (intersect == true) {
      waypoints->clear();
      return false;
    }
    // Align sweep segment.
    if (counter_clockwise) sweep_segment = sweep_segment.opposite();
    // Connect previous sweep.
    waypoints->push_back(sweep_segment.source());
    if (!sweep_segment.is_degenerate())
      waypoints->push_back(sweep_segment.target());
    // Offset sweep.
    sweep = sweep.transform(kOffset);
    has_sweep_segment = findSweepSegment(in, sweep, &sweep_segment, intersect);
    Segment_2 prev_sweep_segment =
        counter_clockwise ? sweep_segment.opposite() : sweep_segment;
    if (!has_sweep_segment &&
        !((!waypoints->empty() &&
           *std::prev(waypoints->end(), 1) == sorted_pts.back()) ||
          (waypoints->size() > 1 &&
           *std::prev(waypoints->end(), 2) == sorted_pts.back()))) {
      sweep = Line_2(sorted_pts.back(), dir);
      has_sweep_segment =
          findSweepSegment(in, sweep, &sweep_segment, intersect);
      // Do not add super close sweep.
      if (CGAL::squared_distance(sweep_segment, prev_sweep_segment) < 0.1)
        break;
    }
    // Swap directions.
    counter_clockwise = !counter_clockwise;
  }
  return true;
}
bool computeConvexSweep(Polygon_2& in, FT offset, vector<Point_2>* waypoints) {
  Polygon_2 convexhull;
  CGAL::convex_hull_2(in.vertices_begin(), in.vertices_end(),
                      std::back_inserter(convexhull));
  simplifyPolygon(convexhull);
  Direction_2 bestdir = getBestEdgeDirection(convexhull);

  bool counter_clockwise = true;
  return computeSweep(in, offset, bestdir, counter_clockwise, waypoints);
}
/*
calculate total length of waypoints
*/
double CalculatePathlength(const vector<Point_2> waypoints) {
  int sz = waypoints.size();
  if (sz < 2)
    return 0;
  else {
    double pathlength = 0;
    for (int i = 0; i < sz - 1; i++) {
      pathlength += distance(waypoints[i], waypoints[i + 1]);
    }
    return pathlength;
  }
}
/*
find SecondOptimaldir in all sweepdir
*/
bool computeSecondOptimalSweep(const Polygon_2& in, const FT offset,
                               Direction_2* SecondOptimaldir) {
  Polygon_2 convexhull;
  CGAL::convex_hull_2(in.vertices_begin(), in.vertices_end(),
                      std::back_inserter(convexhull));
  simplifyPolygon(convexhull);
  bool counter_clockwise = true;
  Direction_2 dir;
  bool hasAcceptableSweep = false;
  double min_distance = std::numeric_limits<double>::max();
  for (EdgeConstIterator edge = convexhull.edges_begin();
       edge != convexhull.edges_end(); ++edge) {
    double Vdistance = std::numeric_limits<double>::min();
    // if not has intersection
    vector<Point_2> waypoints;
    if (computeSweep(in, offset, edge->direction(), counter_clockwise,
                     &waypoints) == true) {
      for (VertexIterator V = convexhull.vertices_begin();
           V != convexhull.vertices_end(); V++) {
        Line_2 edgeLine(edge->source(), edge->target());
        Vdistance = max(Vdistance,
                        CGAL::to_double(CGAL::squared_distance(*V, edgeLine)));
      }
      hasAcceptableSweep = true;
      if (min_distance > Vdistance + eps) {
        min_distance = Vdistance;
        dir = edge->direction();
      }
    }
    //    cout << "hello" << endl;
  }
  if (hasAcceptableSweep == false) return false;
  *SecondOptimaldir = dir;
  return true;
}
/*
try to compute a bestsweep ,otherwise second optimal sweeps;
*/
bool ComputeAllSweep(const Polygon_2& in, double footprint_width,
                     double horizontalOverwrap,
                     vector<vector<Point_2> >* cluster_sweeps,
                     Direction_2& Dir) {
  FT offset = footprint_width * (1 - horizontalOverwrap);
  bool counter_clockwise = true;
  vector<Point_2> sweep;
  // compute bestdir
  Polygon_2 convexhull;
  CGAL::convex_hull_2(in.vertices_begin(), in.vertices_end(),
                      std::back_inserter(convexhull));
  simplifyPolygon(convexhull);
  Direction_2 bestdir = getBestEdgeDirection(convexhull);
  if (computeSweep(in, offset, bestdir, counter_clockwise, &sweep) == false) {
    // can not generate a no-insert zigzag path
    if (computeSecondOptimalSweep(in, offset, &bestdir) == false) {
      cluster_sweeps->clear();
      return false;
    }
  }
  // compute cluster_waypoints
  computeSweep(in, offset, bestdir, counter_clockwise, &sweep);
  cluster_sweeps->push_back(sweep);  // counter_clockwise

  std::reverse(sweep.begin(), sweep.end());
  cluster_sweeps->push_back(sweep);  // reverse
  computeSweep(in, offset, bestdir, !counter_clockwise, &sweep);
  cluster_sweeps->push_back(sweep);  // clockwise
  std::reverse(sweep.begin(), sweep.end());
  cluster_sweeps->push_back(sweep);  // reverse
  Dir = bestdir;
  return true;
}

#endif


#include <cmath>
#include <iostream>
#include <fstream>
#include <assert.h>
#include <string>
#include <vector>
#include <map>

#include <boost/tokenizer.hpp>
#include <boost/algorithm/string/predicate.hpp>

#include "parse_speedup.h"

// Cores for which speedup data is available.
static const size_t NUM_SPEEDUP_POINTS = 4;
static int interp_cores[NUM_SPEEDUP_POINTS] = { 2, 4, 8, 16 };
static std::map<std::string, loop_data*> loop_data_map;

/* This function converts absolute speedups on x cores into marginal speedup.
 * HELIX speedup data is given in absolute terms, but it is easier for the
 * allocators to work with marginal speedups.
 */
/* ========================================================================== */
static void ConvertToMarginalSpeedup(double* speedup, int num_points) {
    double temp1 = speedup[0], temp2 = 0;
    for (int i = 1; i < num_points; i++) {
        temp2 = speedup[i];
        speedup[i] -= temp1;
        temp1 = temp2;
    }
}

/* Linearly interpolates input speedup points. speedup_in is an array that
 * contains speedup values for 2, 4, 8, and 16 cores. speedup_out is an array
 * that has linearly interpolated values for 2-16 cores. The zeroth element of
 * speedup_out = 0, because there is no speedup with just one core.
 */
/* ========================================================================== */
static void InterpolateSpeedup(double* speedup_in, double* speedup_out) {
    // Copy the existing data points.
    for (size_t i = 0; i < NUM_SPEEDUP_POINTS; i++)
        speedup_out[interp_cores[i] - 1] = speedup_in[i];
    for (size_t i = 0; i < NUM_SPEEDUP_POINTS - 1; i++) {
        double slope =
            (speedup_in[i + 1] - speedup_in[i]) / (interp_cores[i + 1] - interp_cores[i]);
        // Interpolate.
        for (int j = interp_cores[i] + 1; j < interp_cores[i + 1]; j++) {
            speedup_out[j - 1] = slope * (j - interp_cores[i + 1]) + speedup_in[i + 1];
        }
    }
}

/* Parses a comma separated value file that contains predicted speedups for
 * each loop when run on 2,4,8,and 16 cores and stores the data in a map.
 */
/* ========================================================================== */
void LoadHelixSpeedupModelData(const std::string filepath) {
    using std::string;
    using boost::tokenizer;
    using boost::escaped_list_separator;
    string line;
    std::ifstream speedup_loop_file(filepath);
    if (speedup_loop_file.is_open()) {
#ifdef PARSE_DEBUG
        std::cout << "Cores:\t\t";
        for (size_t j = 1; j <= NUM_SPEEDUP_POINTS; j++)
            std::cout << j << "\t";
        std::cout << std::endl;
#endif
        while (getline(speedup_loop_file, line)) {
            // Ignore comments (lines starting with //).
            if (!boost::starts_with(line.c_str(), "//")) {
                tokenizer<escaped_list_separator<char>> tok(line);
                string loop_name;
                double* speedup_data = new double[NUM_SPEEDUP_POINTS];
                double serial_runtime = 0;
                double serial_runtime_variance = 0;
                size_t i = 0;
                bool first_iteration = true;
                for (auto it = tok.begin(); it != tok.end(); ++it) {
                    if (first_iteration) {
                        loop_name = *it;
                        first_iteration = false;
                    } else if (i < NUM_SPEEDUP_POINTS) {
                        // Speedup data points.
                        speedup_data[i] = atof(it->c_str());
                        i++;
                    } else {
                        // Serial runtime and variance.
                        serial_runtime = atof(it->c_str());
                        it++;
                        serial_runtime_variance = atof(it->c_str());
                    }
                }
                loop_data* data = new loop_data();
                data->speedup = speedup_data;
                data->serial_runtime = serial_runtime;
                data->serial_runtime_variance = serial_runtime_variance;
                loop_data_map[loop_name] = data;
#ifdef PARSE_DEBUG
                std::cout << loop_name << " speedup:\t";
                for (size_t j = 0; j < NUM_SPEEDUP_POINTS; j++)
                    std::cout << speedup_data[j] << "\t";
                std::cout << std::endl;
#endif
            }
        }
#ifdef PARSE_DEBUG
        std::cout << std::endl;
#endif
    } else {
        std::cerr << "Speedup file could not be opened.";
        exit(1);
    }
}

// Returns the core scaling attributes of a loop.
std::vector<double> GetHelixLoopScaling(const std::string& loop_name) {
    double* res_raw = loop_data_map[loop_name]->speedup;
    assert(res_raw != NULL);
    std::vector<double> res(NUM_SPEEDUP_POINTS + 1);
    res[0] = 1;  // for 1 core, speedup is 1.
    for (size_t i = 0; i < NUM_SPEEDUP_POINTS; i++)
        res[i + 1] = res_raw[i];
    return res;
}

// Returns the full loop scaling attributes of a loop
loop_data* GetHelixFullLoopData(const std::string& loop_name) { return loop_data_map[loop_name]; }


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef TURI_REGULARIZER_INTERFACE_H_
#define TURI_REGULARIZER_INTERFACE_H_

#include <string>
#include <core/data/flexible_type/flexible_type.hpp>

// Eigen
#include <Eigen/Core>
#include <Eigen/SparseCore>

// Optimizaiton
#include <ml/optimization/optimization_interface.hpp>

// TODO: List of todo's for this file
//------------------------------------------------------------------------------
//

namespace turi {

namespace optimization {

/**
 * \ingroup group_optimization
 * \addtogroup regularizers Regularizers
 * \{
 */

/**
 *
 * Interface for regularizers which are separable but not smooth.
 * \note Smooth refers to functions with continuous derivatives of all orders.
 *
*/
class regularizer_interface {

  public:

  /**
   * Default desctuctor.
   */
  virtual ~regularizer_interface() {};

  /**
   * Function to determine if the regularizer is smooth.
   * \returns false
   */
  bool is_smooth() {return false;};

  /**
   * Compute the function value of the regularizer at a given point.
   * \param[in]  point   Point at which we are computing the gradient.
   *
   */
  virtual double compute_function_value(const DenseVector &point) const = 0;

  /**
   * Compute the gradient (or subgradient) at the given point.
   *
   * \param[in]  point    Point at which we are computing the gradient.
   * \param[out] gradient Dense gradient
   *
   */
  virtual void compute_gradient(const DenseVector &point, DenseVector&
      gradient) const = 0;

  /**
   * Compute the proximal operator for the regularizer at a given point.
   *
   * \param[in,out]  point    Point at which we are computing the gradient.
   * \param[in]      penalty  Penalty parameters.
   *
   * \note The proximal operator for a convex function f(.) at the point x is
   * defined as
   *                prox_f(x) = argmin_v ( f(v) + 0.5 * penalty ||x-v||^2)
   *  The idea is an old concept in optimization but it well explained in (1).
   *
   * References:
   *
   * (1) Parikh, Neal, and Stephen Boyd. "Foundations and Trends in
   * Optimization." (2014).
   *
   */
  virtual void apply_proximal_operator(DenseVector &point, const double&
      _penalty=0) const = 0;

};

/**
 *
 * Interface for regularizers that separable and smooth.
 *
*/
class smooth_regularizer_interface: public regularizer_interface {

  public:

  /**
   * Function to determine if the regularizer is smooth.
   * \returns true
   */
  bool is_smooth() {return true;};

  /**
   * Default desctuctor.
   */
  virtual ~smooth_regularizer_interface() {};

  /**
   * Compute the hessian of the regularizer at a given point.
   * \param[in]      point   Point at which we are computing the gradient.
   * \param[in,out]  hessian Diagonal matrix as the hessian gradient.
   *
   */
  virtual void compute_hessian(const DenseVector &point, DiagonalMatrix
      &hessian) const = 0;

};

/// \}

} // optimization
} // turicreate

#endif


//===- conv.cc ------------------------------------------------------------===//
//
//                       The CIM Hardware Simulator Project
//
// See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
#include "conv.hh"
#include "diagnostic/msgHandling.hh"
#include <Eigen/Dense>

namespace cimHW {

enum class AutoPadType {
  NOTSET = 0,
  VALID = 1,
  SAME_UPPER = 2,
  SAME_LOWER = 3,
};

inline AutoPadType StringToAutoPadType(const std::string& str) {
  if (str == "NOTSET") {  // in onnx spec, default value is "NOTSET"
    return AutoPadType::NOTSET;
  }
  if (str == "VALID") {
    return AutoPadType::VALID;
  }
  if (str == "SAME_UPPER") {
    return AutoPadType::SAME_UPPER;
  }
  if (str == "SAME_LOWER") {
    return AutoPadType::SAME_LOWER;
  }
  // default, use NOTSET
  return AutoPadType::NOTSET;
}

dim_type computePadAndOutputShape(
    const int in_dim,
    const int kernel,
    const int stride,
    const int dilation,
    AutoPadType pad_type,
    int* pad_head,
    int* pad_tail)
{
  const int dkernel = dilation * (kernel - 1) + 1;
  int out_dim = 0;
  if (pad_type == AutoPadType::NOTSET)
  {
    out_dim = static_cast<int>(static_cast<float>(in_dim + *pad_head + *pad_tail - dkernel) / stride + 1);
  }
  else
  {
    switch (pad_type) {
      // VALID is nopad
      case AutoPadType::VALID:
        *pad_head = 0;
        *pad_tail = 0;
        out_dim = (in_dim - dkernel) / stride + 1;
        break;
      case AutoPadType::SAME_UPPER:
      case AutoPadType::SAME_LOWER:
      {
        assert(dilation == 1 && "Dilation not supported for AutoPadType::SAME_UPPER or AutoPadType::SAME_LOWER.");
        int legacy_target_size = (in_dim + stride - 1) / stride;
        int pad_needed = (legacy_target_size - 1) * stride + kernel - in_dim;
        out_dim = (in_dim + pad_needed - dkernel) / stride + 1;

        // pad the extra to the end if SAME_UPPER, or to the begining if SAME_LOWER
        *pad_head = (pad_type == AutoPadType::SAME_LOWER)? (pad_needed + 1) / 2 : pad_needed / 2;
        *pad_tail = pad_needed - *pad_head;
      }
      break;
      default:
        assert(false && "pad type not supported.");
    }
  }
  return out_dim;
}

// fetch all posible data cubes (along with kernel height and width) used to do matmul with kernel matrix
void im2col(const float *data_im, const int channels, const int height, const int width,
            const int kernel_h, const int kernel_w,
            const int pad_t, const int pad_b,
            const int pad_l, const int pad_r,
            const int stride_h, const int stride_w,
            const int dilation_h, const int dilation_w,
            float *data_col)
{
  auto is_a_ge_zero_and_a_lt_b = [](int a, int b) { return static_cast<unsigned>(a) < static_cast<unsigned>(b); };
  auto get_conv_output_size = [](const int intput_size,
                                 const int kernel_size,
                                 const int pad_begin,
                                 const int pad_end,
                                 const int stride_step,
                                 const int dilation_rate)
  {
    return (intput_size + pad_begin + pad_end - (dilation_rate * (kernel_size - 1) + 1)) / stride_step + 1;
  };

  const int output_h = get_conv_output_size(height, kernel_h, pad_t, pad_b, stride_h, dilation_h);
  const int output_w = get_conv_output_size(width, kernel_w, pad_l, pad_r, stride_w, dilation_w);
  const int channel_size = height * width;

  for (int channel = channels; channel--; data_im += channel_size)
  {
    for (int kernel_row = 0; kernel_row < kernel_h; kernel_row++)
    {
      for (int kernel_col = 0; kernel_col < kernel_w; kernel_col++)
      {
        int input_row = -pad_t + kernel_row * dilation_h;
        for (int output_rows = output_h; output_rows; output_rows--)
        {
          if (!is_a_ge_zero_and_a_lt_b(input_row, height))
          {
            for (int output_cols = output_w; output_cols; output_cols--)
            {
              *(data_col++) = 0;
            }
          }
          else
          {
            int input_col = -pad_l + kernel_col * dilation_w;
            for (int output_col = output_w; output_col; output_col--)
            {
              if (is_a_ge_zero_and_a_lt_b(input_col, width))
              {
                *(data_col++) = data_im[input_row * width + input_col];
              }
              else
              {
                *(data_col++) = 0;
              }
              input_col += stride_w;
            } // output_col loop
          }
          input_row += stride_h;
        } // output_rows loop
      } // kernel_col loop
    } // kernel_row loop
  } // channel loop
}

// fetch all posible data cubes (along with kernel D1, D2, D3....Dd) used to do matmul with kernel matrix
void im2colNd(const float* data_img, const int* im_shape, const int* col_shape,
              const int* m_kernel_shape, const int N,
              const int* pad,
              const int* stride,
              const int* dilation,
              float* data_col,
              const int padding_value = 0)
{
  int kernel_size = 1;
  for (int i = 0; i < N; ++i) {
    kernel_size *= m_kernel_shape[i];
  }
  int channels_col = col_shape[0];
  std::vector<int> d_offset(N, 0);
  std::vector<int> d_iter(N, 0);
  for (int c_col = 0; c_col < channels_col; ++c_col) {
    // Loop over spatial axes in reverse order to compute a per-axis offset.
    int offset = c_col;
    for (int d_i = N - 1; d_i >= 0; --d_i) {
      if (d_i < N - 1) {
        offset /= m_kernel_shape[d_i + 1];
      }
      d_offset[d_i] = offset % m_kernel_shape[d_i];
    }
    for (bool incremented = true; incremented;) {
      // Loop over spatial axes in forward order to compute the indices in the
      // image and column, and whether the index lies in the padding.
      int index_col = c_col;
      int index_im = c_col / kernel_size;
      bool is_padding = false;
      for (int d_i = 0; d_i < N; ++d_i) {
        int d = d_iter[d_i];
        int d_im = d * stride[d_i] - pad[d_i] + d_offset[d_i] * dilation[d_i];
        is_padding |= d_im < 0 || d_im >= im_shape[d_i + 1];
        index_col *= col_shape[d_i + 1];
        index_col += d;
        index_im *= im_shape[d_i + 1];
        index_im += d_im;
      }
      if (is_padding) {
        data_col[index_col] = padding_value;
      } else {
        data_col[index_col] = data_img[index_im];
      }
      // Loop over spatial axes in reverse order to choose an index,
      // like counting.
      incremented = false;
      for (int d_i = N - 1; d_i >= 0; --d_i) {
        int d_max = col_shape[d_i + 1];
        if (d_iter[d_i] == d_max - 1) {
          d_iter[d_i] = 0;
        } else {  // d_iter[d_i] < d_max - 1
          ++d_iter[d_i];
          incremented = true;
          break;
        }
      }
    }  // while(incremented) {
  }    // for (int c = 0; c < channels_col; ++c) {
}

cimHWConvOp::cimHWConvOp(
  const std::string &configFilePath,
  const_element_type* input_X_, const_dim_type input_X_ndim_, const_dim_type* input_X_dims_,
  const_element_type* input_W_, const_dim_type input_W_ndim_, const_dim_type* input_W_dims_,
  const_element_type* input_B_, const_dim_type input_B_ndim_, const_dim_type* input_B_dims_,
  element_type* output_Y_, const_dim_type output_Y_ndim_, const_dim_type* output_Y_dims_,
  const char* auto_pad_,
  const_dim_type* dilations_, const_dim_type number_of_dilations_,
  const_dim_type group_,
  const_dim_type* kernel_shape_, const_dim_type number_of_kernel_shape_,
  const_dim_type* pads_, const_dim_type number_of_pads_,
  const_dim_type* strides_, const_dim_type number_of_strides_)
  : cimHWOp("Conv")
  , m_cimCU(configFilePath)
  , m_input_X(input_X_)
  , m_input_W(input_W_)
  , m_input_B(input_B_)
  , m_output_Y(output_Y_)
{
  verbose1(opName());

  // input validation
  if(input_X_ndim_ <= 2)
    error("Minimum dimension of X is 3");
  if(input_W_ndim_ <= 2)
    error("Minimum dimension of W is 3");

  m_NInputs = input_X_dims_[0];
  if(m_NInputs != 1)
    error("Only support 1 batch now");

  // basic property of conv
  m_Channel       = input_X_dims_[1];
  m_OutputChannel = input_W_dims_[0];
  m_input_X_shape = std::vector<dim_type>(input_X_dims_, input_X_dims_ + input_X_ndim_);
  m_input_W_shape = std::vector<dim_type>(input_W_dims_, input_W_dims_ + input_W_ndim_);
  if(m_Channel != input_W_dims_[1])
    error("Channel of X and W is not consistent");

  // check kernel related properties
  // find out kernel shape, which is the shape excludes M and C from weights
  m_kernel_shape  = std::vector<dim_type>(input_W_dims_ + 2, input_W_dims_ + input_W_ndim_);
  m_input_B_shape = std::vector<dim_type>(input_B_dims_, input_B_dims_ + input_B_ndim_);
  // check dilation
  m_dilations = std::vector<dim_type>(dilations_, dilations_ + number_of_dilations_);
  if (m_dilations.empty()) {
    m_dilations.resize(m_kernel_shape.size(), 1);
  }
  // check group
  m_group = group_;
  // check pads
  m_pads = std::vector<dim_type>(pads_, pads_ + number_of_pads_);
  if (m_pads.empty()) {
    m_pads.resize(m_kernel_shape.size() * 2, 0);
  }
  // check strides
  m_strides = std::vector<dim_type>(strides_, strides_ + number_of_strides_);
  if (m_strides.empty()) {
    m_strides.resize(m_kernel_shape.size(), 1);
  }

  // infer the shape of output and output_Y from input
  m_image_shape = std::vector<dim_type>(input_X_dims_ + 2, input_X_dims_ + input_X_ndim_);
  m_output_Y_shape = std::vector<dim_type> ({m_NInputs, m_OutputChannel});
  for(int i=0; i< m_image_shape.size(); i++) {
    m_output_Y_shape.push_back(computePadAndOutputShape(m_image_shape[i], m_kernel_shape[i], m_strides[i], m_dilations[i],
                                                        StringToAutoPadType(std::string(auto_pad_)),
                                                        &m_pads[i], &m_pads[ m_image_shape.size()+i]));
  }
  for(int i=2; i<m_output_Y_shape.size(); i++)
    m_output_shape.push_back(m_output_Y_shape[i]);
}

void cimHWConvOp::simulate()
{
  // calculate the size of each tensor
  dim_type m_input_size  = std::accumulate( m_image_shape.cbegin()  ,  m_image_shape.cend()  , 1, std::multiplies<dim_type>());
  dim_type m_weight_size = std::accumulate(m_input_W_shape.cbegin() , m_input_W_shape.cend() , 1, std::multiplies<dim_type>());
  dim_type m_kernel_size = std::accumulate(m_kernel_shape.cbegin()  , m_kernel_shape.cend()  , 1, std::multiplies<dim_type>());
  dim_type m_output_size = std::accumulate(m_output_shape.cbegin()  , m_output_shape.cend()  , 1, std::multiplies<dim_type>());

  // calculate the extended number for easy use
  const dim_type X_offset      = m_Channel / m_group * m_input_size;
  const dim_type W_offset      = m_weight_size / m_group;
  const dim_type kernel_offset = m_kernel_size * m_Channel / m_group;
  const dim_type kernel_dim    = m_Channel / m_group * m_kernel_size;
  const dim_type kernel_rank   = m_kernel_shape.size();

  std::vector<dim_type> col_buffer_shape;
  // Pointwise convolutions can use the original input tensor in place,
  // otherwise a temporary buffer is required for the im2col transform.
  // Todo: pointwise conv can do inplace gemm without dataCube and col_buffer
  //if (kernel_size != 1/* || !HasStridesOneAndNoPadding()*/) {
  if (kernel_rank != 2) {
    // only used in in2colNd
    col_buffer_shape.reserve(1 + m_output_shape.size());
    col_buffer_shape.push_back(kernel_dim);
    col_buffer_shape.insert(col_buffer_shape.end(), m_output_shape.begin(), m_output_shape.end());
  }
  //}

  // Object is to get matrix product N*M(output_channels) x dataCubes(output_image_size) = kernel * dataCubes
  MatrixXfRowMajor result = MatrixXfRowMajor::Zero(m_OutputChannel, m_output_size);
  // prepare dataCubes = dataCubes(output_image_size) x (kernel_size), column-major
  MatrixXfRowMajor dataCubes = MatrixXfRowMajor::Zero(kernel_dim, m_output_size);
  for (int group_id = 0; group_id < m_group; ++group_id)
  {
    // apply im2col for conv2d
    if (kernel_rank == 2) {

      im2col(m_input_X + group_id * X_offset, m_Channel / m_group,  m_image_shape[0],  m_image_shape[1],
               m_kernel_shape[0], m_kernel_shape[1],
               m_pads[0], m_pads[2],
               m_pads[1], m_pads[3],
               m_strides[0], m_strides[1],
               m_dilations[0], m_dilations[1],
               dataCubes.data());
    }
    else
    // otherwise, apply im2colNd, including conv1d and more than conv3d
    {
      im2colNd(m_input_X + group_id * X_offset, m_input_X_shape.data() + 1, col_buffer_shape.data(),
              m_kernel_shape.data(), kernel_rank,
              m_pads.data(),
              m_strides.data(),
              m_dilations.data(),
              dataCubes.data());
    }

    // prepare kernel =  M(output_channels)/group x (kernel_size)
    Eigen::Map<const MatrixXfRowMajor> kernel(m_input_W + group_id * W_offset + group_id * kernel_offset, m_OutputChannel / m_group, kernel_dim);
    // send datacube and each kernel by row to ComputeUnit
    for(int i=0; i<kernel.rows(); i++)
      result.block(group_id * m_OutputChannel / m_group, 0, m_OutputChannel / m_group, m_output_size).row(i) =
                   m_cimCU.compute(dataCubes, kernel.row(i).transpose());
  }

  // finally, plus bias to every kernel
  if(m_input_B != nullptr)
  {
    MatrixXfRowMajor bias_mat(m_OutputChannel, m_output_size);
    bias_mat.colwise() = Eigen::Map<const Eigen::VectorXf>(m_input_B, m_input_B_shape[0]);
    result += bias_mat;
  }

  // copy back to output
  memcpy(m_output_Y, result.data(), sizeof(numType) * m_OutputChannel * m_output_size);
  verbose1("\n");
}

}  // namespace cimHW


#include <mex.h>
#include <math.h>
#include <iostream>
#include <vector>
#include <fstream>
#include <cassert>
#include <utility>

#include <dirent.h>

#include <igl/matlab/MexStream.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/validate_arg.h>
#include <igl/dirname.h>

#include <igl/readMESH.h>
#include <igl/readOBJ.h>

#include <Eigen/Core>

using namespace Eigen;
using namespace std;

void read_mesh_files_from_directory(const std::string& directory, std::vector<std::string>& mesh_list){

	using namespace std;

	std::vector<std::string> file_list;

	DIR *d;
	struct dirent *dir;
	int i=0;
	d = opendir(directory.c_str());
	if (d)
	{
		while ((dir = readdir(d)) != NULL)
		{
			i++;
			file_list.push_back(dir->d_name);
		}
		closedir(d);
	}
	for(int i=0; i<file_list.size(); i++){
		std::string file = file_list[i];
		size_t last_index = file.find_last_of(".");
		if(file.substr(last_index+1) == "mesh"){
			std::string name = file.substr(0,last_index);
			mesh_list.push_back(name);
		}
	}
}

void read_blendshape_list(const std::string& directory, const std::string& list_file,
		std::vector<std::string>& name_list,
		std::vector<Eigen::MatrixXd>& V_list)
{
	FILE* file;
	file= fopen(list_file.c_str(), "rb");
	int num_list;
	fscanf(file, "%d\n", &num_list);

	std::vector<int> reordered_id(num_list);
	for(int i=0; i<num_list; i++)
	{
		//std::string bs_name;
		char bs_char[256];
		fscanf(file, "%s\n", bs_char);
		std::string bs_name(bs_char);
		for(int j=0; j<name_list.size(); j++){
			if(bs_name==name_list[j]){
				reordered_id[i]=j;
			}
		}
	}
	std::vector<std::string> reorder_name_list(num_list);
	for(int i=0; i<reordered_id.size(); i++){
		reorder_name_list[i] = name_list[reordered_id[i]];
	}
	name_list = reorder_name_list;
	fclose(file);

	V_list.resize(name_list.size());
	for(int j=0; j<name_list.size(); j++){
		// std::cout<<"name_list["<<j<<"] : "<<name_list[j]<<std::endl;
		std::string mesh_path = directory+"/"+name_list[j]+".mesh";
		Eigen::MatrixXd BV;
		Eigen::MatrixXi BT, BF;
		igl::readMESH(mesh_path, BV,BT,BF);
		// igl::readOBJ(mesh_path, BV,BF);
		V_list[j] = BV;
	}
}

void read_blendshape_anim(const std::string& anim_file, std::vector<Eigen::VectorXd>& w_list)
{
	FILE* file;
	file= fopen(anim_file.c_str(), "rb");
	int num_bs, num_frame;
	fscanf(file, "%d %d\n", &num_bs, &num_frame);

	w_list.resize(num_frame);
	for(int i=0; i<num_frame; i++){
		Eigen::VectorXd w(num_bs);
		for(int j=0; j<num_bs; j++){
			double val;
			fscanf(file, "%lf", &val);
			w(j) = val;
		}
		w_list[i] = w;
	}
	fclose(file);
}

void construct_blendshape_mat(const Eigen::MatrixXd& V,
		const std::vector<Eigen::MatrixXd>& BV_list,
		//Eigen::RowVectorXd Vcenter, double Vbound,
		Eigen::SparseMatrix<double>& BS)
{
	Eigen::MatrixXd VT = V.transpose();
	Eigen::VectorXd VCol(Eigen::Map<Eigen::VectorXd>(VT.data(), V.cols()*V.rows()));
	std::vector<Eigen::Triplet<double> > coefficients;
	for(int j=0; j<BV_list.size(); j++){
		Eigen::MatrixXd BV = BV_list[j];

		//normalize_obj(BV, Vcenter, Vbound);

		Eigen::MatrixXd BVT = BV.transpose();

		Eigen::VectorXd bVcol = Eigen::Map<Eigen::VectorXd>(BVT.data(), BV.cols()*BV.rows());

		Eigen::VectorXd deltaV = bVcol-VCol;
		for(int i=0; i<deltaV.size(); i++){
			if(deltaV(i)>1e-12){
				coefficients.push_back(Eigen::Triplet<double>(i,j, deltaV(i)));
			}
		}
	}
	BS.resize(VCol.size(), BV_list.size());
	BS.setFromTriplets(coefficients.begin(), coefficients.end());
}

void construct_blendshape_mat(const Eigen::MatrixXd& V,
		const std::vector<Eigen::MatrixXd>& BV_list,
		//Eigen::RowVectorXd Vcenter, double Vbound,
		Eigen::MatrixXd& BS)
{
	Eigen::MatrixXd VT = V.transpose();
	Eigen::VectorXd VCol(Eigen::Map<Eigen::VectorXd>(VT.data(), V.cols()*V.rows()));

	BS.resize(VCol.size(), BV_list.size());
	BS.setZero();
	for(int j=0; j<BV_list.size(); j++){
		Eigen::MatrixXd BV = BV_list[j];

		//normalize_obj(BV, Vcenter, Vbound);

		Eigen::MatrixXd BVT = BV.transpose();

		Eigen::VectorXd bVcol = Eigen::Map<Eigen::VectorXd>(BVT.data(), BV.cols()*BV.rows());

		Eigen::VectorXd deltaV = bVcol-VCol;
		for(int i=0; i<deltaV.size(); i++){
			//if(deltaV(i)>1e-12){
				BS(i,j)=deltaV(i);
			//}
		}
	}
}

void read_blendshape_data(
	std::string bs_list_file, std::string bs_folder, std::string animfile,
	const Eigen::MatrixXd& TV,
	//Eigen::SparseMatrix<double>& BS,
	Eigen::MatrixXd& BS,
	std::vector<Eigen::VectorXd>& T_list
	)
{

	std::vector<std::string> mesh_name_list;
	read_mesh_files_from_directory(bs_folder, mesh_name_list);

	std::vector<Eigen::MatrixXd> BV_list;
	read_blendshape_list(bs_folder, bs_list_file, mesh_name_list, BV_list);
	read_blendshape_anim(animfile, T_list);

	//Eigen::SparseMatrix<double> BS;
	//construct_blendshape_mat(TV, BV_list, Vcenter, Vbound, BS);
	construct_blendshape_mat(TV, BV_list, BS);
}

void mexFunction(
  int          nlhs,
  mxArray      *plhs[],
  int          nrhs,
  const mxArray *prhs[])
{
    Eigen::MatrixXd TV;

    char* bs_list_file = mxArrayToString(prhs[0]);
    char* bs_folder = mxArrayToString(prhs[1]);
    char* animfile = mxArrayToString(prhs[2]);
	igl::matlab::parse_rhs_double(prhs+3,TV);
	//Eigen::SparseMatrix<double> BS;
	Eigen::MatrixXd BS;
    std::vector<Eigen::VectorXd> T_list;

	read_blendshape_data(bs_list_file, bs_folder, animfile, TV, BS, T_list);

	igl::matlab::prepare_lhs_double(BS, plhs);

    plhs[1] = mxCreateCellMatrix(T_list.size(),1);

    mxArray *x;
    for(int i=0; i<T_list.size(); i++){
        const int m = T_list[i].rows();
        const int n = T_list[i].cols();
        x = mxCreateDoubleMatrix(m,n, mxREAL);
        Eigen::Map< Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> > map(mxGetPr(x),m,n);
        map = T_list[i].template cast<double>();
        mxSetCell(plhs[1], i, x);
    }
    return;
}


#pragma once
#include <boost/multiprecision/cpp_int.hpp>
#include <Wren++.h>
#include <sstream>

struct WrenVM;

namespace wrenbase {
   using namespace boost::multiprecision;
   using std::string;
   using std::vector;

   typedef number<cpp_int_backend<256, 256, unsigned_magnitude, unchecked, void> >     uint256;

   class UInt256 {
      public:
        explicit UInt256(double v):value(v){}
        UInt256(uint256 v=0):value(v){}

        void test( WrenVM& vm ) {
           std::cerr << "WrenVM* => " << &vm <<"\n";
        }

        UInt256 operator & ( const UInt256& other )const  { return value & other.value;  }
        UInt256 operator | ( const UInt256& other )const  { return value | other.value;  }
        UInt256 operator ^ ( const UInt256& other )const  { return value ^ other.value;  }
        UInt256 operator + ( const UInt256& other )const  { return value + other.value;  }
        UInt256 operator - ( const UInt256& other )const  { return value - other.value;  }
        UInt256 operator * ( const UInt256& other )const  { return value * other.value;  }
        UInt256 operator / ( const UInt256& other )const  { return value / other.value;  }
        UInt256 operator % ( const UInt256& other )const  { return value % other.value;  }
        bool    operator < ( const UInt256& other )const  { return value < other.value;  }
        bool    operator > ( const UInt256& other )const  { return value > other.value;  }
        bool    operator <= ( const UInt256& other )const { return value <= other.value; }
        bool    operator >= ( const UInt256& other )const { return value >= other.value; }
        bool    operator == ( const UInt256& other )const { return value == other.value; }
        bool    operator != ( const UInt256& other )const { return value != other.value; }
        UInt256 operator << ( double other )const { return value << int64_t(other); }
        UInt256 operator >> ( double other )const { return value >> int64_t(other); }
        bool    operator ! ()const { return !value; }
        UInt256 operator ~ ()const { return ~value; }

        static UInt256 fromString( string s ) { return uint256(s); }
        static UInt256 fromDouble( double s ) { return uint256(s); }
        string toString()const { return string(value); }
        double toDouble()const { return static_cast<double>(value); }
        vector<char> toBinary()const    {
           vector<char> tmp(32);
           memcpy( tmp.data(), &value, 32 );
           return tmp;
        }

        string toHex()const    {
           std::stringstream ss;
           ss << std::hex << value;
           return ss.str();
        }

        static void        wrenBind();
        static std::string wrenScript();

        uint256 value;
   };

} // namespace wrenbase


#pragma once

#include <deal.II/base/tensor.h>
// system includes ------------------------------------------------------------
#include <boost/mpl/if.hpp>
#include <boost/type_traits.hpp>
#include <complex>

namespace boltzmann {

template <int rank, int dimX>
struct tensor_traits
{
  template <typename NUMBER>
  using value_type = dealii::Tensor<rank, dimX, NUMBER>;
};

template <int dimX>
struct tensor_traits<0, dimX>
{
  template <typename NUMBER>
  using value_type = NUMBER;
};

template <typename F>
struct extract_value_type
{
  typedef typename F::value_type type;
};

// ----------------------------------------------------------------------
// tensor number traits
template <typename T>
struct tensor_number_traits
{
  typedef typename boost::mpl::eval_if_c<boost::is_scalar<T>::value || boost::is_complex<T>::value,
                                         boost::mpl::identity<T>,
                                         extract_value_type<T> >::type type;
};

}  // end namespace boltzmann


#include <gtest/gtest.h>
#include <ros/ros.h>
#include <ros/callback_queue.h>
#include "std_msgs/Int32.h"
#include "std_msgs/Float64.h"
#include <nav_msgs/Odometry.h>
#include <tf2_ros/transform_listener.h>
#include <boost/function.hpp>
#include <cmath>

TEST(TreadDistanceNode, Basic)
{

    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener tfListener(tfBuffer);

    double wheel_span, wheelRadius, ticksPerRevolution, drivebaseOdomRate, drivebaseOdomPeriod;
    ros::param::param<double>("/tread_distance_publisher/wheelRadius", wheelRadius, -1);
    ros::param::param<double>("/tread_distance_publisher/ticksPerRevolution", ticksPerRevolution, -1);
    ros::param::param<double>("/drivebase_odom_publisher/wheel_span", wheel_span, -1);
    ros::param::param<double>("/drivebase_odom_publisher/rate", drivebaseOdomRate, -1);

    drivebaseOdomPeriod = 1/drivebaseOdomRate;

    ros::CallbackQueue leftTreadCBQueue;
    ros::NodeHandle leftTreadNH;
    leftTreadNH.setCallbackQueue(&leftTreadCBQueue);
    ros::Publisher leftTreadCountPublisher = leftTreadNH.advertise<std_msgs::Int32>("/left_tread_count", 15);
    double leftTreadDistance;
    boost::function<void(const std_msgs::Float64&)> leftTreadCallback = [&leftTreadDistance](const std_msgs::Float64& msg) {
        leftTreadDistance = msg.data;
    };
    auto leftTreadCountSub = leftTreadNH.subscribe<std_msgs::Float64>("/left_tread_distance", 10, leftTreadCallback);

    ros::CallbackQueue rightTreadCBQueue;
    ros::NodeHandle rightTreadNH;
    rightTreadNH.setCallbackQueue(&rightTreadCBQueue);
    ros::Publisher rightTreadCountPublisher = rightTreadNH.advertise<std_msgs::Int32>("/right_tread_count", 15);

    ros::CallbackQueue drivebaseOdomCBQueue;
    ros::NodeHandle drivebaseOdomNH;
    drivebaseOdomNH.setCallbackQueue(&drivebaseOdomCBQueue);
    geometry_msgs::PoseWithCovariance drivebasePose;
    geometry_msgs::TwistWithCovariance drivbaseTwist;
    boost::function<void(const nav_msgs::Odometry&)> drivebaseOdomCallback = [&drivebasePose, &drivbaseTwist](const nav_msgs::Odometry& msg) {
        drivebasePose = msg.pose;
        drivbaseTwist = msg.twist;
    };

    auto odometrySub = drivebaseOdomNH.subscribe<nav_msgs::Odometry>("/drivebase_odom", 10, drivebaseOdomCallback);

    //wait for nodes to subscribe
    while(ros::ok() && (
        leftTreadCountPublisher.getNumSubscribers() < 1 ||
        rightTreadCountPublisher.getNumSubscribers() < 1)){}
    ROS_DEBUG("Subscribers Connected");

    std_msgs::Int32 treadCountMsg;

    auto waitAndProcessOneMessageCallbackQueue = [](ros::CallbackQueue & cb){
        for (auto result=cb.callOne(); ros::ok() && result != ros::CallbackQueue::CallOneResult::Called; result=cb.callOne()){
            ROS_DEBUG("Queue Empty? %d", cb.empty());
            ROS_DEBUG("Queue Result %d", result);
            ros::Duration(0.1).sleep();
        }
    };

    //give first counts so drivebase_odom has distances to setup with
    treadCountMsg.data=0;
    leftTreadCountPublisher.publish(treadCountMsg);
    rightTreadCountPublisher.publish(treadCountMsg);
    ros::Duration(2*drivebaseOdomPeriod).sleep();

    waitAndProcessOneMessageCallbackQueue(leftTreadCBQueue);
    EXPECT_DOUBLE_EQ(leftTreadDistance, 0);

    //check drivebase is saying we aren't moving
    drivebaseOdomCBQueue.callAvailable();
    waitAndProcessOneMessageCallbackQueue(drivebaseOdomCBQueue);
    EXPECT_DOUBLE_EQ(drivebasePose.pose.position.x, 0);
    EXPECT_DOUBLE_EQ(drivebasePose.pose.position.y, 0);

    //forward a 1/2 revolution
    treadCountMsg.data=(ticksPerRevolution/2);
    leftTreadCountPublisher.publish(treadCountMsg);
    rightTreadCountPublisher.publish(treadCountMsg);
    ros::Duration(2*drivebaseOdomPeriod+.01).sleep();

    waitAndProcessOneMessageCallbackQueue(leftTreadCBQueue);
    EXPECT_DOUBLE_EQ(leftTreadDistance, M_PI*wheelRadius);

    drivebaseOdomCBQueue.callAvailable();
    waitAndProcessOneMessageCallbackQueue(drivebaseOdomCBQueue);
    EXPECT_DOUBLE_EQ(drivebasePose.pose.position.x, M_PI*wheelRadius);
    EXPECT_DOUBLE_EQ(drivebasePose.pose.position.y, 0);

    //forward 1 + 1/2 revolutions
    treadCountMsg.data=(ticksPerRevolution*2);
    leftTreadCountPublisher.publish(treadCountMsg);
    rightTreadCountPublisher.publish(treadCountMsg);
    ros::Duration(2*drivebaseOdomPeriod+.01).sleep();

    waitAndProcessOneMessageCallbackQueue(leftTreadCBQueue);
    EXPECT_DOUBLE_EQ(leftTreadDistance, 3*M_PI*wheelRadius);

    drivebaseOdomCBQueue.callAvailable();
    waitAndProcessOneMessageCallbackQueue(drivebaseOdomCBQueue);
    EXPECT_DOUBLE_EQ(drivebasePose.pose.position.x, 4*M_PI*wheelRadius);
    EXPECT_DOUBLE_EQ(drivebasePose.pose.position.y, 0);

    // back a revolution
    treadCountMsg.data=(ticksPerRevolution);
    leftTreadCountPublisher.publish(treadCountMsg);
    rightTreadCountPublisher.publish(treadCountMsg);
    ros::Duration(2*drivebaseOdomPeriod+.01).sleep();

    waitAndProcessOneMessageCallbackQueue(leftTreadCBQueue);
    EXPECT_DOUBLE_EQ(leftTreadDistance, -2*M_PI*wheelRadius);

    drivebaseOdomCBQueue.callAvailable();
    waitAndProcessOneMessageCallbackQueue(drivebaseOdomCBQueue);
    EXPECT_DOUBLE_EQ(drivebasePose.pose.position.x, 2*M_PI*wheelRadius);
    EXPECT_DOUBLE_EQ(drivebasePose.pose.position.y, 0);
}

int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    ros::init(argc, argv, "drivebase_integration_test");
    return RUN_ALL_TESTS();
}


#include "opencv2/core/core.hpp"
#include "opencv2/contrib/contrib.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/objdetect/objdetect.hpp"
#include <boost/algorithm/string.hpp>
#include <stdlib.h>
#include <vector>
#include <string>
#include <assert.h>
#include <iostream>
#include <fstream>
#include <stdio.h>
#include <iterator>
#include <sstream>

const int IM_WIDTH = 100;
const int IM_HEIGHT = 100;

// Get frame from the camera
IplImage* get_camera_frame(CvCapture* &camera)
{
	IplImage *frame;
	frame = cvQueryFrame(camera);
	if (!frame) {
		std::cout << "Couldn't get a frame \n";
		exit(1);
	}
	return frame;
}

// NCC classifier
// Where x is the value to compare, m is the points to compare and names is the labels of the points, max_thresh is the maximum threhold
// Returns the label of the point with the nearest center
std::string ncc(const cv::Mat &x, const cv::Mat &m, const std::vector<std::string> &names, const float &max_thresh = 2*std::pow(10,20)){

    double v_min, v_max;
    int idx_min[2] = {255,255}, idx_max[2] = {255, 255};
    //each row is a result
    cv::Mat d = cv::Mat::zeros(m.cols, 1, CV_32F);

    // Hypothetically this should be a single operation, but I'm hoping that C++ is fast enough that I won't notice.
    //std::cout << "FOR" << std::endl;
    for(int i = 0; i < m.cols; i++)
    {
        //std::cout << "x shape " << x.t().size() << " m shape " << m.col(i).size() << std::endl;
        d.row(i) = (x.t() * m.col(i) - 0.5*m.col(i).t()*m.col(i));
    }
    //std::cout << "d" << d << std::endl;
    //std::cout << "MAX" << std::endl;
    cv::minMaxIdx(d, &v_min, &v_max, idx_min, idx_max);
    //std::cout << "idx_min=" << idx_min[0] << ", idx_max=" << idx_max[0] << std::endl;

    if(max_thresh < v_max)
    {
        //std::cout << "accessing index " << idx_max[0] << " names length " << names.size() << std::endl;
        std::cout << "Would have returned " << names[idx_max[0]] << " but v_max = " << v_max << std::endl;
        return "unknown";
    }else{
        return names[idx_max[0]];
    }
}
// The main function
int main(int argc, const char* argv[])
{
std::cout << "Start\n";

    // Load the training data
    //std::ifstream infile("./py_unit_test/hist_sean.txt");
    std::ifstream infile("./py_unit_test/hist_sean.txt");
    int number_of_eigenvectors, number_of_people;
    infile >> number_of_eigenvectors;
    std::cout << "K:" << number_of_eigenvectors << std::endl;

    // Read in the eigenvectors
    std::vector<std::string> temp_vector;
    std::vector<float> into_mat;
    std::vector<float> into_avg;
    std::string temp;

    for(int i = 0; i < 2; i++)
    {
        // Apparently I could use an vector::iterator here instead of a for loop, but I don't know what the motivation behind doing that is
        infile >> temp;
        boost::split(temp_vector, temp, boost::is_any_of(","));
        for(int j = 0; j < temp_vector.size(); j++)
        {
            if(i != 0){
                into_mat.push_back(::atof(temp_vector[j].c_str()));
            }else{
                into_avg.push_back(::atof(temp_vector[j].c_str()));
            }
        }
    }
    cv::Mat face_avg = cv::Mat(into_avg, CV_32F);
    cv::Mat eigenvectors = cv::Mat(into_mat, CV_32F);
    std::cout << "face_avg shape " << face_avg.size() << std::endl;
    std::cout << "eigenvectors shape " << eigenvectors.size() << std::endl;
    std::cout << into_mat.size()/number_of_eigenvectors << "," << number_of_eigenvectors << std::endl;
    eigenvectors = eigenvectors.reshape(1, into_mat.size()/number_of_eigenvectors);
    //Okay, if reshaping is failing, maybe it's time to change how the configuration file is loaded so this shit doesn't happen? // Or we could do the projection by hand, so that OpenCV doesn't have a hissy fit? // Or we could just re-write the whole thing in Python?
    //eigenvectors = eigenvectors.reshape(10000, 5);
    //std::cout << "eigenvectors shape " << eigenvectors.size() << std::endl;
    //std::cout << eigenvectors << std::endl;

    infile >> number_of_people;
    // Read in the people locations
    into_mat.clear();

    infile >> temp;
    //std::cout << temp << std::endl;
    boost::split(temp_vector, temp, boost::is_any_of(","));
    for(int j = 0; j < temp_vector.size(); j++)
    {
        into_mat.push_back(::atof(temp_vector[j].c_str()));
    }

    cv::Mat people = cv::Mat(into_mat, CV_32F);
    //std::cout << "People! " << (people.type() == CV_32F) << std::endl;
    // Each column is a person
    people = people.reshape(1, into_mat.size()/number_of_people);
    std::cout << "people size: " << people.size() <<std::endl;

    // Read in the people names
    std::vector<std::string> names;
    infile >> temp;
    boost::split(names, temp, boost::is_any_of(","));

    // Load the face detection using the HAAR classifiers
    cv::CascadeClassifier face_cascade;
    bool file_result = face_cascade.load("/opt/ros/groovy/share/OpenCV/haarcascades/haarcascade_frontalface_alt.xml");
    std::cout << "Did we find the file? " << file_result << std::endl;

	cv::VideoCapture cap(0);
	// Wait a little, so that the camera can auto-adjust its brightness.
	sleep(1);

	cv::Mat current_frame;
	// Quit on "Escape" key.
	while (cvWaitKey(10) != 27) {
		cap >> current_frame;
		cv::Mat original = current_frame.clone();
		cv::Mat gray;
		// Convert to grayscale
		cvtColor(original, gray, CV_BGR2GRAY);
		std::vector< cv::Rect_<int> > faces;
		// Use HAAR to get all the features
		face_cascade.detectMultiScale(gray, faces);

		// iterate through the faces
		for(int i = 0; i < faces.size(); i++){
			cv::Rect face = faces[i];
			// crop the face
			cv::Mat cropped_face = gray(face);
			// resize the face
			cv::Mat resized_face;
			cv::resize(cropped_face, resized_face, cv::Size(IM_WIDTH, IM_HEIGHT), 1.0, 1.0, cv::INTER_CUBIC);

			// NOTE THIS WILL ONLY WORK FOR NON-ROTATED FACES

            // So first we need to project the AVERAGED face into the EigenSpace
            cv::Mat hist_face;
            cv::equalizeHist(resized_face, hist_face);
            //resized_face = hist_face;

            resized_face = resized_face.reshape(1,IM_HEIGHT*IM_WIDTH);
            cv::Mat resized_face_f;
            resized_face.convertTo(resized_face_f, CV_32F);
            //std::cout << "subtract" << std::endl;
            //std::cout << "resized_face shape " << resized_face_f.size() << " face_avg shape " << face_avg.size() << std::endl;
            resized_face_f -= face_avg;
            //std::cout << "multiply" << std::endl;
            //std::cout << "resized_face shape " << resized_face_f.size() << " eigenvectors shape " << eigenvectors.size() << std::endl;
            //std::cout << "resized_face type " << (resized_face_f.type() == CV_32F) << " eigenvectors type " << (eigenvectors.type() == CV_32F) << std::endl;
            resized_face_f = resized_face_f.t() * eigenvectors;
			// then we just compare the centers
			std::string box_text = ncc(resized_face_f.t(), people, names);

			rectangle(original, face, CV_RGB(0, 255, 0), 1);
			int pos_x = std::max(face.tl().x - 10, 0);
			int pos_y = std::max(face.tl().y - 10, 0);
			cv::putText(original, box_text, cv::Point(pos_x, pos_y), cv::FONT_HERSHEY_PLAIN, 1.0, CV_RGB(0, 255, 0), 2.0);
		}
		imshow("face_recognizer", original);
	}

	std::cout << "Done\n";
	return 0;
}


//moveaxis.cpp
/*
* It moves axis in X and/or Y directions (the axis is Z direction)
* (C) 2006 olegabr. All rights reserved.
*/

#include <Magick++.h>
#include <limits>
#include <iostream>
using std::cout; using std::endl;

#include <boost/filesystem/operations.hpp>
namespace fs = boost::filesystem;

#include <boost/lexical_cast.hpp>

namespace {
	void usage()
	{
		std::cout << "moveaxis xvalue yvalue start_angle increment_angle images_directory\n";
	}
}

int main(int argc, char* argv[])
{
	if (argc < 4)
	{
		usage();
		return 0;
	}
	try {
		Magick::InitializeMagick(argv[0]);

		double dXOff = boost::lexical_cast<double>(argv[1]);
		double dYOff = boost::lexical_cast<double>(argv[2]);

		double dAngleStart = 3.1415 * boost::lexical_cast<double>(argv[3]) / 180;
		double dAngleIncr = 3.1415 * boost::lexical_cast<double>(argv[4]) / 180;

		std::string dir_name(argv[argc - 1]);
		fs::path dir_path(dir_name, fs::native);
		if (!exists( dir_path ))
			throw std::runtime_error("The directory '" + dir_name + "' does't exists.");

		fs::path result_path(dir_path/fs::path("axis_moved"));
		if (!fs::exists(result_path)) fs::create_directory(result_path);

		fs::directory_iterator itr(dir_path), end_itr;
		Magick::Image img;
		double dAngle = dAngleStart;
		for (; itr != end_itr; ++itr, dAngle += dAngleIncr) {
			if ( !is_directory( *itr ) )
			{
				cout << "Open image file named: " << complete(*itr).native_file_string() << "\n";
				img.read(complete(*itr).native_file_string());

				//cout << "dAngle = " << dAngle << "\n";

				//cout << "std::cos(dAngle) = " << std::cos(dAngle) << "\n";
				//cout << "std::sin(dAngle) = " << std::sin(dAngle) << "\n";

				//cout << "dXOff * std::cos(dAngle) = " << dXOff * std::cos(dAngle) << "\n";
				//cout << "dYOff * std::sin(dAngle) = " << dYOff * std::sin(dAngle) << "\n";

				//int nXMove = static_cast<int>(dXOff * std::cos(dAngle));
				//int nYMove = static_cast<int>(dYOff * std::sin(dAngle));

				//unsigned int xmove = nXMove > 0 ? nXMove : -nXMove;
				//bool bXNegative = nXMove > 0 ? false : true;
				//unsigned int ymove = nYMove > 0 ? nYMove : -nYMove;
				//bool bYNegative = nYMove > 0 ? false : true;

				//cout << "xmove = " << xmove << "\n";
				//cout << "ymove = " << ymove << "\n";

				//Magick::Geometry geom(img.columns(), img.rows(), xmove, ymove, bXNegative, bYNegative);
				//img.transform(geom);

				Magick::DrawableAffine aff(1.0, 1.0, 1.0, 1.0, dXOff * std::cos(dAngle), dYOff * std::sin(dAngle));
				img.affineTransform(aff);

				img.type(Magick::GrayscaleType);
				img.write(complete(result_path/fs::path(std::string("moved_")+itr->leaf())).native_file_string());
			}
		}
	}
	catch (Magick::Exception &ex) {
		cout << ex.what() << endl;
	}
	catch (std::exception &e) {
		cout << e.what() << endl;
	}
	catch (...) {
		cout << "Unknown exception occured." << endl;
	}

	return 0;
}


//==============================================================================
//         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_IEEE_FUNCTIONS_SCALAR_EPS_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_EPS_HPP_INCLUDED
#include <boost/simd/ieee/functions/eps.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mindenormal.hpp>
#include <boost/simd/include/functions/scalar/is_not_finite.hpp>
#include <boost/simd/include/functions/scalar/fast_ldexp.hpp>
#include <boost/simd/include/functions/scalar/exponent.hpp>
#include <boost/simd/include/functions/scalar/abs.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::eps_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const &)const { return One<A0>(); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::eps_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef std::numeric_limits<A0> lim;
      const A0 a = boost::simd::abs(a0);
      if (is_not_finite(a))
      {
        return Nan<A0>();
      }
      else if (a < lim::min())
      {
        return Mindenormal<A0>();
      }
      else
      {
        return boost::simd::fast_ldexp(One<A0>(), exponent(a)-lim::digits+1);
        // TODO this can surely be made speedier by computing it directly
        // as One is a constant
      }
    }
  };
} } }

#endif


//---------------------------------------------------------------------------//
//!
//! \file   Utility_TranslationSphericalSpatialCoordinateConversionPolicy.hpp
//! \author Alex Robinson
//! \brief  Translation spherical spatial coordinate conversion policy decl.
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_TRANSLATION_SPHERICAL_SPATIAL_COORDINATE_CONVERSION_POLICY_HPP
#define UTILITY_TRANSLATION_SPHERICAL_SPATIAL_COORDINATE_CONVERSION_POLICY_HPP

// Boost Includes
#include <boost/serialization/array_wrapper.hpp>

// FRENSIE Includes
#include "Utility_SphericalSpatialCoordinateConversionPolicy.hpp"

namespace Utility{

//! The translation spherical coordinate conversion policy class
class TranslationSphericalSpatialCoordinateConversionPolicy : public SphericalSpatialCoordinateConversionPolicy
{

public:

  //! Constructor
  TranslationSphericalSpatialCoordinateConversionPolicy(
                                                      const double origin[3] );

  //! Destructor
  ~TranslationSphericalSpatialCoordinateConversionPolicy()
  { /* ... */ }

  //! Convert the spatial coordinates to cartesian coordinates
  void convertToCartesianSpatialCoordinates(
                                      const double primary_spatial_coord,
                                      const double secondary_spatial_coord,
                                      const double tertiary_spatial_coord,
                                      double& x_spatial_coord,
                                      double& y_spatial_coord,
                                      double& z_spatial_coord ) const override;

  //! Convert the cartesian coordinates to the spatial coordinate system
  void convertFromCartesianSpatialCoordinates(
                               const double x_spatial_coord,
                               const double y_spatial_coord,
                               const double z_spatial_coord,
                               double& primary_spatial_coord,
                               double& secondary_spatial_coord,
                               double& tertiary_spatial_coord ) const override;

  //! Convert the spatial coordinates to cartesian coordinates
  using SphericalSpatialCoordinateConversionPolicy::convertToCartesianSpatialCoordinates;

  //! Convert the cartesian coordinates to the spatial coordinate system
  using SphericalSpatialCoordinateConversionPolicy::convertFromCartesianSpatialCoordinates;

private:

  // The default constructor should not be used - if the origin corresponds to
  // the global coordinate system (0,0,0) use the basic conversion policy
  TranslationSphericalSpatialCoordinateConversionPolicy()
  { /* ... */ }

  // We have C-arrays as members - hide the copy constructor and assignment
  // operator
  TranslationSphericalSpatialCoordinateConversionPolicy( const TranslationSphericalSpatialCoordinateConversionPolicy& that );
  TranslationSphericalSpatialCoordinateConversionPolicy& operator=( const TranslationSphericalSpatialCoordinateConversionPolicy& that );

  // Save the policy to an archive
  template<typename Archive>
  void save( Archive& ar, const unsigned version ) const;

  // Load the policy from an archive
  template<typename Archive>
  void load( Archive& ar, const unsigned version );

  BOOST_SERIALIZATION_SPLIT_MEMBER();

  // Declare the boost serialization access object as a friend
  friend class boost::serialization::access;

  // The origin of the local spherical coordinate system w.r.t. the global
  // Cartesian coordinate system
  double d_origin[3];
};

// Save the policy to an archive
template<typename Archive>
void TranslationSphericalSpatialCoordinateConversionPolicy::save( Archive& ar, const unsigned version ) const
{
  // Save the base class
  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP( SphericalSpatialCoordinateConversionPolicy );

  // Save the local data
  ar & boost::serialization::make_nvp( "d_origin", boost::serialization::make_array( d_origin, 3 ) );
}

// Load the policy from an archive
template<typename Archive>
void TranslationSphericalSpatialCoordinateConversionPolicy::load( Archive& ar, const unsigned version )
{
  // Load the base class
  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP( SphericalSpatialCoordinateConversionPolicy );

  // Load the local data
  ar & boost::serialization::make_nvp( "d_origin", boost::serialization::make_array( d_origin, 3 ) );
}

} // end Utility namespace

BOOST_SERIALIZATION_CLASS_VERSION( TranslationSphericalSpatialCoordinateConversionPolicy, Utility, 0 );
BOOST_SERIALIZATION_CLASS_EXPORT_STANDARD_KEY( TranslationSphericalSpatialCoordinateConversionPolicy, Utility );
EXTERN_EXPLICIT_CLASS_SAVE_LOAD_INST( Utility, TranslationSphericalSpatialCoordinateConversionPolicy );

#endif // end UTILITY_TRANSLATION_SPHERICAL_SPATIAL_COORDINATE_CONVERSION_POLICY_HPP

//---------------------------------------------------------------------------//
// end Utility_TranslationSphericalSpatialCoordinateConversionPolicy.hpp
//---------------------------------------------------------------------------//


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

#include "calculate_additional_lines.hpp"

#include <boost/assign/std/vector.hpp>
#include <boost/format.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/numeric.hpp>

#include <core/io/read_pla_to_bdd.hpp>
#include <core/utils/timer.hpp>

#include <reversible/synthesis/dd_synthesis_p.hpp>
#include <reversible/synthesis/synthesis_utils_p.hpp>

#include <cuddInt.h>
#include <gmpxx.h>

using namespace boost::assign;

namespace cirkit
{

void count_output_pattern_recurse( DdManager* mgr, DdNode* node, unsigned num_inputs, unsigned num_outputs,
                                   const std::string& pattern, unsigned depth,
                                   std::vector<mpz_class>& counts, std::vector<std::string>& patterns,
                                   bool verbose )
{
  if ( Cudd_IsConstant( node ) ) { return; }

  if ( depth == num_outputs )
  {
    if ( verbose )
    {
      std::cout << pattern << " has " << Cudd_CountMinterm( mgr, node, num_inputs ) << std::endl;
    }
    patterns += pattern;
    counts += mpz_class( Cudd_CountMinterm( mgr, node, num_inputs ) );
  }
  else
  {
    count_output_pattern_recurse( mgr, Cudd_Regular( cuddT( node ) ), num_inputs, num_outputs, pattern + "1", depth + 1u, counts, patterns, verbose );
    count_output_pattern_recurse( mgr, Cudd_Regular( cuddE( node ) ), num_inputs, num_outputs, pattern + "0", depth + 1u, counts, patterns, verbose );
  }
}

unsigned calculate_additional_lines( const std::string& filename, properties::ptr settings, properties::ptr statistics )
{
  /* Settings */
  auto verbose        = get( settings, "verbose",        false         );
  auto dotname        = get( settings, "dotname",        std::string() );
  auto dumpadd        = get( settings, "dumpadd",        false         );
  auto explicit_zeros = get( settings, "explicit_zeros", false         );

  /* Timer */
  properties_timer t( statistics );

  BDDTable bdd;
  read_pla_to_characteristic_bdd( bdd, filename, false, explicit_zeros );

  std::vector<mpz_class>   counts;
  std::vector<std::string> patterns;

  DdNode *add = Cudd_BddToAdd( bdd.cudd, bdd.outputs.at( 0u ).second );
  Cudd_Ref( add );

  if ( !dotname.empty() )
  {
    FILE * fp = fopen( dotname.c_str(), "w" );
    char ** inames = new char*[bdd.inputs.size()];
    boost::transform( bdd.inputs, inames, []( const std::pair<std::string, DdNode*>& p ) { return const_cast<char*>( p.first.c_str() ); } );
    char* onames[] = { const_cast<char*>( "f" ) };
    if ( dumpadd )
    {
      Cudd_DumpDot( bdd.cudd, 1, &add, inames, onames, fp );
    }
    else
    {
      Cudd_DumpDot( bdd.cudd, 1, &bdd.outputs.at( 0u ).second, inames, onames, fp );
    }
    fclose( fp );
  }

  unsigned n = bdd.inputs.size() - *bdd.num_real_outputs;
  unsigned m = *bdd.num_real_outputs;

  count_output_pattern_recurse( bdd.cudd, add, n, m, "", 0u, counts, patterns, verbose );

  if ( verbose )
  {
    std::cout << "total: " << boost::accumulate( counts, mpz_class( 0u ), []( mpz_class d1, mpz_class d2 ) { return d1 + d2; } ) << std::endl;
    std::cout << "pow2(n): " << pow2(n) << std::endl;
  }
  counts += pow2( n ) - boost::accumulate( counts, mpz_class( 0u ), []( mpz_class d1, mpz_class d2 ) { return d1 + d2; } );
  if ( verbose )
  {
    std::cout << "max: " << *boost::max_element( counts ) << std::endl;
  }

  /* Statistics */
  set( statistics, "num_inputs", n );
  set( statistics, "num_outputs", m );
  set( statistics, "patterns", patterns );

  return calculate_required_lines( n, m, *boost::max_element( counts ) ) - n;
}

unsigned calculate_additional_lines( const bdd_function_t& bdd, properties::ptr settings, properties::ptr statistics )
{
  /* Settings */
  const auto verbose = get( settings, "verbose", false );

  /* Timer */
  properties_timer t( statistics );

  const auto cf = compute_characteristic( bdd, false );
  const auto add = cf.second[0].Add();

  std::vector<mpz_class> counts;
  std::vector<std::string> patterns;
  const unsigned n = bdd.first.ReadSize();
  const unsigned m = bdd.second.size();

  count_output_pattern_recurse( cf.first.getManager(), add.getNode(), n, m, "", 0u, counts, patterns, verbose );

  /* statistics */
  set( statistics, "num_inputs", n );
  set( statistics, "num_outputs", m );
  set( statistics, "patterns", patterns );

  return calculate_required_lines( n, m, *boost::max_element( counts ) ) - n;
}

}

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


//==============================================================================
//         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
//==============================================================================
//////////////////////////////////////////////////////////////////////////////
// cover test behavior of arithmetic components in scalar mode
//////////////////////////////////////////////////////////////////////////////

#include <nt2/arithmetic/include/functions/two_prod.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <vector>
#include <boost/fusion/include/std_pair.hpp>
#include <nt2/include/constants/valmin.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/include/functions/remainder.hpp>
#include <nt2/include/functions/idivround.hpp>
#include <nt2/sdk/meta/strip.hpp>

#include <nt2/sdk/unit/tests/cover.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/include/functions/idivround.hpp>
//#include <nt2/table.hpp>

NT2_TEST_CASE_TPL ( two_prod_real__1_0_1,  (float))
{

  using nt2::two_prod;
  using nt2::tag::two_prod_;
  typedef typename nt2::meta::call<two_prod_(T, T)>::type r_t;
  typedef typename nt2::meta::strip<typename boost::fusion::result_of::at_c<r_t,0>::type>::type r_t1;
  typedef typename nt2::meta::strip<typename boost::fusion::result_of::at_c<r_t,1>::type>::type r_t2;
  typedef double U;

  nt2::uint32_t NR = NT2_NB_RANDOM_TEST;
  std::vector<T> in1(NR), in2(NR);
  std::vector<r_t1> ref1(NR), out1(NR);
  std::vector<r_t2> ref2(NR), out2(NR), res1(NR);
  nt2::roll(in1, -1, 1);
  nt2::roll(in2, -1, 1);
  for(nt2::uint32_t i=0; i < NR ; ++i)
  {
    r_t r = two_prod(in1[i], in2[i]);
    out1[i] = r.first;
    out2[i] = r.second;
    ref1[i] = T(U(in1[i])*U(in2[i]));
    res1[i] = T(U(out1[i])+U(out2[i]));
  }

  NT2_TEST_ULP_EQUAL(ref1, out1, 0);
}

NT2_TEST_CASE_TPL ( two_prod_real__1_0_2,  (double))
{

  using nt2::two_prod;
  using nt2::tag::two_prod_;
  typedef typename nt2::meta::call<two_prod_(T, T)>::type r_t;
  typedef typename nt2::meta::strip<typename boost::fusion::result_of::at_c<r_t,0>::type>::type r_t1;
  typedef typename nt2::meta::strip<typename boost::fusion::result_of::at_c<r_t,1>::type>::type r_t2;
  typedef long double U;

  nt2::uint32_t NR = NT2_NB_RANDOM_TEST;
  std::vector<T> in1(NR), in2(NR);
  std::vector<r_t1> ref1(NR), out1(NR);
  std::vector<r_t2> ref2(NR), out2(NR), res1(NR);
  nt2::roll(in1, -1, 1);
  nt2::roll(in2, -1, 1);
  for(nt2::uint32_t i=0; i < NR ; ++i)
  {
    r_t r = two_prod(in1[i], in2[i]);
    out1[i] = r.first;
    out2[i] = r.second;
    ref1[i] = T(U(in1[i])*U(in2[i]));
    res1[i] = T(U(out1[i])+U(out2[i]));
  }

  NT2_TEST_ULP_EQUAL(ref1, out1, 0);
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    unsigned long long int k, a, b, sum = 1, i = 0; cin >> k >> a >> b;
    while (i <= k) {
        if (b - a <= 2) break;
        if (sum < a) {
            unsigned long long int tmp = (a - sum);
            sum += tmp, i += tmp;
        }
        else if (i + 2 <= k) sum += (b - a), i += 2;
        else {
            sum += (k - i);
            break;
        }
    }
    cout << max(1 + k, sum) << endl;
}


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Mat;
using arma::span;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenMatRowIndRangeColIndRangeGenDouble : public Expected {
    public:
      ExpectedInPlaceGenMatRowIndRangeColIndRangeGenDouble() {
        cout << "Compute ExpectedInPlaceGenMatRowIndRangeColIndRangeGenDouble(): " << endl;

          vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
            InputClass::GenMat,
            InputClass::RowIndRange,
            InputClass::ColIndRange,
            InputClass::GenDouble
          });

          for (vector<pair<string, void*>> input : inputs) {
            _fileSuffix = "";

            int n = 0;
            for (pair<string, void*> value : input) {
              switch (n) {
                case 0:
                  _fileSuffix += value.first;
                  _genMat = *static_cast<Mat<double>*>(value.second);
                  break;
                case 1:
                  _fileSuffix += "," + value.first;
                  _rowIndRange = *static_cast<span*>(value.second);
                  break;
                case 2:
                  _fileSuffix += "," + value.first;
                  _colIndRange = *static_cast<span*>(value.second);
                  break;
                case 3:
                  _fileSuffix += "," + value.first;
                  _genDouble = *static_cast<double*>(value.second);
                  break;
              }
              ++n;
            }

            cout << "Using input: " << _fileSuffix << endl;

            _copyOfGenMatA = _genMat;
            _copyOfRowIndRange = _rowIndRange;
            _copyOfGenMatB = _colIndRange;
            _copyOfGenDouble = _genDouble;

            expectedMatSubmatPlus();

            _genMat = _copyOfGenMatA;
            _rowIndRange = _copyOfRowIndRange;
            _colIndRange = _copyOfGenMatB;
            _genDouble = _copyOfGenDouble;
            expectedMatSubmatMinus();

            _genMat = _copyOfGenMatA;
            _rowIndRange = _copyOfRowIndRange;
            _colIndRange = _copyOfGenMatB;
            _genDouble = _copyOfGenDouble;
            expectedMatSubmatTimes();

            _genMat = _copyOfGenMatA;
            _rowIndRange = _copyOfRowIndRange;
            _colIndRange = _copyOfGenMatB;
            _genDouble = _copyOfGenDouble;
            expectedMatSubmatDivide();
          }

          cout << "done." << endl;
        }

    protected:
      Mat<double> _genMat;
      Mat<double> _copyOfGenMatA;

      span _rowIndRange;
      span _copyOfRowIndRange;

      span _colIndRange;
      span _copyOfGenMatB;

      double _genDouble;
      double _copyOfGenDouble;

      void expectedMatSubmatPlus() {
        if(!_genMat.in_range(_rowIndRange, _colIndRange)) {
          return;
        }

        cout << "- Compute expectedMatSubmatPlus() ... ";

        _genMat.submat(_rowIndRange, _colIndRange) += _genDouble;
        save<double>("Mat.submatPlus", _genMat);

        cout << "done." << endl;
      }

      void expectedMatSubmatMinus() {
        if(!_genMat.in_range(_rowIndRange, _colIndRange)) {
          return;
        }

        cout << "- Compute expectedMatSubmatMinus() ... ";

        _genMat.submat(_rowIndRange, _colIndRange) -= _genDouble;
        save<double>("Mat.submatMinus", _genMat);

        cout << "done." << endl;
      }

      void expectedMatSubmatTimes() {
        if(!_genMat.in_range(_rowIndRange, _colIndRange)) {
          return;
        }

        cout << "- Compute expectedMatSubmatTimes() ... ";

        _genMat.submat(_rowIndRange, _colIndRange) *= _genDouble;
        save<double>("Mat.submatTimes", _genMat);

        cout << "done." << endl;
      }

      void expectedMatSubmatDivide() {
        if(!_genMat.in_range(_rowIndRange, _colIndRange)) {
          return;
        }

        cout << "- Compute expectedMatSubmatDivide() ... ";

        _genMat.submat(_rowIndRange, _colIndRange) /= _genDouble;
        save<double>("Mat.submatDivide", _genMat);

        cout << "done." << endl;
      }
  };
}


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

#include <boost/math/bindings/rr.hpp>
#include <boost/math/tools/test_data.hpp>
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/math/special_functions/ellint_1.hpp>
#include <fstream>
#include <boost/math/tools/test_data.hpp>
#include <boost/tr1/random.hpp>

using namespace boost::math::tools;
using namespace boost::math;
using namespace std;

float extern_val;
// confuse the compilers optimiser, and force a truncation to float precision:
float truncate_to_float(float const * pf)
{
   extern_val = *pf;
   return *pf;
}

template<class T>
T ellint_f_data(T phi, T k)
{
   return ellint_1(k, phi);
}

int test_main(int argc, char*argv [])
{
   using namespace boost::math::tools;

   boost::math::ntl::RR::SetOutputPrecision(50);
   boost::math::ntl::RR::SetPrecision(1000);

   parameter_info<boost::math::ntl::RR> arg1, arg2;
   test_data<boost::math::ntl::RR> data;

   bool cont;
   std::string line;

   if(argc < 1)
      return 1;

   do{
      if(0 == get_user_parameter_info(arg1, "phi"))
         return 1;
      if(0 == get_user_parameter_info(arg2, "k"))
         return 1;

      data.insert(&ellint_f_data<boost::math::ntl::RR>, arg1, arg2);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=ellint_f.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "ellint_f.ipp";
   std::ofstream ofs(line.c_str());
   line.erase(line.find('.'));
   ofs << std::scientific;
   write_code(ofs, data, line.c_str());

   return 0;
}


/*
@copyright Louis Dionne 2014
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/hana/integral.hpp>

#include <boost/hana/detail/assert.hpp>
using namespace boost::hana;

int main() {
    // Integral == other
    BOOST_HANA_CONSTEXPR_ASSERT(equal(int_<0>, int{0}));
    BOOST_HANA_CONSTEXPR_ASSERT(!equal(int_<0>, int{1}));

    BOOST_HANA_CONSTEXPR_ASSERT(equal(int_<0>, long{0}));
    BOOST_HANA_CONSTEXPR_ASSERT(!equal(int_<1>, long{0}));

    BOOST_HANA_CONSTEXPR_ASSERT(equal(int_<0>, float{0}));
    BOOST_HANA_CONSTEXPR_ASSERT(!equal(int_<1>, float{0}));

    // other == Integral
    BOOST_HANA_CONSTEXPR_ASSERT(equal(int{0}, int_<0>));
    BOOST_HANA_CONSTEXPR_ASSERT(!equal(int{1}, int_<0>));

    BOOST_HANA_CONSTEXPR_ASSERT(equal(long{0}, int_<0>));
    BOOST_HANA_CONSTEXPR_ASSERT(!equal(long{1}, int_<0>));

    BOOST_HANA_CONSTEXPR_ASSERT(equal(float{0}, int_<0>));
    BOOST_HANA_CONSTEXPR_ASSERT(!equal(float{1}, int_<0>));
}


//////////////////////////////////////////////////////////////////////////////
///   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
//////////////////////////////////////////////////////////////////////////////
#define NT2_UNIT_MODULE "nt2 polynomials toolbox - unit/simd Mode"

#include <nt2/toolbox/polynomials/include/tchebeval.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/simd/native.hpp>
#include <nt2/sdk/memory/is_aligned.hpp>
#include <nt2/sdk/memory/aligned_type.hpp>
#include <nt2/sdk/memory/load.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/include/functions/random.hpp>
#include <nt2/include/functions/ulpdist.hpp>
#include <boost/array.hpp>
#include <iostream>

//////////////////////////////////////////////////////////////////////////////
// Test behavior of arithmetic components using NT2_TEST_CASE
//////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE_TPL(tchebeval, NT2_REAL_TYPES )
{
 using nt2::tchebeval;
 using nt2::tag::tchebeval_;
 using nt2::load;
 using nt2::simd::native;
 using nt2::meta::cardinal_of;

 typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
 typedef native<T,ext_t>             n_t;
 typedef typename nt2::meta::as_real<T>::type rT;
 typedef native<rT,ext_t>             rn_t;
 typedef typename nt2::meta::call<tchebeval_(n_t, boost::array<rT,4> )>::type call_type;

 static const boost::array<T, 4 > A = {{ 1, 2, 3, 4 }};
 NT2_TEST( (boost::is_same<call_type, rn_t>::value) );
 NT2_ALIGNED_TYPE(T) data[1*cardinal_of<n_t>::value];
 double z, m = 0;
 for(int num = 0; num < 10; num++)
   {
     for(std::size_t i=0;i<1*cardinal_of<n_t>::value;++i){
       data[i] = nt2::random(0.0, 100.0); // good value here for tchebeval
     }
     n_t a0 = load<n_t>(&data[0],0);
     rn_t v  = tchebeval(a0, A);
     for(std::size_t j=0;j<cardinal_of<n_t>::value;++j)
       {
       NT2_TEST_LESSER_EQUAL( z = nt2::ulpdist(v[j], tchebeval(a0[j], A)), 1);
       if (z > m) m = z;
       }
   }
 std::cout << "ulp max = " << m << std::endl;
}