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

#include <fuse_constraints/normal_prior_pose_3d_cost_functor.h>
#include <pluginlib/class_list_macros.h>

#include <boost/serialization/export.hpp>
#include <ceres/autodiff_cost_function.h>
#include <Eigen/Dense>

#include <string>

namespace fuse_constraints
{

AbsolutePose3DStampedConstraint::AbsolutePose3DStampedConstraint(
  const std::string& source,
  const fuse_variables::Position3DStamped& position,
  const fuse_variables::Orientation3DStamped& orientation,
  const fuse_core::Vector7d& mean,
  const fuse_core::Matrix6d& covariance) :
    fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}),  // NOLINT(whitespace/braces)
    mean_(mean),
    sqrt_information_(covariance.inverse().llt().matrixU())
{
}

void AbsolutePose3DStampedConstraint::print(std::ostream& stream) const
{
  stream << type() << "\n"
         << "  source: " << source() << "\n"
         << "  uuid: " << uuid() << "\n"
         << "  position variable: " << variables().at(0) << "\n"
         << "  orientation variable: " << variables().at(1) << "\n"
         << "  mean: " << mean().transpose() << "\n"
         << "  sqrt_info: " << sqrtInformation() << "\n";
}

ceres::CostFunction* AbsolutePose3DStampedConstraint::costFunction() const
{
  return new ceres::AutoDiffCostFunction<NormalPriorPose3DCostFunctor, 6, 3, 4>(
    new NormalPriorPose3DCostFunctor(sqrt_information_, mean_));
}

}  // namespace fuse_constraints

BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePose3DStampedConstraint);
PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePose3DStampedConstraint, fuse_core::Constraint);


/*

Copyright (C) 1997,1998,1999,2000,2001  Franz Josef Och

mkcls - a program for making word classes .

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
USA.

*/

#include <stdlib.h>
#include <math.h>
#include "KategProblem.h"

#ifdef WIN32
#include <boost\math\special_functions\erf.hpp>
using namespace boost::math;
#endif

double rhoLo=0.75;
#define MAX_VERFAELSCHUNG 5000
double verfTab[MAX_VERFAELSCHUNG],verfTabSigma=-1.0;
double verfaelsche(int a,double b)
{

  if( a>=0&&verfTabSigma==b&&a<MAX_VERFAELSCHUNG )
    {

      massert(verfTab[a]== b*(erf(10000.0) - erf(a/b))/2+a);
      return verfTab[a];
    }
  else
    {
      double x = b*(erf(10000.0) - erf(a/b))/2+a;
      return x;
    }
}
double verfaelsche(double,double b)
{
  abort();
  return b;
}

KategProblemKBC::KategProblemKBC(int s,double sv) :
  _n(s),_n1(s,0),_n2(s,0),sigmaVerfaelschung(sv),withVerfaelschung(sv!=0.0),
  _nverf(s),_n1verf(s,0.0),_n2verf(s,0.0),_nWords(0),
  eta0(s*s),eta1(0),c1_0(s),c2_0(s),
  _bigramVerfSum(0.0),_unigramVerfSum1(0.0),_unigramVerfSum2(0.0),nKats(s)

{
  verfInit0=0.0;
  int i;
  if( withVerfaelschung )
    {
      verfInit0=verfaelsche(0,sv);
      cout << "VERFAELSCHUNG wird mitgefuehrt => LANGSAMER!!!\n";
    }
  for(i=0;i<s;i++)
    {
      _n[i].init(s,0);
      _nverf[i].init(s,verfInit0);
      _n1verf[i]=_n2verf[i]=verfInit0;
      _bigramVerfSum+=verfInit0*s;
      _unigramVerfSum1+=verfInit0;
      _unigramVerfSum2+=verfInit0;
    }
  if( withVerfaelschung )
    {
      cout << "VERFAELSCHUNG " << _bigramVerfSum << " " << _unigramVerfSum1 << " " << _unigramVerfSum2 << endl;
    }
  verfTabSigma=sigmaVerfaelschung;

}

void KategProblemKBC::setN(int w1,int w2, FreqType n)

{
  addN(w1,w2,-_n[w1][w2]);
  addN(w1,w2,n);
}

double KategProblemKBC::fullBewertung(int auswertung)
{

  double bewertung=0;
  int c1,c2;

  switch( auswertung )
    {
    case CRITERION_ML:
      for(c1=0;c1<nKats;c1++)
	{
	  for(c2=0;c2<nKats;c2++)
	    bewertung-=kat_h(_n[c1][c2]);
	  bewertung+=kat_h(_n1[c1])+kat_h(_n2[c1]);
	}
      break;
    case CRITERION_MY:
      {
      for(c1=0;c1<nKats;c1++)
	{
	  for(c2=0;c2<nKats;c2++)
	    bewertung-=mkat_h_full((int)n(c1,c2),nverf(c1,c2));
	  bewertung+=mkat_h_part((int)(n1(c1)),n1verf(c1))+mkat_h_part((int)(n2(c1)),n2verf(c1));
	}
      double u1=_unigramVerfSum1-verfInit0*c1_0;
      double u2=_unigramVerfSum2-verfInit0*c2_0;
      double b=_bigramVerfSum-verfInit0*(c1_0*nKats+c2_0*nKats-c1_0*c2_0);
      if( verboseMode>1 )
	{
	  cout << "CRITERION_MY: " << bewertung << endl;
	  cout << "U1:"<<_unigramVerfSum1 << " n:"<<u1<< " "
	       << "U2:"<<_unigramVerfSum2 << " n:"<<u2<< " "
	       << "U3:"<<_bigramVerfSum   << " n:"<<b<< endl;
	}
      if(b>0.000001)
	{

	  if(verboseMode>1 )
	    cout <<  "  NEU: " <<_nWords*log( u1 * u2 / b ) << endl;
	  bewertung -= _nWords*log( u1 * u2 / b );
	  if(verboseMode>1)
	    cout << "SCHLUSSBEWERTUNG: " << bewertung << endl;
	}
      else
	cout << "B zu klein " << b << endl;
      }
      break;
    case CRITERION_LO:
      for(c1=0;c1<nKats;c1++)
	{
	  for(c2=0;c2<nKats;c2++)
	    bewertung-=_n[c1][c2]*kat_mlog(_n[c1][c2]-1-rhoLo);
	  bewertung+=_n1[c1]*kat_mlog(_n1[c1]-1)+_n2[c1]*kat_mlog(_n2[c1]-1);
	}
      bewertung-=kat_etaFkt(eta0,eta1,(c1_0*nKats+c2_0*nKats-c1_0*c2_0),nKats);
      break;
    default:
      cerr << "Error: wrong criterion " << auswertung << endl;
      exit(1);
    }
  return bewertung;
}

double KategProblemKBC::myCriterionTerm()
{
  iassert( withVerfaelschung );
  double r;
      double u1=_unigramVerfSum1-verfInit0*c1_0;
      double u2=_unigramVerfSum2-verfInit0*c2_0;
      double b=_bigramVerfSum-verfInit0*(c1_0*nKats+c2_0*nKats-c1_0*c2_0);

  if( verboseMode>1 )
    {
      cout << "nwords divisor:"<<_nWords << " " << u1 * u2 / b << endl;
      cout << "ergebnis:      "<<_nWords*log( u1 * u2 / b ) << endl;
      cout << "0:             "<<c1_0 << endl;
    }
  r       = _nWords*log( u1 * u2 / b );

  return -r;
}

double KategProblemKBC::bigramVerfSum()
{
  double sum=0;
  for(int c1=0;c1<nKats;c1++)
    for(int c2=0;c2<nKats;c2++)
      sum+=nverf(c1,c2);
  cout << "BIGRAMVERFSUM: " << sum << endl;
  return sum;
}

double KategProblemKBC::unigramVerfSum1()
{
  double sum=0;
  for(int c1=0;c1<nKats;c1++)
    sum+=n1verf(c1);
  cout << "UNIGRAMVERFSUM1: " << sum << endl;
  return sum;
}

double KategProblemKBC::unigramVerfSum2()
{
  double sum=0;
  for(int c1=0;c1<nKats;c1++)
    sum+=n2verf(c1);
  cout << "UNIGRAMVERFSUM2: " << sum << endl;
 return sum;
}


//!
//! Contains common interface used by both gravity and magnetic forward models.
//!
//! \file fwdmodel/gravmag.hpp
//! \author Darren Shen
//! \author Alistair Reid
//! \date 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#pragma once

#include <Eigen/Dense>
#include "world/voxelise.hpp"

namespace obsidian
{
  namespace fwd
  {
    //! Structure containing gravity and magnetic interpolation parameters.
    //!
    struct GravmagInterpolatorParams
    {
      //! The indices of each sensor.
      Eigen::MatrixXi sensorIndices;

      //! The weights of each sensor.
      Eigen::MatrixXd sensorWeights;

      //! The locations of each sensor.
      Eigen::MatrixXd gridLocations;
    };

    //! Create a GravmagInterpolatorParams object.
    //!
    //! \param voxelisation The voxelised world.
    //! \param locations The sensor locations.
    //! \param worldSpec The world specifications.
    //!
    GravmagInterpolatorParams makeInterpParams(const VoxelSpec& voxelisation, const Eigen::MatrixXd& locations, const WorldSpec& worldSpec);

    //! Computes the field values for either gravity or magnetic.
    //!
    //! \param sens The gravity or magnetic sensitivity matrix.
    //! \param sensorIndices The indices of each of the sensors.
    //! \param sensorWeights The weights of each of the sensors.
    //! \param properties The rock property for the sensor type.
    //!
    Eigen::VectorXd computeField(const Eigen::MatrixXd &sens, const Eigen::MatrixXi sensorIndices, const Eigen::MatrixXd sensorWeights,
                                 const Eigen::VectorXd &properties);

    namespace detail
    {
      //! A small number added to denominators to prevent them from being zero.
      //!
      const double EPS = 1e-12;

      //! Computes the sensitivity for either gravity or magnetic.
      //!
      //! \param xEdges, yEdges, zEdges The coordinates of the mesh grid in
      //!                               the x, y, and z directions.
      //! \param xField, yField, zField The geological field values at each of the
      //!                               mesh grid points.
      //! \param locations A Nx3 matrix containing the coordinates of each.
      //!                  sensor observation location.
      //! \param sensFunc The function to compute the sensitivity.
      //!
      Eigen::MatrixXd computeSensitivity(const Eigen::VectorXd &xEdges, const Eigen::VectorXd &yEdges, const Eigen::VectorXd &zEdges,
                                         const Eigen::VectorXd &xField, const Eigen::VectorXd &yField, const Eigen::VectorXd &zField,
                                         const Eigen::MatrixXd &locations,
                                         double (*sensFunc)(double, double, double, double, double, double));
    } // namespace detail
  } // namespace fwd
} // namespace obsidian


#include "defence.h"
#include "../Log.h"
#include "../Config.h"

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

int AntiRageRequiredShips(const GameState &state, const PlanetPtr& my_planet, const PlanetPtr& enemy_planet);

// Lock the required number of ships onto planets that are under attack
void Defence(GameState& state, Player player) {
    static bool defence = Config::Value<bool>("defence");
    if ( ! defence ) return;

    LOG("Defence phase");

    std::vector<PlanetPtr> my_planets = state.PlanetsOwnedBy(player);
    foreach ( PlanetPtr& p, my_planets ) {
        // TODO: IF this is an important planet then we must protect
        // Else we can run away if AFTER all order have been issued we are still
        // under attack

        int required_ships = p->RequiredShips();

        if ( required_ships > 0 ) {
            // TODO: This might be impacting the prediction so see if we can fix it
            //       (Idea send a zero day fleet)
            p->LockShips(required_ships);

            LOG( " Locking " << required_ships << " ships on planet " << p->id );
        }
    }
}

DefenceExclusions AntiRage(GameState& state, Player player) {
    DefenceExclusions defence_exclusions;

    // Anti rge level
    // 0: None    |
    // 1: closest | Increasing number of ships locked
    // 2: max     |
    // 3: sum     v
    static int anti_rage_level = Config::Value<int>("antirage");
    static bool have_defence_exclusions = Config::Value<bool>("antirage.exlusions");
    if ( anti_rage_level == 0 ) return defence_exclusions;

    LOG("Antirage phase");

    std::map<int,bool> frontier_planets = state.FrontierPlanets(player);
    std::pair<int,bool> item;
    foreach ( item, frontier_planets ) {
        if ( ! item.second ) continue;

        PlanetPtr p = state.Planet(item.first);

        int ships_locked = 0;

        // the planet we are protecting ourselves from
        PlanetPtr rage_planet;

        if ( anti_rage_level == 1 ) {
            // defend against only the closest enemy
            PlanetPtr closest_enemy = state.ClosestPlanetByOwner(p, -player);
            if ( closest_enemy ) {
                ships_locked = AntiRageRequiredShips(state, p, closest_enemy );
                rage_planet = closest_enemy;
            }
        }
        else {
            // defend against all enemies
            int max_required_ships = 0;
            int sum_required_ships = 0;
            std::vector<PlanetPtr> enemy_planets = state.PlanetsOwnedBy(-player);
            foreach ( PlanetPtr& enemy_planet, enemy_planets ) {
                int required_ships = AntiRageRequiredShips(state, p, enemy_planet );
                if ( required_ships > max_required_ships ) {
                    max_required_ships = required_ships;
                    rage_planet = enemy_planet;
                }
                sum_required_ships += required_ships;
            }

            ships_locked = anti_rage_level == 2 ? max_required_ships : sum_required_ships;
        }

        if ( ships_locked > 0 ) {
            ships_locked = p->LockShips(ships_locked);
            if (  have_defence_exclusions ) {
                defence_exclusions[p->id] = std::pair<int,int>(rage_planet->id, ships_locked);
            }
            LOG( " Locking " << ships_locked << " ships on planet " << p->id );
        }
    }

    return defence_exclusions;
}

int AntiRageRequiredShips(const GameState &state, const PlanetPtr& my_planet, const PlanetPtr& enemy_planet) {
    int distance = Map::Distance(enemy_planet->id, my_planet->id);
    int required_ships = enemy_planet->Ships() - distance*my_planet->GrowthRate();
    if ( required_ships <= 0 ) return 0;

    Player player = my_planet->Owner();

    // enslist help
    const std::vector<int>& sorted = Map::PlanetsByDistance( my_planet->id );
    foreach ( int i, sorted ) {
        const PlanetPtr p = state.Planet(i);
        if ( p->Owner() != player ) continue;

        int help_distance =  Map::Distance(my_planet->id, i);
        if ( help_distance >= distance ) break;
        required_ships -= p->Ships(true) + p->ShipExcess(distance-help_distance-1);
    }

    if ( required_ships <= 0 ) return 0;

    return required_ships;
}


#include <eos/types/Asset.hpp>
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <fc/reflect/variant.hpp>

namespace eos { namespace types {
      typedef boost::multiprecision::int128_t  int128_t;

      uint8_t Asset::decimals()const {
         auto a = (const char*)&symbol;
         return a[0];
      }
      void Asset::set_decimals(uint8_t d){
         auto a = (char*)&symbol;
         a[0] = d;
      }

      String Asset::symbol_name()const {
         auto a = (const char*)&symbol;
         assert( a[7] == 0 );
         return &a[1];
      }

       int64_t Asset::precision()const {

         static int64_t table[] = {
                           1, 10, 100, 1000, 10000,
                           100000, 1000000, 10000000, 100000000ll,
                           1000000000ll, 10000000000ll,
                           100000000000ll, 1000000000000ll,
                           10000000000000ll, 100000000000000ll
                         };
         return table[ decimals() ];
      }

      String Asset::toString()const {
         String result = fc::to_string( static_cast<int64_t>(amount) / precision());
         if( decimals() )
         {
            auto fract = static_cast<int64_t>(amount) % precision();
            result += "." + fc::to_string(precision() + fract).erase(0,1);
         }
         return result + " " + symbol_name();
      }

      Asset Asset::fromString( const String& from )
      {
         try
         {
            String s = fc::trim( from );
            auto space_pos = s.find( " " );
            auto dot_pos = s.find( "." );

            Asset result;
            result.symbol = uint64_t(0);
            auto sy = (char*)&result.symbol;
            *sy = (char) dot_pos; // Mask due to undefined architecture behavior

            auto intpart = s.substr( 0, dot_pos );
            result.amount = fc::to_int64(intpart);
            String fractpart;
            if( dot_pos != String::npos )
            {
               auto fractpart = "1" + s.substr( dot_pos + 1, space_pos - dot_pos - 1 );
               result.set_decimals( fractpart.size() - 1 );

               result.amount *= Int64(result.precision());
               result.amount += Int64(fc::to_int64(fractpart));
               result.amount -= Int64(result.precision());
            }
            auto symbol = s.substr( space_pos + 1 );

            if( symbol.size() )
               memcpy( sy+1, symbol.c_str(), std::min(symbol.size(),size_t(6)) );

            return result;
         }
         FC_CAPTURE_LOG_AND_RETHROW( (from) )
      }

      bool operator == ( const Price& a, const Price& b )
      {
         if( std::tie( a.base.symbol, a.quote.symbol ) != std::tie( b.base.symbol, b.quote.symbol ) )
             return false;

         const auto amult = UInt128( b.quote.amount ) * UInt128(a.base.amount);
         const auto bmult = UInt128( a.quote.amount ) * UInt128(b.base.amount);

         return amult == bmult;
      }

      bool operator < ( const Price& a, const Price& b )
      {
         if( a.base.symbol < b.base.symbol ) return true;
         if( a.base.symbol > b.base.symbol ) return false;
         if( a.quote.symbol < b.quote.symbol ) return true;
         if( a.quote.symbol > b.quote.symbol ) return false;

         const auto amult = UInt128( b.quote.amount ) * UInt128(a.base.amount);
         const auto bmult = UInt128( a.quote.amount ) * UInt128(b.base.amount);

         return amult < bmult;
      }

      bool operator <= ( const Price& a, const Price& b )
      {
         return (a == b) || (a < b);
      }

      bool operator != ( const Price& a, const Price& b )
      {
         return !(a == b);
      }

      bool operator > ( const Price& a, const Price& b )
      {
         return !(a <= b);
      }

      bool operator >= ( const Price& a, const Price& b )
      {
         return !(a < b);
      }

      Asset operator * ( const Asset& a, const Price& b )
      {
         if( a.symbol_name() == b.base.symbol_name() )
         {
            FC_ASSERT( static_cast<int64_t>(b.base.amount) > 0 );
            auto result = (UInt128(a.amount) * UInt128(b.quote.amount))/UInt128(b.base.amount);
            return Asset( Int64(result), b.quote.symbol );
         }

         else if( a.symbol_name() == b.quote.symbol_name() )
         {
            FC_ASSERT( static_cast<int64_t>(b.quote.amount) > 0 );
            auto result = (UInt128(a.amount) *UInt128(b.base.amount))/UInt128(b.quote.amount);
            return Asset( Int64(result), b.base.symbol );
         }
         FC_THROW_EXCEPTION( fc::assert_exception, "invalid Asset * Price", ("Asset",a)("Price",b) );
      }

      Price operator / ( const Asset& base, const Asset& quote )
      { try {
         FC_ASSERT( base.symbol_name() != quote.symbol_name() );
         return Price{ base, quote };
      } FC_CAPTURE_AND_RETHROW( (base)(quote) ) }

      Price Price::max( AssetSymbol base, AssetSymbol quote ) { return Asset( ShareType(EOS_MAX_SHARE_SUPPLY), base ) / Asset( ShareType(1), quote); }
      Price Price::min( AssetSymbol base, AssetSymbol quote ) { return Asset( 1, base ) / Asset( EOS_MAX_SHARE_SUPPLY, quote); }

      bool Price::is_null() const { return *this == Price(); }

      void Price::validate() const
      { try {
         FC_ASSERT( base.amount > ShareType(0) );
         FC_ASSERT( quote.amount > ShareType(0) );
         FC_ASSERT( base.symbol_name() != quote.symbol_name() );
      } FC_CAPTURE_AND_RETHROW( (base)(quote) ) }

}}  // eos::types


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_LOG2_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_LOG2_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>
#include <cmath>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/ilog2.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/plain.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/std.hpp>

#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/invlog_2.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>

#include <boost/simd/detail/constant/invlog_2hi.hpp>
#include <boost/simd/detail/constant/invlog_2lo.hpp>

#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

#include <cmath>
namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 const& a0) const BOOST_NOEXCEPT
    {
      return musl_(log2)(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 const& a0) const BOOST_NOEXCEPT
    {
      return bs::ilog2(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &, A0 a0
                                    ) const BOOST_NOEXCEPT
    {
      return std::log2(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0)
                          , bd::cpu_
                          , bs::plain_tag
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &, A0 a0
                                    ) const BOOST_NOEXCEPT
    {
      return musl_(log2(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0)
                          , bd::cpu_
                          , bs::musl_tag
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
    /* origin: FreeBSD /usr/src/lib/msun/src/e_log2f.c */
    /*
     * ====================================================
     * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
     *
     * Developed at SunPro, a Sun Microsystems, Inc. business.
     * Permission to use, copy, modify, and distribute this
     * software is freely granted, provided that this notice
     * is preserved.
     * ====================================================
     */
     BOOST_FORCEINLINE A0 operator() (const musl_tag &, A0 x) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      uiA0 ix = bitwise_cast<uiA0>(x);
      iA0 k = 0;
      if (ix < 0x00800000 || ix>>31)         /* x < 2**-126  */
      {
        if (ix<<1 == 0) return Minf<A0>();  /* log(+-0)=-inf */
        if (ix>>31) return Nan<A0>();       /* log(-#) = NaN */
#ifndef BOOST_SIMD_NO_DENORMALS
        /* subnormal number, scale up x */
        k -= 25;
        x *= 33554432.0f;
         ix = bitwise_cast<iA0>(x);
#endif
      }
      else if (ix >= 0x7f800000)
      {
        return x;
      }
      else if (ix == 0x3f800000)
        return 0;

      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      ix += 0x3f800000 - 0x3f3504f3;
      k += bitwise_cast<iA0>(ix>>23) - 0x7f;
      ix = (ix&0x007fffff) + 0x3f3504f3;
      x =  bitwise_cast<A0>(ix);
      A0 f = dec(x);
      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3eccce13, 0x3e789e26>(w);
      A0 t2= z*horn<A0, 0x3f2aaaaa, 0x3e91e9ee>(w);
      A0 R = t2 + t1;
      A0 hfsq = 0.5f*sqr(f);
      return -(hfsq-(s*(hfsq+R))-f)*Invlog_2<A0>()+k;
      // The original algorithm does some extra calculation in place of the return line
      // to get extra precision but this is uneeded for float as the exhaustive test shows
      // a 0.5 ulp maximal error on the full range.
      // Moreover all log2(exp2(i)) i =  1..31 are flint
      // I leave the code here in case an exotic proc will not play the game.
      //       A0  hi = f - hfsq;
      //       hi =  bitwise_and(hi, uiA0(0xfffff000ul));
      //       A0  lo = fma(s, hfsq+R, f - hi - hfsq);
      //       return fma((lo+hi), Invlog_2lo<A0>(), lo*Invlog_2hi<A0>() + hi*Invlog_2hi<A0>() + k);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0)
                          , bd::cpu_
                          , bs::musl_tag
                          , bd::scalar_< bd::double_<A0> >
                          )
  {
    /* origin: FreeBSD /usr/src/lib/msun/src/e_log2.c */
    /*
     * ====================================================
     * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
     *
     * Developed at SunSoft, a Sun Microsystems, Inc. business.
     * Permission to use, copy, modify, and distribute this
     * software is freely granted, provided that this notice
     * is preserved.
     * ====================================================
     */
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, A0 x) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      uiA0 hx = bitwise_cast<uiA0>(x) >> 32;
      iA0 k = 0;
      if (hx < 0x00100000 || hx>>31)
      {
        if(is_eqz(x))
          return Minf<A0>();  /* log(+-0)=-inf */
        if (hx>>31)
          return Nan<A0>(); /* log(-#) = NaN */
        /* subnormal number, scale x up */
#ifndef BOOST_SIMD_NO_DENORMALS
        k -= 54;
        x *=  18014398509481984.0;
        hx = bitwise_cast<uiA0>(x) >> 32;
#endif
      }
      else if (hx >= 0x7ff00000)
      {
        return x;
      }
      else if (x == One<A0>())
        return Zero<A0>();
      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      hx += 0x3ff00000 - 0x3fe6a09e;
      k += bitwise_cast<iA0>(hx>>20) - 0x3ff;
      hx = (hx&0x000fffff) + 0x3fe6a09e;
      x = bitwise_cast<A0>( (uint64_t)hx<<32 | (bitwise_and(0xffffffffull, bitwise_cast<uiA0>(x))));
      A0 f = dec(x);
      A0 hfsq = 0.5*sqr(f);
      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3fd999999997fa04ll, 0x3fcc71c51d8e78afll, 0x3fc39a09d078c69fll > (w);
      A0 t2= z*horn<A0, 0x3fe5555555555593ll, 0x3fd2492494229359ll
                      , 0x3fc7466496cb03dell, 0x3fc2f112df3e5244ll> (w);
      A0 R = t2 + t1;
//        return -(hfsq-(s*(hfsq+R))-f)*Invlog_2<A0>()+dk;  // fast ?

      /*
       * f-hfsq must (for args near 1) be evaluated in extra precision
       * to avoid a large cancellation when x is near sqrt(2) or 1/sqrt(2).
       * This is fairly efficient since f-hfsq only depends on f, so can
       * be evaluated in parallel with R.  Not combining hfsq with R also
       * keeps R small (though not as small as a true `lo' term would be),
       * so that extra precision is not needed for terms involving R.
       *
       * Compiler bugs involving extra precision used to break Dekker's
       * theorem for spitting f-hfsq as hi+lo, unless double_t was used
       * or the multi-precision calculations were avoided when double_t
       * has extra precision.  These problems are now automatically
       * avoided as a side effect of the optimization of combining the
       * Dekker splitting step with the clear-low-bits step.
       *
       * y must (for args near sqrt(2) and 1/sqrt(2)) be added in extra
       * precision to avoid a very large cancellation when x is very near
       * these values.  Unlike the above cancellations, this problem is
       * specific to base 2.  It is strange that adding +-1 is so much
       * harder than adding +-ln2 or +-log10_2.
       *
       * This uses Dekker's theorem to normalize y+val_hi, so the
       * compiler bugs are back in some configurations, sigh.  And I
       * don't want to used double_t to avoid them, since that gives a
       * pessimization and the support for avoiding the pessimization
       * is not yet available.
       *
       * The multi-precision calculations for the multiplications are
       * routine.
       */

      /* hi+lo = f - hfsq + s*(hfsq+R) ~ log(1+f) */
      A0  hi = f - hfsq;
      hi =  bitwise_and(hi, (Allbits<uiA0>() << 32));
      A0 lo = f - hi - hfsq + s*(hfsq+R);

      A0 val_hi = hi*Invlog_2hi<A0>();
      A0 val_lo = fma(lo+hi, Invlog_2lo<A0>(), lo*Invlog_2hi<A0>());

      A0 dk = k;
      A0 w1 = dk + val_hi;
      val_lo += (dk - w1) + val_hi;
      val_hi = w1;
      return val_lo + val_hi;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0)
                          , bd::cpu_
                          , bs::plain_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, A0 x) const BOOST_NOEXCEPT
    {
      return musl_(log2)(x); //the "plain" version of the algorithm is never speedier than the "musl" version.
      // the call is here to allow a scalar fallback to simd calls
    }
  };

} } }

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2011-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2011-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_UTIL_RATIONAL_HPP
#define BOOST_GEOMETRY_UTIL_RATIONAL_HPP

#include <boost/rational.hpp>
#include <boost/numeric/conversion/bounds.hpp>

#include <boost/geometry/util/coordinate_cast.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost{ namespace geometry
{

// Specialize for Boost.Geometry's coordinate cast
// (from string to coordinate type)
namespace detail
{

template <typename T>
struct coordinate_cast<rational<T> >
{
    static inline void split_parts(std::string const& source, std::string::size_type p,
        T& before, T& after, bool& negate, std::string::size_type& len)
    {
        std::string before_part = source.substr(0, p);
        std::string const after_part = source.substr(p + 1);

        negate = false;

        if (before_part.size() > 0 && before_part[0] == '-')
        {
            negate = true;
            before_part.erase(0, 1);
        }
        before = atol(before_part.c_str());
        after = atol(after_part.c_str());
        len = after_part.length();
    }

    static inline rational<T> apply(std::string const& source)
    {
        T before, after;
        bool negate;
        std::string::size_type len;

        // Note: decimal comma is not (yet) supported, it does (and should) not
        // occur in a WKT, where points are comma separated.
        std::string::size_type p = source.find(".");
        if (p == std::string::npos)
        {
            p = source.find("/");
            if (p == std::string::npos)
            {
                return rational<T>(atol(source.c_str()));
            }
            split_parts(source, p, before, after, negate, len);

            return negate
                ? -rational<T>(before, after)
                : rational<T>(before, after)
                ;

        }

        split_parts(source, p, before, after, negate, len);

        T den = 1;
        for (std::string::size_type i = 0; i < len; i++)
        {
            den *= 10;
        }

        return negate
            ? -rational<T>(before) - rational<T>(after, den)
            : rational<T>(before) + rational<T>(after, den)
            ;
    }
};

} // namespace detail

// Specialize for Boost.Geometry's select_most_precise
template <typename T1, typename T2>
struct select_most_precise<geofeatures_boost::rational<T1>, geofeatures_boost::rational<T2> >
{
    typedef typename geofeatures_boost::rational
        <
            typename select_most_precise<T1, T2>::type
        > type;
};

template <typename T>
struct select_most_precise<geofeatures_boost::rational<T>, double>
{
    typedef typename geofeatures_boost::rational<T> type;
};

}} // namespace geofeatures_boost::geometry

// Specializes geofeatures_boost::rational to geofeatures_boost::numeric::bounds
namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace numeric
{

template<class T>
struct bounds<rational<T> >
{
    static inline rational<T> lowest()
    {
        return rational<T>(bounds<T>::lowest(), 1);
    }
    static inline rational<T> highest()
    {
        return rational<T>(bounds<T>::highest(), 1);
    }
};

}} // namespace geofeatures_boost::numeric

// Support for geofeatures_boost::numeric_cast to int and to double (necessary for SVG-mapper)
namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace numeric
{

template
<
    typename T,
    typename Traits,
    typename OverflowHandler,
    typename Float2IntRounder,
    typename RawConverter,
    typename UserRangeChecker
>
struct converter<int, rational<T>, Traits, OverflowHandler, Float2IntRounder, RawConverter, UserRangeChecker>
{
    static inline int convert(rational<T> const& arg)
    {
        return int(rational_cast<double>(arg));
    }
};

template
<
    typename T,
    typename Traits,
    typename OverflowHandler,
    typename Float2IntRounder,
    typename RawConverter,
    typename UserRangeChecker
>
struct converter<double, rational<T>, Traits, OverflowHandler, Float2IntRounder, RawConverter, UserRangeChecker>
{
    static inline double convert(rational<T> const& arg)
    {
        return rational_cast<double>(arg);
    }
};

}}

#endif // BOOST_GEOMETRY_UTIL_RATIONAL_HPP


/*  Copyright (C) 2017 NEC Laboratories America, Inc. ("NECLA"). All rights reserved.
 *
 * This source code is licensed under the license found in the LICENSE file in
 * the root directory of this source tree. An additional grant of patent rights
 * can be found in the PATENTS file in the same directory.
 */

#include "optimizelu.hh"
#include "utils.h"
#include "boost/iterator/counting_iterator.hpp"
#include <boost/numeric/conversion/bounds.hpp>  // boost::numeric::bounds<T>
#include <cmath>
#include <iostream>

#include "constants.h"

#define __restricted /* __restricted seems to be an error */

namespace mcsolver_detail{

  using namespace std;
  /** get optimal {l,u} bounds given projection and class order.
   * Computationally expensive, so it should be done sparingly.
   */
  void optimizeLU(VectorXd& l, VectorXd& u,
		  const VectorXd& projection, const SparseMb& y,
		  const vector<int>& class_order, const vector<int>& sorted_class,
		  const VectorXd& wc, const VectorXi& nclasses,
		  const VectorXd& inside_weight, const VectorXd& outside_weight,
		  const boolmatrix& filtered,
		  const double C1, const double C2,
		  const param_struct& params)
  {
#define BOUNDGRAD_THREAD 1 // 1=good

    // -------------- and sanity checks
#if MCTHREADS && defined(_OPENMP)
    if( omp_in_parallel() )
      throw runtime_error(" ERROR: please don't call optimizeLU from and omp parallel section");
#endif
    static bool check_once=false;
    if( !check_once ){
      size_t nErase=0U;
#if MCTHREADS && defined(_OPENMP)
#pragma omp parallel for schedule(static)
#endif
      for(size_t o=0U; o<y.outerSize(); ++o){
	for(SparseMb::InnerIterator it(y,o); it; ++it) {
	  if( it.value()==false ){
	    ++nErase;
	  }
	}
      }
      if( nErase )
	throw runtime_error(" ERROR: SparseMb 'y' should have only 'true' entries");
      if( ! y.isCompressed() )
	throw runtime_error(" ERROR: expect SparseMb 'y' to be compressed sparse");
      check_once=true;
    }
    // ------------------------------------------------------
    size_t const n = projection.size();
    size_t const noClasses = y.cols();
    bool const none_filtered = filtered.count()==0;
    VectorXd allproj(2*n);
    allproj << (projection.array() - 1), (projection.array() + 1);
    std::vector<size_t> indices(allproj.size());
    sort_index(allproj, indices);

    // yes, co[sc[c]] == c
    // initialize the gradients for u and l
    VectorXd gradu(noClasses); // by ranked classes, to minimize cache misses/false sharing
    VectorXd gradl(noClasses); // by ranked classes, to minimize cache misses/false sharing
#if MCTHREADS && defined(_OPENMP)
    size_t const ck = std::max(size_t{256U},size_t{noClasses/omp_get_max_threads()});
#pragma omp parallel for schedule(static,ck)
#endif
    for (size_t sc = 0; sc <noClasses; ++sc) {
      int const c = sorted_class[sc];
      double const classweight = wc.coeff(c);
      if (classweight <= 0.0) { // no examples of this class
	// classes with no weight (wc[]) get {l,u} bounds set to high/low values.
	u.coeffRef(c) = boost::numeric::bounds<double>::lowest()/10; // the /10 is needed because octave has trouble reading back ascii files that are written with the highest/lowest limits
	l.coeffRef(c) = boost::numeric::bounds<double>::highest()/10; // the /10 is needed because octave has trouble reading back ascii files that are written with the highest/lowest limits;
	gradu.coeffRef(sc) = -1.0;
	gradl.coeffRef(sc) = -1.0;
      } else {
	gradu.coeffRef(sc) = C1*classweight;
	gradl.coeffRef(sc) = C1*classweight;
      }
    }

#if BOUNDGRAD_THREAD && MCTHREADS && defined(_OPENMP)
    int const max_n_chunks = omp_get_max_threads();

    int const min_chunk_size = 10000;     // need a test case to set this value XXX
#endif

#if MCTHREADS
#pragma omp parallel default(none) shared(l, u, allproj, indices, filtered, y, class_order, sorted_class, wc, nclasses, inside_weight, outside_weight, params, gradu, gradl)
#endif
    {
#if MCTHREADS
#pragma omp single nowait
#endif
      { // calculate the optimal value for upper bounds -- iterate from beginning
        std::vector<int> classes;
        classes.reserve(nclasses.maxCoeff());
        for (std::vector<size_t>::const_iterator i = indices.begin(); i != indices.end(); ++i) {
	  bool plus = false;
	  size_t idx = *i;
	  if (idx >= n) { plus = true; idx -= n; }

	  if (plus) { // only the upper bounds of the classes of this example are affected
	    double const class_weight = C1*inside_weight.coeff(idx);
	    for (SparseMb::InnerIterator it(y,idx); it; ++it) {
	      int const cs = it.col();                // raw [unsorted] class
	      int const sc = class_order[cs];         // sorted class number
	      if( gradu.coeff(sc) >= 0 && (gradu.coeffRef(sc) -= class_weight) <  std::numeric_limits<double>::epsilon()*class_weight*10){
		u.coeffRef(cs) = allproj.coeff(*i);
	      }
	    }
	  }else{
	    // only the classes ranked lower than the classes of this example are affected
	    sortedClasses( /*OUT*/ classes, /*IN*/ class_order, y, idx );
	    if (classes.size() == 0) continue;
	    if (classes.back() == 0) continue;

	    double const other_weight = C2*outside_weight.coeff(idx);
	    // how many classes of the curent instance should be ranked higher
	    //  times the weight of each
	    //  if each class has its own weight will need to
	    //  be calculated below (or have it precomputed for each example
	    //  as a corresponding wclasses to nclasses to be wclasses the same as wc
	    //  corresponds to nc
	    double const right_update = other_weight * nclasses.coeff(idx);
#if BOUNDGRAD_THREAD && MCTHREADS && defined(_OPENMP)
	    // make sure there is enough work to do to paralelize this
	    int n_chunks = classes.back()/min_chunk_size + 1;
	    n_chunks = n_chunks < max_n_chunks?n_chunks:max_n_chunks;
	    if( n_chunks > 1 ){
	      int chunk_size = classes.back()/n_chunks;
	      int remaining = classes.back()%n_chunks;
	      for (int chunk=0; chunk < n_chunks; ++chunk)
		{
#pragma omp task default(shared) firstprivate(chunk) shared(gradu, u, idx, i, sorted_class, classes, allproj, filtered, chunk_size, remaining)
		  {
		    int sc_start = chunk*chunk_size + (chunk<remaining?chunk:remaining);
		    int sc_incr = chunk_size + (chunk<remaining);
		    getBoundGrad(gradu, u, idx, *i, sorted_class, sc_start, sc_start + sc_incr, classes, right_update, -other_weight, allproj, none_filtered, filtered);
		  }
#pragma omp taskwait // need to wait for all tasks to finish before moving to the next example
		}
	    }else{
	      getBoundGrad(gradu, u, idx, *i, sorted_class, 0,classes.back(),classes,right_update,-other_weight,allproj,none_filtered,filtered);
	    }
#else // not _OPENMP
	    getBoundGrad(gradu, u, idx, *i, sorted_class, 0,classes.back(),classes,right_update,-other_weight,allproj,none_filtered,filtered);
#endif // _OPENMP
	  }
        }
      }

#if MCTHREADS
#pragma omp single nowait
#endif
      { // calculate the optimal value for lower bounds -- iterate from end
        std::vector<int> classes;
        classes.reserve(nclasses.maxCoeff());
        for (std::vector<size_t>::const_reverse_iterator i = indices.rbegin(); i != indices.rend(); i++) {
	  bool plus = false;
	  size_t idx = *i;
	  if (idx >= n) { plus = true; idx -= n; }

	  if (!plus){ // only the lower bounds of the classes of this example are affected
	    double const class_weight = inside_weight.coeff(idx)*C1;
	    for (SparseMb::InnerIterator it(y,idx); it; ++it) {
	      //assert( it.value() ); //if (it.value())
	      int cs = it.col();
	      int sc = class_order[cs];
	      if (gradl.coeff(sc) >= 0 && (gradl.coeffRef(sc) -= class_weight) < std::numeric_limits<double>::epsilon()*class_weight*10){
		l.coeffRef(cs) = allproj.coeff(*i);
	      }
	    }
	  }else{
	    // only the classes ranked higher than the classes of this example are affected
	    // calling y.coeff is expensive so get the classes in the ranked order here
	    sortedClasses( /*OUT*/ classes, /*IN*/ class_order, y, idx );
	    if(classes.size() == 0U) continue;
	    // if a class has lower rank than the lowest rank class of this example
	    // it's lower bound will not be influenced by this example
	    int n_active = noClasses - classes.front() - 1;
	    if (n_active <= 0)
	      continue;

	    double const other_weight = outside_weight.coeff(idx)*C2;
#if BOUNDGRAD_THREAD && MCTHREADS && defined(_OPENMP)
	    // make sure there is enough work to do to paralelize this
	    int n_chunks = n_active/min_chunk_size + 1;
	    n_chunks = n_chunks < max_n_chunks?n_chunks:max_n_chunks;
	    if( n_chunks > 1 ){
	      int chunk_size = n_active/n_chunks;
	      int remaining = n_active%n_chunks;
	      for (int chunk=0; chunk < n_chunks; ++chunk)
		{
#pragma omp task default(shared) firstprivate(chunk) shared(gradl, l, idx, i, sorted_class, classes, allproj, filtered, chunk_size, remaining)
		  {
		    int sc_start = classes.front() + 1 + chunk*chunk_size + (chunk<remaining?chunk:remaining);
		    int sc_incr = chunk_size + (chunk<remaining);
		    getBoundGrad(gradl, l, idx, *i, sorted_class, sc_start, sc_start + sc_incr, classes, 0.0, other_weight, allproj, none_filtered, filtered);
		  }
#pragma omp taskwait  // need to wait for all tasks to finish before moving to the next example
		}
	    }else{
	      getBoundGrad(gradl, l, idx, *i, sorted_class, classes.front() + 1,noClasses,classes, 0.0, other_weight, allproj, none_filtered, filtered);
	    }
#else // not _OPENMP
	    getBoundGrad(gradl, l, idx, *i, sorted_class, classes.front() + 1,noClasses,classes, 0.0, other_weight, allproj, none_filtered, filtered);
#endif // _OPENMP
	  }
        }
      } // omp single
    } // omp parallel
  }
}


// Copyright (c) 2021 fortiss GmbH
//
// Authors: Klemens Esterle and Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef MIQP_COMMON_GEOMETRY_HPP_
#define MIQP_COMMON_GEOMETRY_HPP_

#include <Eigen/Core>
#include <vector>

#include <boost/geometry/geometries/adapted/boost_polygon.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include "bark/geometry/polygon.hpp"

namespace miqp {
namespace common {
namespace geometry {

namespace bg = boost::geometry;

using PointXY = bark::geometry::Point2d;
using BoostPolygon = bg::model::polygon<PointXY>;
using BoostMultiPolygon = bg::model::multi_polygon<BoostPolygon>;
using BoostLinestring = bg::model::linestring<PointXY>;

typedef unsigned int PolygonId;
typedef std::map<PolygonId, bark::geometry::Polygon> PolygonMap;

/**
 * @brief prints points of polygon
 *
 * @param poly polygon
 */
void PrintPolygon(BoostPolygon const& poly);

bark::geometry::Polygon BoostPolygonToBarkPolygon(BoostPolygon const& poly);

/**
 * @brief conveerts Boost linestring to BARK line type
 *
 * @param line
 * @return bark::geometry::Line
 */
bark::geometry::Line BoostLinestringToBarkLine(BoostLinestring const& line);

/**
 * @brief converts BARK's polygon to Boost polygon type
 *
 * @param poly BARK polygon to be converted
 * @return BoostPolygon converted polygon of type BoostPolygon
 */
BoostPolygon BarkPolygonToBoostPolygon(bark::geometry::Polygon const& poly);

bool IsConvex(BoostPolygon const& poly);

/**
 * @brief buffer all elements in a boost MultiPolygone, overrides the entries!
 * @param polynoms_in ... the multipoly
 * @param buffer_radius .. the buffer distance
 */
void BufferPolygonCollection(BoostMultiPolygon& polygons_in,
                             const double buffer_radius);

/**
 * @brief merging convex polygons to still convex ones. suboptimal algorithm,
 * that heavily depends on the sequence of the input polygons
 *
 * @param polygons_in ... polygons that will be merged
 * @param tolerance ... tolerance for merging
 * @param buffered_env_for_merging ... polygon, which all merged cells must in
 * within
 *
 * @note to account for numerical issues with the boost geometry union_ function
 * we buffer the input polygons by a small radius. Atm we only observe this
 * problems with points of doubles. see the unit test convexified_map_test.cc
 * TEST(convexified_map_test, polymerge) for a demo.
 *
 */
void MergeToConvexPolygons(BoostMultiPolygon& polygons_in,
                           const float tolerance,
                           bark::geometry::Polygon& buffered_env_for_merging);

/**
 * @brief convert BARK polygon to definition that is used by MIQP planner
 *
 * @param poly polygon to be converted
 * @return Eigen::MatrixXd converted polygon
 */
Eigen::MatrixXd Polygon2MiqpPolygonDefinition(
    const bark::geometry::Polygon& poly);

/**
 * @brief creates hash code from polygon
 *
 * @param polygon polygon used for hash calculation
 * @return std::size_t hash code
 */
std::size_t GetHash(const bark::geometry::Polygon& polygon);

/**
 * @brief convert array to BARK line type
 *
 * @param ref_in points array
 * @param ref_size size of points array
 * @param simplification_distance distance used for simplification of line
 * @param line_out converted line
 */
void ConvertToBarkLine(double ref_in[], const int ref_size,
                       double simplification_distance,
                       bark::geometry::Line& line_out);

/**
 * @brief converter from multipolygon to polygon map
 *
 * @param polygons_in ... input as multipolygon
 * @return PolygonMap ... output as polygon map
 */
PolygonMap BoostMultiPolygonToBarkPolygonMap(
    miqp::common::geometry::BoostMultiPolygon& polygons_in);

}  // namespace geometry
}  // namespace common
}  // namespace miqp

#endif  // MIQP_COMMON_GEOMETRY_HPP_


//
//  TDF SDK
//
//  Created by Sujan Reddy on 2019/03/07.
//  Copyright 2019 Virtru Corporation
//

#define BOOST_TEST_MODULE test_rsa_key_pair

#include "rsa_key_pair.h"
#include "crypto_utils.h"

#include <string>
#include <iostream>
#include <time.h>

#include <boost/test/included/unit_test.hpp>
#include <openssl/bio.h>
#include <openssl/pem.h>

BOOST_AUTO_TEST_SUITE(test_rsa_key_pair_suite)

namespace vc = virtru::crypto;
struct BioDeleter { void operator()(BIO* bio) {::BIO_free(bio);} };
struct RsaDeleter {	void operator()(RSA* rsa) {	::RSA_free(rsa); }	};

BOOST_AUTO_TEST_CASE(rsa_key_pair_2048)
{
    auto keyPairOf2048 = vc::RsaKeyPair::Generate(2048);
    auto privateKey = keyPairOf2048->PrivateKeyInPEMFormat();
    auto publicKey = keyPairOf2048->PublicKeyInPEMFormat();

    std::unique_ptr<BIO, BioDeleter> privateKeyBuffer { BIO_new_mem_buf(privateKey.data(), privateKey.size()) };
    std::unique_ptr<RSA, RsaDeleter> privateRSA { PEM_read_bio_RSAPrivateKey(privateKeyBuffer.get(),
            nullptr, nullptr, nullptr) };

    std::unique_ptr<BIO, BioDeleter> publicKeyBuffer { BIO_new_mem_buf(privateKey.data(), privateKey.size()) };
    std::unique_ptr<RSA, RsaDeleter> publicRSA { PEM_read_bio_RSAPrivateKey(publicKeyBuffer.get(), nullptr,
            nullptr, nullptr) };

    BOOST_TEST(RSA_size(privateRSA.get()) == 256, "Checking RSA private key length - key size 256 bytes");
    BOOST_TEST(RSA_size(publicRSA.get()) == 256, "Checking RSA public key length - key size 256 bytes");
}

BOOST_AUTO_TEST_CASE(rsa_key_pair_4096)
{
    auto keyPairOf4096 = vc::RsaKeyPair::Generate(4096);
    auto privateKey = keyPairOf4096->PrivateKeyInPEMFormat();
    auto publicKey = keyPairOf4096->PublicKeyInPEMFormat();

    std::unique_ptr<BIO, BioDeleter> privateKeyBuffer { BIO_new_mem_buf(privateKey.data(), privateKey.size()) };
    std::unique_ptr<RSA, RsaDeleter> privateRSA { PEM_read_bio_RSAPrivateKey(privateKeyBuffer.get(),
            nullptr, nullptr, nullptr) };

    std::unique_ptr<BIO, BioDeleter> publicKeyBuffer { BIO_new_mem_buf(privateKey.data(), privateKey.size()) };
    std::unique_ptr<RSA, RsaDeleter> publicRSA { PEM_read_bio_RSAPrivateKey(publicKeyBuffer.get(),
            nullptr, nullptr, nullptr) };

    BOOST_TEST(RSA_size(privateRSA.get()) == 512, "Checking RSA private key length - key size 512 bytes");
    BOOST_TEST(RSA_size(publicRSA.get()) == 512, "Checking RSA public key length - key size 512 bytes");
}

BOOST_AUTO_TEST_CASE(rsa_key_pair_negative_test)
{
    try {
        auto desginedToFailkeyPair = vc::RsaKeyPair::Generate(-1);
        auto privateKey = desginedToFailkeyPair->PrivateKeyInPEMFormat();
        auto publicKey = desginedToFailkeyPair->PublicKeyInPEMFormat();
        BOOST_FAIL("We should not get here" );
    } catch ( const vc::CryptoException& exception) {
        BOOST_TEST_MESSAGE("Expect crypto exception");
        std :: cout << exception.what() << std::endl;
    } catch ( ... ) {
        BOOST_FAIL("Crypto exception should be thrown" );
        std :: cout << "...\n";
    }
}

BOOST_AUTO_TEST_SUITE_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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 swar toolbox - split/simd Mode"
//COMMENTED
//////////////////////////////////////////////////////////////////////////////
// unit test behavior of swar components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 24/02/2011
///
#include <boost/simd/toolbox/swar/include/functions/split.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/simd/include/constants/one.hpp>

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

//COMMENTED

NT2_TEST_CASE_TPL ( split_lt_64__1_0, BOOST_SIMD_SIMD_SPLITABLE_TYPES)
{
  using boost::simd::split;
  using boost::simd::tag::split_;
  using boost::simd::native;

  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>   vT;
  typedef typename boost::dispatch::meta::call<split_(vT)>::type r_t;

  // specific values tests
  typedef typename boost::dispatch::meta::strip<typename boost::fusion::result_of::at_c<r_t,0>::type>::type r_t0;
  typedef typename boost::dispatch::meta::strip<typename boost::fusion::result_of::at_c<r_t,1>::type>::type r_t1;

  {
    r_t res = split(boost::simd::One<vT>());
    NT2_TEST_EQUAL( boost::fusion::get<0>(res), boost::simd::One<r_t0>());
    NT2_TEST_EQUAL( boost::fusion::get<1>(res), boost::simd::One<r_t1>());
  }
  {
    r_t res = split(boost::simd::Zero<vT>());
    NT2_TEST_EQUAL( boost::fusion::get<0>(res), boost::simd::Zero<r_t0>());
    NT2_TEST_EQUAL( boost::fusion::get<1>(res), boost::simd::Zero<r_t1>());
  }
} // end of test for lt_64_


// This file is a part of the OpenSurgSim project.
// Copyright 2013 - 2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <boost/assign/list_of.hpp>
#include <gtest/gtest.h>

#include "SurgSim/Blocks/KnotIdentificationBehavior.h"
#include "SurgSim/Framework/BasicSceneElement.h"
#include "SurgSim/Framework/Runtime.h"
#include "SurgSim/Framework/Scene.h"
#include "SurgSim/Physics/Fem1DRepresentation.h"
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Math/MathConvert.h"
#include <random>

namespace
{
std::vector<SurgSim::Blocks::KnotIdentificationBehavior::Crossing> createGaussCode(const std::vector<int>& ids,
	const std::vector<int>& signs = {})
{
	std::vector<SurgSim::Blocks::KnotIdentificationBehavior::Crossing> result;
	for (size_t i = 0; i < ids.size(); ++i)
	{
		result.emplace_back(ids[i], 0, 0.0, 1);
	}
	for (size_t i = 0; i < std::min(signs.size(), result.size()); ++i)
	{
		result[i].sign = signs[i];
	}
	return result;
}
}

namespace SurgSim
{
namespace Blocks
{

class MockKnotIdentificationBehavior : public KnotIdentificationBehavior
{
public:
	explicit MockKnotIdentificationBehavior(const std::string& name) : KnotIdentificationBehavior(name) {}

	bool tryReidmeisterMove1(std::vector<Crossing>* gaussCode, std::vector<int>* erased)
	{
		return KnotIdentificationBehavior::tryReidmeisterMove1(gaussCode, erased);
	}

	bool tryReidmeisterMove2(std::vector<Crossing>* gaussCode, std::vector<int>* erased)
	{
		return KnotIdentificationBehavior::tryReidmeisterMove2(gaussCode, erased);
	}

	bool tryReidmeisterMove3(std::vector<Crossing>* gaussCode)
	{
		return KnotIdentificationBehavior::tryReidmeisterMove3(gaussCode, &data);
	}

	void adjustGaussCodeForErasedCrossings(std::vector<Crossing>* gaussCode)
	{
		return KnotIdentificationBehavior::adjustGaussCodeForErasedCrossings(gaussCode);
	}

	std::string identifyKnot(const std::vector<Crossing>& gaussCode)
	{
		return KnotIdentificationBehavior::identifyKnot(gaussCode);
	}

	KnotIdentificationBehavior::ReidmeisterMove3Data data;
};

class KnotIdentificationBehaviorTest : public testing::Test
{
public:
	KnotIdentificationBehaviorTest() : testing::Test(), mockReidmeisterMove3Behavior("Test3")
	{

	}

	void SetUp()
	{

	}

	std::shared_ptr<SurgSim::Physics::Fem1DRepresentation> makeFem1D(
		const std::string& filename,
		SurgSim::Math::RigidTransform3d t = SurgSim::Math::RigidTransform3d::Identity())
	{
		auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");
		auto physics = std::make_shared<SurgSim::Physics::Fem1DRepresentation>("Physics");
		physics->setFemElementType("SurgSim::Physics::Fem1DElementBeam");
		physics->setLocalPose(SurgSim::Math::RigidTransform3d::Identity());
		physics->loadFem(filename);
		physics->setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT);
		physics->setLinearSolver(SurgSim::Math::LINEARSOLVER_LU);
		physics->setRayleighDampingMass(5.0);
		physics->setRayleighDampingStiffness(0.001);
		physics->setIsGravityEnabled(true);
		physics->initialize(runtime);
		return physics;
	}

	bool testKnot(std::string filename, std::string knotname)
	{
		KnotIdentificationBehavior knotId("Knot ID");

		using SurgSim::Math::makeRigidTransform;
		using SurgSim::Math::makeRotationMatrix;

		SurgSim::Math::Vector3d axis = SurgSim::Math::Vector3d::UnitX();
		for (int i = 0; i < 4; ++i)
		{
			double angle = 0.25 * i * M_PI * 2.0;
			auto  t = makeRigidTransform(makeRotationMatrix(angle, axis), SurgSim::Math::Vector3d::Zero());
			auto fem = makeFem1D(filename, t);
			knotId.setFem1d(fem);
			knotId.doInitialize();
			knotId.update(0.0);
			EXPECT_EQ(knotname, knotId.getKnotName());
		}

		axis = SurgSim::Math::Vector3d::UnitY();
		for (int i = 0; i < 4; ++i)
		{
			double angle = 0.25 * i * M_PI * 2.0;
			auto  t = makeRigidTransform(makeRotationMatrix(angle, axis), SurgSim::Math::Vector3d::Zero());
			auto fem = makeFem1D(filename, t);
			knotId.setFem1d(fem);
			knotId.doInitialize();
			knotId.update(0.0);
			EXPECT_EQ(knotname, knotId.getKnotName());
		}

		axis = SurgSim::Math::Vector3d::UnitZ();
		for (int i = 0; i < 4; ++i)
		{
			double angle = 0.25 * i * M_PI * 2.0;
			auto  t = makeRigidTransform(makeRotationMatrix(angle, axis), SurgSim::Math::Vector3d::Zero());
			auto fem = makeFem1D(filename, t);
			knotId.setFem1d(fem);
			knotId.doInitialize();
			knotId.update(0.0);
			EXPECT_EQ(knotname, knotId.getKnotName());
		}
		return true;
	}

	bool testKnotRandom(std::string filename, std::string knotname, int iterations = 1000, unsigned int seed = 1000)
	{
		std::default_random_engine generator;
		std::uniform_real_distribution<double> distribution(0, 1);

		generator.seed(seed);

		using SurgSim::Math::makeRigidTransform;
		using SurgSim::Math::makeRotationMatrix;

		auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");

		auto mesh = std::make_shared<SurgSim::Physics::Fem1D>();
		mesh->load(filename);

		std::cout << knotname << ":";
		SurgSim::Math::Vector3d axis = SurgSim::Math::Vector3d::UnitX();
		for (int i = 0; i < iterations; ++i)
		{
			KnotIdentificationBehavior knotId("Knot ID");
			if (i % 100 == 0) std::cout << "+";

			double angle = distribution(generator) * M_PI * 2.0;
			auto axis = SurgSim::Math::Vector3d
			(distribution(generator), distribution(generator), distribution(generator));
			axis.normalize();
			auto  t = makeRigidTransform(makeRotationMatrix(angle, axis), SurgSim::Math::Vector3d::Zero());

			auto fem = std::make_shared<SurgSim::Physics::Fem1DRepresentation>("Physics");
			fem->setFemElementType("SurgSim::Physics::Fem1DElementBeam");
			fem->setLocalPose(t);
			fem->setFem(mesh);
			fem->setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT);
			fem->setLinearSolver(SurgSim::Math::LINEARSOLVER_LU);
			fem->setRayleighDampingMass(5.0);
			fem->setRayleighDampingStiffness(0.001);
			fem->setIsGravityEnabled(true);
			fem->initialize(runtime);

			knotId.setFem1d(fem);
			knotId.doInitialize();
			knotId.update(0.0);
			EXPECT_EQ(knotname, knotId.getKnotName()) << "Axis: " << axis.transpose() << " Angle: " << angle;
			if (knotname != knotId.getKnotName())
			{
				using SurgSim::Math::toBytes;
				std::vector<uint8_t> bytes;
				toBytes(angle, &bytes);
				toBytes(axis, &bytes);

				std::ofstream out("knoterror.bin", std::ios_base::binary);
				SURGSIM_ASSERT(out.is_open());

				for (auto b : bytes) out << b;
				return false;
			}
		}

		std::cout << "\n";
		return true;
	}

	bool testKnotFromFailure(std::string filename, std::string knotname, std::string failureFile)
	{

		KnotIdentificationBehavior knotId("Knot ID");

		using SurgSim::Math::makeRigidTransform;
		using SurgSim::Math::makeRotationMatrix;

		auto runtime = std::make_shared<SurgSim::Framework::Runtime>("config.txt");

		auto mesh = std::make_shared<SurgSim::Physics::Fem1D>();
		mesh->load(filename);

		double angle;
		SurgSim::Math::Vector3d axis;

		using SurgSim::Math::fromBytes;
		std::ifstream in(failureFile, std::ios_base::binary);
		EXPECT_TRUE(!in.bad());
		std::vector<uint8_t> bytes(std::istreambuf_iterator<char>(in), {});

		auto end = fromBytes(bytes, &angle);
		end += fromBytes(bytes, &axis, end);
		EXPECT_EQ(end, bytes.size());
		auto  t = makeRigidTransform(makeRotationMatrix(angle, axis), SurgSim::Math::Vector3d::Zero());

		auto fem = std::make_shared<SurgSim::Physics::Fem1DRepresentation>("Physics");
		fem->setFemElementType("SurgSim::Physics::Fem1DElementBeam");
		fem->setLocalPose(t);
		fem->setFem(mesh);
		fem->setIntegrationScheme(SurgSim::Math::INTEGRATIONSCHEME_EULER_IMPLICIT);
		fem->setLinearSolver(SurgSim::Math::LINEARSOLVER_LU);
		fem->setRayleighDampingMass(5.0);
		fem->setRayleighDampingStiffness(0.001);
		fem->setIsGravityEnabled(true);
		fem->initialize(runtime);

		knotId.setFem1d(fem);
		knotId.doInitialize();
		knotId.update(0.0);
		EXPECT_EQ(knotname, knotId.getKnotName()) << "Axis: " << axis.transpose() << " Angle: " << angle;
		return true;
	}

	void testReidmeisterMove1(std::vector<KnotIdentificationBehavior::Crossing> gaussCodeBefore,
		std::vector<KnotIdentificationBehavior::Crossing> gaussCodeAfter,
							  std::vector<int> erased = std::vector<int>())
	{
		MockKnotIdentificationBehavior mockKnotIdentificationBehavior("Test ReidmeisterMove1");
		std::vector<int> erasedCalculated;
		bool expectedReturn = gaussCodeBefore != gaussCodeAfter;
		bool actualReturn = mockKnotIdentificationBehavior.tryReidmeisterMove1(&gaussCodeBefore, &erasedCalculated);
		EXPECT_EQ(expectedReturn, actualReturn);
		EXPECT_EQ(gaussCodeBefore, gaussCodeAfter);
		std::sort(erased.begin(), erased.end(), std::greater<int>());
		std::sort(erasedCalculated.begin(), erasedCalculated.end(), std::greater<int>());
		EXPECT_EQ(erased, erasedCalculated);
	}

	void testReidmeisterMove2(std::vector<KnotIdentificationBehavior::Crossing> gaussCodeBefore,
		std::vector<KnotIdentificationBehavior::Crossing> gaussCodeAfter,
							  std::vector<int> erased = std::vector<int>())
	{
		MockKnotIdentificationBehavior mockKnotIdentificationBehavior("Test ReidmeisterMove2");
		std::vector<int> erasedCalculated;
		bool expectedReturn = gaussCodeBefore != gaussCodeAfter;
		bool actualReturn = mockKnotIdentificationBehavior.tryReidmeisterMove2(&gaussCodeBefore, &erasedCalculated);
		EXPECT_EQ(expectedReturn, actualReturn);
		EXPECT_EQ(gaussCodeBefore, gaussCodeAfter);
		std::sort(erased.begin(), erased.end(), std::greater<int>());
		std::sort(erasedCalculated.begin(), erasedCalculated.end(), std::greater<int>());
		EXPECT_EQ(erased, erasedCalculated);
	}

	void testReidmeisterMove3(std::vector<KnotIdentificationBehavior::Crossing> gaussCodeBefore,
		std::vector<KnotIdentificationBehavior::Crossing> gaussCodeAfter, bool expectedReturn)
	{
		bool actualReturn = mockReidmeisterMove3Behavior.tryReidmeisterMove3(&gaussCodeBefore);
		EXPECT_EQ(expectedReturn, actualReturn);
		EXPECT_EQ(gaussCodeBefore, gaussCodeAfter);
	}

	void testAdjustGaussCodeForErasedCrossings(std::vector<KnotIdentificationBehavior::Crossing> gaussCodeBefore,
		std::vector<KnotIdentificationBehavior::Crossing> gaussCodeAfter, std::vector<int> erased)
	{
		MockKnotIdentificationBehavior mockKnotIdentificationBehavior("Test AdjustGaussCodeForErasedCrossings");
		mockKnotIdentificationBehavior.adjustGaussCodeForErasedCrossings(&gaussCodeBefore);
		EXPECT_EQ(gaussCodeBefore, gaussCodeAfter);
	}

	void testIdentifyKnot(std::vector<KnotIdentificationBehavior::Crossing> gaussCode, std::string expected)
	{
		MockKnotIdentificationBehavior mockKnotIdentificationBehavior("Test IdentifyKnot");
		auto actual = mockKnotIdentificationBehavior.identifyKnot(gaussCode);
		EXPECT_EQ(expected, actual);
		// Clear the list of known knot codes and identify again. If gaussCode is empty, it should say "No Knot",
		// otherwise "Unknown Knot".
		mockKnotIdentificationBehavior.clearKnownKnotCodes();
		actual = mockKnotIdentificationBehavior.identifyKnot(gaussCode);
		if (gaussCode.empty())
		{
			EXPECT_EQ("No Knot", actual);
		}
		else
		{
			EXPECT_EQ("Unknown Knot", actual);
		}
	}

	MockKnotIdentificationBehavior mockReidmeisterMove3Behavior;
};

TEST_F(KnotIdentificationBehaviorTest, Constructor)
{
	EXPECT_NO_THROW(KnotIdentificationBehavior knotId("KnotId"));
	EXPECT_NO_THROW(new KnotIdentificationBehavior("KnotId"));
	EXPECT_NO_THROW(auto x = std::make_shared<KnotIdentificationBehavior>("KnotId"));
}

TEST_F(KnotIdentificationBehaviorTest, GetSetFem1D)
{
	KnotIdentificationBehavior knotId("KnotId");
	EXPECT_EQ(nullptr, knotId.getFem1d());
	auto physics = std::make_shared<SurgSim::Physics::Representation>("Physics Representation");
	EXPECT_THROW(knotId.setFem1d(physics), SurgSim::Framework::AssertionFailure);
	EXPECT_FALSE(knotId.doWakeUp());
	auto fem1D = std::make_shared<SurgSim::Physics::Fem1DRepresentation>("Fem1D");
	EXPECT_NO_THROW(knotId.setFem1d(fem1D));
	EXPECT_TRUE(knotId.doWakeUp());
	EXPECT_EQ(fem1D->getName(), knotId.getFem1d()->getName());
}

TEST_F(KnotIdentificationBehaviorTest, ReidmeisterMove1)
{
	{
		std::vector<int> input = boost::assign::list_of(1)(-2)(3)(-1)(2)(-3);
		std::vector<int> expected = boost::assign::list_of(1)(-2)(3)(-1)(2)(-3);
		testReidmeisterMove1(createGaussCode(input), createGaussCode(expected));
	}
	{
		std::vector<int> input = boost::assign::list_of(1)(-2)(3)(-3)(-1)(2);
		std::vector<int> expected = boost::assign::list_of(1)(-2)(-1)(2);
		std::vector<int> erased = boost::assign::list_of(3);
		testReidmeisterMove1(createGaussCode(input), createGaussCode(expected), erased);
	}
	{
		std::vector<int> input = boost::assign::list_of(3)(1)(-2)(-1)(2)(-3);
		std::vector<int> expected = boost::assign::list_of(1)(-2)(-1)(2);
		std::vector<int> erased = boost::assign::list_of(3);
		testReidmeisterMove1(createGaussCode(input), createGaussCode(expected), erased);
	}
	{
		std::vector<int> input = boost::assign::list_of(3)(1)(4)(-4)(-2)(6)(-6)(-1)(-5)(5)(2)(-3);
		std::vector<int> expected = boost::assign::list_of(1)(-2)(-1)(2);
		std::vector<int> erased = boost::assign::list_of(3)(4)(5)(6);
		testReidmeisterMove1(createGaussCode(input), createGaussCode(expected), erased);
	}
	{
		std::vector<int> input = boost::assign::list_of(3)(1)(4)(-4)(-2)(6)(7)(-6)(-7)(-1)(-5)(5)(2)(-3);
		std::vector<int> expected = boost::assign::list_of(1)(-2)(6)(7)(-6)(-7)(-1)(2);
		std::vector<int> erased = boost::assign::list_of(3)(4)(5);
		testReidmeisterMove1(createGaussCode(input), createGaussCode(expected), erased);
	}
}

TEST_F(KnotIdentificationBehaviorTest, ReidmeisterMove2)
{
	{
		std::vector<int> input = boost::assign::list_of(3)(1)(-2)(-1)(2)(-3)(4)(5)(-4)(-5);
		std::vector<int> expected = boost::assign::list_of(3)(1)(-2)(-1)(2)(-3);
		std::vector<int> erased = boost::assign::list_of(4)(5);
		testReidmeisterMove2(createGaussCode(input), createGaussCode(expected), erased);
	}
	{
		std::vector<int> input = boost::assign::list_of(-5)(3)(1)(-2)(-1)(2)(-3)(4)(5)(-4);
		std::vector<int> expected = boost::assign::list_of(3)(1)(-2)(-1)(2)(-3);
		std::vector<int> erased = boost::assign::list_of(4)(5);
		testReidmeisterMove2(createGaussCode(input), createGaussCode(expected), erased);
	}
	{
		std::vector<int> input = boost::assign::list_of(3)(1)(-2)(7)(6)(-1)(2)(-3)(4)(5)(-7)(-6)(-4)(-5);
		std::vector<int> expected = boost::assign::list_of(3)(1)(-2)(-1)(2)(-3)(4)(5)(-4)(-5);
		std::vector<int> erased = boost::assign::list_of(6)(7);
		testReidmeisterMove2(createGaussCode(input), createGaussCode(expected), erased);
	}
}

TEST_F(KnotIdentificationBehaviorTest, ReidmeisterMove3Test1)
{
	std::vector<int> input = boost::assign::list_of(-1)(-3)(-2)(1)(2)(3);
	std::vector<int> expected = boost::assign::list_of(-1)(-3)(3)(-2)(1)(2);
	testReidmeisterMove3(createGaussCode(input), createGaussCode(expected), true);
	std::vector<int> expected2 = boost::assign::list_of(2)(-3)(-2)(3)(1)(-1);
	testReidmeisterMove3(createGaussCode(expected), createGaussCode(expected2), true);
	std::vector<int> expected3 = boost::assign::list_of(3)(-1)(-3)(1)(2)(-2);
	testReidmeisterMove3(createGaussCode(expected2), createGaussCode(expected3), true);
	std::vector<int> expected4 = boost::assign::list_of(-2)(-1)(1)(-3)(2)(3);
	testReidmeisterMove3(createGaussCode(expected3), createGaussCode(expected4), true);
	testReidmeisterMove3(createGaussCode(expected4), createGaussCode(input), false);
}

TEST_F(KnotIdentificationBehaviorTest, ReidmeisterMove3Test2)
{
	std::vector<int> input = boost::assign::list_of(3)(1)(-4)(-2)(-1)(-5)(2)(-3)(4)(5);
	std::vector<int> expected = boost::assign::list_of(3)(1)(-4)(-3)(-2)(-5)(-1)(2)(4)(5);
	testReidmeisterMove3(createGaussCode(input), createGaussCode(expected), true);
}

TEST_F(KnotIdentificationBehaviorTest, AdjustGaussCodeForErasedCrossings)
{
	{
		std::vector<int> input = boost::assign::list_of(3)(-4)(5)(-3)(4)(-5);
		std::vector<int> expected = boost::assign::list_of(1)(-2)(3)(-1)(2)(-3);
		std::vector<int> erased = boost::assign::list_of(1)(2);
		testAdjustGaussCodeForErasedCrossings(createGaussCode(input), createGaussCode(expected), erased);
	}
	{
		std::vector<int> input = boost::assign::list_of(1)(-4)(5)(-1)(4)(-5);
		std::vector<int> expected = boost::assign::list_of(1)(-2)(3)(-1)(2)(-3);
		std::vector<int> erased = boost::assign::list_of(2)(3);
		testAdjustGaussCodeForErasedCrossings(createGaussCode(input), createGaussCode(expected), erased);
	}
}

TEST_F(KnotIdentificationBehaviorTest, IdentifyKnotTest1)
{
	testIdentifyKnot(createGaussCode(std::vector<int>()), "No Knot");
}

TEST_F(KnotIdentificationBehaviorTest, IdentifyKnotTest2)
{
	std::vector<int> input = boost::assign::list_of(1)(-4)(5)(-1)(4)(-5);
	testIdentifyKnot(createGaussCode(input), "Unknown Knot");
}

TEST_F(KnotIdentificationBehaviorTest, IdentifyKnotTest3)
{
	std::vector<int> ids = boost::assign::list_of(1)(-2)(3)(-1)(2)(-3);
	std::vector<int> signs = boost::assign::list_of(1)(1)(1)(1)(1)(1);
	testIdentifyKnot(createGaussCode(ids, signs), "Trefoil Knot");
}

TEST_F(KnotIdentificationBehaviorTest, IdentifyKnotTest4)
{
	std::vector<int> ids = boost::assign::list_of(-1)(2)(-3)(1)(-2)(3);
	std::vector<int> signs = boost::assign::list_of(1)(1)(1)(1)(1)(1);
	testIdentifyKnot(createGaussCode(ids, signs), "Trefoil Knot");
}

TEST_F(KnotIdentificationBehaviorTest, IdentifyKnotTest5)
{
	std::vector<int> ids = boost::assign::list_of(1)(-2)(3)(-4)(5)(-6)(4)(-5)(6)(-1)(2)(-3);
	std::vector<int> signs = boost::assign::list_of(1)(1)(1)(1)(1)(1)(1)(1)(1)(1)(1)(1);
	testIdentifyKnot(createGaussCode(ids, signs), "Granny Knot");
}

TEST_F(KnotIdentificationBehaviorTest, IdentifyKnotTest6)
{
	std::vector<int> ids = boost::assign::list_of(-1)(2)(-3)(4)(-5)(6)(-4)(5)(-6)(1)(-2)(3);
	std::vector<int> signs = boost::assign::list_of(1)(1)(1)(1)(1)(1)(1)(1)(1)(1)(1)(1);
	testIdentifyKnot(createGaussCode(ids, signs), "Granny Knot");
}

TEST_F(KnotIdentificationBehaviorTest, IdentifyKnotTest7)
{
	std::vector<int> ids = boost::assign::list_of(1)(-2)(3)(4)(-5)(6)(-4)(5)(-6)(-1)(2)(-3);
	std::vector<int> signs = boost::assign::list_of(1)(1)(1)(-1)(-1)(-1)(-1)(-1)(-1)(1)(1)(1);
	testIdentifyKnot(createGaussCode(ids, signs), "Square Knot");
}

TEST_F(KnotIdentificationBehaviorTest, Fem1DNoKnot)
{
	KnotIdentificationBehavior knotId("Knot ID");
	knotId.setFem1d(makeFem1D("Geometry/loop.ply"));
	knotId.doInitialize();
	knotId.update(0.0);
	EXPECT_EQ("No Knot", knotId.getKnotName());
}

TEST_F(KnotIdentificationBehaviorTest, Fem1DTrefoil)
{
	EXPECT_TRUE(testKnot("Geometry/trefoil_knot.ply", "Trefoil Knot"));
}

TEST_F(KnotIdentificationBehaviorTest, Fem1DSquare)
{
	EXPECT_TRUE(testKnot("Geometry/square_knot.ply", "Square Knot"));
}

TEST_F(KnotIdentificationBehaviorTest, Fem1DGranny)
{
	EXPECT_TRUE(testKnot("Geometry/granny_knot.ply", "Granny Knot"));
}

/*
TEST_F(KnotIdentificationBehaviorTest, Fem1DTrefoilRandom)
{
	std::default_random_engine generator;
	std::uniform_int_distribution<unsigned int> distribution;

	for (;;)
	{
		auto seed = distribution(generator);

		std::cout << seed << "\n";

		ASSERT_TRUE(testKnotRandom("Geometry/trefoil_knot.ply", "Trefoil Knot", 10000, seed));
		ASSERT_TRUE(testKnotRandom("Geometry/square_knot.ply", "Square Knot", 10000, seed));
		ASSERT_TRUE(testKnotRandom("Geometry/granny_knot.ply", "Granny Knot", 10000, seed));
	}
}

TEST_F(KnotIdentificationBehaviorTest, ErrorFiles)
{
		//ASSERT_TRUE(testKnotFromFailure("Geometry/square_knot.ply", "Square Knot", "knoterror1.bin"));
		ASSERT_TRUE(testKnotFromFailure("Geometry/square_knot.ply", "Square Knot", "knoterror.bin"));
		//ASSERT_TRUE(testKnotFromFailure("Geometry/square_knot.ply", "Square Knot", "knoterror3.bin"));
}
*/
}
}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-predicates
    Function object implementing is_not_equal capabilities

    Returns @ref True if x and y are not equal else @ref False.
    Infix notation can be used with operator '!='.

    @par Semantic:

    @code
    auto r = is_not_equal(x,y);
    @endcode

    or

    @code
    auto r = x != y;
    @endcode

    @see  is_equal, is_nez, is_not_equal_with_equal_nans

  **/
  as_logical_t<Value> is_not_equal(Value const& x);

} }
#endif

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

#endif


// (C) Copyright Renaud Detry   2007-2015.
// Distributed under the GNU General Public License and under the
// BSD 3-Clause License (See accompanying file LICENSE.txt).

/** @file */

#include <nuklei/Kernel.h>
#include <nuklei/Common.h>
#include <nuklei/Indenter.h>
#include <boost/static_assert.hpp>

namespace nuklei
{

  namespace kernel
  {

    const std::string base::TypeNames[] = { "r3", "r3xs2", "r3xs2p", "se3" };

    std::ostream& operator<<(std::ostream &out, const kernel::base &k)
    {
      return k.polyPrint(out);
    }

    // SE(3)

    void se3::assertConsistency() const
    {
      NUKLEI_TRACE_BEGIN();
      NUKLEI_ASSERT(w_ >= 0);
      la::check(ori_, __FUNCTION__);
      NUKLEI_ASSERT(loc_h_ >= 0);
      NUKLEI_ASSERT(ori_h_ >= 0);
      NUKLEI_TRACE_END();
    }

    std::ostream& se3::print(std::ostream &out) const
    {
      Indenter idt(out);
      idt << "Kernel SE(3): [ weight = " << w_ << " ]" << std::endl;
      {
        Indenter idt2(out);
        idt2 << "Location: " << loc_ << std::endl;
        idt2 << "LocWidth: " << loc_h_ << std::endl;
        idt2 << "Orientation: ";
        ::nuklei::operator<<(idt2, ori_) << std::endl;
        idt2 << "OriWidth: " << ori_h_ << std::endl;
      }
      return out;
    }

    kernel::base::Type se3::type() const
    {
      return SE3;
    }

    //coord_t se3::eval(const kernel::se3& k) const
    //cf. header.

    kernel::se3 se3::sample() const
    {
      kernel::se3 s;
      typedef nuklei::sampler<PositionKernel> PositionSampler;

      typedef nuklei::sampler<OrientationKernel> OrientationSampler;

      s.loc_ = PositionSampler::s(loc_, loc_h_);
      s.ori_ = OrientationSampler::s(ori_, ori_h_);

      if (hasDescriptor()) s.setDescriptor(getDescriptor());
      return s;
    }

    kernel::se3 se3::se3Sample() const
    {
      return sample();
    }

    kernel::se3 se3::se3Proj() const
    {
      kernel::se3 k;
      k.loc_ = loc_;
      k.ori_ = ori_;
      if (hasDescriptor()) k.setDescriptor(getDescriptor());
      return *this;
    }

    kernel::se3 se3::projectedOn(const kernel::se3& k) const
    {
      kernel::se3 p;
      la::project(p.loc_, p.ori_, k.loc_, k.ori_, loc_, ori_);
      if (hasDescriptor()) p.setDescriptor(getDescriptor());
      return p;
    }

    kernel::se3 se3::transformedWith(const kernel::se3& k) const
    {
      kernel::se3 p;
      la::transform(p.loc_, p.ori_, k.loc_, k.ori_, loc_, ori_);
      if (hasDescriptor()) p.setDescriptor(getDescriptor());
      return p;
    }

    void se3::makeTransformWith(const kernel::se3& k)
    {
      la::transform(loc_, ori_, k.loc_, k.ori_, loc_, ori_);
    }

    kernel::se3 se3::transformationFrom(const kernel::se3& k) const
    {
      kernel::se3 p;
      la::transfoTo(p.loc_, p.ori_, k.loc_, k.ori_, loc_, ori_);
      if (hasDescriptor()) p.setDescriptor(getDescriptor());
      return p;
    }

    kernel::se3 se3::inverseTransformation() const
    {
      kernel::se3 origin;
      kernel::se3 it = origin.transformationFrom(*this);
      if (hasDescriptor()) it.setDescriptor(getDescriptor());
      return it;
    }

    kernel::se3
    kernel::se3::linearInterpolation(const kernel::se3& k,
                                     const coord_t x) const
    {
      NUKLEI_ASSERT(0 <= x && x <= 1);
      kernel::se3 i;
      i.loc_ = (1-x) * loc_ + x * k.loc_;
      Quaternion q = k.ori_;
      if (ori_.Dot(q) < 0) q = - q;
      //fixme: this should use slerp.
      i.ori_ = (1-x) * ori_ + x * q;
      i.ori_.Normalize();
      return i;
    }

    void kernel::se3::updateWidth(const kernel::se3& k,
                                  const coord_t x)
    {
      loc_h_ = std::sqrt( (1-x) * loc_h_*loc_h_ +
                         x * std::pow(dist<groupS::r3>::d(loc_, k.loc_), 2) );
      ori_h_ = std::sqrt( (1-x) * ori_h_*ori_h_ +
                         x * std::pow(dist<groupS::so3>::d(ori_, k.ori_), 2) );
    }

    coord_pair kernel::se3::distanceTo(const kernel::se3& k) const
    {
      return std::make_pair(dist<groupS::r3>::d(loc_, k.loc_),
                            dist<groupS::so3>::d(ori_, k.ori_));
    }

    // S^2(_+)

    // R^3

    void r3::assertConsistency() const
    {
      NUKLEI_TRACE_BEGIN();
      NUKLEI_ASSERT(w_ >= 0);
      NUKLEI_ASSERT(loc_h_ >= 0);
      NUKLEI_TRACE_END();
    }

    std::ostream& r3::print(std::ostream &out) const
    {
      Indenter idt(out);
      idt << "Kernel R^3: [ weight = " << w_ << " ]" << std::endl;
      {
        Indenter idt2(out);
        idt2 << "Location: " << loc_ << std::endl;
        idt2 << "LocWidth: " << loc_h_ << std::endl;
      }
      return out;
    }

    kernel::base::Type r3::type() const
    {
      return R3;
    }

    //coord_t r3::eval(const kernel::r3& k) const
    //cf. header.

    kernel::r3 r3::sample() const
    {
      kernel::r3 s;

      typedef nuklei::sampler<PositionKernel> PositionSampler;

      s.loc_ = PositionSampler::s(loc_, loc_h_);

      if (hasDescriptor()) s.setDescriptor(getDescriptor());
      return s;
    }

    kernel::se3 r3::se3Sample() const
    {
      kernel::r3 r3k = sample();
      kernel::se3 se3k;
      se3k.loc_ = r3k.loc_;
      se3k.ori_ = random_element<groupS::so3>::r();
      if (hasDescriptor()) se3k.setDescriptor(getDescriptor());
      return se3k;
    }

    kernel::se3 r3::se3Proj() const
    {
      kernel::r3 r3k = *this;
      kernel::se3 se3k;
      se3k.loc_ = r3k.loc_;
      se3k.ori_ = random_element<groupS::so3>::r();
      if (hasDescriptor()) se3k.setDescriptor(getDescriptor());
      return se3k;
    }

    kernel::r3 r3::projectedOn(const kernel::se3& k) const
    {
      kernel::r3 p;
      p.loc_ = la::project(k.loc_, k.ori_, loc_);
      if (hasDescriptor()) p.setDescriptor(getDescriptor());
      return p;
    }

    kernel::r3 r3::transformedWith(const kernel::se3& k) const
    {
      kernel::r3 p;
      p.loc_ = la::transform(k.loc_, k.ori_, loc_);
      if (hasDescriptor()) p.setDescriptor(getDescriptor());
      return p;
    }

    void r3::makeTransformWith(const kernel::se3& k)
    {
      loc_ = la::transform(k.loc_, k.ori_, loc_);
    }

    kernel::r3
    kernel::r3::linearInterpolation
    (const kernel::r3& k,
     const coord_t x) const
    {
      NUKLEI_ASSERT(0 <= x && x <= 1);
      kernel::r3 i;
      i.loc_ = (1-x) * loc_ + x * k.loc_;
      return i;
    }

    void kernel::r3::updateWidth(const kernel::r3& k,
                                 const coord_t x)
    {
      loc_h_ = std::sqrt( (1-x) * loc_h_*loc_h_ +
                         x * std::pow(dist<groupS::r3>::d(loc_, k.loc_), 2) );
    }

    coord_pair
    kernel::r3::distanceTo(const kernel::r3& k) const
    {
      return std::make_pair(dist<groupS::r3>::d(loc_, k.loc_), -1.);
    }

  }

  namespace enforce_data_align {

    BOOST_STATIC_ASSERT(sizeof(kernel::base) % 8 == 0);

  }

}

#if BOOST_VERSION < 104100

BOOST_CLASS_EXPORT_GUID(nuklei::kernel::se3, "mdfh_kernel_se3")
BOOST_CLASS_EXPORT_GUID(nuklei::kernel::r3xs2, "mdfh_kernel_r3xs2")
BOOST_CLASS_EXPORT_GUID(nuklei::kernel::r3xs2p, "mdfh_kernel_r3xs2p")
BOOST_CLASS_EXPORT_GUID(nuklei::kernel::r3, "mdfh_kernel_r3")

#else

BOOST_CLASS_EXPORT_IMPLEMENT(nuklei::kernel::se3)
BOOST_CLASS_EXPORT_IMPLEMENT(nuklei::kernel::r3xs2)
BOOST_CLASS_EXPORT_IMPLEMENT(nuklei::kernel::r3xs2p)
BOOST_CLASS_EXPORT_IMPLEMENT(nuklei::kernel::r3)

#endif // BOOST_VERSION


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

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

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

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

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

/** \example multithreaded.cpp
*
*   This tutorial shows how to use ViennaCL with multiple threads, one thread per GPU.
*   The use of one thread per context (host, CUDA, OpenCL) is supported.
*   However, using more than one thread per context is not fully covered by the OpenCL standard.
*   It is, however, perfectly fine to use multiple OpenCL contexts simultaneously, each using one thread.
*
*   We start with including the necessary headers:
**/

#ifndef VIENNACL_WITH_OPENCL
  #define VIENNACL_WITH_OPENCL
#endif

// include necessary system headers
#include <iostream>

//include basic scalar and vector types of ViennaCL
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"

#include "viennacl/ocl/device.hpp"
#include "viennacl/ocl/platform.hpp"
#include "viennacl/ocl/backend.hpp"

//include the generic inner product functions of ViennaCL
#include "viennacl/linalg/norm_2.hpp"

// Some helper functions for this tutorial:
#include "Random.hpp"

#include <boost/thread.hpp>

/**
*  Each thread runs a separate task, for which we provide the following functor.
*  For each thread, vector operations and a vector norm gets computed.
**/
template<typename NumericT>
class worker
{
public:
  worker(std::size_t tid) : thread_id_(tid) {}

  void operator()()
  {
    std::size_t N = 6;

    viennacl::context ctx(viennacl::ocl::get_context(static_cast<long>(thread_id_)));
    viennacl::vector<NumericT> u = viennacl::scalar_vector<NumericT>(N, NumericT(1) * NumericT(thread_id_ + 1), ctx);
    viennacl::vector<NumericT> v = viennacl::scalar_vector<NumericT>(N, NumericT(2) * NumericT(thread_id_ + 1), ctx);
    viennacl::matrix<NumericT> A = viennacl::linalg::outer_prod(u, v);
    viennacl::vector<NumericT> x(u);

    u += v;
    NumericT result = viennacl::linalg::norm_2(u);

    std::stringstream ss;
    ss << "Result of thread " << thread_id_ << " on device " << viennacl::ocl::get_context(static_cast<long>(thread_id_)).devices()[0].name() << ": " << result << std::endl;
    ss << "  A: " << A << std::endl;
    ss << "  x: " << x << std::endl;
    message_ = ss.str();
  }

  std::string message() const { return message_; }

private:
  std::string message_;
  std::size_t thread_id_;
};

/**
*   In the main routine we create two OpenCL contexts and then use one thread per context to run the operations in the functor defined above.
**/
int main()
{
  // Change this type definition to double if your gpu supports that
  typedef float       ScalarType;

  if (viennacl::ocl::get_platforms().size() == 0)
  {
    std::cerr << "Error: No platform found!" << std::endl;
    return EXIT_FAILURE;
  }

  /**
  *   Part 1: Setup first device for first context, second device for second context:
  **/
  viennacl::ocl::platform pf = viennacl::ocl::get_platforms()[0];
  std::vector<viennacl::ocl::device> const & devices = pf.devices();

  // Set first device to first context:
  viennacl::ocl::setup_context(0, devices[0]);

  // Set second device for second context (use the same device for the second context if only one device available):
  if (devices.size() > 1)
    viennacl::ocl::setup_context(1, devices[1]);
  else
    viennacl::ocl::setup_context(1, devices[0]);

  /**
  *   Part 2: Now let two threads operate on two GPUs in parallel.
  **/

  worker<ScalarType> work_functor0(0);
  worker<ScalarType> work_functor1(1);
  boost::thread worker_thread_0(boost::ref(work_functor0));
  boost::thread worker_thread_1(boost::ref(work_functor1));

  worker_thread_0.join();
  worker_thread_1.join();

  std::cout << work_functor0.message() << std::endl;
  std::cout << work_functor1.message() << std::endl;

  /**
  *  We're done - print a success message.
  **/
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return EXIT_SUCCESS;
}


#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <message_filters/subscriber.h>
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>
#include <boost/math/constants/constants.hpp>

namespace
{

using namespace std;
using namespace ros;

static const double ANGULAR_VELOCITY_THRESHOLD = 0.01; // radians per second
static const double ANGULAR_TOLERANCE = 2 * boost::math::constants::pi<double>() / 100.0;

class FallReadiness
{
private:

    //! Node handle
    ros::NodeHandle nh;

    //! Private nh
    ros::NodeHandle pnh;

    //! IMU subscription
    auto_ptr<message_filters::Subscriber<sensor_msgs::Imu> > imuSub;

    //! Whether the pole is currently stopped
    bool isStopped;

    //! Whether the pole is allowably close to vertical
    bool isWithinAngularTolerance;

    //! Whether this is the first iteration
    bool isFirstIteration;

public:
    FallReadiness() :
        pnh("~")
    {
        imuSub.reset(new message_filters::Subscriber<sensor_msgs::Imu>(nh, "/human/imu", 1));
        imuSub->registerCallback(boost::bind(&FallReadiness::imuCallback, this, _1));
        imuSub->subscribe();

        isStopped = false;
        isWithinAngularTolerance = false;
        isFirstIteration = true;

        ROS_DEBUG("Fall readiness initialized successfully");
    }

    void imuCallback(sensor_msgs::ImuConstPtr data)
    {
        ROS_DEBUG("Angular velocity [%f] [%f] [%f]", data->angular_velocity.x, data->angular_velocity.y, data->angular_velocity.z);
        bool previousIsStopped = isStopped;
        isStopped = fabs(data->angular_velocity.x) < ANGULAR_VELOCITY_THRESHOLD && fabs(data->angular_velocity.y) < ANGULAR_VELOCITY_THRESHOLD
                    && fabs(data->angular_velocity.z) < ANGULAR_VELOCITY_THRESHOLD;

        tf::Quaternion q;
        tf::quaternionMsgToTF(data->orientation, q);
        tf::Matrix3x3 m(q);
        double roll, pitch, yaw;
        m.getRPY(roll, pitch, yaw);
        ROS_DEBUG("Roll, Pitch, Yaw [%f] [%f] [%f]", roll, pitch, yaw);

        bool previousWithinTolerance = isWithinAngularTolerance;
        isWithinAngularTolerance = pitch - boost::math::constants::pi<double>() / 2.0 < ANGULAR_TOLERANCE
          && fabs(yaw) < ANGULAR_TOLERANCE;

        if (isFirstIteration || isStopped != previousIsStopped) {
            ROS_INFO("Currently stopped: %s", isStopped ? "true" : "false");
        }
        if (isFirstIteration || previousWithinTolerance != isWithinAngularTolerance) {
            ROS_INFO("Is within angular tolerance: %s", isWithinAngularTolerance ? "true" : "false");
        }
        isFirstIteration = false;
    }
};
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "fall_readiness");
    FallReadiness fs;
    ros::spin();
}


// Copyright 2015-2019 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/histogram.hpp>
#include "../test/utility_histogram.hpp"

#include <cassert>
#include <cstdio>
#include <ctime>
#include <memory>
#include <random>

using namespace boost::histogram;
using reg = axis::regular<>;

std::unique_ptr<double[]> random_array(unsigned n, int type) {
  std::unique_ptr<double[]> r(new double[n]);
  std::default_random_engine gen(1);
  if (type) { // type == 1
    std::normal_distribution<> d(0.5, 0.3);
    for (unsigned i = 0; i < n; ++i) r[i] = d(gen);
  } else { // type == 0
    std::uniform_real_distribution<> d(0.0, 1.0);
    for (unsigned i = 0; i < n; ++i) r[i] = d(gen);
  }
  return r;
}

double baseline(unsigned n) {
  auto r = random_array(n, 0);
  auto t = clock();
  for (auto it = r.get(), end = r.get() + n; it != end;) {
    volatile auto x = *it++;
    (void)(x);
  }
  return (double(clock()) - t) / CLOCKS_PER_SEC / n * 1e9;
}

template <typename Tag, typename Storage>
double compare_1d(unsigned n, int distrib) {
  auto r = random_array(n, distrib);
  auto h = make_s(Tag(), Storage(), reg(100, 0, 1));
  auto t = clock();
  for (auto it = r.get(), end = r.get() + n; it != end;) h(*it++);
  return (double(clock()) - t) / CLOCKS_PER_SEC / n * 1e9;
}

template <typename Tag, typename Storage>
double compare_2d(unsigned n, int distrib) {
  auto r = random_array(n, distrib);
  auto h = make_s(Tag(), Storage(), reg(100, 0, 1), reg(100, 0, 1));
  auto t = clock();
  for (auto it = r.get(), end = r.get() + n; it != end;) {
    const auto x = *it++;
    const auto y = *it++;
    h(x, y);
  }
  return (double(clock()) - t) / CLOCKS_PER_SEC / n * 1e9;
}

template <typename Tag, typename Storage>
double compare_3d(unsigned n, int distrib) {
  auto r = random_array(n, distrib);
  auto h = make_s(Tag(), Storage(), reg(100, 0, 1), reg(100, 0, 1), reg(100, 0, 1));
  auto t = clock();
  for (auto it = r.get(), end = r.get() + n; it != end;) {
    const auto x = *it++;
    const auto y = *it++;
    const auto z = *it++;
    h(x, y, z);
  }
  return (double(clock()) - t) / CLOCKS_PER_SEC / n * 1e9;
}

template <typename Tag, typename Storage>
double compare_6d(unsigned n, int distrib) {
  auto r = random_array(n, distrib);
  auto h = make_s(Tag(), Storage(), reg(10, 0, 1), reg(10, 0, 1), reg(10, 0, 1),
                  reg(10, 0, 1), reg(10, 0, 1), reg(10, 0, 1));
  auto t = clock();
  for (auto it = r.get(), end = r.get() + n; it != end;) {
    const auto x1 = *it++;
    const auto x2 = *it++;
    const auto x3 = *it++;
    const auto x4 = *it++;
    const auto x5 = *it++;
    const auto x6 = *it++;
    h(x1, x2, x3, x4, x5, x6);
  }
  return (double(clock()) - t) / CLOCKS_PER_SEC / n * 1e9;
}

int main() {
  const unsigned nfill = 6000000;
  using SStore = std::vector<int>;
  using DStore = unlimited_storage<>;

  printf("baseline %.1f\n", baseline(nfill));

  for (int itype = 0; itype < 2; ++itype) {
    const char* d = itype == 0 ? "uniform" : "normal ";
    printf("1D-boost:SS-%s %5.1f\n", d, compare_1d<static_tag, SStore>(nfill, itype));
    printf("1D-boost:SD-%s %5.1f\n", d, compare_1d<static_tag, DStore>(nfill, itype));
    printf("1D-boost:DS-%s %5.1f\n", d, compare_1d<dynamic_tag, SStore>(nfill, itype));
    printf("1D-boost:DD-%s %5.1f\n", d, compare_1d<dynamic_tag, DStore>(nfill, itype));
    printf("2D-boost:SS-%s %5.1f\n", d, compare_2d<static_tag, SStore>(nfill, itype));
    printf("2D-boost:SD-%s %5.1f\n", d, compare_2d<static_tag, DStore>(nfill, itype));
    printf("2D-boost:DS-%s %5.1f\n", d, compare_2d<dynamic_tag, SStore>(nfill, itype));
    printf("2D-boost:DD-%s %5.1f\n", d, compare_2d<dynamic_tag, DStore>(nfill, itype));
    printf("3D-boost:SS-%s %5.1f\n", d, compare_3d<static_tag, SStore>(nfill, itype));
    printf("3D-boost:SD-%s %5.1f\n", d, compare_3d<static_tag, DStore>(nfill, itype));
    printf("3D-boost:DS-%s %5.1f\n", d, compare_3d<dynamic_tag, SStore>(nfill, itype));
    printf("3D-boost:DD-%s %5.1f\n", d, compare_3d<dynamic_tag, DStore>(nfill, itype));
    printf("6D-boost:SS-%s %5.1f\n", d, compare_6d<static_tag, SStore>(nfill, itype));
    printf("6D-boost:SD-%s %5.1f\n", d, compare_6d<static_tag, DStore>(nfill, itype));
    printf("6D-boost:DS-%s %5.1f\n", d, compare_6d<dynamic_tag, SStore>(nfill, itype));
    printf("6D-boost:DD-%s %5.1f\n", d, compare_6d<dynamic_tag, DStore>(nfill, itype));
  }
}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [touint]
#include <boost/simd/arithmetic.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/constant/inf.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft = bs::pack <float, 4>;
using pack_it = bs::pack <std::int16_t, 4>;

int main()
{
  pack_ft pf = {-1.0f, 2.6f, 3.4f, bs::Inf<float>()};
  pack_it pi = { -1, 2, 3, bs::Inf<std::int16_t>() };

  std::cout
    << "---- simd" << '\n'
    << "<- pf =                        " << pf << '\n'
    << "-> bs::touint(pf) =             " << bs::touint(pf) << '\n'
    << "-> saturated_(bs::touint)(pf) = " << bs::saturated_(bs::touint)(pf) << '\n'
    << "<- pi =                        " << pi << '\n'
    << "-> bs::touint(pi) =             " << bs::touint(pi) << '\n'
    << "-> saturated_(bs::touint)(pi) = " << bs::saturated_(bs::touint)(pi) << '\n';

  float xf = -2.50f;
  std::int16_t xi = -1;

  std::cout
    << "---- scalar"  << '\n'
    << "<- xf =                        " << xf << '\n'
    << "-> bs::touint(xf) =             " << bs::touint(xf) << '\n'
    << "-> saturated_(bs::touint)(xf) = " << bs::saturated_(bs::touint)(xf) << '\n'
    << "<- xi =                        " << xi << '\n'
    << "-> bs::touint(xi) =             " << bs::touint(xi) << '\n'
    << "-> saturated_(bs::touint)(xi) = " << bs::saturated_(bs::touint)(xi) << '\n';
    return 0;
}
//! [touint]