// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2011 Gael Guennebaud <g.gael@free.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "sparse_solver.h"

#include <Eigen/UmfPackSupport>

template<typename T> void test_umfpack_support_T()
{
  UmfPackLU<SparseMatrix<T, ColMajor> > umfpack_colmajor;
  UmfPackLU<SparseMatrix<T, RowMajor> > umfpack_rowmajor;

  check_sparse_square_solving(umfpack_colmajor);
  check_sparse_square_solving(umfpack_rowmajor);

  check_sparse_square_determinant(umfpack_colmajor);
  check_sparse_square_determinant(umfpack_rowmajor);
}

void test_umfpack_support()
{
  CALL_SUBTEST_1(test_umfpack_support_T<double>());
  CALL_SUBTEST_2(test_umfpack_support_T<std::complex<double> >());
}


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

// Copyright (c) 2012-2014 Barend Gehrels, 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_STRATEGIES_AGNOSTIC_BUFFER_DISTANCE_ASYMMETRIC_HPP
#define BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_BUFFER_DISTANCE_ASYMMETRIC_HPP

#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/strategies/buffer.hpp>
#include <boost/geometry/util/math.hpp>

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

namespace strategy { namespace buffer
{

/*!
\brief Let the buffer for linestrings be asymmetric
\ingroup strategies
\tparam NumericType \tparam_numeric
\details This strategy can be used as DistanceStrategy for the buffer algorithm.
    It can be applied for (multi)linestrings. It uses a (potentially) different
    distances for left and for right. This means the (multi)linestrings are
    interpreted having a direction.

\qbk{
[heading Example]
[buffer_distance_asymmetric]
[heading Output]
[$img/strategies/buffer_distance_asymmetric.png]
[heading See also]
\* [link geometry.reference.algorithms.buffer.buffer_7_with_strategies buffer (with strategies)]
\* [link geometry.reference.strategies.strategy_buffer_distance_symmetric distance_symmetric]
}
 */
template<typename NumericType>
class distance_asymmetric
{
public :
    //! \brief Constructs the strategy, two distances must be specified
    //! \param left The distance (or radius) of the buffer on the left side
    //! \param right The distance on the right side
    distance_asymmetric(NumericType const& left,
                NumericType const& right)
        : m_left(left)
        , m_right(right)
    {}

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    //! Returns the distance-value for the specified side
    template <typename Point>
    inline NumericType apply(Point const& , Point const& ,
                buffer_side_selector side)  const
    {
        NumericType result = side == buffer_side_left ? m_left : m_right;
        return negative() ? math::abs(result) : result;
    }

    //! Used internally, returns -1 for deflate, 1 for inflate
    inline int factor() const
    {
        return negative() ? -1 : 1;
    }

    //! Returns true if both distances are negative
    inline bool negative() const
    {
        return m_left < 0 && m_right < 0;
    }

    //! Returns the max distance distance up to the buffer will reach
    template <typename JoinStrategy, typename EndStrategy>
    inline NumericType max_distance(JoinStrategy const& join_strategy,
            EndStrategy const& end_strategy) const
    {
        geofeatures_boost::ignore_unused(join_strategy, end_strategy);

        NumericType const left = geometry::math::abs(m_left);
        NumericType const right = geometry::math::abs(m_right);
        NumericType const dist = (std::max)(left, right);
        return (std::max)(join_strategy.max_distance(dist),
                          end_strategy.max_distance(dist));
    }

    //! Returns the distance at which the input is simplified before the buffer process
    inline NumericType simplify_distance() const
    {
        NumericType const left = geometry::math::abs(m_left);
        NumericType const right = geometry::math::abs(m_right);
        return (std::min)(left, right) / 1000.0;
    }

#endif // DOXYGEN_SHOULD_SKIP_THIS

private :
    NumericType m_left;
    NumericType m_right;
};

}} // namespace strategy::buffer

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_BUFFER_DISTANCE_ASYMMETRIC_HPP


/*
 * main_optimize.cpp
 * Date: 2013-01-28
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 */

#define FUSION_MAX_VECTOR_SIZE 20

#include <gpcxx/tree.hpp>
#include <gpcxx/generate.hpp>
#include <gpcxx/operator.hpp>
#include <gpcxx/eval.hpp>
#include <gpcxx/evolve.hpp>
#include <gpcxx/io.hpp>
#include <gpcxx/stat.hpp>
#include <gpcxx/app.hpp>

#include <boost/fusion/include/make_vector.hpp>

#include <iostream>
#include <random>
#include <vector>
#include <functional>

const std::string tab = "\t";

namespace pl = std::placeholders;
namespace fusion = boost::fusion;

int main( int argc , char *argv[] )
{
    typedef std::mt19937 rng_type ;
    typedef double value_type;
    typedef char symbol_type;
    typedef gpcxx::regression_training_data< value_type , 3 > training_data_type;
    typedef gpcxx::regression_context< value_type , 3 > context_type;
    typedef std::vector< value_type > fitness_type;

    auto eval = gpcxx::make_static_eval_erc< value_type , symbol_type , context_type >(
        fusion::make_vector( 1.0 , std::normal_distribution<>( 0.0 , 1.0 ) ) ,
        fusion::make_vector(
            fusion::make_vector( 'x' , []( context_type const& t ) { return t[0]; } )
          , fusion::make_vector( 'y' , []( context_type const& t ) { return t[1]; } )
          , fusion::make_vector( 'z' , []( context_type const& t ) { return t[2]; } )
          ) ,
        fusion::make_vector(
            fusion::make_vector( 's' , []( double v ) -> double { return std::sin( v ); } )
          , fusion::make_vector( 'c' , []( double v ) -> double { return std::cos( v ); } )
          ) ,
        fusion::make_vector(
            fusion::make_vector( '+' , std::plus< double >() )
          , fusion::make_vector( '-' , std::minus< double >() )
          , fusion::make_vector( '*' , std::multiplies< double >() )
          , fusion::make_vector( '/' , std::divides< double >() )
          ) );
    typedef decltype( eval ) eval_type;
    typedef eval_type::node_attribute_type node_attribute_type;

    typedef gpcxx::basic_tree< node_attribute_type > tree_type;
    typedef std::vector< tree_type > population_type;
    typedef gpcxx::static_pipeline< population_type , fitness_type , rng_type > evolver_type;

    rng_type rng;

    training_data_type c;
    gpcxx::generate_regression_test_data( c , 1024 , rng , []( double x1 , double x2 , double x3 )
                    { return  x1 * x1 * x1 + 1.0 / 10.0 * x2 * x2 - 3.0 / 4.0 * ( x3 - 4.0 ) + 1.0 ; } );

    size_t population_size = 512;
    size_t number_elite = 1;
    double mutation_rate = 0.2;
    double crossover_rate = 0.6;
    double reproduction_rate = 0.3;
    size_t min_tree_height = 8 , max_tree_height = 8;

    auto terminal_gen = eval.get_terminal_symbol_distribution();
    auto unary_gen = eval.get_unary_symbol_distribution();
    auto binary_gen = eval.get_binary_symbol_distribution();
    std::array< double , 3 > weights = {{ 2.0 * double( terminal_gen.num_symbols() ) ,
                                       double( unary_gen.num_symbols() ) ,
                                       double( binary_gen.num_symbols() ) }};
    auto tree_generator = gpcxx::make_ramp( rng , terminal_gen , unary_gen , binary_gen , min_tree_height , max_tree_height , 0.5 , weights );

    evolver_type evolver( number_elite , mutation_rate , crossover_rate , reproduction_rate , rng );
    fitness_type fitness( population_size , 0.0 );
    population_type population( population_size );

    auto fitness_f = gpcxx::regression_fitness< eval_type >( eval );
    evolver.mutation_function() = gpcxx::make_mutation(
        gpcxx::make_simple_mutation_strategy( rng , terminal_gen , unary_gen , binary_gen ) ,
        gpcxx::make_random_selector( rng ) );
    evolver.crossover_function() = gpcxx::make_crossover(
        gpcxx::make_one_point_crossover_strategy( rng , 10 ) ,
        gpcxx::make_random_selector( rng ) );
    evolver.reproduction_function() = gpcxx::make_reproduce( gpcxx::make_random_selector( rng ) );

    // initialize population with random trees and evaluate fitness
    for( size_t i=0 ; i<population.size() ; ++i )
    {
        tree_generator( population[i] );
        fitness[i] = fitness_f( population[i] , c );
    }
    std::cout << "Best individuals" << std::endl << gpcxx::best_individuals( population , fitness ) << std::endl;
    std::cout << "Statistics : " << gpcxx::calc_population_statistics( population ) << std::endl;
    std::cout << std::endl << std::endl;

    for( size_t i=0 ; i<100 ; ++i )
    {
        evolver.next_generation( population , fitness );
        for( size_t i=0 ; i<population.size() ; ++i )
            fitness[i] = fitness_f( population[i] , c );

        std::cout << "Iteration " << i << std::endl;
        std::cout << "Best individuals" << std::endl << gpcxx::best_individuals( population , fitness , 1 ) << std::endl;
        std::cout << "Statistics : " << gpcxx::calc_population_statistics( population ) << std::endl << std::endl;
    }

    return 0;
}


#include <Eigen/Geometry>
#include <Eigen/Core>
#include "util.h"
#include "localization.h"
#include "geometry_msgs/PoseArray.h"

ros::Time p_start, p_end,
    p_weight_start, p_weight_end,
    p_resample_start, p_resample_end,
    p_perturb_start, p_perturb_end,
    p_move_start, p_move_end
    ;

Localization::ParamHelper::ParamHelper()
{
    bool ok = true;
    ok &= ros::param::get("~map_image", params.map_image);
    ok &= ros::param::get("~map_pixel_size", params.map_pixel_size);
    ok &= ros::param::get("~map_threshold", params.map_threshold);
    if (!ok) {
	ROS_ERROR("Missing parameters");
    }
}

Localization::Localization(ros::NodeHandle handle) :
    particleFilter(config, paramHelper.params),
    nh(handle),
    particlePublisher(handle.advertise<geometry_msgs::PoseArray>("localization/viz/particles", 1)),
    positionPublisher(handle.advertise<geometry_msgs::PoseStamped>("position/estimate", 1)),
    mapPublisher(handle.advertise<nav_msgs::OccupancyGrid>("localization/viz/map", 1)),
    sonarSubscriber(handle.subscribe("sonar/scan/walls", 1, &Localization::sonarCallback, this)),
    positionSubscriber(handle.subscribe("/initialpose", 1, &Localization::positionCallback, this)),
    imuSubscriber(handle.subscribe("imu", 10, &Localization::imuCallback, this)),
    leftThrusterSubscriber(handle.subscribe("motors/left", 1, &Localization::leftThrusterCallback, this)),
    rightThrusterSubscriber(handle.subscribe("motors/right", 1, &Localization::rightThrusterCallback, this)),
    leftSpeed(0),
    rightSpeed(0)
{
    reconfigServer.setCallback(boost::bind(&Localization::reconfigure, this, _1, _2));
    particleFilter.resetPosition();
}

void Localization::reconfigure(hanse_sonarlocalization::ParticleFilterConfig &newConfig, uint32_t level)
{
    particleFilter.reconfigure(newConfig);
}

void Localization::positionCallback(const geometry_msgs::PoseWithCovarianceStamped &pose)
{
    particleFilter.setPosition(localization::positionFromPose(pose.pose.pose));
}

void Localization::sonarCallback(const hanse_msgs::WallDetection &msg)
{
    p_start = ros::Time::now();
    bool update = false;
    while ((!imuQueue.empty()) && imuQueue.front().header.stamp < msg.header.stamp) {
	particleFilter.addImuMessage(imuQueue.front());
	imuQueue.pop_front();
	update = true;
    }

    p_perturb_start = ros::Time::now();
    particleFilter.perturb();
    p_perturb_end = ros::Time::now();
    p_move_start = ros::Time::now();
    if (!lastMsgTime.isZero()) {
	ros::Duration timePassed = msg.header.stamp - lastMsgTime;
	particleFilter.move(timePassed.toSec());
        particleFilter.thrusterUpdate(timePassed.toSec(), leftSpeed, rightSpeed);
    }
    p_move_end = ros::Time::now();
    lastMsgTime = msg.header.stamp;

    if (update) {
	particleFilter.imuUpdate();
    }

    p_weight_start = ros::Time::now();
    particleFilter.weightParticles(msg);
    p_weight_end = ros::Time::now();
    p_resample_start = ros::Time::now();
    particleFilter.resample();
    p_resample_end = ros::Time::now();

    geometry_msgs::PoseStamped position;
    position.header.frame_id = "/map";
    position.pose = poseFrom2DPosition(particleFilter.estimatedPosition(), 1.0);

    positionPublisher.publish(position);

    geometry_msgs::PoseArray particles;

    particles.header.frame_id = "/map";

    int count = 0;
    for (auto &particle : particleFilter.getParticles()) {
	particles.poses.push_back(poseFrom2DPosition(particle.position));
	count++;
	if (count > 1000)
	    break;
    }
    particlePublisher.publish(particles);
    p_end = ros::Time::now();
    /*
    ROS_INFO("Duration %f (w: %f, r: %f, p: %f, m: %f)",
	     (p_end-p_start).toSec(),
	     (p_weight_end-p_weight_start).toSec(),
	     (p_resample_end-p_resample_start).toSec(),
	     (p_perturb_end-p_perturb_start).toSec(),
	     (p_move_end-p_move_start).toSec()
	     );
    */

    ros::Time now = ros::Time::now();

    if (lastMapTime == ros::Time() || (now - lastMapTime).toSec() > 5) {
	mapPublisher.publish(particleFilter.map().occupancyGrid());
	lastMapTime = now;
    }
}

void Localization::imuCallback(const sensor_msgs::Imu &msg)
{
    if (!imuQueue.empty() && imuQueue.front().header.stamp > msg.header.stamp)
	imuQueue.clear();
    imuQueue.push_back(msg);
}

geometry_msgs::Pose Localization::poseFrom2DPosition(Eigen::Affine2f position2D, float z)
{
    geometry_msgs::Pose pose;

    Eigen::Vector2f position = position2D.translation();
    pose.position.x = position(0);
    pose.position.y = position(1);
    pose.position.z = z;
    Eigen::Rotation2D<float> rotation(0);
    rotation.fromRotationMatrix(position2D.rotation());
    Eigen::Quaternion<float> rotation3D(Eigen::AngleAxis<float>(rotation.angle(), Eigen::Vector3f(0,0,1)));
    pose.orientation.x = rotation3D.x();
    pose.orientation.y = rotation3D.y();
    pose.orientation.z = rotation3D.z();
    pose.orientation.w = rotation3D.w();
    return pose;
}

void Localization::leftThrusterCallback(const hanse_msgs::sollSpeed &msg)
{
    leftSpeed = msg.data;
}

void Localization::rightThrusterCallback(const hanse_msgs::sollSpeed &msg)
{
    rightSpeed = msg.data;
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "localization");
    ros::NodeHandle n;
    Localization localization(n);

    ros::spin();

    return 0;
}


/*
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The ASF licenses this file to You 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.
 */

#ifdef _WIN32
#   include <windows.h>
#endif

#include <vector>
#include <string>

#include <boost/test/unit_test.hpp>

#include "test_type.h"
#include "sql_test_suite_fixture.h"
#include <ignite/common/decimal.h>

using namespace ignite;

BOOST_FIXTURE_TEST_SUITE(SqlOperatorTestSuite, ignite::SqlTestSuiteFixture)

BOOST_AUTO_TEST_CASE(TestOperatorAddInt)
{
    CheckSingleResult<SQLINTEGER>("SELECT 123 + 51", 123 + 51);
};

BOOST_AUTO_TEST_CASE(TestOperatorSubInt)
{
    CheckSingleResult<SQLINTEGER>("SELECT 123 - 51", 123 - 51);
};

BOOST_AUTO_TEST_CASE(TestOperatorDivInt)
{
    CheckSingleResult<SQLINTEGER>("SELECT 123 / 51", 123 / 51);
};

BOOST_AUTO_TEST_CASE(TestOperatorModInt)
{
    CheckSingleResult<SQLINTEGER>("SELECT 123 % 51", 123 % 51);
};

BOOST_AUTO_TEST_CASE(TestOperatorMultInt)
{
    CheckSingleResult<SQLINTEGER>("SELECT 123 * 51", 123 * 51);
};

BOOST_AUTO_TEST_CASE(TestOperatorAddDouble)
{
    CheckSingleResult<double>("SELECT 123.0 + 51.0", 123.0 + 51.0);
};

BOOST_AUTO_TEST_CASE(TestOperatorSubDouble)
{
    CheckSingleResult<double>("SELECT 123.0 - 51.0", 123.0 - 51.0);
};

BOOST_AUTO_TEST_CASE(TestOperatorDivDouble)
{
    CheckSingleResult<double>("SELECT 123.0 / 51.0", 123.0 / 51.0);
};

BOOST_AUTO_TEST_CASE(TestOperatorModDouble)
{
    CheckSingleResult<double>("SELECT 123.0 % 51.0", 123 % 51);
};

BOOST_AUTO_TEST_CASE(TestOperatorMultDouble)
{
    CheckSingleResult<double>("SELECT 123.0 * 51.0", 123.0 * 51.0);
};

BOOST_AUTO_TEST_CASE(TestOperatorConcatString)
{
    CheckSingleResult<std::string>("SELECT 'Hello' || ' ' || 'World' || '!'", "Hello World!");
};

BOOST_AUTO_TEST_CASE(TestOperatorGreaterInt)
{
    CheckSingleResult<bool>("SELECT 2 > 3", false);
    CheckSingleResult<bool>("SELECT 3 > 3", false);
    CheckSingleResult<bool>("SELECT 34 > 3", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorLessInt)
{
    CheckSingleResult<bool>("SELECT 4 < 4", false);
    CheckSingleResult<bool>("SELECT 4 < 4", false);
    CheckSingleResult<bool>("SELECT 8 < 42", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorGreaterEquealInt)
{
    CheckSingleResult<bool>("SELECT 2 >= 3", false);
    CheckSingleResult<bool>("SELECT 3 >= 3", true);
    CheckSingleResult<bool>("SELECT 34 >= 3", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorLessEquealInt)
{
    CheckSingleResult<bool>("SELECT 4 <= 3", false);
    CheckSingleResult<bool>("SELECT 4 <= 4", true);
    CheckSingleResult<bool>("SELECT 8 <= 42", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorEquealInt)
{
    CheckSingleResult<bool>("SELECT 4 = 3", false);
    CheckSingleResult<bool>("SELECT 4 = 4", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorNotEquealInt)
{
    CheckSingleResult<bool>("SELECT 4 != 3", true);
    CheckSingleResult<bool>("SELECT 4 != 4", false);
};

BOOST_AUTO_TEST_CASE(TestOperatorGreaterDouble)
{
    CheckSingleResult<bool>("SELECT 2 > 3", false);
    CheckSingleResult<bool>("SELECT 3 > 3", false);
    CheckSingleResult<bool>("SELECT 34 > 3", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorLessDouble)
{
    CheckSingleResult<bool>("SELECT 4.0 < 4.0", false);
    CheckSingleResult<bool>("SELECT 4.0 < 4.0", false);
    CheckSingleResult<bool>("SELECT 8.0 < 42.0", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorGreaterEquealDouble)
{
    CheckSingleResult<bool>("SELECT 2.0 >= 3.0", false);
    CheckSingleResult<bool>("SELECT 3.0 >= 3.0", true);
    CheckSingleResult<bool>("SELECT 34.0 >= 3.0", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorLessEquealDouble)
{
    CheckSingleResult<bool>("SELECT 4.0 <= 3.0", false);
    CheckSingleResult<bool>("SELECT 4.0 <= 4.0", true);
    CheckSingleResult<bool>("SELECT 8.0 <= 42.0", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorEquealDouble)
{
    CheckSingleResult<bool>("SELECT 4.0 = 3.0", false);
    CheckSingleResult<bool>("SELECT 4.0 = 4.0", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorNotEquealDouble)
{
    CheckSingleResult<bool>("SELECT 4.0 != 3.0", true);
    CheckSingleResult<bool>("SELECT 4.0 != 4.0", false);
};

BOOST_AUTO_TEST_CASE(TestOperatorGreaterString)
{
    CheckSingleResult<bool>("SELECT 'abc' > 'bcd'", false);
    CheckSingleResult<bool>("SELECT 'abc' > 'abc'", false);
    CheckSingleResult<bool>("SELECT 'bcd' > 'abc'", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorLessString)
{
    CheckSingleResult<bool>("SELECT 'bcd' < 'abc'", false);
    CheckSingleResult<bool>("SELECT 'abc' < 'abc'", false);
    CheckSingleResult<bool>("SELECT 'abc' < 'bcd'", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorGreaterEquealString)
{
    CheckSingleResult<bool>("SELECT 'abc' >= 'bcd'", false);
    CheckSingleResult<bool>("SELECT 'abc' >= 'abc'", true);
    CheckSingleResult<bool>("SELECT 'bcd' >= 'abc'", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorLessEquealString)
{
    CheckSingleResult<bool>("SELECT 'bcd' <= 'abc'", false);
    CheckSingleResult<bool>("SELECT 'abc' <= 'bcd'", true);
    CheckSingleResult<bool>("SELECT 'abc' <= 'abc'", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorEquealString)
{
    CheckSingleResult<bool>("SELECT 'bcd' = 'abc'", false);
    CheckSingleResult<bool>("SELECT 'abc' = 'abc'", true);
};

BOOST_AUTO_TEST_CASE(TestOperatorNotEquealString)
{
    CheckSingleResult<bool>("SELECT 'abc' != 'abc'", false);
    CheckSingleResult<bool>("SELECT 'bcd' != 'abc'", true);
};

BOOST_AUTO_TEST_SUITE_END()


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "tests/Unit/TestingFramework.hpp"

#include <array>
#include <boost/optional.hpp>
#include <random>

#include "Domain/CoordinateMaps/Frustum.hpp"
#include "Domain/OrientationMap.hpp"
#include "ErrorHandling/Error.hpp"
#include "Utilities/TypeTraits.hpp"
#include "tests/Unit/Domain/CoordinateMaps/TestMapHelpers.hpp"

namespace {
void test_suite_for_frustum(const bool with_equiangular_map) {
  // Set up random number generator
  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_real_distribution<> lower_bound_dis(-14, -2);
  std::uniform_real_distribution<> upper_bound_dis(2, 14);

  const double lower_x_lower_base = lower_bound_dis(gen);
  CAPTURE_PRECISE(lower_x_lower_base);
  const double lower_y_lower_base = lower_bound_dis(gen);
  CAPTURE_PRECISE(lower_y_lower_base);
  const double upper_x_lower_base = upper_bound_dis(gen);
  CAPTURE_PRECISE(upper_x_lower_base);
  const double upper_y_lower_base = upper_bound_dis(gen);
  CAPTURE_PRECISE(upper_y_lower_base);
  const double lower_x_upper_base = lower_bound_dis(gen);
  CAPTURE_PRECISE(lower_x_upper_base);
  const double lower_y_upper_base = lower_bound_dis(gen);
  CAPTURE_PRECISE(lower_y_upper_base);
  const double upper_x_upper_base = upper_bound_dis(gen);
  CAPTURE_PRECISE(upper_x_upper_base);
  const double upper_y_upper_base = upper_bound_dis(gen);
  CAPTURE_PRECISE(upper_y_upper_base);

  for (OrientationMapIterator<3> map_i{}; map_i; ++map_i) {
    const std::array<std::array<double, 2>, 4> face_vertices{
        {{{lower_x_lower_base, lower_y_lower_base}},
         {{upper_x_lower_base, upper_y_lower_base}},
         {{lower_x_upper_base, lower_y_upper_base}},
         {{upper_x_upper_base, upper_y_upper_base}}}};
    const CoordinateMaps::Frustum frustum_map(face_vertices, -1.0, 2.0, map_i(),
                                              with_equiangular_map);
    test_suite_for_map_on_unit_cube(frustum_map);
  }
}

void test_frustum_fail() noexcept {
  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{2.0, 2.0}}, {{-4.0, -4.0}}, {{4.0, 4.0}}}};
  const CoordinateMaps::Frustum map(face_vertices, 2.0, 5.0,
                                    OrientationMap<3>{});

  // For the choice of params above, any point with z<=-1 should fail.
  const std::array<double, 3> test_mapped_point1{{3.0, 3.0, -1.0}};
  const std::array<double, 3> test_mapped_point2{{6.0, -7.0, -1.0}};
  const std::array<double, 3> test_mapped_point3{{6.0, -7.0, -3.0}};

  // This is outside the mapped frustum, so inverse should either
  // return the correct inverse (which happens to be computable for
  // this point) or it should return boost::none.
  const std::array<double, 3> test_mapped_point4{{0.0, 0.0, 9.0}};

  CHECK_FALSE(static_cast<bool>(map.inverse(test_mapped_point1)));
  CHECK_FALSE(static_cast<bool>(map.inverse(test_mapped_point2)));
  CHECK_FALSE(static_cast<bool>(map.inverse(test_mapped_point3)));
  if(map.inverse(test_mapped_point4)) {
    CHECK_ITERABLE_APPROX(map(map.inverse(test_mapped_point4).get()),
                          test_mapped_point4);
  }
}

}  // namespace

SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Fail", "[Domain][Unit]") {
  test_frustum_fail();
}

SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Equidistant",
                  "[Domain][Unit]") {
  test_suite_for_frustum(false);
}

SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Equiangular",
                  "[Domain][Unit]") {
  test_suite_for_frustum(true);
}

SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Alignment",
                  "[Domain][Unit]") {
  // This test tests that the logical axes point along the expected directions
  // in physical space

  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{2.0, 2.0}}, {{-4.0, -4.0}}, {{4.0, 4.0}}}};
  const double lower_bound = 2.0;
  const double upper_bound = 5.0;

  const auto wedge_directions = all_wedge_directions();
  const CoordinateMaps::Frustum map_upper_zeta(
      face_vertices, lower_bound, upper_bound,
      wedge_directions[0]);  // Upper Z frustum
  const CoordinateMaps::Frustum map_upper_eta(
      face_vertices, lower_bound, upper_bound,
      wedge_directions[2]);  // Upper Y frustum
  const CoordinateMaps::Frustum map_upper_xi(
      face_vertices, lower_bound, upper_bound,
      wedge_directions[4]);  // Upper X Frustum
  const CoordinateMaps::Frustum map_lower_zeta(
      face_vertices, lower_bound, upper_bound,
      wedge_directions[1]);  // Lower Z frustum
  const CoordinateMaps::Frustum map_lower_eta(
      face_vertices, lower_bound, upper_bound,
      wedge_directions[3]);  // Lower Y frustum
  const CoordinateMaps::Frustum map_lower_xi(
      face_vertices, lower_bound, upper_bound,
      wedge_directions[5]);  // Lower X frustum
  const std::array<double, 3> lowest_corner{{-1.0, -1.0, -1.0}};
  const std::array<double, 3> along_xi{{1.0, -1.0, -1.0}};
  const std::array<double, 3> along_eta{{-1.0, 1.0, -1.0}};
  const std::array<double, 3> along_zeta{{-1.0, -1.0, 1.0}};

  const std::array<double, 3> lowest_physical_corner_in_map_upper_zeta{
      {-2.0, -2.0, 2.0}};
  const std::array<double, 3> lowest_physical_corner_in_map_upper_eta{
      {-2.0, 2.0, -2.0}};
  const std::array<double, 3> lowest_physical_corner_in_map_upper_xi{
      {2.0, -2.0, -2.0}};
  const std::array<double, 3> lowest_physical_corner_in_map_lower_zeta{
      {-2.0, 2.0, -2.0}};
  const std::array<double, 3> lowest_physical_corner_in_map_lower_eta{
      {-2.0, -2.0, 2.0}};
  const std::array<double, 3> lowest_physical_corner_in_map_lower_xi{
      {-2.0, 2.0, -2.0}};

  // Test that this map's logical axes point along +X, +Y, +Z:
  CHECK(map_upper_zeta(along_xi)[0] == approx(2.0));
  CHECK(map_upper_zeta(along_eta)[1] == approx(2.0));
  CHECK(map_upper_zeta(along_zeta)[2] == approx(5.0));
  CHECK_ITERABLE_APPROX(map_upper_zeta(lowest_corner),
                        lowest_physical_corner_in_map_upper_zeta);

  // Test that this map's logical axes point along +Z, +X, +Y:
  CHECK(map_upper_eta(along_xi)[2] == approx(2.0));
  CHECK(map_upper_eta(along_eta)[0] == approx(2.0));
  CHECK(map_upper_eta(along_zeta)[1] == approx(5.0));
  CHECK_ITERABLE_APPROX(map_upper_eta(lowest_corner),
                        lowest_physical_corner_in_map_upper_eta);

  // Test that this map's logical axes point along +Y, +Z, +X:
  CHECK(map_upper_xi(along_xi)[1] == approx(2.0));
  CHECK(map_upper_xi(along_eta)[2] == approx(2.0));
  CHECK(map_upper_xi(along_zeta)[0] == approx(5.0));
  CHECK_ITERABLE_APPROX(map_upper_xi(lowest_corner),
                        lowest_physical_corner_in_map_upper_xi);

  // Test that this map's logical axes point along +X, -Y, -Z:
  CHECK(map_lower_zeta(along_xi)[0] == approx(2.0));
  CHECK(map_lower_zeta(along_eta)[1] == approx(-2.0));
  CHECK(map_lower_zeta(along_zeta)[2] == approx(-5.0));
  CHECK_ITERABLE_APPROX(map_lower_zeta(lowest_corner),
                        lowest_physical_corner_in_map_lower_zeta);

  // Test that this map's logical axes point along -Z, +X, -Y:
  CHECK(map_lower_eta(along_xi)[2] == approx(-2.0));
  CHECK(map_lower_eta(along_eta)[0] == approx(2.0));
  CHECK(map_lower_eta(along_zeta)[1] == approx(-5.0));
  CHECK_ITERABLE_APPROX(map_lower_eta(lowest_corner),
                        lowest_physical_corner_in_map_lower_eta);

  // Test that this map's logical axes point along -Y, +Z, -X:
  CHECK(map_lower_xi(along_xi)[1] == approx(-2.0));
  CHECK(map_lower_xi(along_eta)[2] == approx(2.0));
  CHECK(map_lower_xi(along_zeta)[0] == approx(-5.0));
  CHECK_ITERABLE_APPROX(map_lower_xi(lowest_corner),
                        lowest_physical_corner_in_map_lower_xi);
}

// [[OutputRegex, The lower bound for a coordinate must be numerically less
// than the upper bound for that coordinate.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Assert1",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{-3.0, 2.0}}, {{-4.0, -4.0}}, {{4.0, 4.0}}}};
  const double lower_bound = 2.0;
  const double upper_bound = 5.0;

  auto failed_frustum = CoordinateMaps::Frustum(
      face_vertices, lower_bound, upper_bound, OrientationMap<3>{});
  static_cast<void>(failed_frustum);

  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, The lower bound for a coordinate must be numerically less
// than the upper bound for that coordinate.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Assert2",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{2.0, -3.0}}, {{-4.0, -4.0}}, {{4.0, 4.0}}}};
  const double lower_bound = 2.0;
  const double upper_bound = 5.0;

  auto failed_frustum = CoordinateMaps::Frustum(
      face_vertices, lower_bound, upper_bound, OrientationMap<3>{});
  static_cast<void>(failed_frustum);

  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, The lower bound for a coordinate must be numerically less
// than the upper bound for that coordinate.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Assert3",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{2.0, 2.0}}, {{-4.0, -4.0}}, {{-5.0, 4.0}}}};
  const double lower_bound = 2.0;
  const double upper_bound = 5.0;

  auto failed_frustum = CoordinateMaps::Frustum(
      face_vertices, lower_bound, upper_bound, OrientationMap<3>{});
  static_cast<void>(failed_frustum);

  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, The lower bound for a coordinate must be numerically less
// than the upper bound for that coordinate.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Assert4",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{2.0, 2.0}}, {{-4.0, -4.0}}, {{4.0, -5.0}}}};
  const double lower_bound = 2.0;
  const double upper_bound = 5.0;

  auto failed_frustum = CoordinateMaps::Frustum(
      face_vertices, lower_bound, upper_bound, OrientationMap<3>{});
  static_cast<void>(failed_frustum);

  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, The lower bound for a coordinate must be numerically less
// than the upper bound for that coordinate.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Assert5",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{2.0, 2.0}}, {{-4.0, -4.0}}, {{4.0, 4.0}}}};
  const double lower_bound = 2.0;
  const double upper_bound = -2.0;

  auto failed_frustum = CoordinateMaps::Frustum(
      face_vertices, lower_bound, upper_bound, OrientationMap<3>{});
  static_cast<void>(failed_frustum);

  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}


// ValueUtilities.cpp
// Copyright 2019 Mikko Lauri
//
// 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 "ValueUtilities.h"
#include "ValueFunction.h"
#include "BeliefUtilities.h"
#include "JointPolicyHistories.h"
#include "combinations.hpp"
#include <boost/random/discrete_distribution.hpp>
#include <boost/random/uniform_int.hpp>

namespace pgi {

std::size_t best_value_index(const value_map_t& vm) {
  auto it = std::max_element(
      vm.begin(), vm.end(), [](const std::pair<std::size_t, stats_t>& a,
                               const std::pair<std::size_t, stats_t>& b) {
        return boost::accumulators::weighted_sum(a.second) <
               boost::accumulators::weighted_sum(b.second);
      });
  return it->first;
}

namespace local_node_value {
// lower bound for the values of local policies of node qlocal in local policy
// idx_local, evaluated by taking the expectation under expected beliefs of
// joint nodes in qj.
value_map_t lower_bound(const DecPOMDPDiscrete& d, HistoryCache& hc,
                        JointPolicy jp, std::size_t idx_local, vertex_t qlocal,
                        const std::vector<JointPolicy::joint_vertex_t>& qj) {
  value_map_t vm;
  PolicyGraph& local = jp.local_policy(idx_local);
  for (const auto& q : qj) {
    auto jb = joint_belief_with_probability(q, jp,d, hc);

    if (is_almost_zero(jb.second)) continue;
    for (std::size_t a = 0; a < local[boost::graph_bundle].num_actions_; ++a) {
      HistoryCache hcache(HistoryData{1.0, 0.0, 0.0, jb.first});

      local[qlocal] = a;
      for (std::size_t e = 0; e < num_out_edge_configurations(qlocal, local);
           ++e) {
        set_out_edge_configuration(qlocal, e, local);
        double value = pgi::value(jp, q, History(), d, hcache);
        const std::size_t i = get_local_policy(qlocal, a, e, local);
        vm[i](value, boost::accumulators::weight = jb.second);
      }
    }
  }
  return vm;
}

value_map_t exact(const DecPOMDPDiscrete& d,
                  HistoryCache& hc, JointPolicy jp,
                  std::size_t idx_local, vertex_t qlocal,
                  const std::vector<JointPolicy::joint_vertex_t>& qj) {
  value_map_t vm;
  PolicyGraph& local = jp.local_policy(idx_local);
  for (std::size_t a = 0; a < local[boost::graph_bundle].num_actions_; ++a) {
    local[qlocal] = a;
    for (std::size_t e = 0; e < num_out_edge_configurations(qlocal, local);
         ++e) {
      set_out_edge_configuration(qlocal, e, local);
      const std::size_t i = get_local_policy(qlocal, a, e, local);

      for (const auto& q : qj) {
        const std::vector<vertex_t> qv = jp.to_local(q);
        const std::vector<std::vector<History>> local_histories = local_histories_at(qv, jp);

        if (local_histories.empty())
          continue;

        for (auto h : make_combinations(local_histories)) {
          const History history = local_to_joint_history(get_combination(h),
                                     d.joint_action_space(),
                                     d.joint_observation_space());
          const HistoryData& hd = get_data_or_insert_missing(history,
              hc, d, jp.is_terminal(q));
          if (!is_reachable(hd))
            continue;

          double value = pgi::value(jp, q, history, d, hc);
          vm[i](value, boost::accumulators::weight = hd.probability_);
          hc.ensure_size_within_limits();
        }
      }
    }
  }
  return vm;
}

value_map_t heuristic_estimate(const DecPOMDPDiscrete& d,
                               HistoryCache& hc,
                               const JointPolicy& jp, std::size_t idx_local,
                               vertex_t qlocal, PRNG& rng,
                               bool use_random_history) {
  const PolicyGraph& local = jp.local_policy(idx_local);
  std::pair<PolicyState, bool> ps(PolicyState{}, false);
  if (!use_random_history) {
    ps = sample_history_and_joint_vertex_from_policy(
        d.joint_action_space(), d.joint_observation_space(), jp,
        steps_remaining(qlocal, local), rng);
  }
  if (!ps.second) {
    const HistoryData& rootdata = get_data_or_insert_missing(
        History(), hc, d, jp.is_terminal(ps.first.joint_vertex_));
    ps = sample_random_history_and_joint_vertex(d, rootdata.belief_, jp,
                                                idx_local, qlocal, rng);
  }

  return policy_state_value(d, jp, ps.first, idx_local, qlocal, hc);
}

value_map_t policy_state_value(const DecPOMDPDiscrete& d,
                               JointPolicy jp, const PolicyState& ps,
                               std::size_t idx_local, vertex_t qlocal,
                               HistoryCache& hc) {
  value_map_t vm;
  PolicyGraph& local = jp.local_policy(idx_local);
  for (std::size_t a = 0; a < local[boost::graph_bundle].num_actions_; ++a) {
    local[qlocal] = a;
    for (std::size_t e = 0; e < num_out_edge_configurations(qlocal, local);
         ++e) {
      set_out_edge_configuration(qlocal, e, local);
      const std::size_t i = get_local_policy(qlocal, a, e, local);
      double value = pgi::value(jp, ps.joint_vertex_, ps.history_, d, hc);
      vm[i](value, boost::accumulators::weight = 1.0);
    }
  }
  return vm;
}

}  // namespace local_node_value
}  // namespace pgi


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <vector>
#include <utility>
#include <string>

#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map.hpp>

/*
  Sample Output

  graph name: foo
  0  --joe--> 1
  1  --joe--> 0   --curly--> 2   --dick--> 3
  2  --curly--> 1   --tom--> 4
  3  --dick--> 1   --harry--> 4
  4  --tom--> 2   --harry--> 3
  (0,1) (1,2) (1,3) (2,4) (3,4)

  removing edge (1,3):
  0  --joe--> 1
  1  --joe--> 0   --curly--> 2
  2  --curly--> 1   --tom--> 4
  3  --harry--> 4
  4  --tom--> 2   --harry--> 3
  (0,1) (1,2) (2,4) (3,4)

 */

int main(int , char* [])
{
  using namespace boost;
  using namespace std;

  typedef property<edge_name_t, std::string> EdgeProperty;
  typedef property<vertex_color_t, default_color_type> ColorProperty;
  typedef property<vertex_index_t, std::size_t, ColorProperty> VertexProperty;

  typedef adjacency_list<vecS, listS, undirectedS,
    VertexProperty, EdgeProperty> Graph;

  const int V = 5;
  Graph g(V);

  property_map<Graph, vertex_index_t>::type id = get(vertex_index, g);

  property_map<Graph, edge_name_t>::type name = get(edge_name, g);

  boost::graph_traits<Graph>::vertex_iterator vi, viend;
  int vnum = 0;

  for (boost::tie(vi,viend) = vertices(g); vi != viend; ++vi)
    id[*vi] = vnum++;

  add_edge(vertex(0, g), vertex(1, g), EdgeProperty("joe"), g);
  add_edge(vertex(1, g), vertex(2, g), EdgeProperty("curly"), g);
  add_edge(vertex(1, g), vertex(3, g), EdgeProperty("dick"), g);
  add_edge(vertex(2, g), vertex(4, g), EdgeProperty("tom"), g);
  add_edge(vertex(3, g), vertex(4, g), EdgeProperty("harry"), g);

  graph_traits<Graph>::vertex_iterator i, end;
  graph_traits<Graph>::out_edge_iterator ei, edge_end;
  for (boost::tie(i,end) = vertices(g); i != end; ++i) {
    cout << id[*i] << " ";
    for (boost::tie(ei,edge_end) = out_edges(*i, g); ei != edge_end; ++ei)
      cout << " --" << name[*ei] << "--> " << id[target(*ei, g)] << "  ";
    cout << endl;
  }
  print_edges(g, id);

  cout << endl << "removing edge (1,3): " << endl;
  remove_edge(vertex(1, g), vertex(3, g), g);

  ei = out_edges(vertex(1, g), g).first;
  cout << "removing edge (" << id[source(*ei, g)]
       << "," << id[target(*ei, g)] << ")" << endl;
  remove_edge(ei, g);

  for(boost::tie(i,end) = vertices(g); i != end; ++i) {
    cout << id[*i] << " ";
    for (boost::tie(ei,edge_end) = out_edges(*i, g); ei != edge_end; ++ei)
      cout << " --" << name[*ei] << "--> " << id[target(*ei, g)] << "  ";
    cout << endl;
  }

  print_edges(g, id);

  return 0;
}


#include <iostream>
#include <armadillo>
#include <vector>
#include <string>
#include "KS2sample.hpp"

int main(int argc, char **argv)
{

    std::vector<std::string> files(4);
    files[0] = "Rayleigh1.bin";
    files[1] = "Rayleigh2.bin";
    files[2] = "Rayleigh3.bin";
    files[3] = "Exponential.bin";

    for(int ii=0; ii<4; ii++)
    {
        arma::fvec alldata;
        alldata.load(files[ii], arma::raw_binary);

        int nrows = alldata.size() / 2;
        arma::fmat x(alldata.memptr(), nrows,2, false);

        KS2sample worker(nrows);

        double prob = worker.test(x.colptr(0), x.colptr(1));
        std::cout << "File: " << files[ii] << " Prob: " << prob << "\n";
    }

}


#ifndef PYTHONIC_INCLUDE_NUMPY_ANGLEINDEG_HPP
#define PYTHONIC_INCLUDE_NUMPY_ANGLEINDEG_HPP

#include "pythonic/include/numpy/angle_in_rad.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/indeg.hpp>

/* NOTE: angle_in_deg is ! part of the official Numpy API,
 * this file is here only to split the angle function in two parts
 */

PYTHONIC_NS_BEGIN

namespace numpy
{

  namespace wrapper
  {
    template <class T>
    auto angle_in_deg(T const &t)
        -> decltype(boost::simd::indeg(angle_in_rad(t)))
    {
      return boost::simd::indeg(angle_in_rad(t));
    }
  }
#define NUMPY_NARY_FUNC_NAME angle_in_deg
#define NUMPY_NARY_FUNC_SYM wrapper::angle_in_deg
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>
#include <Eigen/Dense>
#include <unordered_map>
#include <utility>
#include <tuple>
#include <map>
#include <set>
#include <vector>
#include <algorithm>

#include "tuple_hash.h"

using MatrixXi = Eigen::Matrix<long, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
using VectorXi = Eigen::Matrix<long, Eigen::Dynamic, 1>;
using MatrixXiRM = Eigen::Matrix<long, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using MatrixXdRM = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using namespace std;

class FJaccard{
protected:
	unordered_map<tuple<long, long>, double> similarity_cache_;
	vector<vector<long>> item_users_;
	int cache_limit_;

	double calculate_jaccard_(const vector<long> &i1v, const vector<long> &i2v){
		std::vector<long> intersection;
		intersection.reserve(min(i1v.size(), i2v.size()));
		set_intersection(
			i1v.begin(), i1v.end(),
			i2v.begin(), i2v.end(),
			std::back_inserter(intersection)
		);
		if(i1v.size() + i2v.size() == 0){
			return 0;
		}
		return double(intersection.size()) / (i1v.size() + i2v.size() - intersection.size());
	}
	void build_userlists_(
		const Eigen::Ref<const MatrixXi> &user_vector,
		const Eigen::Ref<const MatrixXi> &item_vector
	){
		int items_max = 0;
		for(uint i=0; i< item_vector.rows(); i++){
			if(item_vector(i,0) > items_max){
				items_max = item_vector(i,0);
			}
		}
		item_users_ = vector<vector<long>>(items_max+1, vector<long>());
		for(uint i=0; i < item_vector.rows(); i++){
			item_users_[item_vector(i,0)].push_back(user_vector(i,0));
		}
		for(auto &v : item_users_){
			sort(v.begin(), v.end());
		}
	}

public:
	FJaccard(
		const Eigen::Ref<const MatrixXi> &user_vector,
		const Eigen::Ref<const MatrixXi> &item_vector,
		int cache_limit
	) {
		build_userlists_(user_vector, item_vector);
		cache_limit_ = cache_limit;
	}

	double query(long i1, long i2){
		if(i1 == i2){
			return 1;
		}
		if(i1 >= item_users_.size() || i2 >= item_users_.size()){
			return 0;
		}
		bool need_cache = true;
		vector<long> *i1v, *i2v;
		if(cache_limit_ != 0){
			i1v = &item_users_[i1];
			i2v = &item_users_[i2];
			need_cache = i1v->size() >= cache_limit_ && i2v->size() >= cache_limit_;
		}
		if(need_cache){
			auto cache_hit = similarity_cache_.find(make_tuple(min(i1, i2), max(i1, i2)));
			if(cache_hit != similarity_cache_.end()){
				return cache_hit->second;
			}
		}
		if(cache_limit_ == 0){
			i1v = &item_users_[i1];
			i2v = &item_users_[i2];
		}
		double jaccard = calculate_jaccard_(*i1v, *i2v);
		if(need_cache){
			similarity_cache_[make_tuple(min(i1, i2), max(i1, i2))] = jaccard;
		}
		return jaccard;
	}

	MatrixXdRM query_square(const Eigen::Ref<const MatrixXi> &items){
		MatrixXdRM result = MatrixXdRM::Identity(items.rows(), items.rows());
		for(uint i=0; i<items.rows(); i++){
			for(uint j=i+1; j<items.rows(); j++){
				double jaccard = query(items(i,0),items(j,0));
				result(i,j) = jaccard;
				result(j,i) = jaccard;
			}
		}
		return result;
	}

	MatrixXdRM query_pairs(
		const Eigen::Ref<const MatrixXi> &items1,
		const Eigen::Ref<const MatrixXi> &items2
	){
		MatrixXdRM result = MatrixXdRM::Zero(items1.rows(), items2.rows());
		for(uint i=0; i<items1.rows(); i++){
			for(uint j=0; j<items2.rows(); j++){
				result(i,j) = query(items1(i,0),items2(j,0));
			}
		}
		return result;
	}
};

namespace py = pybind11;
PYBIND11_MODULE(fjaccard, m) {
    py::class_<FJaccard>(m, "FJaccard")
        .def(
        	py::init(
        		[](
					const Eigen::Ref<const MatrixXi> &user_vector,
					const Eigen::Ref<const MatrixXi> &item_vector,
					int cache_limit
				) -> unique_ptr<FJaccard> {
					return unique_ptr<FJaccard>(new FJaccard(user_vector, item_vector, cache_limit));
				}
	        ),
			py::arg().noconvert(),
			py::arg().noconvert(),
			py::arg() = 0
		)
		.def("query", &FJaccard::query)
		.def("query_square",
			[](FJaccard &self, const Eigen::Ref<const MatrixXi> &items) -> MatrixXdRM {
				return self.query_square(items);
			},
			py::arg().noconvert()
		)
		.def("query_pairs",
			[](
				FJaccard &self,
				const Eigen::Ref<const MatrixXi> &items1,
				const Eigen::Ref<const MatrixXi> &items2
			) -> MatrixXdRM {
				return self.query_pairs(items1, items2);
			},
			py::arg().noconvert(),
			py::arg().noconvert()
		);

	#ifdef VERSION_INFO
	    m.attr("__version__") = VERSION_INFO;
	#else
	    m.attr("__version__") = "dev";
	#endif
}


#ifndef STAN_MATH_REV_MAT_FUN_TCROSSPROD_HPP
#define STAN_MATH_REV_MAT_FUN_TCROSSPROD_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/typedefs.hpp>
#include <stan/math/rev/core.hpp>
#include <stan/math/rev/mat/fun/Eigen_NumTraits.hpp>
#include <stan/math/rev/mat/fun/typedefs.hpp>
#include <stan/math/rev/mat/fun/dot_product.hpp>
#include <stan/math/rev/mat/fun/dot_self.hpp>
#include <stan/math/rev/mat/fun/columns_dot_self.hpp>
#include <boost/math/tools/promotion.hpp>
#include <vector>

namespace stan {
namespace math {

/**
 * Returns the result of post-multiplying a matrix by its
 * own transpose.
 * @param M Matrix to multiply.
 * @return M times its transpose.
 */
inline matrix_v tcrossprod(const matrix_v& M) {
  if (M.rows() == 0)
    return matrix_v(0, 0);
  // if (M.rows() == 1)
  //   return M * M.transpose();

  // WAS JUST THIS
  // matrix_v result(M.rows(), M.rows());
  // return result.setZero().selfadjointView<Eigen::Upper>().rankUpdate(M);

  matrix_v MMt(M.rows(), M.rows());

  vari** vs
      = reinterpret_cast<vari**>(ChainableStack::instance().memalloc_.alloc(
          (M.rows() * M.cols()) * sizeof(vari*)));
  int pos = 0;
  for (int m = 0; m < M.rows(); ++m)
    for (int n = 0; n < M.cols(); ++n)
      vs[pos++] = M(m, n).vi_;
  for (int m = 0; m < M.rows(); ++m)
    MMt(m, m) = var(new dot_self_vari(vs + m * M.cols(), M.cols()));
  for (int m = 0; m < M.rows(); ++m) {
    for (int n = 0; n < m; ++n) {
      MMt(m, n) = var(new dot_product_vari<var, var>(
          vs + m * M.cols(), vs + n * M.cols(), M.cols()));
      MMt(n, m) = MMt(m, n);
    }
  }
  return MMt;
}

}  // namespace math
}  // namespace stan
#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2015 Jakub Pola <jakub.pola@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

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

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/scatter_if.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/constant_buffer_iterator.hpp>
#include <boost/compute/iterator/counting_iterator.hpp>
#include <boost/compute/functional.hpp>
#include "check_macros.hpp"
#include "context_setup.hpp"

namespace bc = boost::compute;

BOOST_AUTO_TEST_CASE(scatter_if_int)
{
    int input_data[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    bc::vector<int> input(input_data, input_data + 10, queue);

    int map_data[] = {9, 8, 7, 6, 5, 4, 3, 2, 1, 0};

    bc::vector<int> map(map_data, map_data + 10, queue);

    int stencil_data[] = {0, 1, 0, 1, 0, 1, 0, 1, 0, 1};

    bc::vector<bc::uint_> stencil(stencil_data, stencil_data + 10, queue);

    bc::vector<int> output(input.size(), -1, queue);

    bc::scatter_if(input.begin(), input.end(),
                   map.begin(), stencil.begin(),
                   output.begin());

    CHECK_RANGE_EQUAL(int, 10, output, (9, -1, 7, -1, 5, -1, 3, -1, 1, -1) );
}

BOOST_AUTO_TEST_CASE(scatter_if_constant_indices)
{
    int input_data[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    bc::vector<int> input(input_data, input_data + 10, queue);

    int map_data[] = {9, 8, 7, 6, 5, 4, 3, 2, 1, 0};
    bc::buffer map_buffer(context,
                          10 * sizeof(int),
                          bc::buffer::read_only | bc::buffer::use_host_ptr,
                          map_data);

    int stencil_data[] = {0, 1, 0, 1, 0, 1, 0, 1, 0, 1};
    bc::buffer stencil_buffer(context,
                              10 * sizeof(bc::uint_),
                              bc::buffer::read_only | bc::buffer::use_host_ptr,
                              stencil_data);

    bc::vector<int> output(input.size(), -1, queue);

    bc::scatter_if(input.begin(),
                   input.end(),
                   bc::make_constant_buffer_iterator<int>(map_buffer, 0),
                   bc::make_constant_buffer_iterator<int>(stencil_buffer, 0),
                   output.begin(),
                   queue);

    CHECK_RANGE_EQUAL(int, 10, output, (9, -1, 7, -1, 5, -1, 3, -1, 1, -1) );
}

BOOST_AUTO_TEST_CASE(scatter_if_function)
{
    int input_data[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    bc::vector<int> input(input_data, input_data + 10, queue);

    int map_data[] = {9, 8, 7, 6, 5, 4, 3, 2, 1, 0};
    bc::vector<int> map(map_data, map_data + 10, queue);

    int stencil_data[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    bc::vector<bc::uint_> stencil(stencil_data, stencil_data + 10, queue);

    bc::vector<int> output(input.size(), -1, queue);

    BOOST_COMPUTE_FUNCTION(int, gt_than_5, (int x),
    {
        if (x > 5)
            return true;
        else
            return false;
    });

    bc::scatter_if(input.begin(),
                   input.end(),
                   map.begin(),
                   stencil.begin(),
                   output.begin(),
                   gt_than_5,
                   queue);

    CHECK_RANGE_EQUAL(int, 10, output, (9, 8, 7, 6, -1, -1, -1, -1, -1, -1) );
}

BOOST_AUTO_TEST_CASE(scatter_if_counting_iterator)
{
    int input_data[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    bc::vector<int> input(input_data, input_data + 10, queue);

    int map_data[] = {9, 8, 7, 6, 5, 4, 3, 2, 1, 0};
    bc::vector<int> map(map_data, map_data + 10, queue);

    bc::vector<int> output(input.size(), -1, queue);

    BOOST_COMPUTE_FUNCTION(int, gt_than_5, (int x),
    {
        if (x > 5)
            return true;
        else
            return false;
    });

    bc::scatter_if(input.begin(),
                   input.end(),
                   map.begin(),
                   bc::make_counting_iterator<int>(0),
                   output.begin(),
                   gt_than_5,
                   queue);

    CHECK_RANGE_EQUAL(int, 10, output, (9, 8, 7, 6, -1, -1, -1, -1, -1, -1) );

}

BOOST_AUTO_TEST_SUITE_END()


#ifndef SKYLARK_COMBBLAS_MIXED_GEMM_HPP
#define SKYLARK_COMBBLAS_MIXED_GEMM_HPP

#include <boost/mpi.hpp>
#include "../exception.hpp"

#if SKYLARK_HAVE_COMBBLAS
#include <CombBLAS.h>
#include <CommGrid.h>
#endif

#include "../../utility/external/view.hpp"
#include "../../utility/external/combblas_comm_grid.hpp"
#include "../../utility/external/elemental_comm_grid.hpp"

#if SKYLARK_HAVE_COMBBLAS

namespace skylark { namespace base {

namespace detail {

/// only compute local product:
///   elem(n x k) x local_part_cb(k x m) -> array(n x m)
template<typename index_type, typename value_type>
inline void mixed_gemm_local_part_nn (
        const double alpha,
        const SpParMat<index_type, value_type, SpDCCols<index_type, value_type> > &A,
        const El::DistMatrix<value_type, El::STAR, El::STAR> &S,
        const double beta,
        std::vector<value_type> &local_matrix) {

    typedef SpDCCols< index_type, value_type > col_t;
    typedef SpParMat< index_type, value_type, col_t > matrix_type;
    matrix_type &_A = const_cast<matrix_type&>(A);
    col_t &data = _A.seq();

    //FIXME
    local_matrix.resize(S.Width() * data.getnrow(), 0);
    size_t cb_col_offset = utility::cb_my_col_offset(A);

    for(typename col_t::SpColIter col = data.begcol();
        col != data.endcol(); col++) {
        for(typename col_t::SpColIter::NzIter nz = data.begnz(col);
            nz != data.endnz(col); nz++) {

            // we want local index here to fill local dense matrix
            index_type rowid = nz.rowid();
            // column needs to be global
            index_type colid = col.colid() + cb_col_offset;

            // compute application of S to yield a partial row in the result.
            for(size_t bcol = 0; bcol < S.Width(); ++bcol) {
                local_matrix[rowid * S.Width() + bcol] +=
                        alpha * S.Get(colid, bcol) * nz.value();
            }
        }
    }
}

/// implementing gemm for CB * (*/*) = (SOMETHING/*)
//FIXME: benchmark against one-sided
template<typename index_type, typename value_type, El::Distribution col_d>
inline void inner_panel_mixed_gemm_impl_nn(
        const double alpha,
        const SpParMat<index_type, value_type, SpDCCols<index_type, value_type> > &A,
        const El::DistMatrix<value_type, El::STAR, El::STAR> &S,
        const double beta,
        El::DistMatrix<value_type, col_d, El::STAR> &C) {

    int n_proc_side   = A.getcommgrid()->GetGridRows();
    int output_width  = S.Width();
    int output_height = A.getnrow();

    size_t rank = A.getcommgrid()->GetRank();
    size_t cb_row_offset = utility::cb_my_row_offset(A);

    typedef SpDCCols< index_type, value_type > col_t;
    typedef SpParMat< index_type, value_type, col_t > matrix_type;
    matrix_type &_A = const_cast<matrix_type&>(A);
    col_t &data = _A.seq();

    // 1) compute the local values still using the CombBLAS distribution (2D
    //    processor grid). We assume the result is dense.
    std::vector<double> local_matrix;
    mixed_gemm_local_part_nn(alpha, A, S, 0.0, local_matrix);

    // 2) reduce first along rows so that each processor owns the values in
    //    the output row of the SOMETHING/* matrix and values for processors in
    //    the same processor column.
    boost::mpi::communicator my_row_comm(
            A.getcommgrid()->GetRowWorld(), boost::mpi::comm_duplicate);

    // storage for other procs in same row communicator: rank -> (row, values)
    typedef std::vector<std::pair<int, std::vector<double> > > for_rank_t;
    std::vector<for_rank_t> for_rank(n_proc_side);

    for(size_t local_row = 0; local_row < data.getnrow(); ++local_row) {

        size_t row = local_row + cb_row_offset;

        // the owner for VR/* and VC/* matrices is independent of the column
        size_t target_proc = utility::owner(C, row, static_cast<size_t>(0));

        // if the target processor is not in the current row communicator, get
        // the value in the processor grid sharing the same row.
        if(!A.getcommgrid()->OnSameProcRow(target_proc))
            target_proc = static_cast<int>(rank / n_proc_side) *
                            n_proc_side + target_proc % n_proc_side;

        size_t target_row_rank = A.getcommgrid()->GetRankInProcRow(target_proc);

        // reduce partial row (FIXME: if the resulting matrix is still
        // expected to be sparse, change this to communicate only nnz).
        // Working on local_width columns concurrently per column processing
        // group.
        size_t local_width = S.Width();
        const value_type* buffer = &local_matrix[local_row * local_width];
        std::vector<value_type> new_values(local_width);
        boost::mpi::reduce(my_row_comm, buffer, local_width,
                &new_values[0], std::plus<value_type>(), target_row_rank);

        // processor stores result directly if it is the owning rank of that
        // row, save for subsequent communication along rows otherwise
        if(rank == utility::owner(C, row, static_cast<size_t>(0))) {
            int elem_lrow = C.LocalRow(row);
            for(size_t idx = 0; idx < local_width; ++idx) {
                int elem_lcol = C.LocalCol(idx);
                C.SetLocal(elem_lrow, elem_lcol,
                    new_values[idx] + beta * C.GetLocal(elem_lrow, elem_lcol));
            }
        } else if (rank == target_proc) {
            // store for later comm across rows
            for_rank[utility::owner(C, row, static_cast<size_t>(0)) / n_proc_side].push_back(
                    std::make_pair(row, new_values));
        }
    }

    // 3) gather remaining values along rows: we exchange all the values with
    //    other processors in the same communicator row and then add them to
    //    our local part.
    boost::mpi::communicator my_col_comm(
            A.getcommgrid()->GetColWorld(), boost::mpi::comm_duplicate);

    std::vector<for_rank_t> new_values;
    for(int i = 0; i < n_proc_side; ++i)
        boost::mpi::gather(my_col_comm, for_rank[i], new_values, i);

    // insert new values
    for(size_t proc = 0; proc < new_values.size(); ++proc) {
        const for_rank_t &cur  = new_values[proc];

        for(size_t i = 0; i < cur.size(); ++i) {
            int elem_lrow = C.LocalRow(cur[i].first);
            for(size_t j = 0; j < cur[i].second.size(); ++j) {
                size_t elem_lcol = C.LocalCol(j);
                C.SetLocal(elem_lrow, elem_lcol,
                        cur[i].second[j] + beta *
                        C.GetLocal(elem_lrow, elem_lcol));
            }
        }
    }
}

//FIXME: benchmark against one-sided
template<typename index_type, typename value_type, El::Distribution col_d>
inline void outer_panel_mixed_gemm_impl_nn(
        const double alpha,
        const SpParMat<index_type, value_type, SpDCCols<index_type, value_type> > &A,
        const El::DistMatrix<value_type, El::STAR, El::STAR> &S,
        const double beta,
        El::DistMatrix<value_type, col_d, El::STAR> &C) {

    utility::combblas_slab_view_t<index_type, value_type> cbview(A, false);

    //FIXME: factor
    size_t slab_size = 2 * C.Grid().Height();
    for(size_t cur_row_idx = 0; cur_row_idx < cbview.nrows();
        cur_row_idx += slab_size) {

        size_t cur_slab_size =
            std::min(slab_size, cbview.nrows() - cur_row_idx);

        // get the next slab_size columns of B
        El::DistMatrix<value_type, col_d, El::STAR>
            A_row(cur_slab_size, S.Height());

        cbview.extract_elemental_row_slab_view(A_row, cur_slab_size);

        // assemble the distributed column vector
        for(size_t l_col_idx = 0; l_col_idx < A_row.LocalWidth();
            l_col_idx++) {

            size_t g_col_idx = l_col_idx * A_row.RowStride()
                               + A_row.RowShift();

            for(size_t l_row_idx = 0; l_row_idx < A_row.LocalHeight();
                ++l_row_idx) {

                size_t g_row_idx = l_row_idx * A_row.ColStride()
                                   + A_row.ColShift() + cur_row_idx;

                A_row.SetLocal(l_row_idx, l_col_idx,
                               cbview(g_row_idx, g_col_idx));
            }
        }

        El::DistMatrix<value_type, col_d, El::STAR>
            C_slice(cur_slab_size, C.Width());
        El::View(C_slice, C, cur_row_idx, 0, cur_slab_size, C.Width());
        El::LocalGemm(El::NORMAL, El::NORMAL, alpha, A_row, S,
                        beta, C_slice);
    }
}

//FIXME: benchmark against one-sided
template<typename index_type, typename value_type, El::Distribution col_d>
inline void outer_panel_mixed_gemm_impl_tn(
        const double alpha,
        const SpParMat<index_type, value_type, SpDCCols<index_type, value_type> > &A,
        const El::DistMatrix<value_type, col_d, El::STAR> &S,
        const double beta,
        El::DistMatrix<value_type, El::STAR, El::STAR> &C) {

    El::DistMatrix<value_type, El::STAR, El::STAR>
        tmp_C(C.Height(), C.Width());
    El::Zero(tmp_C);

    utility::combblas_slab_view_t<index_type, value_type> cbview(A, false);

    //FIXME: factor
    size_t slab_size = 2 * S.Grid().Height();
    for(size_t cur_row_idx = 0; cur_row_idx < cbview.ncols();
        cur_row_idx += slab_size) {

        size_t cur_slab_size =
            std::min(slab_size, cbview.ncols() - cur_row_idx);

        // get the next slab_size columns of B
        El::DistMatrix<value_type, El::STAR, El::STAR>
            A_row(cur_slab_size, S.Height());

        // transpose is column
        //cbview.extract_elemental_column_slab_view(A_row, cur_slab_size);
        cbview.extract_full_slab_view(cur_slab_size);

        // matrix mult (FIXME only iter nz)
        for(size_t l_row_idx = 0; l_row_idx < A_row.LocalHeight();
            ++l_row_idx) {

            size_t g_row_idx = l_row_idx * A_row.ColStride()
                               + A_row.ColShift() + cur_row_idx;

            for(size_t l_col_idx = 0; l_col_idx < A_row.LocalWidth();
                l_col_idx++) {

                //XXX: should be the same as l_col_idx
                size_t g_col_idx = l_col_idx * A_row.RowStride()
                                   + A_row.RowShift();

                // continue if we don't own values in S in this row
                if(!S.IsLocalRow(g_col_idx))
                    continue;

                //get transposed value
                value_type val = alpha * cbview(g_col_idx, g_row_idx);

                for(size_t s_col_idx = 0; s_col_idx < S.LocalWidth();
                    s_col_idx++) {

                    tmp_C.UpdateLocal(g_row_idx, s_col_idx,
                                val * S.GetLocal(S.LocalRow(g_col_idx), s_col_idx));
                }
            }
        }
    }

    //FIXME: scaling
    if(A.getcommgrid()->GetRank() == 0) {
        for(size_t col_idx = 0; col_idx < C.Width(); col_idx++)
            for(size_t row_idx = 0; row_idx < C.Height(); row_idx++)
                tmp_C.UpdateLocal(row_idx, col_idx,
                        beta * C.GetLocal(row_idx, col_idx));
    }

    //FIXME: Use utility getter
    boost::mpi::communicator world(
            A.getcommgrid()->GetWorld(), boost::mpi::comm_duplicate);
    boost::mpi::all_reduce (world,
                        tmp_C.LockedBuffer(),
                        C.Height() * C.Width(),
                        C.Buffer(),
                        std::plus<value_type>());
}

} // namespace detail

/**
 * Mixed GEMM for Elental and CombBLAS matrices. For a distributed Elemental
 * input matrix, the output has the same distribution.
 */

/// Gemm for distCombBLAS x distElental(* / *) -> distElental (SOMETHING / *)
template<typename index_type, typename value_type, El::Distribution col_d>
void Gemm(El::Orientation oA, El::Orientation oB, double alpha,
          const SpParMat<index_type, value_type, SpDCCols<index_type, value_type> > &A,
          const El::DistMatrix<value_type, El::STAR, El::STAR> &B,
          double beta,
          El::DistMatrix<value_type, col_d, El::STAR> &C) {

    if(oA == El::NORMAL && oB == El::NORMAL) {

        if(A.getnol() != B.Height())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        if(A.getnrow() != C.Height())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        if(B.Width() != C.Width())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        //XXX: simple heuristic to decide what to communicate (improve!)
        //     or just if A.getncol() < B.Width..
        if(A.getnnz() < B.Height() * B.Width())
            detail::outer_panel_mixed_gemm_impl_nn(alpha, A, B, beta, C);
        else
            detail::inner_panel_mixed_gemm_impl_nn(alpha, A, B, beta, C);
    }
}

/// Gemm for distCombBLAS x distElental(SOMETHING / *) -> distElental (* / *)
template<typename index_type, typename value_type, El::Distribution col_d>
void Gemm(El::Orientation oA, El::Orientation oB, double alpha,
          const SpParMat<index_type, value_type, SpDCCols<index_type, value_type> > &A,
          const El::DistMatrix<value_type, col_d, El::STAR> &B,
          double beta,
          El::DistMatrix<value_type, El::STAR, El::STAR> &C) {

    if(oA == El::TRANSPOSE && oB == El::NORMAL) {

        if(A.getrow() != B.Height())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        if(A.getncol() != C.Height())
            SKYLARK_THROW_EXCEPTION (
                    base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        if(B.Width() != C.Width())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        detail::outer_panel_mixed_gemm_impl_tn(alpha, A, B, beta, C);
    }

}

} // namespace base
} // namespace skylark

#endif // SKYLARK_HAVE_COMBBLAS

#endif // SKYLARK_COMBBLAS_MIXED_GEMM_HPP_


#include "random.h"
#include <boost/random.hpp>
#include <ctime>
#include <cmath>
#include <vector>

namespace c2pool::random
{
    using namespace boost::random;

    boost::random::mt19937 generator(std::time(0));

    ///[min, max)
    int RandomInt(int min, int max)
    {
        boost::random::uniform_int_distribution<> rnd(min, max - 1);
        return rnd(generator);
    }

    ///[min, max]
    float RandomFloat(float min, float max)
    {
        float Min = float(min), Max = float(max);
        boost::random::uniform_int_distribution<> rnd(Min, Max);
        float res = ((float)rnd(generator) / Max);
        float range = Max - Min;
        res = (res * range) + Min;
        return res;
    }

    std::vector<unsigned char> random_bytes(int32_t length)
    {
        std::vector<unsigned char> bytes;

        for (int i = 0; i < length; i++)
        {
            bytes.emplace_back(c2pool::random::RandomInt(0, 256));
        }

    }

    template <typename T>
    T RandomChoice(std::vector<T> &list)
    {
        int pos = RandomInt(0, list.size());
        return list[pos];
    }

    // template <typename K, typename V, typename Compare = std::less<K>,
    //     typename Alloc = std::allocator<std::pair<const K, V> > >
    // V RandomChoice(std::map<K, V, Compare, Alloc> map)
    // { //TODO: THIS WANNA TEST
    //     int pos = RandomInt(0, map.size());
    //     return std::advance(map.begin(), pos);
    // }

    ///l = 1.0/<среднее желаемое число>
    float Expovariate(float l)
    {
        return (log(RandomInt(1, RAND_MAX) + 1) - log(RAND_MAX)) / (-1 / l);
    }

    unsigned long long randomNonce()
    {
        boost::random::uniform_int_distribution<unsigned long long> rnd(0, 0xFFFFFFFFFFFFFFFF);
        return rnd(generator);
    }

} // namespace c2pool::random


//
//  FrameStorage.cpp
//  CardRecognizer
//
//  Created by Vladimir Tchernitski on 11/01/16.
//  Copyright © 2016 Vladimir Tchernitski. All rights reserved.
//

#include <Eigen>

#include "FrameStorage.h"
#include "warp.h"
#include "Utils.h"

static const Size lowSize = Size(660,416);

CFrameStorage::CFrameStorage() : _grayOrientation(PayCardsRecognizerOrientationUnknown), _yuvOrientation(PayCardsRecognizerOrientationUnknown)
{
    int bufferHeightY = 1280;
    int bytesPerRowY = 720;

    int bufferHeightUV = 640;
    int bytesPerRowUV = 720;

    _bufferSizeY = bufferHeightY * bytesPerRowY;
    _bufferY = malloc(_bufferSizeY);

    _bufferSizeUV = bufferHeightUV * bytesPerRowUV;
    _bufferUV = malloc(_bufferSizeUV);
}

CFrameStorage::~CFrameStorage()
{
    _edges.clear();
    free(_bufferY);
    free(_bufferUV);
}

bool CFrameStorage::SetRawFrame(const Mat& rawFrame, const vector<ParametricLine>& edges, PayCardsRecognizerOrientation orientation)
{
    if (!NormalizeMatrix(rawFrame, edges, _frame)) return false;

    _grayOrientation = orientation;

    if (_grayOrientation != PayCardsRecognizerOrientationPortraitUpsideDown && _grayOrientation != PayCardsRecognizerOrientationPortrait) {

        CUtils::RotateMatrix90n(_frame, _frame, 90);
    }

    cv::resize(_frame, _frame, lowSize, CV_INTER_CUBIC);

    return true;
}

bool CFrameStorage::NormalizeMatrix(const Mat& matrix, const vector<ParametricLine>& edges, Mat& resultMatrix)
{
    vector<cv::Point> corners = {cv::Point(0,0),cv::Point(0,0),cv::Point(0,0),cv::Point(0,0)};

    if (!FindIntersectCorners(corners, edges)) return false;

    NormalizeMatrix(matrix, resultMatrix, 0, corners);

    return true;

}

void CFrameStorage::SetRawY(const void* rawY, const void* rawUV, const vector<ParametricLine>& edges, PayCardsRecognizerOrientation orientation)
{
    memcpy(_bufferY, rawY, _bufferSizeY);
    memcpy(_bufferUV, rawUV, _bufferSizeUV);
    _edges.clear();
    _edges = edges;
    _yuvOrientation = orientation;
}

bool CFrameStorage::GetCurrentFrame(Mat& mat)
{
    mat = _frame;
    return true;
}

void CFrameStorage::PopFrame()
{
    _frame = Scalar(0,0,0);
}

const void* CFrameStorage::GetUVMat()
{
    return _bufferUV;
}

const void* CFrameStorage::GetYMat()
{
    return _bufferY;
}

const PayCardsRecognizerOrientation CFrameStorage::GetYUVOrientation()
{
    return _yuvOrientation;
}

const vector<ParametricLine> CFrameStorage::GetEdges()
{
    return _edges;
}

//////////////////////////////////////

// 0 - top-left
// 1 - bottom-left
// 2 - top-right
// 3 - bottom-right
bool CFrameStorage::FindIntersectCorners(vector<cv::Point>& corners, vector<ParametricLine> edges)
{

    bool intersectsTL = ParametricIntersect(edges[0], edges[2], &corners[0].x, &corners[0].y);
    bool intersectsBL = ParametricIntersect(edges[1], edges[2], &corners[1].x, &corners[1].y);
    bool intersectsTR = ParametricIntersect(edges[0], edges[3], &corners[2].x, &corners[2].y);
    bool intersectsBR = ParametricIntersect(edges[1], edges[3], &corners[3].x, &corners[3].y);

    bool allIntersect = intersectsTL && intersectsBL && intersectsTR && intersectsBR;

    return allIntersect;
}

void CFrameStorage::NormalizeMatrix(const Mat& src, Mat& dst, int intend, vector<cv::Point>& corner_points)
{
    vector<cv::Point> srcPoints;

    srcPoints.push_back(corner_points[0]);
    srcPoints.push_back(corner_points[2]);
    srcPoints.push_back(corner_points[1]);
    srcPoints.push_back(corner_points[3]);

    llcv_unwarp(src, srcPoints, cv::Rect(intend/2, intend/2, src.cols-intend, src.rows-intend), dst);
}

bool CFrameStorage::ParametricIntersect(const ParametricLine& line1, const ParametricLine& line2, int *x, int *y)
{
    if(IsParametricLineNone(line1) || IsParametricLineNone(line2)) {
        return false;
    }

    Eigen::Matrix2f t;
    Eigen::Vector2f r;
    t << cosf(line1.theta), sinf(line1.theta), cosf(line2.theta), sinf(line2.theta);
    r << line1.rho, line2.rho;

    if(t.determinant() < 1e-10) {
        return false;
    }

    Eigen::Vector2f intersection = t.inverse() * r;
    *x = cvRound(intersection(0));
    *y = cvRound(intersection(1));
    return true;
}

bool CFrameStorage::IsParametricLineNone(ParametricLine line)
{
    return ((line).theta == FLT_MAX);
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <boost/units/base_dimension.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/unit.hpp>
#include <boost/units/quantity.hpp>

//[runtime_conversion_factor_snippet_1

using boost::units::base_dimension;
using boost::units::base_unit;

static const long currency_base = 1;

struct currency_base_dimension : base_dimension<currency_base_dimension, 1> {};

typedef currency_base_dimension::dimension_type currency_type;

template<long N>
struct currency_base_unit :
    base_unit<currency_base_unit<N>, currency_type, currency_base + N> {};

typedef currency_base_unit<0> us_dollar_base_unit;
typedef currency_base_unit<1> euro_base_unit;

typedef us_dollar_base_unit::unit_type us_dollar;
typedef euro_base_unit::unit_type euro;

// an array of all possible conversions
double conversion_factors[2][2] = {
    {1.0, 1.0},
    {1.0, 1.0}
};

double get_conversion_factor(long from, long to) {
    return(conversion_factors[from][to]);
}

void set_conversion_factor(long from, long to, double value) {
    conversion_factors[from][to] = value;
    conversion_factors[to][from] = 1.0 / value;
}

BOOST_UNITS_DEFINE_CONVERSION_FACTOR_TEMPLATE((long N1)(long N2),
    currency_base_unit<N1>,
    currency_base_unit<N2>,
    double, get_conversion_factor(N1, N2));

//]

int main() {
    boost::units::quantity<us_dollar> dollars = 2.00 * us_dollar();
    boost::units::quantity<euro> euros(dollars);
    set_conversion_factor(0, 1, 2.0);
    dollars = static_cast<boost::units::quantity<us_dollar> >(euros);
    set_conversion_factor(0, 1, .5);
    euros = static_cast<boost::units::quantity<euro> >(dollars);
    double value = euros.value(); // = .5
    if(value != .5) {
        return(1);
    } else {
        return(0);
    }
}


// Copyright (c) 2019 The Thincoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <chainparams.h>
#include <coins.h>
#include <consensus/merkle.h>
#include <pow.h>
#include <validation.h>
#include <miner.h>
#include <uint256.h>

#include <test/test_bitcoin.h>

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(interblock_tests, TestChain100Setup)

bool BetweenWithWrapping(arith_uint256 min, arith_uint256 value, arith_uint256 max)
{
    if (min <= max && (value < min || value > max))
        return false;
    else if (max < min && (value < min && value > max))
        return false;

    return true;
}

uint256 GetSaltedMerkle(uint256 merkleRoot, uint32_t merkleSalt)
{
    std::vector<unsigned char> vch(36);
    CVectorWriter ss(SER_NETWORK, PROTOCOL_VERSION, vch, 0);
    ss << merkleRoot << merkleSalt;
    return HashX11((const char *)vch.data(), (const char *)vch.data() + vch.size());
}

BOOST_AUTO_TEST_CASE(InterblockHash_validity)
{
    // Check validation from no skipping to its allowed maximum value
    for(unsigned int skip = 0; skip <= MAX_SKIP; ++skip)
    {
        // Check every block from regtest
        for(unsigned int i = skip + 1; chainActive[i + skip + 1] != nullptr; ++i)
        {
            // Get current block and previous block with skipping
            CBlockIndex *pblock = chainActive[i];
            CBlockIndex *pprev  = chainActive[i - skip - 1];
            CBlockIndex *pnext  = chainActive[i + skip + 1];
            arith_uint256 bnTarget, targetLowBound, targetUpBound, skipLowTarget, skipMidTarget, skipUpTarget;
            arith_uint256 MAX_VERIFIABLE = UintToArith256(uint256S("0xffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff"));

            // Validate current block's validity
            arith_uint256 blockSaltedMerkle = UintToArith256(pblock->GetBlockSaltedMerkle());
            arith_uint256 blockPoW = UintToArith256(pblock->GetBlockPoWHash());
            bnTarget.SetCompact(pblock->nBits);
            targetLowBound = blockSaltedMerkle;
            targetUpBound  = targetLowBound + bnTarget;
            BOOST_ASSERT(BetweenWithWrapping(targetLowBound, blockPoW, targetUpBound));
            targetUpBound  = blockPoW;
            targetLowBound = targetLowBound - bnTarget;
            BOOST_ASSERT(BetweenWithWrapping(targetLowBound, blockSaltedMerkle, targetUpBound));

            BOOST_WARN(MAX_VERIFIABLE / (skip + 1) < bnTarget);
            skipLowTarget  = bnTarget * skip;
            skipMidTarget  = bnTarget * (skip + 1);
            skipUpTarget   = bnTarget * (skip + 2);

            // Validate previous blocks' hash valid range of PoW with skip
            arith_uint256 prevSaltedMerkle = UintToArith256(pprev->GetBlockSaltedMerkle());
            arith_uint256 prevPoW = UintToArith256(pprev->GetBlockPoWHash());
            targetLowBound = blockSaltedMerkle - skipMidTarget;
            targetUpBound  = blockSaltedMerkle + skipMidTarget;
            BOOST_ASSERT(BetweenWithWrapping(targetLowBound, prevSaltedMerkle, targetUpBound));
            targetLowBound = blockPoW - skipUpTarget;
            targetUpBound  = blockPoW + skipMidTarget;
            BOOST_ASSERT(BetweenWithWrapping(targetLowBound, prevSaltedMerkle, targetUpBound));
            targetLowBound = blockSaltedMerkle - skipLowTarget;
            targetUpBound  = blockSaltedMerkle + skipMidTarget;
            BOOST_ASSERT(BetweenWithWrapping(targetLowBound, prevPoW, targetUpBound));
            targetLowBound = blockPoW - skipMidTarget;
            targetUpBound  = blockPoW + skipMidTarget;
            BOOST_ASSERT(BetweenWithWrapping(targetLowBound, prevPoW, targetUpBound));

            // Validate next blocks' hash valid range of PoW with skip
            arith_uint256 nextSaltedMerkle = UintToArith256(pnext->GetBlockSaltedMerkle());
            arith_uint256 nextPoW = UintToArith256(pnext->GetBlockPoWHash());
            targetLowBound = blockSaltedMerkle - skipMidTarget;
            targetUpBound  = blockSaltedMerkle + skipMidTarget;
            BOOST_ASSERT(BetweenWithWrapping(targetLowBound, nextSaltedMerkle, targetUpBound));
            targetLowBound = blockPoW - skipMidTarget;
            targetUpBound  = blockPoW + skipLowTarget;
            BOOST_ASSERT(BetweenWithWrapping(targetLowBound, nextSaltedMerkle, targetUpBound));
            targetLowBound = blockSaltedMerkle - skipMidTarget;
            targetUpBound  = blockSaltedMerkle + skipUpTarget;
            BOOST_ASSERT(BetweenWithWrapping(targetLowBound, nextPoW, targetUpBound));
            targetLowBound = blockPoW - skipMidTarget;
            targetUpBound  = blockPoW + skipMidTarget;
            BOOST_ASSERT(BetweenWithWrapping(targetLowBound, nextPoW, targetUpBound));
        }
    }
}

BOOST_AUTO_TEST_CASE(InterblockTransaction_validity)
{
    // Check validation from no skipping to its allowed maximum value
    for(unsigned int skip = 0; skip <= MAX_SKIP; ++skip)
    {
        // Check every block from regtest
        for(unsigned int i = skip + 1; chainActive[i + skip + 1] != nullptr; ++i)
        {
            // Get current block and previous block with skipping
            CBlockIndex *pblock = chainActive[i];
            CBlockIndex *pprev  = chainActive[i - skip - 1];
            CBlockIndex *pnext  = chainActive[i + skip + 1];
            arith_uint256 bnTarget, targetLowBound, targetUpBound, skipLowTarget, skipMidTarget, skipUpTarget;
            arith_uint256 MAX_VERIFIABLE = UintToArith256(uint256S("0xffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff"));

            // Get their CBlock counterpart in order to access their transactions
            CBlock prevblock, nextblock;
            BOOST_ASSERT(ReadBlockFromDisk(prevblock, pprev, Params().GetConsensus()));
            BOOST_ASSERT(ReadBlockFromDisk(nextblock, pnext, Params().GetConsensus()));

            // Get their coinbase transaction, which is always located at index 0
            const CTransaction *pprevcb = prevblock.vtx[0].get();
            const CTransaction *pnextcb = nextblock.vtx[0].get();

            // Get their merkle branch
            std::vector<uint256> prevbranch = BlockMerkleBranch(prevblock, 0);
            std::vector<uint256> nextbranch = BlockMerkleBranch(nextblock, 0);

            // Get their merkle tree root
            uint256 prevmerkleroot = ComputeMerkleRootFromBranch(pprevcb->GetHash(), prevbranch, 0);
            uint256 nextmerkleroot = ComputeMerkleRootFromBranch(pnextcb->GetHash(), nextbranch, 0);

            // Get the computed salted merkle
            arith_uint256 prevSaltedMerkle = UintToArith256(GetSaltedMerkle(prevmerkleroot, prevblock.nMerkleSalt));
            arith_uint256 nextSaltedMerkle = UintToArith256(GetSaltedMerkle(nextmerkleroot, nextblock.nMerkleSalt));

            // Compute necessary data to determine the valid range
            arith_uint256 blockSaltedMerkle = UintToArith256(pblock->GetBlockSaltedMerkle());
            arith_uint256 blockPoW = UintToArith256(pblock->GetBlockPoWHash());
            bnTarget.SetCompact(pblock->nBits);
            BOOST_WARN(MAX_VERIFIABLE / (skip + 1) < bnTarget);
            skipLowTarget  = bnTarget * skip;
            skipMidTarget  = bnTarget * (skip + 1);
            skipUpTarget   = bnTarget * (skip + 2);

            // Validate previous blocks' hash valid range of salted merkle with skip
            targetLowBound = blockSaltedMerkle - skipMidTarget;
            targetUpBound  = blockSaltedMerkle + skipMidTarget;
            BOOST_ASSERT(BetweenWithWrapping(targetLowBound, prevSaltedMerkle, targetUpBound));
            targetLowBound = blockPoW - skipUpTarget;
            targetUpBound  = blockPoW + skipMidTarget;
            BOOST_ASSERT(BetweenWithWrapping(targetLowBound, prevSaltedMerkle, targetUpBound));

            // Validate next blocks' hash valid range of salted merkle with skip
            targetLowBound = blockSaltedMerkle - skipMidTarget;
            targetUpBound  = blockSaltedMerkle + skipMidTarget;
            BOOST_ASSERT(BetweenWithWrapping(targetLowBound, nextSaltedMerkle, targetUpBound));
            targetLowBound = blockPoW - skipMidTarget;
            targetUpBound  = blockPoW + skipLowTarget;
            BOOST_ASSERT(BetweenWithWrapping(targetLowBound, nextSaltedMerkle, targetUpBound));
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


/* Copyright (c) 2018-2019 the `graphkernels` developers
 * All rights reserved.
 */

#include "connected_graphlet.h"

#include <Eigen/Sparse>

#include <algorithm>

using std::sort;
using std::vector;

using Eigen::MatrixXd;
using Eigen::MatrixXi;
using Eigen::SparseMatrix;
using Eigen::VectorXd;

constexpr auto FREQ_SIZE_3 = 2;
constexpr auto FREQ_SIZE_4 = 6;
constexpr auto FREQ_SIZE_5 = 21;

void getMinValue(const MatrixXi& iam, const vector<int>& idx, vector<int>& sums) {
    SparseMatrix<int> am = iam.sparseView();

    sums.clear();
    sums.resize(idx.size());
    fill(sums.begin(), sums.end(), 0);
    for (auto i = 0; i < idx.size(); ++i) {
        for (SparseMatrix<int>::InnerIterator it(am, idx[i]); it; ++it) {
            if (find(idx.cbegin(), idx.cend(), it.row()) != idx.cend()) {
                sums[i] += it.value();
            }
        }
    }
    sums.push_back(1);
}

VectorXd countConnectedGraphletsFive(
        const MatrixXi& am,
        const vector<vector<int>>& al) {
    vector<double> w = {
        1.0 / 120.0, 1.0 / 72.0, 1.0 / 48.0, 1.0 / 36.0, 1.0 / 28.0, 1.0 / 20.0,
        1.0 / 14.0,  1.0 / 10.0, 1.0 / 12.0, 1.0 / 8.0,  1.0 / 8.0,  1.0 / 4.0,
        1.0 / 2.0,   1.0 / 12.0, 1.0 / 12.0, 1.0 / 4.0,  1.0 / 4.0,  1.0 / 2.0,
        0.0,         0.0,        0.0};

    const auto n = al.size();
    VectorXd count_gr = VectorXd::Zero(FREQ_SIZE_5);

    vector<int> idx(5);
    vector<int> sums;

    for (auto i = 0; i < n; ++i) {
        for (auto&& j : al[i]) {
            for (auto&& k : al[j]) {
                if (k != i) {
                    for (auto&& l : al[k]) {
                        if (l != i && l != j) {
                            for (auto&& m : al[l]) {
                                if (m != i && m != j && m != k) {
                                    const auto aux =
                                        am.coeff(i, k) + am.coeff(i, l) +
                                        am.coeff(i, m) + am.coeff(j, l) +
                                        am.coeff(j, m) + am.coeff(k, m);
                                    if (aux == 6) {
                                        count_gr[0] += w[0];
                                    } else if (aux == 5) {
                                        count_gr[1] += w[1];
                                    } else if (aux == 4) {
                                        idx[0] = i;
                                        idx[1] = j;
                                        idx[2] = k;
                                        idx[3] = l;
                                        idx[4] = m;
                                        getMinValue(am, idx, sums);
                                        const auto aux1 = *min_element(
                                                sums.cbegin(), sums.cend());
                                        if (aux1 == 2) {
                                            count_gr[3] += w[3];
                                        } else {
                                            count_gr[2] += w[2];
                                        }
                                    } else if (aux == 3) {
                                        idx[0] = i;
                                        idx[1] = j;
                                        idx[2] = k;
                                        idx[3] = l;
                                        idx[4] = m;
                                        getMinValue(am, idx, sums);
                                        sort(sums.begin(), sums.end());
                                        if (sums[0] == 1) {
                                            count_gr[8] += w[8];
                                        } else if (sums[1] == 3) {
                                            count_gr[4] += w[4];
                                        } else if (sums[2] == 2) {
                                            count_gr[13] += w[13];
                                        } else {
                                            count_gr[5] += w[5];
                                        }
                                    } else if (aux == 2) {
                                        idx[0] = i;
                                        idx[1] = j;
                                        idx[2] = k;
                                        idx[3] = l;
                                        idx[4] = m;
                                        getMinValue(am, idx, sums);
                                        vector<int> aux1;
                                        copy(sums.cbegin(), sums.cend(),
                                                back_inserter(aux1));
                                        sort(aux1.begin(), aux1.end());
                                        if (aux1[0] == 1) {
                                            if (aux1[2] == 2) {
                                                count_gr[15] += w[15];
                                            } else {
                                                count_gr[9] += w[9];
                                            }
                                        } else {
                                            if (aux1[3] == 2) {
                                                count_gr[10] += w[10];
                                            } else {
                                                vector<int> ind;
                                                for (auto ii = 0; ii < sums.size(); ++ii) {
                                                    if (sums[ii] == 3) {
                                                        ind.push_back(ii);
                                                    }
                                                }
                                                if (am.coeff(idx[ind[0]], idx[ind[1]]) == 1) {
                                                    count_gr[6] += w[6];
                                                } else {
                                                    count_gr[14] += w[14];
                                                }
                                            }
                                        }
                                    } else if (aux == 1) {
                                        idx[0] = i;
                                        idx[1] = j;
                                        idx[2] = k;
                                        idx[3] = l;
                                        idx[4] = m;
                                        getMinValue(am, idx, sums);
                                        vector<int> aux1;
                                        copy(sums.cbegin(), sums.cend(),
                                                back_inserter(aux1));
                                        sort(aux1.begin(), aux1.end());
                                        if (aux1[0] == 2) {
                                            count_gr[7] += w[7];
                                        } else if (aux1[1] == 1) {
                                            count_gr[17] += w[17];
                                        } else {
                                            vector<int> ind;
                                            for (auto ii = 0; ii < sums.size(); ++ii) {
                                                if (sums[ii] == 3) {
                                                    ind.push_back(ii);
                                                }
                                            }
                                            for (auto ii = 0; ii < sums.size(); ++ii) {
                                                if (sums[ii] == 1) {
                                                    ind.push_back(ii);
                                                }
                                            }
                                            if (am.coeff(idx[ind[0]], idx[ind[1]]) == 1) {
                                                count_gr[16] += w[16];
                                            } else {
                                                count_gr[11] += w[11];
                                            }
                                        }
                                    } else {
                                        count_gr[12] += w[12];
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
        // count graphlets of type 20
        for (auto&& j : al[i]) {
            for (auto&& k : al[j]) {
                if (k != i && am.coeff(i, k) == 0) {
                    for (auto&& l : al[k]) {
                        if (l != i && l != j && am.coeff(i, l) == 0 &&
                                am.coeff(j, l) == 0) {
                            for (auto&& m : al[k]) {
                                if (m != i && m != j && m != l
                                        && am.coeff(i, m) == 0
                                        && am.coeff(j, m) == 0
                                        && am.coeff(l, m) == 0) {
                                    count_gr[19] += w[19];
                                }
                            }
                        }
                    }
                }
            }
        }
        // count graphlets of type 19 and 21
        for (auto m = al[i].size(); m-- > 3;) {
            for (auto l = m; l-- > 2;) {
                for (auto k = l; k-- > 1;) {
                    for (auto j = k; j-- > 0;) {
                        auto aux =
                            am.coeff(al[i][j], al[i][k]) +
                            am.coeff(al[i][j], al[i][l]) +
                            am.coeff(al[i][j], al[i][m]) +
                            am.coeff(al[i][k], al[i][l]) +
                            am.coeff(al[i][k], al[i][m]) +
                            am.coeff(al[i][l], al[i][m]);
                        if (aux == 1) {
                            count_gr[18]++;
                        } else if (aux == 0) {
                            count_gr[20]++;
                        }
                    }
                }
            }
        }
    }
    const auto csum = count_gr.sum();
    return (csum == 0.0) ? count_gr : count_gr / csum;
}

VectorXd countConnectedGraphletsFour(
        const MatrixXi& am,
        const vector<vector<int>>& al) {
    vector<double> w = {1.0 / 24.0, 1.0 / 12.0, 1.0 / 4.0,
                        0.0,        1.0 / 8.0,  1.0 / 2.0};

    VectorXd count_gr = VectorXd::Zero(FREQ_SIZE_4);
    const auto n = am.rows();
    for (auto i = 0; i < n; ++i) {
        for (auto&& j : al[i]) {
            for (auto&& k : al[j]) {
                if (k != i) {
                    for (auto&& l : al[k]) {
                        if (l != i && l != j) {
                            const auto aux =
                                am.coeff(i, k) +
                                am.coeff(i, l) +
                                am.coeff(j, l);
                            if (aux == 3) {
                                count_gr[0] += w[0];
                            } else if (aux == 2) {
                                count_gr[1] += w[1];
                            } else if (aux == 1) {
                                if (am.coeff(i, l) == 1) {
                                    count_gr[4] += w[4];
                                } else {
                                    count_gr[2] += w[2];
                                }
                            } else {
                                count_gr[5] += w[5];
                            }
                        }
                    }
                }
            }
        }

        // count "stars"
        for (auto l = al[i].size(); l-- > 2;) {
            for (auto k = l; k-- > 1;) {
                for (auto j = k; j-- > 0;) {
                    if (am.coeff(al[i][j], al[i][k]) == 0 &&
                            am.coeff(al[i][j], al[i][l]) == 0 &&
                            am.coeff(al[i][k], al[i][l]) == 0) {
                        count_gr[3]++;
                    }
                }
            }
        }
    }
    const auto csum = count_gr.sum();
    return (csum == 0.0) ? count_gr : count_gr / csum;
}

VectorXd countConnectedGraphletsThree(
        const MatrixXi& am,
        const vector<vector<int>>& al) {
    vector<double> w = {1.0 / 2.0, 1.0 / 6.0};

    VectorXd count_gr = VectorXd::Zero(FREQ_SIZE_3);
    const auto n = am.rows();
    for (auto i = 0; i < n; ++i) {
        for (auto&& j : al[i]) {
            for (auto&& k : al[j]) {
                if (k != i) {
                    if (am.coeff(i, k) == 1) {
                        count_gr[1] += w[1];
                    } else {
                        count_gr[0] += w[0];
                    }
                }
            }
        }
    }
    const auto csum = count_gr.sum();
    return (csum == 0.0) ? count_gr : count_gr / csum;
}

MatrixXd CalculateConnectedGraphletKernelThreePy(
        const vector<MatrixXi>& graph_adj_all,
        const vector<vector<vector<int>>>& graph_adjlist_all) {
    MatrixXd freq(FREQ_SIZE_3, graph_adjlist_all.size());

    for (auto i = 0; i < graph_adjlist_all.size(); ++i) {
        freq.col(i) = countConnectedGraphletsThree(
                graph_adj_all[i], graph_adjlist_all[i]);
    }

    return freq.transpose() * freq;
}

MatrixXd CalculateConnectedGraphletKernelFourPy(
        const vector<MatrixXi>& graph_adj_all,
        const vector<vector<vector<int>>>& graph_adjlist_all) {
    MatrixXd freq(FREQ_SIZE_4, graph_adjlist_all.size());

    for (auto i = 0; i < graph_adjlist_all.size(); ++i) {
        freq.col(i) = countConnectedGraphletsFour(
                graph_adj_all[i], graph_adjlist_all[i]);
    }

    return freq.transpose() * freq;
}

MatrixXd CalculateConnectedGraphletKernelFivePy(
        const vector<MatrixXi>& graph_adj_all,
        const vector<vector<vector<int>>>& graph_adjlist_all) {
    MatrixXd freq(FREQ_SIZE_5, graph_adjlist_all.size());

    for (auto i = 0; i < graph_adjlist_all.size(); ++i) {
        freq.col(i) = countConnectedGraphletsFive(
                graph_adj_all[i], graph_adjlist_all[i]);
    }

    return freq.transpose() * freq;
}