#include <miopen/scgemm/conv.hpp>
#include <miopen/logger.hpp>
#include <miopen/scgemm/scgemm.hpp>
#include <boost/algorithm/string.hpp>
#include <cstring>
#include <vector>
#include <unordered_map>

namespace miopen {
namespace scgemm {

bool is_fconv(const std::string& kernel_name)
{
    return kernel_name.find("sconv_") != std::string::npos;
}

scgemm_conv_routine_t get_fconv_routine(const std::string& kernel_name)
{
    std::vector<std::string> ss;
    boost::split(ss, kernel_name, [](char c) { return c == '_'; });
    static const std::unordered_map<std::string, scgemm_conv_routine_t> data{
        {"6x4", sconv_064x016},
        {"6x5", sconv_064x032},
        {"6x6", sconv_064x064},
        {"7x5", sconv_128x032},
        {"7x6", sconv_128x064},
        {"7x7", sconv_128x128},
        {"8x5", sconv_256x032},
        {"8x6", sconv_256x064},
    };
    auto result = data.find(ss[1]);
    if(result != data.end())
    {
        return result->second;
    }
    else
    {
        MIOPEN_LOG_E("Conv kernel: " << kernel_name << ", cannot find associate routine.");
        return sconv_064x016;
    }
}

std::string get_conv_kernel_tile_string(scgemm_conv_routine_t routine)
{
    switch(routine)
    {
    case sconv_064x016: return "6x4";
    case sconv_064x032: return "6x5";
    case sconv_064x064: return "6x6";
    case sconv_128x032: return "7x5";
    case sconv_128x064: return "7x6";
    case sconv_128x128: return "7x7";
    case sconv_256x032: return "8x5";
    case sconv_256x064: return "8x6";
    }
    MIOPEN_LOG_E("Conv routine " << routine << " tile not found.");
    return "6x4";
}

static void gen_fconv_kernel_prop(kernel_prop_t* prop, scgemm_conv_routine_t routine, bool is_relu)
{
    prop->name = "sconv_";
    prop->name.append(get_conv_kernel_tile_string(routine));
    if(is_relu)
    {
        prop->name.append("_relu");
    }

    switch(routine)
    {
    case sconv_064x016:
        prop->tile_x     = 6;
        prop->tile_y     = 4;
        prop->block_size = 64;
        break;
    case sconv_064x032:
        prop->tile_x     = 6;
        prop->tile_y     = 5;
        prop->block_size = 64;
        break;
    case sconv_064x064:
        prop->tile_x     = 6;
        prop->tile_y     = 6;
        prop->block_size = 64;
        break;
    case sconv_128x032:
        prop->tile_x     = 7;
        prop->tile_y     = 5;
        prop->block_size = 128;
        break;
    case sconv_128x064:
        prop->tile_x     = 7;
        prop->tile_y     = 6;
        prop->block_size = 128;
        break;
    case sconv_128x128:
        prop->tile_x     = 7;
        prop->tile_y     = 7;
        prop->block_size = 256;
        break;
    case sconv_256x032:
        prop->tile_x     = 8;
        prop->tile_y     = 5;
        prop->block_size = 256;
        break;
    case sconv_256x064:
        prop->tile_x     = 8;
        prop->tile_y     = 6;
        prop->block_size = 256;
        break;
    }
}

scgemm_fconv_params create_fconv_params(std::string& kernel_name,
                                        std::vector<uint32_t>& grids,
                                        std::vector<uint32_t>& blocks,
                                        scgemm_conv_routine_t routine,
                                        const group_prop_t* gprop,
                                        uint32_t mask,
                                        uint32_t ng)
{
    kernel_prop_t kprop{};
    size_t nb_span, nb_amap;
    uint32_t k, ls, ocs, onpx, inc, onc, snpx, sgs, gdx, gdy, ntidx;
    uint32_t aozero, bozero;
    snpx = gprop->pnx * gprop->pny * gprop->pnz;
    onpx = gprop->qnx * gprop->qny * gprop->qnz;
    inc  = gprop->pnc;
    onc  = gprop->qnc;
    k    = gprop->fnx * gprop->fny * gprop->fnz * inc;
    ocs  = gprop->bat * onpx;
    sgs  = gprop->bat * snpx * inc;
    ls   = PSIZE(k, 8);
    gen_fconv_kernel_prop(&kprop, routine, static_cast<bool>(mask & 1u));
    gdx     = (ocs + (1u << kprop.tile_x) - 1) >> kprop.tile_x;
    gdy     = (onc + (1u << kprop.tile_y) - 1) >> kprop.tile_y;
    ntidx   = gdx << kprop.tile_x;
    nb_span = (ls + 16) << 2;
    nb_amap = ntidx << 2;
    aozero  = 0;
    bozero  = 0;
    if((k & 7) != 0)
    {
        aozero = (sgs * ng) << 2;
        bozero = k;
    }

    (void)nb_span; // Suppress warning

    kernel_name = kprop.name;

    grids[0]  = routine == 0 ? gdx : gdy;
    grids[1]  = routine == 0 ? gdy : gdx;
    grids[2]  = ng;
    blocks[0] = kprop.block_size;
    blocks[1] = 1;
    blocks[2] = 1;

    scgemm_fconv_params params{ntidx,      static_cast<uint32_t>(nb_amap),
                               aozero,     bozero,
                               onc,        ocs,
                               ls,         sgs,
                               onpx,       gprop->pnx,
                               gprop->pny, gprop->pnz,
                               gprop->qnx, gprop->qny,
                               gprop->qnz, gprop->fnx,
                               gprop->fny, gprop->fnz,
                               gprop->pnc, gprop->qnc,
                               gprop->bat};

    return params;
}

/// This fucintion is used to present how to fill in the kernel arguments for a
/// Static Compiled GEMM - Forwared Convolution Kernel.
/// Left the code here for reference.
/*
scgemm_kernel_args_t compiled_fconv_args(size_t& args_size,
                                         const void* src,
                                         const void* wei,
                                         const void* bias,
                                         void* dst,
                                         void* auxbuf,
                                         scgemm_fconv_params* params,
                                         float alpha)
{
    (void)bias; // Suppress warning
    args_size = 72;
    scgemm_kernel_args_t args(new char[args_size]);
    auto _args                                = reinterpret_cast<char*>(args.get());
    *(reinterpret_cast<void**>(&_args[0x00])) = auxbuf;
    *(reinterpret_cast<void**>(&_args[0x08])) =
        reinterpret_cast<void*>(reinterpret_cast<char*>(auxbuf) + params->nb_amap);
    *(reinterpret_cast<uint32_t*>(&_args[0x10]))    = params->onc;
    *(reinterpret_cast<uint32_t*>(&_args[0x14]))    = params->ocs;
    *(reinterpret_cast<uint32_t*>(&_args[0x18]))    = params->ls;
    *(reinterpret_cast<uint32_t*>(&_args[0x1c]))    = params->sgs;
    *(reinterpret_cast<const void**>(&_args[0x20])) = src;
    *(reinterpret_cast<const void**>(&_args[0x28])) = wei;
    *(reinterpret_cast<void**>(&_args[0x30]))       = dst;
    *(reinterpret_cast<uint32_t*>(&_args[0x38]))    = params->onpx;
    memcpy(&_args[0x3c], &alpha, sizeof(float));
    // *(reinterpret_cast<float*>(&_args[0x3c]))       = alpha;
    *(reinterpret_cast<void**>(&_args[0x40])) =
        reinterpret_cast<void*>(reinterpret_cast<char*>(auxbuf) + (params->nb_amap << 1));
    return args;
}
*/

} // namespace scgemm
} // namespace miopen


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>
#include <boost/lambda/lambda.hpp>

#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/SimulationSetup/EstimationSetup/createCartesianStatePartials.h"
#include "Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/rotationMatrixPartial.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

using namespace tudat::ephemerides;
using namespace tudat::estimatable_parameters;
using namespace tudat::observation_partials;

BOOST_AUTO_TEST_SUITE( test_rotation_matrix_partaisl )

//! Test whether partial derivatives of rotation matrix computed by SimpleRotationalEphemeris works correctly
BOOST_AUTO_TEST_CASE( testSimpleRotationalEphemerisPartials )
{
    // Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Create rotation model
    double nominalRotationRate = 2.0 * mathematical_constants::PI / 86400.0;
    std::shared_ptr< SimpleRotationalEphemeris > rotationalEphemeris =
            std::make_shared< SimpleRotationalEphemeris >(
                spice_interface::computeRotationQuaternionBetweenFrames( "ECLIPJ2000", "IAU_Earth", 1.0E7 ),
                nominalRotationRate, 1.0E7, "ECLIPJ2000", "IAU_Earth" );

    {
        // Create partial object.
        std::shared_ptr< RotationMatrixPartialWrtConstantRotationRate > rotationMatrixPartialObject =
                std::make_shared< RotationMatrixPartialWrtConstantRotationRate >( rotationalEphemeris );

        // Compute partial analytically
        double testTime = 1.0E6;
        Eigen::Matrix3d rotationMatrixPartial =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixToBaseFrameWrParameter(
                    testTime ).at( 0 );

        Eigen::Matrix3d rotationMatrixDerivativePartial =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixDerivativeToBaseFrameWrParameter(
                    testTime ).at( 0 );

        // Compute partial numerically.
        double perturbation = 1.0E-12;
        rotationalEphemeris->resetRotationRate( nominalRotationRate + perturbation );
        Eigen::Matrix3d upperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame(
                    testTime).toRotationMatrix( );
        Eigen::Matrix3d upperturbedRotationMatrixDerivative = rotationalEphemeris->getDerivativeOfRotationToBaseFrame(
                    testTime );

        rotationalEphemeris->resetRotationRate( nominalRotationRate - perturbation );
        Eigen::Matrix3d downperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame(
                    testTime).toRotationMatrix( );
        Eigen::Matrix3d downperturbedRotationMatrixDerivative = rotationalEphemeris->getDerivativeOfRotationToBaseFrame(
                    testTime );

        Eigen::Matrix3d numericalRotationMatrixPartial =
                ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation );
        Eigen::Matrix3d numericalRotationMatrixDerivativePartial =
                ( upperturbedRotationMatrixDerivative - downperturbedRotationMatrixDerivative ) / ( 2.0 * perturbation );

        Eigen::Matrix3d matrixDifference = rotationMatrixPartial - numericalRotationMatrixPartial;

        // Compare analytical and numerical result.
        for( unsigned int i = 0; i < 3; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 0.1 );
            }
        }

        matrixDifference = rotationMatrixDerivativePartial - numericalRotationMatrixDerivativePartial;

        // Compare analytical and numerical result.
        for( unsigned int i = 0; i < 3; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-5 );
            }
        }
    }

    {
        // Create partial object.
        std::shared_ptr< RotationMatrixPartialWrtPoleOrientation > rotationMatrixPartialObject =
                std::make_shared< RotationMatrixPartialWrtPoleOrientation >( rotationalEphemeris );

        // Compute partial analytically
        double testTime = 1.0E6;
        std::vector< Eigen::Matrix3d > rotationMatrixPartials =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixToBaseFrameWrParameter(
                    testTime );

        std::vector< Eigen::Matrix3d > rotationMatrixDerivativePartials =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixDerivativeToBaseFrameWrParameter(
                    testTime );

        Eigen::Vector3d nominalEulerAngles = rotationalEphemeris->getInitialEulerAngles( );
        double perturbedAngle;

        // Compute partial numerically.
        double perturbation = 1.0E-6;
        {

            // Compute partial for right ascension numerically.
            {
                perturbedAngle = nominalEulerAngles( 0 ) + perturbation;
                rotationalEphemeris->resetInitialPoleRightAscensionAndDeclination( perturbedAngle, nominalEulerAngles( 1 ) );
                Eigen::Matrix3d upperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame(
                            testTime).toRotationMatrix( );
                Eigen::Matrix3d upperturbedRotationMatrixDerivative = rotationalEphemeris->getDerivativeOfRotationToBaseFrame(
                            testTime );

                perturbedAngle = nominalEulerAngles( 0 ) - perturbation;
                rotationalEphemeris->resetInitialPoleRightAscensionAndDeclination( perturbedAngle, nominalEulerAngles( 1 ) );
                Eigen::Matrix3d downperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame(
                            testTime).toRotationMatrix( );
                Eigen::Matrix3d downperturbedRotationMatrixDerivative =
                        rotationalEphemeris->getDerivativeOfRotationToBaseFrame(
                            testTime );

                Eigen::Matrix3d numericalRotationMatrixPartial =
                        ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation );
                Eigen::Matrix3d numericalRotationMatrixDerivativePartial =
                        ( upperturbedRotationMatrixDerivative - downperturbedRotationMatrixDerivative ) /
                        ( 2.0 * perturbation );

                Eigen::Matrix3d matrixDifference = rotationMatrixPartials.at( 0 ) - numericalRotationMatrixPartial;

                // Compare analytical and numerical result.
                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-8 );
                    }
                }

                matrixDifference = rotationMatrixDerivativePartials.at( 0 ) - numericalRotationMatrixDerivativePartial;

                // Compare analytical and numerical result.
                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-13 );
                    }
                }
            }

            // Compute partial for declination numerically.
            {
                perturbedAngle = nominalEulerAngles( 1 ) + perturbation;
                rotationalEphemeris->resetInitialPoleRightAscensionAndDeclination( nominalEulerAngles( 0 ), perturbedAngle );
                Eigen::Matrix3d upperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame(
                            testTime).toRotationMatrix( );
                Eigen::Matrix3d upperturbedRotationMatrixDerivative = rotationalEphemeris->getDerivativeOfRotationToBaseFrame(
                            testTime );

                perturbedAngle = nominalEulerAngles( 1 ) - perturbation;
                rotationalEphemeris->resetInitialPoleRightAscensionAndDeclination( nominalEulerAngles( 0 ), perturbedAngle );
                Eigen::Matrix3d downperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame(
                            testTime).toRotationMatrix( );
                Eigen::Matrix3d downperturbedRotationMatrixDerivative =
                        rotationalEphemeris->getDerivativeOfRotationToBaseFrame( testTime );

                Eigen::Matrix3d numericalRotationMatrixPartial =
                        ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation );
                Eigen::Matrix3d numericalRotationMatrixDerivativePartial =
                        ( upperturbedRotationMatrixDerivative -
                          downperturbedRotationMatrixDerivative ) / ( 2.0 * perturbation );

                Eigen::Matrix3d matrixDifference = rotationMatrixPartials.at( 1 ) - numericalRotationMatrixPartial;

                // Compare analytical and numerical result.
                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-8 );
                    }
                }

                matrixDifference = rotationMatrixDerivativePartials.at( 1 ) - numericalRotationMatrixDerivativePartial;

                // Compare analytical and numerical result.
                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-13 );
                    }
                }
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


#include "VectorizedNetwork.hpp"
#include <boost/timer/timer.hpp>
#include <boost/filesystem.hpp>
#include <chrono>

using namespace MiindLib;

VectorizedNetwork::VectorizedNetwork(MPILib::Time time_step) :
    _num_nodes(0),
    _n_steps(10),
    _network_time_step(time_step)
{
}

void VectorizedNetwork::addGridNode(TwoDLib::Mesh mesh, TwoDLib::TransitionMatrix tmat, double start_v, double start_w,
    std::vector<TwoDLib::Redistribution> vec_rev, std::vector<TwoDLib::Redistribution> vec_res, double tau_refractive, unsigned int finite_size) {

    _num_nodes++;
    _grid_node_ids.push_back(_num_nodes - 1);
    _grid_vec_mesh.push_back(mesh);
    _grid_vec_vec_rev.push_back(vec_rev);
    _grid_vec_vec_res.push_back(vec_res);
    _grid_vec_tau_refractive.push_back(tau_refractive);

    _vec_transforms.push_back(tmat);
    _start_vs.push_back(start_v);
    _start_ws.push_back(start_w);

    _num_grid_objects.push_back(finite_size);
}

void VectorizedNetwork::addMeshNode(TwoDLib::Mesh mesh,
    std::vector<TwoDLib::Redistribution> vec_rev, std::vector<TwoDLib::Redistribution> vec_res, double tau_refractive, unsigned int finite_size) {

    _num_nodes++;
    _mesh_node_ids.push_back(_num_nodes - 1);
    _mesh_vec_mesh.push_back(mesh);
    _mesh_vec_vec_rev.push_back(vec_rev);
    _mesh_vec_vec_res.push_back(vec_res);
    _mesh_vec_tau_refractive.push_back(tau_refractive);
    _num_mesh_objects.push_back(finite_size);
}

void VectorizedNetwork::addRateNode(function_pointer functor) {
    _num_nodes++;
    _rate_functions.push_back(function_association(_num_nodes - 1, functor));
}

void VectorizedNetwork::addRateNode(rate_functor functor) {
    _num_nodes++;
    _rate_functors[_num_nodes - 1] = functor;
}

void VectorizedNetwork::initOde2DSystem(unsigned int min_solve_steps) {

    int mesh_count = 0;
    for (int g = 0; g < _grid_node_ids.size(); g++) {
        _node_id_to_group_mesh.insert(std::pair<MPILib::NodeId, MPILib::Index>(_grid_node_ids[g], mesh_count));
        _group_mesh_to_node_id.insert(std::pair<MPILib::Index, MPILib::NodeId>(mesh_count, _grid_node_ids[g]));
        _grid_meshes.push_back(mesh_count);
        mesh_count++;
        _vec_mesh.push_back(_grid_vec_mesh[g]);
        _vec_vec_rev.push_back(_grid_vec_vec_rev[g]);
        _vec_vec_res.push_back(_grid_vec_vec_res[g]);
        _vec_tau_refractive.push_back(_grid_vec_tau_refractive[g]);
        _num_objects.push_back(_num_grid_objects[g]);
    }

    for (int m = 0; m < _mesh_node_ids.size(); m++) {
        _node_id_to_group_mesh.insert(std::pair<MPILib::NodeId, MPILib::Index>(_mesh_node_ids[m], mesh_count));
        _group_mesh_to_node_id.insert(std::pair<MPILib::Index, MPILib::NodeId>(mesh_count, _mesh_node_ids[m]));
        _mesh_meshes.push_back(mesh_count);
        mesh_count++;
        _vec_mesh.push_back(_mesh_vec_mesh[m]);
        _vec_vec_rev.push_back(_mesh_vec_vec_rev[m]);
        _vec_vec_res.push_back(_mesh_vec_vec_res[m]);
        _vec_tau_refractive.push_back(_mesh_vec_tau_refractive[m]);
        _num_objects.push_back(_num_mesh_objects[m]);
    }

#ifdef IZHIKEVICH_TEST
    _network_time_step = 0.0001;
    _display_nodes.clear();
    _num_objects.clear();
    _num_objects.push_back(50000);
    _group = new TwoDLib::Ode2DSystemGroup(_vec_mesh, _vec_vec_rev, _vec_vec_res, _vec_tau_refractive, _num_objects);
#else
    _group = new TwoDLib::Ode2DSystemGroup(_vec_mesh, _vec_vec_rev, _vec_vec_res, _vec_tau_refractive, _num_objects);
#endif

    for (MPILib::Index i = 0; i < _grid_meshes.size(); i++) {
        vector<TwoDLib::Coordinates> coords = _vec_mesh[_grid_meshes[i]].findPointInMeshSlow(TwoDLib::Point(_start_vs[i], _start_ws[i]));
        _group->Initialize(_grid_meshes[i], coords[0][0], coords[0][1]);

        //create CSR Matrix for each transforms
        _csrs.push_back(TwoDLib::CSRMatrix(_vec_transforms[i], *(_group), _grid_meshes[i]));
        _grid_transform_indexes.push_back(i);
    }

    for (MPILib::Index i = 0; i < _mesh_meshes.size(); i++) {
        _group->Initialize(_mesh_meshes[i], 0, 0);
    }

    _master_steps = min_solve_steps;

    // All grids/meshes must have the same timestep
    TwoDLib::MasterParameter par(static_cast<MPILib::Number>(ceil(_network_time_step / _vec_mesh[0].TimeStep())));
    _n_steps = par._N_steps;
    std::cout << "Using master solver n_steps = " << _master_steps << "\n";

    _group_adapter = new CudaTwoDLib::CudaOde2DSystemAdapter(*(_group), _network_time_step);
}

void VectorizedNetwork::reportNodeActivities(MPILib::Time sim_time) {
    for (int i = 0; i < _rate_nodes.size(); i++) {
        if (std::fabs(std::remainder(sim_time, _rate_intervals[i])) > 0.00000001)
            continue;
        std::ostringstream ost2;
        ost2 << "rate_" << _rate_nodes[i];
        std::ofstream ofst_rate(ost2.str(), std::ofstream::app);
        ofst_rate.precision(10);
        ofst_rate << sim_time << "\t" << _current_node_rates[_rate_nodes[i]] << std::endl;
        ofst_rate.close();
    }
}

void VectorizedNetwork::reportNodeDensities(MPILib::Time sim_time) {
    for (int i = 0; i < _density_nodes.size(); i++) {
        if (sim_time < _density_start_times[i] || sim_time > _density_end_times[i] || std::fabs(std::remainder(sim_time, _density_intervals[i])) > 0.00000001)
            continue;

        std::ostringstream ost;
        ost << _density_nodes[i] << "_" << sim_time << "1";
        string fn("node_" + ost.str());

        std::string model_path("densities");
        boost::filesystem::path path(model_path);

        // MdK 27/01/2017. grid file is now created in the cwd of the program and
        // not in the directory where the mesh resides.
        const std::string dirname = path.filename().string();

        if (!boost::filesystem::exists(dirname)) {
            boost::filesystem::create_directory(dirname);
        }
        std::ofstream ofst(dirname + "/" + fn);
        _group->DumpSingleMesh(&ofst, _node_id_to_group_mesh[_density_nodes[i]]);
    }
}

void VectorizedNetwork::addGridConnection(MPILib::NodeId in, MPILib::NodeId out, std::map<std::string, std::string> params) {
    _grid_connections.push_back(NodeGridConnection(in, out, params));
}

void VectorizedNetwork::addGridConnection(MPILib::NodeId out, std::map<std::string, std::string> params, MPILib::NodeId ext_id) {
    _grid_connections.push_back(NodeGridConnection(out, params, ext_id));
}

void VectorizedNetwork::addMeshConnection(MPILib::NodeId in, MPILib::NodeId out, double efficacy, int n_conns, double delay, TwoDLib::TransitionMatrix* tmat) {
    _mesh_connections.push_back(NodeMeshConnection(in, out, efficacy, n_conns, delay, tmat));
}

void VectorizedNetwork::addMeshConnection(MPILib::NodeId out, double efficacy, int n_conns, double delay, TwoDLib::TransitionMatrix* tmat, MPILib::NodeId ext_id) {
    _mesh_connections.push_back(NodeMeshConnection(out, efficacy, n_conns, delay, tmat, ext_id));
}

void VectorizedNetwork::addMeshCustomConnection(MPILib::NodeId in, MPILib::NodeId out, std::map<std::string, std::string> params, TwoDLib::TransitionMatrix* tmat) {
    _mesh_custom_connections.push_back(NodeMeshCustomConnection(in, out, params, tmat));
}

void VectorizedNetwork::addMeshCustomConnection(MPILib::NodeId out, std::map<std::string, std::string> params, TwoDLib::TransitionMatrix* tmat, MPILib::NodeId ext_id) {
    _mesh_custom_connections.push_back(NodeMeshCustomConnection(out, params, tmat, ext_id));
}

void VectorizedNetwork::addExternalMonitor(MPILib::NodeId nid) {
    _monitored_nodes.push_back(nid);
}

void VectorizedNetwork::setupLoop(bool write_displays) {

    for (unsigned int i = 0; i < _display_nodes.size(); i++) {
        TwoDLib::Display::getInstance()->addOdeSystem(_display_nodes[i], _group, _node_id_to_group_mesh[_display_nodes[i]]);
    }

    const MPILib::Time h = 1. / _master_steps * _vec_mesh[0].TimeStep();

    // Setup the OpenGL displays (if there are any required)
    if (_display_nodes.size() > 0) {
        TwoDLib::Display::getInstance()->animate(write_displays, _display_nodes, _network_time_step);
    }

    for (unsigned int i = 0; i < _grid_connections.size(); i++) {
        // for each connection, which of group's meshes is being affected
        _connection_out_group_mesh.push_back(_node_id_to_group_mesh[_grid_connections[i]._out]);
        _effs.push_back(std::stod(_grid_connections[i]._params["efficacy"]));

        _connection_queue.push_back(MPILib::DelayedConnectionQueue(_network_time_step, std::stod(_grid_connections[i]._params["delay"])));
        if (_grid_connections[i]._external)
            if (_external_to_connection_queue.find(_grid_connections[i]._external_id) == _external_to_connection_queue.end()) {
                _external_to_connection_queue.insert(
                    std::pair<MPILib::NodeId, std::vector<MPILib::NodeId>>(_grid_connections[i]._external_id, std::vector<MPILib::NodeId>()));
                _external_to_connection_queue[_grid_connections[i]._external_id].push_back(_connection_queue.size() - 1);
            }
            else {
                _external_to_connection_queue[_grid_connections[i]._external_id].push_back(_connection_queue.size() - 1);
            }
        else
            if (_node_to_connection_queue.find(_grid_connections[i]._in) == _node_to_connection_queue.end()) {
                _node_to_connection_queue.insert(
                    std::pair<MPILib::NodeId, std::vector<MPILib::NodeId>>(_grid_connections[i]._in, std::vector<MPILib::NodeId>()));
                _node_to_connection_queue[_grid_connections[i]._in].push_back(_connection_queue.size() - 1);
            }
            else {
                _node_to_connection_queue[_grid_connections[i]._in].push_back(_connection_queue.size() - 1);
            }
    }

    for (unsigned int i = 0; i < _mesh_connections.size(); i++) {
        _connection_out_group_mesh.push_back(_node_id_to_group_mesh[_mesh_connections[i]._out]);
        _csrs.push_back(TwoDLib::CSRMatrix(*(_mesh_connections[i]._transition), *(_group), _node_id_to_group_mesh[_mesh_connections[i]._out]));
        // _csrs contains all the grid transforms first (see initOde2DSystem)
        // now we're adding all the mesh transition matrices so set the correct index value
        _mesh_transform_indexes.push_back(_grid_meshes.size() + i);

        _connection_queue.push_back(MPILib::DelayedConnectionQueue(_network_time_step, _mesh_connections[i]._delay));
        if (_mesh_connections[i]._external)
            if (_external_to_connection_queue.find(_mesh_connections[i]._external_id) == _external_to_connection_queue.end()) {
                _external_to_connection_queue.insert(
                    std::pair<MPILib::NodeId, std::vector<MPILib::NodeId>>(_mesh_connections[i]._external_id, std::vector<MPILib::NodeId>()));
                _external_to_connection_queue[_mesh_connections[i]._external_id].push_back(_connection_queue.size() - 1);
            }
            else {
                _external_to_connection_queue[_mesh_connections[i]._external_id].push_back(_connection_queue.size() - 1);
            }
        else
            if (_node_to_connection_queue.find(_mesh_connections[i]._in) == _node_to_connection_queue.end()) {
                _node_to_connection_queue.insert(
                    std::pair<MPILib::NodeId, std::vector<MPILib::NodeId>>(_mesh_connections[i]._in, std::vector<MPILib::NodeId>()));
                _node_to_connection_queue[_mesh_connections[i]._in].push_back(_connection_queue.size() - 1);
            }
            else {
                _node_to_connection_queue[_mesh_connections[i]._in].push_back(_connection_queue.size() - 1);
            }
    }

    for (unsigned int i = 0; i < _mesh_custom_connections.size(); i++) {
        // for each connection, which of group's meshes is being affected
        _connection_out_group_mesh.push_back(_node_id_to_group_mesh[_mesh_custom_connections[i]._out]);

        TwoDLib::TransitionMatrix mat = *(_mesh_custom_connections[i]._transition);
        auto id = _node_id_to_group_mesh[_mesh_custom_connections[i]._out];
        auto ffs = TwoDLib::CSRMatrix(*(_mesh_custom_connections[i]._transition), *(_group), _node_id_to_group_mesh[_mesh_custom_connections[i]._out]);
        _csrs.push_back(ffs);

        // _csrs contains all the grid transforms first (see initOde2DSystem)
        // now we're adding all the mesh transition matrices so set the correct index value
        _mesh_transform_indexes.push_back(_grid_meshes.size() + i);

        _connection_queue.push_back(MPILib::DelayedConnectionQueue(_network_time_step, std::stod(_mesh_custom_connections[i]._params["delay"])));
        if (_mesh_custom_connections[i]._external)
            if (_external_to_connection_queue.find(_mesh_custom_connections[i]._external_id) == _external_to_connection_queue.end()) {
                _external_to_connection_queue.insert(
                    std::pair<MPILib::NodeId, std::vector<MPILib::NodeId>>(_mesh_custom_connections[i]._external_id, std::vector<MPILib::NodeId>()));
                _external_to_connection_queue[_mesh_custom_connections[i]._external_id].push_back(_connection_queue.size() - 1);
            }
            else {
                _external_to_connection_queue[_mesh_custom_connections[i]._external_id].push_back(_connection_queue.size() - 1);
            }
        else
            if (_node_to_connection_queue.find(_mesh_custom_connections[i]._in) == _node_to_connection_queue.end()) {
                _node_to_connection_queue.insert(
                    std::pair<MPILib::NodeId, std::vector<MPILib::NodeId>>(_mesh_custom_connections[i]._in, std::vector<MPILib::NodeId>()));
                _node_to_connection_queue[_mesh_custom_connections[i]._in].push_back(_connection_queue.size() - 1);
            }
            else {
                _node_to_connection_queue[_mesh_custom_connections[i]._in].push_back(_connection_queue.size() - 1);
            }
    }

    _csr_adapter = new CudaTwoDLib::CSRAdapter(*_group_adapter, _csrs,
        _effs.size(), h, _mesh_transform_indexes, _grid_transform_indexes);

    _csr_adapter->InitializeStaticGridEfficacies(_connection_out_group_mesh, _effs);

    const auto p1 = std::chrono::system_clock::now();
    _csr_adapter->setRandomSeeds(std::chrono::duration_cast<std::chrono::seconds>(
        p1.time_since_epoch()).count());
}

std::vector<double> VectorizedNetwork::singleStep(std::vector<double> activities, unsigned int i_loop) {
    MPILib::Time time = _network_time_step * i_loop;
#ifndef IZHIKEVICH_TEST
    for (const auto& element : _rate_functions) {
        for (unsigned int i = 0; i < _node_to_connection_queue[element.first].size(); i++) {
            _connection_queue[_node_to_connection_queue[element.first][i]].updateQueue(element.second(time));
        }
    }

    for (const auto& element : _rate_functors) {
        for (unsigned int i = 0; i < _node_to_connection_queue[element.first].size(); i++) {
            _connection_queue[_node_to_connection_queue[element.first][i]].updateQueue(element.second(time));
        }
    }

    for (int i = 0; i < activities.size(); i++) {
        for (unsigned int j = 0; j < _external_to_connection_queue[i].size(); j++)
            _connection_queue[_external_to_connection_queue[i][j]].updateQueue(activities[i]);
    }

    std::vector<fptype> rates;
    int connection_count = 0;
    for (unsigned int i = 0; i < _grid_connections.size(); i++) {
        rates.push_back(_connection_queue[connection_count].getCurrentRate() * std::stod(_grid_connections[i]._params["num_connections"]));
        connection_count++;
    }
    for (unsigned int i = 0; i < _mesh_connections.size(); i++) {
        rates.push_back(_connection_queue[connection_count].getCurrentRate() * _mesh_connections[i]._n_connections);
        connection_count++;
    }

    for (unsigned int i = 0; i < _mesh_custom_connections.size(); i++) {
        rates.push_back(_connection_queue[connection_count].getCurrentRate() * std::stod(_mesh_custom_connections[i]._params["num_connections"]));
        connection_count++;
    }

    for (MPILib::Index i_part = 0; i_part < _n_steps; i_part++) {
        _group_adapter->EvolveOnDevice(_mesh_meshes);
        _group_adapter->RemapReversal();
        _group_adapter->RemapReversalFiniteObjects();

        _csr_adapter->ClearDerivative();
        _csr_adapter->SingleTransformStep();
        _csr_adapter->AddDerivativeFull();

        _csr_adapter->SingleTransformStepFiniteSize();
    }

    _group_adapter->RedistributeProbability(_grid_meshes);
    _group_adapter->MapFinish(_grid_meshes);

    for (unsigned int i = 0; i < rates.size(); i++)
        rates[i] *= _n_steps;

    for (MPILib::Index i_part = 0; i_part < _master_steps; i_part++) {
        _csr_adapter->ClearDerivative();
        _csr_adapter->CalculateMeshGridDerivativeWithEfficacy(_connection_out_group_mesh, rates);
        _csr_adapter->AddDerivative();
    }

    _csr_adapter->CalculateMeshGridDerivativeWithEfficacyFinite(_connection_out_group_mesh, rates, _effs, _vec_mesh[0].TimeStep());

    _group_adapter->RedistributeFiniteObjects(_mesh_meshes, _vec_mesh[0].TimeStep(), _csr_adapter->getCurandState());
    _group_adapter->RedistributeGridFiniteObjects(_grid_meshes, _csr_adapter->getCurandState());

    _group_adapter->RedistributeProbability(_mesh_meshes);
    _group_adapter->MapFinish(_mesh_meshes);
#else
    _csr_adapter->IzhTest(_group_adapter->getSpikes());
#endif

    const std::vector<fptype>& group_rates = _group_adapter->F(_n_steps);

    for (unsigned int i = 0; i < group_rates.size(); i++) {
        _current_node_rates[_group_mesh_to_node_id[i]] = group_rates[i];
        for (unsigned int j = 0; j < _node_to_connection_queue[_group_mesh_to_node_id[i]].size(); j++) {
            _connection_queue[_node_to_connection_queue[_group_mesh_to_node_id[i]][j]].updateQueue(group_rates[i]);
        }
    }

    std::vector<double> monitored_rates(_monitored_nodes.size());
    for (unsigned int i = 0; i < _monitored_nodes.size(); i++) {
        monitored_rates[i] = group_rates[_node_id_to_group_mesh[_monitored_nodes[i]]];
    }

    if (_display_nodes.size() > 0) {
        _group_adapter->updateGroupMass();
        _group_adapter->updateFiniteObjects();
        for (MPILib::Index i_part = 0; i_part < _n_steps; i_part++) {
            _group_adapter->EvolveWithoutTransfer(_mesh_meshes);
        }
        TwoDLib::Display::getInstance()->updateDisplay(i_loop);
    }

    if (_density_nodes.size() > 0) {
        _group_adapter->updateGroupMass();
        reportNodeDensities(time);
    }

    reportNodeActivities(time);

    return monitored_rates;
}

void VectorizedNetwork::mainLoop(MPILib::Time t_begin, MPILib::Time t_end, MPILib::Time t_report, bool write_displays) {
    MPILib::Number n_iter = static_cast<MPILib::Number>(ceil((t_end - t_begin) / _network_time_step));

    setupLoop(write_displays);

    MPILib::utilities::ProgressBar* pb = new MPILib::utilities::ProgressBar(n_iter);
    boost::timer::auto_cpu_timer timer;
    for (MPILib::Index i_loop = 0; i_loop < n_iter; i_loop++) {
        singleStep(std::vector<double>(), i_loop);
        (*pb)++;
    }
}


#ifndef POWER_FILE_LAYER_HPP_
#define POWER_FILE_LAYER_HPP_

#include <boost/shared_ptr.hpp>
#include <gflags/gflags.h>
#include <glog/logging.h>

#include <cmath>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/layer.hpp"
#include "caffe/proto/caffe.pb.h"

namespace caffe
{
namespace ultinous
{

template<typename Dtype>
class AffineMatrixLayer : public Layer<Dtype>
{

public:
  explicit AffineMatrixLayer(const LayerParameter &param)
  : Layer<Dtype>(param)
  {}

  virtual void LayerSetUp(const vector<Blob<Dtype> *> &bottom,
                          const vector<Blob<Dtype> *> &top);

  virtual void Reshape(const vector<Blob<Dtype> *> &bottom,
                       const vector<Blob<Dtype> *> &top);

  virtual inline const char *type() const
  { return "AffineMatrix"; }

  virtual inline int ExactNumBottomBlobs() const
  { return 1; }

  virtual inline int ExactNumTopBlobs() const
  { return 1; }

protected:
  virtual void Forward_cpu(const vector<Blob<Dtype> *> &bottom,
                           const vector<Blob<Dtype> *> &top);

  virtual void Backward_cpu(const vector<Blob<Dtype> *> &top,
                            const vector<bool> &propagate_down, const vector<Blob<Dtype> *> &bottom);

private:
  Dtype m_base_sx;
  Dtype m_base_sy;

  Dtype m_min_sx, m_max_sx, m_min_sy, m_max_sy;
  Dtype m_min_hx, m_max_hx, m_min_hy, m_max_hy;
  Dtype m_min_tx, m_max_tx, m_min_ty, m_max_ty;
  Dtype m_min_alpha, m_max_alpha;

  Dtype m_max_diff;

  bool m_normalize_params;
  bool m_bias;

  Dtype m_moving_average_fraction;
  Dtype m_boundary_violation_step;

  int m_iter;
};

}  // namespace ultinous
}  // namespace caffe

#endif  // CAFFE_COMMON_HPP_


#include "lib/header.h"

#include "glog/logging.h"

#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/qi_int.hpp>
#include <boost/spirit/include/qi_real.hpp>

namespace qi = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;

using qi::int_;
using qi::double_;
using qi::char_;
using qi::phrase_parse;
using ascii::space;
using qi::_1;
using qi::_val;

struct Function;

using Node = boost::variant<int, boost::recursive_wrapper<Function>>;

struct Function {
  std::string name;
  std::vector<Node> children;

  Function& operator+=(const Node& node) {
    children.emplace_back(node);
    return *this;
  }

  void operator=(const std::string& name_) { name = name_; }
};

void print(int x) { LOG(INFO) << "print: " << x; }

template <typename Iterator>
struct FunctionGrammar
    : public qi::grammar<Iterator, Node(), ascii::space_type> {
  FunctionGrammar() : FunctionGrammar::base_type(expr, "expr") {
    atom = int_[_val = _1];
    functionName = +char_("a-zA-Z")[_val += _1];
    function = (functionName[_val = _1] >> "(" >> expr[_val += _1] >>
                *("," >> expr[_val += _1]) >> ")") ||
               (functionName[_val = _1] >> "()");
    expr = function[_val = _1] || atom[_val = _1];
  }

  qi::rule<Iterator, Node(), ascii::space_type> expr;
  qi::rule<Iterator, Function(), ascii::space_type> function;
  qi::rule<Iterator, std::string(), ascii::space_type> functionName;
  qi::rule<Iterator, int(), ascii::space_type> atom;
};

int eval(const Node& node) {
  int result = 0;
  switch (node.which()) {
    case 0:
      result = boost::get<int>(node);
      LOG(INFO) << OUT(result);
      break;
    case 1:
      const auto& f = boost::get<Function>(node);
      LOG(INFO) << OUT(f.name) << OUT(f.children.size());
      for (const auto& x : f.children) {
        result += eval(x);
      }
      break;
  }
  return result;
}

int main() {
  /*
  string input("876,1234.56");
  auto iter = input.cbegin();
  auto endIter = input.cend();
  int varI = -1;
  double varD = -1;
  phrase_parse(iter, endIter,
               int_[boost::phoenix::ref(varI) = _1] >> ',' >>
                   double_[boost::phoenix::ref(varD) = _1],
               space);
  LOG(INFO) << OUT(varI) << OUT(varD);
  */

  string input("sum(sum(2, 2), 2)");
  auto iter = input.cbegin();
  auto endIter = input.cend();
  FunctionGrammar<string::const_iterator> grammar;
  Node parsedNode;
  phrase_parse(iter, endIter, grammar, space, parsedNode);
  LOG(INFO) << (iter == endIter);
  LOG(INFO) << OUT(eval(parsedNode));
  return 0;
}


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

#include <boost/mpl/vector.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/quote.hpp>
#include <boost/mpl/protect.hpp>
#include <boost/mpl/bind.hpp>
#include <boost/mpl/list.hpp>

#include <cmath>

#include <limits>
#include <boost/safe_float.hpp>
#include <boost/safe_float/convenience.hpp>

//types to be tested
using test_types=boost::mpl::list<
    float, double, long double
>;

using namespace boost::safe_float;

/**
  This test suite checks the numeric limits are the same as the wrapped type unless required.
  */

BOOST_AUTO_TEST_SUITE( safe_float_numeric_limits_suite )
BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_numeric_limits_basic_fp_types, FPT, test_types){
    //define a safe_float with base policies
    using number_type = safe_float<FPT>;
    using exact_number_type = safe_float<FPT, policy::check_inexact_rounding>;

    //check the specialization equal methods and attributes
    BOOST_CHECK(std::numeric_limits<number_type>::is_specialized == std::numeric_limits<FPT>::is_specialized);
    BOOST_CHECK(std::numeric_limits<number_type>::digits == std::numeric_limits<FPT>::digits);
    BOOST_CHECK(std::numeric_limits<number_type>::digits10 == std::numeric_limits<FPT>::digits10);
    BOOST_CHECK(std::numeric_limits<number_type>::max_digits10 == std::numeric_limits<FPT>::max_digits10);
    BOOST_CHECK(std::numeric_limits<number_type>::is_signed == std::numeric_limits<FPT>::is_signed);
    BOOST_CHECK(std::numeric_limits<number_type>::is_integer == std::numeric_limits<FPT>::is_integer);
    BOOST_CHECK(std::numeric_limits<number_type>::radix == std::numeric_limits<FPT>::radix);
    BOOST_CHECK(std::numeric_limits<number_type>::min_exponent == std::numeric_limits<FPT>::min_exponent);
    BOOST_CHECK(std::numeric_limits<number_type>::min_exponent10 == std::numeric_limits<FPT>::min_exponent10);
    BOOST_CHECK(std::numeric_limits<number_type>::max_exponent == std::numeric_limits<FPT>::max_exponent);
    BOOST_CHECK(std::numeric_limits<number_type>::max_exponent10 == std::numeric_limits<FPT>::max_exponent10);
    BOOST_CHECK(std::numeric_limits<number_type>::has_infinity == std::numeric_limits<FPT>::has_infinity);
    BOOST_CHECK(std::numeric_limits<number_type>::has_denorm == std::numeric_limits<FPT>::has_denorm);
    BOOST_CHECK(std::numeric_limits<number_type>::has_denorm_loss == std::numeric_limits<FPT>::has_denorm_loss);
    BOOST_CHECK(std::numeric_limits<number_type>::is_iec559 == std::numeric_limits<FPT>::is_iec559);
    BOOST_CHECK(std::numeric_limits<number_type>::is_bounded == std::numeric_limits<FPT>::is_bounded);
    BOOST_CHECK(std::numeric_limits<number_type>::is_modulo == std::numeric_limits<FPT>::is_modulo);
    BOOST_CHECK(std::numeric_limits<number_type>::traps == std::numeric_limits<FPT>::traps);
    BOOST_CHECK(std::numeric_limits<number_type>::tinyness_before == std::numeric_limits<FPT>::tinyness_before);

    //if safe_float has a policy declaring inexact is handled in every operation it should be considered exact
    BOOST_CHECK(std::numeric_limits<number_type>::is_exact == std::numeric_limits<FPT>::is_exact);
    BOOST_CHECK(std::numeric_limits<exact_number_type>::is_exact); //need to be implemented

    //The way NaNs are handled makes reference to the internal datatype
    BOOST_CHECK(std::numeric_limits<number_type>::has_quiet_NaN == std::numeric_limits<FPT>::has_quiet_NaN);
    BOOST_CHECK(std::numeric_limits<number_type>::has_signaling_NaN == std::numeric_limits<FPT>::has_signaling_NaN);
    BOOST_CHECK(std::numeric_limits<number_type>::round_style == std::numeric_limits<FPT>::round_style);

    //round error is zero if the number is_exactly represented
    BOOST_CHECK(std::numeric_limits<number_type>::round_error().get_stored_value() == std::numeric_limits<FPT>::round_error());
    BOOST_CHECK(std::numeric_limits<exact_number_type>::round_error().get_stored_value() == 0.0f);

    //check the special methods and attributes that are wrapped are internally the same values
    BOOST_CHECK(std::numeric_limits<number_type>::min().get_stored_value() == std::numeric_limits<FPT>::min());
    BOOST_CHECK(std::numeric_limits<number_type>::max().get_stored_value() == std::numeric_limits<FPT>::max());
    BOOST_CHECK(std::numeric_limits<number_type>::lowest().get_stored_value() == std::numeric_limits<FPT>::lowest());
    BOOST_CHECK(std::numeric_limits<number_type>::epsilon().get_stored_value() == std::numeric_limits<FPT>::epsilon());
    BOOST_CHECK(std::numeric_limits<number_type>::infinity().get_stored_value() == std::numeric_limits<FPT>::infinity());
    BOOST_CHECK(std::numeric_limits<number_type>::quiet_NaN().get_stored_value() == std::numeric_limits<FPT>::quiet_NaN());
    BOOST_CHECK(std::numeric_limits<number_type>::signaling_NaN().get_stored_value() == std::numeric_limits<FPT>::signaling_NaN());
    BOOST_CHECK(std::numeric_limits<number_type>::denorm_min().get_stored_value() == std::numeric_limits<FPT>::denorm_min());
}

BOOST_AUTO_TEST_SUITE_END() // policy


OBSOLETE

// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// 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)
//
// Doxygen Examples, referred to from the sources

#include <boost/tuple/tuple.hpp>

#if defined(_MSC_VER)
// We deliberately mix float/double's here so turn off warning
#pragma warning( disable : 4244 )
#endif // defined(_MSC_VER)

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/geometries.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>

// All functions below are referred to in the documentation of Boost.Geometry
// Don't rename them.
void example_area_polygon()
{
    //[area_polygon
    //` Calculate the area of a polygon
    boost::geometry::polygon<boost::geometry::point_xy<double> > poly; /*< Declare >*/
    boost::geometry::read_wkt("POLYGON((0 0,0 7,4 2,2 0,0 0))", poly); /*< Fill, in this case with WKT >*/
    double area = boost::geometry::area(poly); /*< Calculate area >*/
    //]

    //[area_polygon_spherical
    //` Calculate the area of a *spherical* polygon
    namespace bg = boost::geometry;
    bg::polygon<bg::point<float, 2, bg::cs::spherical<bg::degree> > > sph_poly;
    bg::read_wkt("POLYGON((0 0,0 45,45 0,0 0))", sph_poly);
    double area = bg::area(sph_poly);
    //]
}

void example_as_wkt_point()
{
    typedef boost::geometry::point_xy<double> P;
    P p(5.12, 6.34);
    // Points can be streamed like this:
    std::cout << boost::geometry::dsv<P>(p) << std::endl;

    // or like this:
    std::cout << boost::geometry::dsv(p) << std::endl;

    // or (with extension) like this:
    std::cout << boost::geometry::wkt(p) << std::endl;
}

void example_as_wkt_vector()
{
    std::vector<boost::geometry::point_xy<int> > v;
    boost::geometry::read_wkt<boost::geometry::point_xy<int> >("linestring(1 1,2 2,3 3,4 4)", std::back_inserter(v));

    std::cout << boost::geometry::dsv(std::make_pair(v.begin(), v.end())) << std::endl;
}

void example_centroid_polygon()
{
    boost::geometry::polygon<boost::geometry::point_xy<double> > poly;
    boost::geometry::read_wkt("POLYGON((0 0,0 7,4 2,2 0,0 0))", poly);
    // Center of polygon might have different type than points of polygon
    boost::geometry::point_xy<float> center;
    boost::geometry::centroid(poly, center);
    std::cout << "Centroid: " << boost::geometry::dsv(center) << std::endl;
}

void example_distance_point_point()
{
    boost::geometry::point_xy<double> p1(1, 1);
    boost::geometry::point_xy<double> p2(2, 3);
    std::cout << "Distance p1-p2 is "
        << boost::geometry::distance(p1, p2)
        << " units" << std::endl;

    /*
    Extension, other coordinate system:
    // Read 2 Dutch cities from WKT texts (in decimal degrees)
    boost::geometry::point_ll<double, boost::geometry::cs::geographic<boost::geometry::degree> >  a, r;
    boost::geometry::read_wkt("POINT(4.89222 52.3731)", a);
    boost::geometry::read_wkt("POINT(4.47917 51.9308)", r);

    std::cout << "Distance Amsterdam-Rotterdam is "
        << boost::geometry::distance(a, r) / 1000.0
        << " kilometers " << std::endl;
    */
}

void example_distance_point_point_strategy()
{
    /*
    Extension, other coordinate system:
    typedef boost::geometry::point_ll<double, boost::geometry::cs::geographic<boost::geometry::degree> > LL;
    LL a, r;
    boost::geometry::read_wkt("POINT(4.89222 52.3731)", a);
    boost::geometry::read_wkt("POINT(4.47917 51.9308)", r);

    std::cout << "Distance Amsterdam-Rotterdam is "
        << boost::geometry::distance(a, r,
                boost::geometry::strategy::distance::vincenty<LL>() )
                 / 1000.0
        << " kilometers " << std::endl;
    */
}

void example_from_wkt_point()
{
    boost::geometry::point_xy<int> point;
    boost::geometry::read_wkt("Point(1 2)", point);
    std::cout << point.x() << "," << point.y() << std::endl;
}

void example_from_wkt_output_iterator()
{
    std::vector<boost::geometry::point_xy<int> > v;
    boost::geometry::read_wkt<boost::geometry::point_xy<int> >("linestring(1 1,2 2,3 3,4 4)", std::back_inserter(v));
    std::cout << "vector has " << v.size() << " coordinates" << std::endl;
}

void example_from_wkt_linestring()
{
    boost::geometry::linestring<boost::geometry::point_xy<double> > line;
    boost::geometry::read_wkt("linestring(1 1,2 2,3 3,4 4)", line);
    std::cout << "linestring has " << line.size() << " coordinates" << std::endl;
}

void example_from_wkt_polygon()
{
    boost::geometry::polygon<boost::geometry::point_xy<double> > poly;
    boost::geometry::read_wkt("POLYGON((0 0,0 1,1 1,1 0,0 0))", poly);
    std::cout << "Polygon has " << poly.outer().size() << " coordinates in outer ring" << std::endl;
}

void example_point_ll_convert()
{
    /*
    Extension, other coordinate system:
    boost::geometry::point_ll<double, boost::geometry::cs::geographic<boost::geometry::degree> > deg(boost::geometry::latitude<>(33.0), boost::geometry::longitude<>(-118.0));
    boost::geometry::point_ll<double, boost::geometry::cs::geographic<boost::geometry::radian> > rad;
    boost::geometry::transform(deg, rad);

    std::cout << "point in radians: " << rad << std::endl;
    */
}

void example_clip_linestring1()
{
    typedef boost::geometry::point_xy<double> P;
    boost::geometry::linestring<P> line;
    boost::geometry::read_wkt("linestring(1.1 1.1, 2.5 2.1, 3.1 3.1, 4.9 1.1, 3.1 1.9)", line);
    boost::geometry::box<P> cb(P(1.5, 1.5), P(4.5, 2.5));
    std::cout << "Clipped linestring(s) " << std::endl;

    std::vector<boost::geometry::linestring<P> > intersection;
    boost::geometry::intersection_inserter<boost::geometry::linestring<P> >(cb, line, std::back_inserter(intersection));
}

void example_clip_linestring2()
{
    typedef boost::geometry::point_xy<double> P;
    std::vector<P> vector_in;
    boost::geometry::read_wkt<P>("linestring(1.1 1.1, 2.5 2.1, 3.1 3.1, 4.9 1.1, 3.1 1.9)",
                    std::back_inserter(vector_in));

    boost::geometry::box<P> cb(P(1.5, 1.5), P(4.5, 2.5));
    typedef std::vector<std::vector<P> > VV;
    VV vector_out;
    boost::geometry::intersection_inserter<std::vector<P>  >(cb, vector_in, std::back_inserter(vector_out));

    std::cout << "Clipped vector(s) " << std::endl;
    for (VV::const_iterator it = vector_out.begin(); it != vector_out.end(); it++)
    {
        std::copy(it->begin(), it->end(), std::ostream_iterator<P>(std::cout, " "));
        std::cout << std::endl;
    }
}

void example_intersection_polygon1()
{
    typedef boost::geometry::point_xy<double> P;
    typedef std::vector<boost::geometry::polygon<P> > PV;

    boost::geometry::box<P> cb(P(1.5, 1.5), P(4.5, 2.5));
    boost::geometry::polygon<P> poly;
    boost::geometry::read_wkt("POLYGON((2 1.3,2.4 1.7,2.8 1.8,3.4 1.2,3.7 1.6,3.4 2,4.1 3,5.3 2.6,5.4 1.2,4.9 0.8,2.9 0.7,2 1.3)"
            ",(4 2,4.2 1.4,4.8 1.9,4.4 2.2,4 2))", poly);

    PV v;
    boost::geometry::intersection_inserter<boost::geometry::polygon<P> >(cb, poly, std::back_inserter(v));

    std::cout << "Clipped polygon(s) " << std::endl;
    for (PV::const_iterator it = v.begin(); it != v.end(); it++)
    {
        std::cout << boost::geometry::dsv(*it) << std::endl;
    }
}

void example_simplify_linestring1()
{
    //[simplify
    //` Simplify a linestring
    boost::geometry::linestring<boost::geometry::point_xy<double> > line, simplified;
    boost::geometry::read_wkt("linestring(1.1 1.1, 2.5 2.1, 3.1 3.1, 4.9 1.1, 3.1 1.9)", line);
    boost::geometry::simplify(line, simplified, 0.5); /*< Simplify it, using distance of 0.5 units >*/
    std::cout
        << "  original line: " << boost::geometry::dsv(line) << std::endl
        << "simplified line: " << boost::geometry::dsv(simplified) << std::endl;
    //]
}

void example_simplify_linestring2()
{
    //[simplify_inserter
    //` Simplify a linestring using an output iterator
    typedef boost::geometry::point_xy<double> P;
    typedef boost::geometry::linestring<P> L;
    L line;

    boost::geometry::read_wkt("linestring(1.1 1.1, 2.5 2.1, 3.1 3.1, 4.9 1.1, 3.1 1.9)", line);

    typedef boost::geometry::strategy::distance::projected_point<P, P> DS;
    typedef boost::geometry::strategy::simplify::douglas_peucker<P, DS> simplification;
    boost::geometry::simplify_inserter(line, std::ostream_iterator<P>(std::cout, "\n"), 0.5, simplification());
    //]
}

void example_within()
{
    boost::geometry::polygon<boost::geometry::point_xy<double> > poly;
    boost::geometry::read_wkt("POLYGON((0 0,0 7,4 2,2 0,0 0))", poly);
    boost::geometry::point_xy<float> point(3, 3);
    std::cout << "Point is "
        << (boost::geometry::within(point, poly) ? "IN" : "NOT in")
        << " polygon"
        << std::endl;
}

/*
void example_within_strategy()
{
    // TO BE UPDATED/FINISHED
    typedef boost::geometry::point_xy<double> P;
    typedef boost::geometry::polygon<P> POLY;
    P p;
    std::cout << within(p, poly, strategy::within::cross_count<P>) << std::endl;
}
*/

void example_length_linestring()
{
    using namespace boost::geometry;
    linestring<point_xy<double> > line;
    read_wkt("linestring(0 0,1 1,4 8,3 2)", line);
    std::cout << "linestring length is "
        << length(line)
        << " units" << std::endl;

    /*
    Extension, other coordinate system:
    // Linestring in latlong, filled with
    // explicit degree-minute-second values
    typedef point_ll<float, boost::geometry::cs::geographic<boost::geometry::degree> > LL;
    linestring<LL> line_ll;
    line_ll.push_back(LL(
        latitude<float>(dms<north, float>(52, 22, 23)),
        longitude<float>(dms<east, float>(4, 53, 32))));
    line_ll.push_back(LL(
        latitude<float>(dms<north, float>(51, 55, 51)),
        longitude<float>(dms<east, float>(4, 28, 45))));
    line_ll.push_back(LL(
        latitude<float>(dms<north, float>(52, 4, 48)),
        longitude<float>(dms<east, float>(4, 18, 0))));
    std::cout << "linestring length is "
        << length(line_ll) / 1000
        << " kilometers " << std::endl;
        */
}

void example_length_linestring_iterators1()
{
    boost::geometry::linestring<boost::geometry::point_xy<double> > line;
    boost::geometry::read_wkt("linestring(0 0,1 1,4 8,3 2)", line);
    std::cout << "linestring length is "
        << boost::geometry::length(line)
        << " units" << std::endl;
}

void example_length_linestring_iterators2()
{
    std::vector<boost::geometry::point_xy<double> > line;
    boost::geometry::read_wkt<boost::geometry::point_xy<double> >("linestring(0 0,1 1,4 8,3 2)", std::back_inserter(line));
    std::cout << "linestring length is "
        << boost::geometry::length(line)
        << " units" << std::endl;
}

void example_length_linestring_iterators3()
{
    /*
    Extension, other coordinate system:
    using namespace boost::geometry;
    typedef point_ll<float, boost::geometry::cs::geographic<boost::geometry::degree> > LL;
    std::deque<LL> line;
    boost::geometry::read_wkt<LL>("linestring(0 51,1 51,2 52)", std::back_inserter(line));
    std::cout << "linestring length is "
        << 0.001 * boost::geometry::length(line, boost::geometry::strategy::distance::vincenty<LL>())
        << " kilometers" << std::endl;
    */
}

void example_length_linestring_strategy()
{
    /*
    Extension, other coordinate system:
    using namespace boost::geometry;
    typedef point_ll<float, boost::geometry::cs::geographic<boost::geometry::degree> > LL;
    linestring<LL> line_ll;
    line_ll.push_back(LL(latitude<float>(dms<north, float>(52, 22, 23)), longitude<float>(dms<east, float>(4, 53, 32))));
    line_ll.push_back(LL(latitude<float>(dms<north, float>(51, 55, 51)), longitude<float>(dms<east, float>(4, 28, 45))));
    line_ll.push_back(LL(latitude<float>(dms<north, float>(52, 4, 48)), longitude<float>(dms<east, float>(4, 18, 0))));
    std::cout << "linestring length is "
        << length(line_ll, strategy::distance::vincenty<LL, LL>() )/(1000)
        << " kilometers " << std::endl;
    */
}

void example_envelope_linestring()
{
    boost::geometry::linestring<boost::geometry::point_xy<double> > line;
    boost::geometry::read_wkt("linestring(0 0,1 1,4 8,3 2)", line);
    boost::geometry::box<boost::geometry::point_xy<double> > box;
    boost::geometry::envelope(line, box);

    std::cout << "envelope is " << boost::geometry::dsv(box) << std::endl;
}

void example_envelope_polygon()
{
    /*
    Extension, other coordinate system:
    using namespace boost::geometry;
    typedef point_ll<double, boost::geometry::cs::geographic<boost::geometry::degree> >  LL;

    // Wrangel island, 180 meridian crossing island above Siberia.
    polygon<LL> wrangel;
    wrangel.outer().push_back(LL(latitude<>(dms<north>(70, 47, 7)), longitude<>(dms<west>(178, 47, 9))));
    wrangel.outer().push_back(LL(latitude<>(dms<north>(71, 14, 0)), longitude<>(dms<east>(177, 28, 33))));
    wrangel.outer().push_back(LL(latitude<>(dms<north>(71, 34, 24)), longitude<>(dms<east>(179, 44, 37))));
    // Close it
    wrangel.outer().push_back(wrangel.outer().front());

    boost::geometry::box<LL> box;
    boost::geometry::envelope(wrangel, box);

    dms<cd_lat> minlat(box.min_corner().lat());
    dms<cd_lon> minlon(box.min_corner().lon());

    dms<cd_lat> maxlat(box.max_corner().lat());
    dms<cd_lon> maxlon(box.max_corner().lon());

    std::cout << wrangel << std::endl;
    std::cout << "min: " << minlat.get_dms() << " , " << minlon.get_dms() << std::endl;
    std::cout << "max: " << maxlat.get_dms() << " , " << maxlon.get_dms() << std::endl;
    */
}

void example_dms()
{
    /*
    Extension, other coordinate system:
    // Construction with degree/minute/seconds
    boost::geometry::dms<boost::geometry::east> d1(4, 53, 32.5);

    // Explicit conversion to double.
    std::cout << d1.as_value() << std::endl;

    // Conversion to string, with optional strings
    std::cout << d1.get_dms(" deg ", " min ", " sec") << std::endl;

    // Combination with latitude/longitude and cardinal directions
    {
        using namespace boost::geometry;
        point_ll<double, boost::geometry::cs::geographic<boost::geometry::degree> > canberra(
            latitude<>(dms<south>(35, 18, 27)),
            longitude<>(dms<east>(149, 7, 27.9)));
        std::cout << canberra << std::endl;
    }
    */
}

void example_point_ll_construct()
{
    /*
    Extension, other coordinate system:
    using namespace boost::geometry;
    typedef point_ll<double, boost::geometry::cs::geographic<boost::geometry::degree> > ll;

    // Constructions in both orders possible
    ll juneau(
        latitude<>(dms<north>(58, 21, 5)),
        longitude<>(dms<west>(134, 30, 42)));
    ll wladiwostok(
        longitude<>(dms<east>(131, 54)),
        latitude<>(dms<north>(43, 8))
        );
    */
}

int main(void)
{
    example_area_polygon();

    example_centroid_polygon();

    example_distance_point_point();
    example_distance_point_point_strategy();

    example_from_wkt_point();
    example_from_wkt_output_iterator();
    example_from_wkt_linestring();
    example_from_wkt_polygon();

    example_as_wkt_point();

    example_clip_linestring1();
    example_clip_linestring2();
    example_intersection_polygon1();

    example_simplify_linestring1();
    example_simplify_linestring2();

    example_length_linestring();
    example_length_linestring_iterators1();
    example_length_linestring_iterators2();
    example_length_linestring_iterators3();
    example_length_linestring_strategy();

    example_envelope_linestring();
    example_envelope_polygon();

    example_within();

    example_point_ll_convert();
    example_point_ll_construct();
    example_dms();

    return 0;
}


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

#pragma once

#include <vector>
#include <memory>
#include <ze/common/logging.hpp>
#include <Eigen/Core>

#include <ze/common/types.hpp>
#include <ze/common/statistics.hpp>

namespace ze {

// -----------------------------------------------------------------------------
// Scale Estimators

//! Dummy scale estimator.
template <typename Scalar>
struct UnitScaleEstimator
{
  static constexpr Scalar compute(const VectorX& /*errors*/)
  {
    return Scalar{1.0};
  }
};

//! Estimates scale by computing the median absolute deviation (MAD).
template <typename Scalar>
struct MADScaleEstimator
{
  using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
  static Scalar compute(const VectorX& errors)
  {
    VectorX absolute_error(errors.size());
    absolute_error = errors.array().abs();
    std::pair<Scalar, bool> res = median(absolute_error);
    CHECK(res.second);
    return Scalar{1.48} * res.first; // 1.48f / 0.6745
  }
};

//! Estimates scale by computing the standard deviation.
template <typename Scalar>
struct NormalDistributionScaleEstimator
{
  using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
  static Scalar compute(const VectorX& errors)
  {
    // normed_errors should not have absolute values.
    const int n = errors.size();
    CHECK(n > 1);
    const Scalar mean = errors.sum() / n;
    Scalar sum2 = Scalar{0.0};
    for(int i = 0; i < errors.size(); ++i)
    {
      sum2 += (errors(i) - mean) * (errors(i) - mean);
    }
    return std::sqrt(sum2 / (n - 1));
  }
};

// -----------------------------------------------------------------------------
// Scale Estimators
// Weight-Functions for M-Estimators
// http://research.microsoft.com/en-us/um/people/zhang/inria/publis/tutorial-estim/node24.html

template <typename Scalar, typename Implementation>
struct WeightFunction
{
  using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
  static VectorX weightVectorized(const VectorX& error_vec)
  {
    VectorX weights(error_vec.size());
    for(int i = 0; i < error_vec.size(); ++i)
    {
      weights(i) = Implementation::weight(error_vec(i));
    }
    return weights;
  }

  static real_t weight(const real_t error)
  {
    return Implementation::weight(error);
  }
};

template <typename Scalar>
struct UnitWeightFunction : public WeightFunction<Scalar, UnitWeightFunction<Scalar>>
{
  static constexpr Scalar weight(const Scalar& /*normed_error*/)
  {
    return Scalar{1.0};
  }
};

template <typename Scalar>
struct TukeyWeightFunction : public WeightFunction<Scalar, TukeyWeightFunction<Scalar>>
{
  static constexpr Scalar b_square = 4.6851 * 4.6851;
  static Scalar weight(const Scalar& error)
  {
    const Scalar x_square = error * error;
    if(x_square <= b_square)
    {
      const Scalar tmp = Scalar{1.0} - x_square / b_square;
      return tmp * tmp;
    }
    else
    {
      return Scalar{0.0};
    }
  }
};

template <typename Scalar>
struct HuberWeightFunction : public WeightFunction<Scalar, HuberWeightFunction<Scalar>>
{
  static constexpr Scalar k = 1.345;
  static Scalar weight(const Scalar& normed_error)
  {
    const Scalar abs_error = std::abs(normed_error);
    return (abs_error < k) ? Scalar{1.0} : k / abs_error;
  }
};

} // namespace ze


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

  @copyright 2017 NumScale SAS

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

#include <boost/simd/range/segmented_aligned_range.hpp>
#include <boost/simd/function/enumerate.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/as.hpp>
#include <boost/simd/pack.hpp>
#include <iterator>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Fills the range [first, last) with sequentially increasing values,
    starting with seed  and repetitively evaluating value += step.

    @param first  Beginning of the range of elements
    @param last   End of the range of elements
    @param seed   initial value to store
    @param step   value to add (default to 1)

    @par Requirement

      - @c first, @c last and @c out must be pointer to Vectorizable type.

    @par Example:

      @snippet iota.cpp iota

    @par Possible output:

      @snippet iota.txt iota
  **/
  template<typename T,  typename T1,  typename T2>
  void iota(T * first, T * last, T1 seed, T2 step)
  {
    // Scalar seeder
    struct sg
    {
      sg(T1 seed, T2 step) : i_(seed),  step_(step) {}
      T operator()() { auto z = i_; i_+= step_; return z; }
      T i_, step_;
    };

    // SIMD seeder
    struct vg
    {
      using p_t =  pack<T>;
      vg(T1 seed, T2 step) : i_(bs::enumerate<p_t>(seed, step)), step_(step) {}
      p_t operator()() { auto z = i_; i_+=step_*p_t::static_size; return z; }
      p_t i_, step_;
    };

    auto pr = segmented_aligned_range(first,last);

    sg gg0(seed, step);
    std::generate(pr.head.begin(), pr.head.end(), gg0);

    // This lambda wrapping is not useless, it prevent MSVC bitching about passing gg1
    // by value while it stores a pack :| and hence is not aligned for the stack
    seed += std::distance(pr.head.begin(), pr.head.end());
    vg gg1(seed, step);
    std::generate(pr.body.begin(), pr.body.end(), [&gg1](){ return gg1(); } );

    seed += std::distance(pr.body.begin(), pr.body.end())*pack<T>::static_size;
    sg gg2(seed, step);
    std::generate(pr.tail.begin(), pr.tail.end(), gg2);
  }

  /*!
    @ingroup group-algo

    Fills the range [first, last) with sequentially increasing values starting with seed.

    @param first  Beginning of the range of elements
    @param last   End of the range of elements
    @param seed   initial value to store

    @par Requirement

      - @c first, @c last and @c out must be pointer to Vectorizable type.

    @par Example:

      @snippet iota.cpp iota

    @par Possible output:

      @snippet iota.txt iota
  **/
  template<typename T,  typename T1> void iota(T * first, T * last, T1 seed)
  {
    // Scalar seeder
    struct sg
    {
      sg(T1 seed) : i_(seed) {}
      T operator()() { return i_++; }
      T i_;
    };

    // SIMD seeder
    struct vg
    {
      using p_t =  pack<T>;
      vg(T seed) : i_(bs::enumerate<p_t>(seed)) {}
      p_t operator()() { auto z = i_; i_+= p_t::static_size; return z; }
      p_t i_;
    };

    auto pr = segmented_aligned_range(first,last);

    sg gg0(seed);
    std::generate(pr.head.begin(), pr.head.end(), gg0 );

    seed += std::distance(pr.head.begin(), pr.head.end());
    vg gg1(seed);
    std::generate(pr.body.begin(), pr.body.end(), [&gg1](){ return gg1(); });

    seed += std::distance(pr.body.begin(), pr.body.end())*pack<T>::static_size;
    sg gg2(seed);
    std::generate(pr.tail.begin(), pr.tail.end(), gg2 );
  }
} }

#endif


// Copyright (c)       2014 Martin Stumpf
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef MANDELBROT_WORKLOAD_H_
#define MANDELBROT_WORKLOAD_H_

#include <cstdlib>
#include <vector>

#include <hpx/config.hpp>
#include <boost/shared_ptr.hpp>

/*
 * A workload, defines a mandelbrot line and will be filled by workers with
 * the calculated results
 */
class workload
{

    public:
        workload(size_t num_pixels_x_,
                 size_t num_pixels_y_,
                 double topleft_x_,
                 double topleft_y_,
                 double hor_pixdist_x_,
                 double hor_pixdist_y_,
                 double vert_pixdist_x_,
                 double vert_pixdist_y_,
                 size_t img_id_,
                 size_t pos_in_img_x_,
                 size_t pos_in_img_y_,
                 size_t line_offset_);

        // Will hold the calculated pixels
        boost::shared_ptr<std::vector<char>> pixeldata;
        // the number of pixels on the rectangle
        size_t num_pixels_x;
        size_t num_pixels_y;
        // the top left point of the rectangle
        double topleft_x;
        double topleft_y;
        // the horizontal offset between pixels
        double hor_pixdist_x;
        double hor_pixdist_y;
        // the vertical offset between pixels
        double vert_pixdist_x;
        double vert_pixdist_y;
        // metadata for correct mapping to image
        size_t img_id;
        size_t pos_in_img_x;
        size_t pos_in_img_y;
        size_t line_offset;

};

#endif


/*
 [auto_generated]
 libs/numeric/odeint/test/trivial_state.cpp

 [begin_description]
 This file tests if the steppers can integrate the trivial state consisting of a single double.
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

 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)
 */

// disable checked iterator warning for msvc
#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_trivial_state

#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/at.hpp>

#include <boost/numeric/odeint/stepper/euler.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include <boost/numeric/odeint/stepper/generation.hpp>
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

namespace mpl = boost::mpl;

typedef double state_type;
typedef double value_type;
typedef double deriv_type;
typedef double time_type;

void constant_system( const state_type &x , state_type &dxdt , const time_type t ) { dxdt = 1.0; }

BOOST_AUTO_TEST_SUITE( trivial_state_test )

/* test different do_step methods of simple steppers */

typedef mpl::vector<
        euler< state_type , value_type , deriv_type , time_type ,
               vector_space_algebra , default_operations , never_resizer > ,
        runge_kutta4< state_type , value_type , deriv_type , time_type ,
                      vector_space_algebra , default_operations , never_resizer >
        >::type stepper_types;

BOOST_AUTO_TEST_CASE_TEMPLATE( test_do_step , T, stepper_types )
{
    typedef T stepper_type;
    stepper_type stepper;
    state_type x = 0.0;
    time_type t = 0.0;
    time_type dt = 0.1;
    stepper.do_step( constant_system , x , t , dt );
    BOOST_CHECK_CLOSE( x , 0.1 , 1.0e-10 );

    // this overload is not allowed if the types of dxdt and dt are the same
    // deriv_type dxdt = 1.0;
    // stepper.do_step( constant_system , x , dxdt , t , dt );

    state_type x_out;
    stepper.do_step( constant_system , x , t , x_out , dt );
    BOOST_CHECK_CLOSE( x , 0.1 , 1.0e-10 );
    BOOST_CHECK_CLOSE( x_out , 0.2 , 1.0e-10 );
}

/* test integrate_adaptive with controlled steppers */

typedef mpl::vector<
    runge_kutta_cash_karp54< state_type , value_type , deriv_type , time_type ,
                             vector_space_algebra , default_operations , never_resizer > ,
    runge_kutta_dopri5< state_type , value_type , deriv_type , time_type ,
                        vector_space_algebra , default_operations , never_resizer >
    > error_stepper_types;

BOOST_AUTO_TEST_CASE_TEMPLATE( test_integrate , T , error_stepper_types )
{
    typedef T stepper_type;
    state_type x = 0.0;
    time_type t0 = 0.0;
    time_type t1 = 1.0;
    time_type dt = 0.1;
    integrate_adaptive( make_controlled< stepper_type >( 1e-6 , 1e-6 ) , constant_system , x , t0 , t1 , dt );
    BOOST_CHECK_CLOSE( x , 1.0 , 1.0e-10 );
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/program_options.hpp>
#include <iostream>

namespace po = boost::program_options;

template <typename type_qi = std::uint32_t, typename type_Ri = std::uint32_t,
          typename type_ni = std::uint32_t>
class ProblemSize {
public:
  using type_q = type_qi;
  using type_R = type_Ri;
  using type_n = type_ni;

  ProblemSize(type_q const qi, type_R const Ri, type_n const ni)
      : q(qi), R(Ri), n(ni) {}

public:
  // Values are read only and public!
  type_q const q;
  type_R const R;
  type_n const n;
};

template <typename type_q = std::uint32_t, typename type_R = std::uint32_t,
          typename type_n = std::uint32_t, typename type_cnt = std::uint64_t>
class Params {
public:
  using ProblemType = ProblemSize<type_q, type_R, type_n>;

  Params(int argc, char *argv[], bool with_cnt = true);

  ProblemType const &get_problem_size() const { return ps; }
  type_cnt get_cnt() const { return cnt; }

private:
  po::variables_map vm;
  ProblemType const ps;
  type_cnt const cnt;
};

namespace {

po::variables_map init_variables_map(int argc, char *argv[]) {
  po::variables_map vm;

  po::options_description desc("Allowed options");
  desc.add_options()("help", "produce help message")(
      "q", po::value<std::uint32_t>(), "set q - alphabet size")(
      "R", po::value<std::uint32_t>(), "set R - max distance")(
      "n", po::value<std::uint32_t>(), "set n - size / dimension")(
      "cnt", po::value<std::uint64_t>(), "set cnt - number of codes / rooks");
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << "\n";
    exit(1);
  }

  return vm;
}

template <typename type_q, typename type_R, typename type_n>
ProblemSize<type_q, type_R, type_n>
init_problem_type(po::variables_map const &vm) {
  type_q q;
  if (vm.count("q")) {
    q = vm["q"].as<type_q>();
  } else {
    std::cout << "Size (q) was not set.\n";
    exit(1);
  }

  type_R R;
  if (vm.count("R")) {
    R = vm["R"].as<type_R>();
  } else {
    std::cout << "Maximum distance was not set.\n";
    exit(1);
  }

  type_n n;
  if (vm.count("n")) {
    n = vm["n"].as<type_n>();
  } else {
    std::cout << "Size was not set.\n";
    exit(1);
  }

  return ProblemSize<type_q, type_R, type_n>(q, R, n);
}

template <typename type_cnt> type_cnt init_cnt(po::variables_map const &vm) {
  if (vm.count("cnt")) {
    return vm["cnt"].as<type_cnt>();
  } else {
    std::cout << "Count was not set.\n";
    exit(1);
  }
}

} // namespace

template <typename type_q, typename type_R, typename type_n, typename type_cnt>
Params<type_q, type_R, type_n, type_cnt>::Params(int argc, char *argv[],
                                                 bool with_cnt)
    : vm(init_variables_map(argc, argv)),
      ps(init_problem_type<type_q, type_R, type_n>(vm)),
      cnt(with_cnt ? init_cnt<type_cnt>(vm) : 0) {}


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#define BOOST_TEST_MODULE icl::interval_map_infix_mixed unit test
#include <libs/icl/test/disable_test_warnings.hpp>
#include <string>
#include <boost/mpl/list.hpp>
#include "../unit_test_unwarned.hpp"

// interval instance types
#include "../test_type_lists.hpp"
#include "../test_value_maker.hpp"

#include <boost/icl/interval_set.hpp>
#include <boost/icl/separate_interval_set.hpp>
#include <boost/icl/split_interval_set.hpp>
#include <boost/icl/interval_map.hpp>
#include <boost/icl/split_interval_map.hpp>

using namespace std;
using namespace boost;
using namespace unit_test;
using namespace boost::icl;

BOOST_AUTO_TEST_CASE_TEMPLATE(test_icl_interval_map_mixed_infix_plus_overload_4_bicremental_types, T, bicremental_types)
{
    typedef int U;
    typedef interval_map<T,U>  IntervalMapT;
    interval_map<T,U>          join_a;
    split_interval_map<T,U>    split_a;

    join_a .add(CDv(1,3,1)).add(IDv(8,9,1)).add(IIv(6,11,3));
    split_a.add(IDv(0,9,2)).add(IIv(3,6,1)).add(IDv(5,7,1));

    BOOST_CHECK_EQUAL(split_a + join_a, join_a + split_a);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_icl_interval_map_mixed_infix_pipe_overload_4_bicremental_types, T, bicremental_types)
{
    typedef int U;
    typedef interval_map<T,U>  IntervalMapT;
    interval_map<T,U>          join_a;
    split_interval_map<T,U>    split_a;

    join_a .add(CDv(1,3,1)).add(IDv(8,9,1)).add(IIv(6,11,3));
    split_a.add(IDv(0,9,2)).add(IIv(3,6,1)).add(IDv(5,7,1));

    BOOST_CHECK_EQUAL(split_a | join_a, join_a | split_a);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_icl_interval_map_mixed_infix_minus_overload_4_bicremental_types, T, bicremental_types)
{
    typedef int U;
    typedef interval_map<T,U>  IntervalMapT;
    interval_map<T,U>          join_a, join_b;
    split_interval_map<T,U>    split_a, split_b;

    join_a .add(CDv(1,3,1)).add(IDv(8,9,1)).add(IIv(6,11,3));
    split_a.add(IDv(0,9,2)).add(IIv(3,6,1)).add(IDv(5,7,1));

    join_b .add(CDv(1,3,1)).add(IIv(6,11,3));
    split_b.add(IDv(0,9,2)).add(IIv(3,6,1));

    BOOST_CHECK_EQUAL(split_a - join_a, (split_b = split_a) -= join_a);
    BOOST_CHECK_EQUAL(split_a - join_a, split_b);

    BOOST_CHECK_EQUAL(join_a - split_a, (join_b = join_a) -= split_a);
    BOOST_CHECK_EQUAL(join_a - split_a, join_b);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_icl_interval_map_mixed_infix_et_overload_4_bicremental_types, T, bicremental_types)
{
    typedef int U;
    typedef interval_map<T,U>  IntervalMapT;
    interval_map<T,U>          join_a, join_b;
    split_interval_map<T,U>    split_a, split_b;

    join_a .add(CDv(1,3,1)).add(IDv(8,9,1)).add(IIv(6,11,3));
    split_a.add(IDv(0,9,2)).add(IIv(3,6,1)).add(IDv(5,7,1));

    BOOST_CHECK_EQUAL(split_a & join_a, join_a & split_a);
    BOOST_CHECK_EQUAL(split_a & join_a, (split_b = split_a) &= join_a);
    BOOST_CHECK_EQUAL(split_a & join_a, split_b);

    BOOST_CHECK_EQUAL(join_a & split_a, (split_b = split_a) &= join_a);
    BOOST_CHECK_EQUAL(join_a & split_a, split_b);
}


// Copyright 2005 The Trustees of Indiana University.

// 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)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#include "graph_types.hpp"
#include <boost/graph/sequential_vertex_coloring.hpp>

namespace boost { namespace graph { namespace python {

template<typename Graph>
int
sequential_vertex_coloring
  (const Graph& g,
   vector_property_map<
     int,
     typename property_map<Graph, vertex_index_t>::const_type>* in_color)
{
  typedef vector_property_map<
            int, typename property_map<Graph, vertex_index_t>::const_type>
    ColorMap;

  ColorMap color =
    in_color? *in_color
    : ColorMap(num_vertices(g), get(vertex_index, g));

  return boost::sequential_vertex_coloring(g, color);
}

void export_sequential_vertex_coloring()
{
  using boost::python::arg;
  using boost::python::def;

#define UNDIRECTED_GRAPH(Name,Type)                                     \
  {                                                                     \
    typedef vector_property_map<                                        \
              int, property_map<Type, vertex_index_t>::const_type> ColorMap; \
                                                                        \
    def("sequential_vertex_coloring",                                   \
        &sequential_vertex_coloring<Type>,                             \
        (arg("graph"), arg("color_map") = static_cast<ColorMap*>(0)));  \
  }
#include "graphs.hpp"
}

} } } // end namespace boost::graph::python


/* Copyright (c) 2012, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See LICENSE.txt or
 * http://www.opensource.org/licenses/mit-license.php */

#pragma once

#include <hdp_var.hpp>
#include <hdp_var_base_py.hpp>

#include <assert.h>
#include <stddef.h>
#include <stdint.h>
#include <typeinfo>

#include <armadillo>

#include <boost/python.hpp>
#include <boost/python/wrapper.hpp>
#include <numpy/ndarrayobject.h> // for PyArrayObject

#ifdef PYTHON_2_6
  #include <python2.6/object.h> // for PyArray_FROM_O
#endif
#ifdef PYTHON_2_7
  #include <python2.7/object.h> // for PyArray_FROM_O
#endif

using namespace boost::python;

template <class U>
class HDP_var_py : public HDP_var_base_py, public HDP_var<U>
{
public:
  HDP_var_py(const BaseMeasure<U>& base, double alpha, double gamma)
  :  HDP_var_base_py(0,0,0), HDP_var<U>(base,alpha,gamma)
  { };

  bool densityEst(uint32_t Nw, double kappa, uint32_t K, uint32_t T, uint32_t S)
  {
    return HDP_var<U>::densityEst(Nw,kappa,K,T,S);
  }

  /*
   * makes no copy of the external data x_i
   */
  uint32_t addDoc(const numeric::array& x_i)
  {
    return HDP_var<U>::addDoc(np2mat<U>(x_i));
  };

  uint32_t addHeldOut(const numeric::array& x_i)
  {
    return HDP_var<U>::addHeldOut(np2mat<U>(x_i));
  };

  /*
   * after an initial densitiy estimate has been made using addDoc() and densityEst()
   * can use this to update the estimate with information from additional x
   */
  bool updateEst(const numeric::array& x, double ro=0.75)
  {
    return HDP_var<U>::updateEst(np2mat<U>(x),ro);
  }
  bool updateEst_batch(double kappa, uint32_t S){
    return HDP_var<U>::updateEst_batch(kappa,S);
  }

  uint32_t getTopicPriorDescriptionLength()
  {
    return HDP<U>::mH0.rowDim();
  };
  /*
   * works on the data in lambda -> size has to be correct in order for this to work!
   * makes a copy of the internal labels vector
   */
  bool getLambda_py(numeric::array& lambda, uint32_t k)
  {
    Row<double> lambda_row;
    if(!HDP_var<U>::getLambda(lambda_row, k)){return false;} // works on the data in _mat
    Row<double> lambda_wrap=np2row<double>(lambda);
    if(lambda_row.n_cols != lambda_wrap.n_cols)
      return false;
    else{
      lambda_wrap = lambda_row;
      return true;
    }
  };

  uint32_t getTopicsDescriptionLength()
  {
    return HDP<U>::mH0.mode()->rowDim();
  };

  bool getCorpTopics_py(numeric::array& beta)
  {
    Mat<double> beta_mat;
    if(!HDP_var<U>::getCorpTopics(beta_mat)){return false;} // works on the data in _mat
    assignMat2np(beta_mat,beta);
    return true;
  };

};

typedef HDP_var_py<uint32_t> HDP_var_Dir_py;
typedef HDP_var_py<double> HDP_var_NIW_py;

//class HDP_var_ss_py : public HDP_var_base_py, public HDP_var_ss
//{
//public:
//  HDP_var_ss_py(const BaseMeasure<uint32_t>& base, double alpha, double gamma)
//  : HDP_var_base_py(0,0,0), HDP_var_ss(base,alpha,gamma)
//  {
//    //cout<<"Creating "<<typeid(this).name()<<endl;
//  };
//
//  bool densityEst(const numeric::array& x, const numeric::array& x_ho, double kappa, uint32_t K, uint32_t T, uint32_t S)
//  {
////    cout<<"mX.n_rows="<<x_mat.n_rows<<endl;
////    cout<<"mX_ho.n_rows="<<x_ho_mat.n_rows<<endl;
//    HDP_var_ss::densityEst(np2mat<uint32_t>(x),np2mat<uint32_t>(x_ho),kappa,K,T,S);
//    return true;
//  }
//
//  /*
//   * after an initial densitiy estimate has been made using addDoc() and densityEst()
//   * can use this to update the estimate with information from additional x
//   */
//  bool updateEst(const numeric::array& x, double kappa)
//  {
//    return HDP_var_ss::updateEst(np2row<uint32_t>(x),kappa);
//  }
//
//};


/*
 * OperatorConstraint.cpp
 * @brief OperatorConstraint constraint
 * @date Apr 3, 2022
 * @author Yoonwoo Kim
 */

#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <cpp/planning/OperatorConstraint.h>
#include <cpp/planning/MultiValueConstraint.h>
#include <cpp/planning/NullConstraint.h>

#include <boost/make_shared.hpp>
using namespace gtsam;
using namespace std;

namespace gtsam_planner {

/* ************************************************************************* */
OperatorConstraint::OperatorConstraint(const DiscreteKeys& multi_keys,
  const vector<size_t>& values, const DiscreteKeys& null_keys, const DiscreteKeys& dkeys)
  : DiscreteFactor(dkeys.indices()) {
  multi_ = multi_keys;
  values_ = values;
  null_ = null_keys;
  dkeys_ = dkeys;
  // multi_constraint_ = MultiValueConstraint(multi_keys, values);
  // null_constraint_ = NullOperatorConstraint(null_keys);

}

/* ************************************************************************* */
void OperatorConstraint::print(const string& s, const KeyFormatter& formatter) const {
  cout << s << "OperatorConstraint on ";
  for (DiscreteKey dkey : dkeys_) cout << formatter(dkey.first) << " ";
  cout << endl;
}

/* ************************************************************************* */
DiscreteKeys OperatorConstraint::discreteKeys() const {
  return dkeys_;
}

/* ************************************************************************* */
double OperatorConstraint::operator()(const DiscreteValues& values) const {
  MultiValueConstraint multi_constraint(multi_, values_);
  NullConstraint null_constraint(null_);
  return multi_constraint(values) && null_constraint(values);
}

/* ************************************************************************* */
DecisionTreeFactor OperatorConstraint::toDecisionTreeFactor() const {
  MultiValueConstraint multi_constraint(multi_, values_);
  NullConstraint null_constraint(null_);
  return multi_constraint.toDecisionTreeFactor() * null_constraint.toDecisionTreeFactor();
}

/* ************************************************************************* */
DecisionTreeFactor OperatorConstraint::operator*(const DecisionTreeFactor& f) const {
  // TODO: can we do this more efficiently?
  return toDecisionTreeFactor() * f;
}

}  // namespace gtsam_planner


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

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

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

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_ASSIGN_HPP
#define BOOST_GEOMETRY_ALGORITHMS_ASSIGN_HPP

#include <cstddef>

#include <boost/concept/requires.hpp>
#include <boost/concept_check.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/if.hpp>
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/type_traits.hpp>

#include <boost/geometry/algorithms/detail/assign_values.hpp>
#include <boost/geometry/algorithms/convert.hpp>

#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/algorithms/append.hpp>
#include <boost/geometry/algorithms/clear.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/util/for_each_coordinate.hpp>

namespace boost { namespace geometry
{

/*!
\brief Assign a range of points to a linestring, ring or polygon
\note The point-type of the range might be different from the point-type of the geometry
\ingroup assign
\tparam Geometry \tparam_geometry
\tparam Range \tparam_range_point
\param geometry \param_geometry
\param range \param_range_point

\qbk{
[heading Notes]
[note Assign automatically clears the geometry before assigning (use append if you don't want that)]
[heading Example]
[assign_points] [assign_points_output]

[heading See also]
\* [link geometry.reference.algorithms.append append]
}
 */
template <typename Geometry, typename Range>
inline void assign_points(Geometry& geometry, Range const& range)
{
    concept::check<Geometry>();

    clear(geometry);
    geometry::append(geometry, range, -1, 0);
}

/*!
\brief assign to a box inverse infinite
\details The assign_inverse function initialize a 2D or 3D box with large coordinates, the
min corner is very large, the max corner is very small. This is a convenient starting point to
collect the minimum bounding box of a geometry.
\ingroup assign
\tparam Geometry \tparam_geometry
\param geometry \param_geometry

\qbk{
[heading Example]
[assign_inverse] [assign_inverse_output]

[heading See also]
\* [link geometry.reference.algorithms.make.make_inverse make_inverse]
}
 */
template <typename Geometry>
inline void assign_inverse(Geometry& geometry)
{
    concept::check<Geometry>();

    dispatch::assign_inverse
        <
            typename tag<Geometry>::type,
            Geometry
        >::apply(geometry);
}

/*!
\brief assign zero values to a box, point
\ingroup assign
\details The assign_zero function initializes a 2D or 3D point or box with coordinates of zero
\tparam Geometry \tparam_geometry
\param geometry \param_geometry

 */
template <typename Geometry>
inline void assign_zero(Geometry& geometry)
{
    concept::check<Geometry>();

    dispatch::assign_zero
        <
            typename tag<Geometry>::type,
            Geometry
        >::apply(geometry);
}

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{
// Note: this is moved to namespace detail because the names and parameter orders
// are not yet 100% clear.

/*!
\brief Assign the four points of a 2D box
\ingroup assign
\note The order is crucial. Most logical is LOWER, UPPER and sub-order LEFT, RIGHT
    so this is how it is implemented.
\tparam Box \tparam_box
\tparam Point \tparam_point
\param box \param_box
\param lower_left point being assigned to lower left coordinates of the box
\param lower_right point being assigned to lower right coordinates of the box
\param upper_left point being assigned to upper left coordinates of the box
\param upper_right point being assigned to upper right coordinates of the box

\qbk{
[heading Example]
[assign_box_corners] [assign_box_corners_output]
}
*/
template <typename Box, typename Point>
inline void assign_box_corners(Box const& box,
        Point& lower_left, Point& lower_right,
        Point& upper_left, Point& upper_right)
{
    concept::check<Box const>();
    concept::check<Point>();

    detail::assign::assign_box_2d_corner
            <min_corner, min_corner>(box, lower_left);
    detail::assign::assign_box_2d_corner
            <max_corner, min_corner>(box, lower_right);
    detail::assign::assign_box_2d_corner
            <min_corner, max_corner>(box, upper_left);
    detail::assign::assign_box_2d_corner
            <max_corner, max_corner>(box, upper_right);
}

template <bool Reverse, typename Box, typename Range>
inline void assign_box_corners_oriented(Box const& box, Range& corners)
{
    if (Reverse)
    {
        // make counterclockwise ll,lr,ur,ul
        assign_box_corners(box, corners[0], corners[1], corners[3], corners[2]);
    }
    else
    {
        // make clockwise ll,ul,ur,lr
        assign_box_corners(box, corners[0], corners[3], corners[1], corners[2]);
    }
}

/*!
\brief Assign a box or segment with the value of a point
\ingroup assign
\tparam Index indicates which box-corner, min_corner (0) or max_corner (1)
    or which point of segment (0/1)
\tparam Point \tparam_point
\tparam Geometry \tparam_box_or_segment
\param point \param_point
\param geometry \param_box_or_segment

\qbk{
[heading Example]
[assign_point_to_index] [assign_point_to_index_output]
}
*/
template <std::size_t Index, typename Geometry, typename Point>
inline void assign_point_to_index(Point const& point, Geometry& geometry)
{
    concept::check<Point const>();
    concept::check<Geometry>();

    detail::assign::assign_point_to_index
        <
            Geometry, Point, Index, 0, dimension<Geometry>::type::value
        >::apply(point, geometry);
}

/*!
\brief Assign a point with a point of a box or segment
\ingroup assign
\tparam Index indicates which box-corner, min_corner (0) or max_corner (1)
    or which point of segment (0/1)
\tparam Geometry \tparam_box_or_segment
\tparam Point \tparam_point
\param geometry \param_box_or_segment
\param point \param_point

\qbk{
[heading Example]
[assign_point_from_index] [assign_point_from_index_output]
}
*/
template <std::size_t Index, typename Point, typename Geometry>
inline void assign_point_from_index(Geometry const& geometry, Point& point)
{
    concept::check<Geometry const>();
    concept::check<Point>();

    detail::assign::assign_point_from_index
        <
            Geometry, Point, Index, 0, dimension<Geometry>::type::value
        >::apply(geometry, point);
}

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Assigns one geometry to another geometry
\details The assign algorithm assigns one geometry, e.g. a BOX, to another geometry, e.g. a RING. This only
if it is possible and applicable.
\ingroup assign
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\param geometry1 \param_geometry (target)
\param geometry2 \param_geometry (source)

\qbk{
[heading Example]
[assign] [assign_output]

[heading See also]
\* [link geometry.reference.algorithms.convert convert]
}
 */
template <typename Geometry1, typename Geometry2>
inline void assign(Geometry1& geometry1, Geometry2 const& geometry2)
{
    concept::check_concepts_and_equal_dimensions<Geometry1, Geometry2 const>();

    dispatch::convert
        <
            typename tag<Geometry2>::type,
            typename tag<Geometry1>::type,
            dimension<Geometry1>::type::value,
            Geometry2,
            Geometry1
        >::apply(geometry2, geometry1);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_ASSIGN_HPP