/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * 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 Autonomous Systems Lab, ETH Zurich 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 Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm 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.
 *
*/
/*
 * GlobalAngularVelocity.hpp
 *
 *  Created on: Jul 25, 2016
 *      Author: Christian Gehring
 */

#pragma once

#include <Eigen/Core>

#include "kindr/math/LinearAlgebra.hpp"
#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/rotations/RotationDiffBase.hpp"
#include "kindr/rotations/Rotation.hpp"
#include "kindr/quaternions/Quaternion.hpp"
#include "kindr/phys_quant/PhysicalQuantities.hpp"

namespace kindr {

/*! \class GlobalAngularVelocity
 * \brief Angular velocity in 3D-space expressed in global coordinates (inertial frame).
 *
 * This class implements an angular velocity of a rigid body in 3D-space expressed in inertial (global) frame.
 *
 * The angular velocity should represent the absolute rotational velocity of a rigid body with respect to an inertial (global) frame I
 * and its coordinates are expressed in the inertial (global) frame (\f$\I_Omega = I_\omega_{IB}$\f)
 *
 * \tparam PrimType_  Primitive type of the coordinates.
 * \ingroup rotations
 */
template<typename PrimType_>
class GlobalAngularVelocity : public RotationDiffBase<GlobalAngularVelocity<PrimType_>>, public AngularVelocity<PrimType_, 3> {
 private:
   /*! \brief The base type.
    */
   typedef AngularVelocity<PrimType_, 3> Base;
  public:
   /*! \brief The implementation type.
    *
    *  The implementation type is always an Eigen object.
    */
   typedef typename Base::Implementation Implementation;

   /*! \brief The primitive type of the velocities.
    */
   typedef PrimType_ Scalar;

   /*! \brief Default constructor initializes all velocities with zero.
    */
   GlobalAngularVelocity()
     : Base(Base::Zero()) {
   }

   /*! Constructor with three components (x,y,z)
    * \param x   x-coordinate expressed in inertial (global) frame
    * \param y   y-coordinate expressed in inertial (global) frame
    * \param z   z-coordinate expressed in inertial (global) frame
    */
   GlobalAngularVelocity(Scalar x, Scalar y, Scalar z)
     : Base(x, y, z) {
   }

   /*! \brief Constructor using vector class.
    *  \param other   AngularVelocity<PrimType_, 3>
    */
   explicit GlobalAngularVelocity(const Base& other)
     : Base(other) {
    }

   /*! Constructor with three components (x,y,z)
    * \param vector  Eigen::Matrix<Scalar,3,1>
    */
   GlobalAngularVelocity(const Implementation & vector)
     : Base(vector) {
   }

   /*! Constructor with a time derivative of a rotation with a different parameterization
    *
    * \param rotation    rotation the time derivative is taken at
    * \param other       other time derivative
    */
   template<typename RotationDerived_, typename OtherDerived_>
   inline explicit GlobalAngularVelocity(const RotationBase<RotationDerived_>& rotation, const RotationDiffBase<OtherDerived_>& other)
     : Base(internal::RotationDiffConversionTraits<GlobalAngularVelocity, OtherDerived_, RotationDerived_>::convert(rotation.derived(), other.derived())){
   }

   inline Implementation vector() const {
     return Base::toImplementation();
   }

   inline Implementation& vector() {
     return Base::toImplementation();
   }

   inline Implementation& toImplementation() {
     return Base::toImplementation();
   }

   inline const Implementation& toImplementation() const {
     return Base::toImplementation();
   }

   /*! \brief Cast to the base type.
    *  \returns the base (recommended only for advanced users)
    */
   inline Base& toBase() {
     return static_cast<Base&>(*this);
   }

   /*! \brief Cast to the base type.
    *  \returns the base (recommended only for advanced users)
    */
   inline const Base& toBase() const {
     return static_cast<const Base&>(*this);
   }

   /*! \brief Addition of two angular velocities.
    */
   template<typename Other_>
   GlobalAngularVelocity operator +(const Other_& other) const {
     return GlobalAngularVelocity(this->toBase() + other.toBase());
   }

   /*! \brief Subtraction of two angular velocities.
    */
   template<typename Other_>
   GlobalAngularVelocity operator -(const Other_& other) const {
     return GlobalAngularVelocity(this->toBase() - other.toBase());
   }

   /*! \brief Multiplication of an angular velocity with a scalar.
    */
   template<typename PrimTypeFactor_>
   GlobalAngularVelocity operator *(const PrimTypeFactor_& factor) const {
     return GlobalAngularVelocity(this->toBase()*factor);
   }

   /*! \brief Addition of two angular velocities.
    * \param other   other angular velocity
    */
   template<typename Other_>
   GlobalAngularVelocity& operator +=(const Other_& other) {
     this->toBase() += other.toBase();
     return *this;
   }

   /*! \brief Subtraction of two angular velocities.
    * \param other   other angular velocity
    */
   template<typename Other_>
   GlobalAngularVelocity& operator -=(const Other_& other) {
     this->toBase() -= other.toBase();
     return *this;
   }

   /*! \brief Sets all components of the angular velocity to zero.
    * \returns reference
    */
   GlobalAngularVelocity& setZero() {
     Base::setZero();
     return *this;
   }

   /*! \brief Get zero element.
    * \returns zero element
    */
   static GlobalAngularVelocity Zero() {
     return GlobalAngularVelocity(Implementation::Zero());
   }
 };

/*! \brief Multiplication of an angular velocity with a scalar.
 */
template<typename PrimType_, typename PrimTypeFactor_>
GlobalAngularVelocity<PrimType_> operator *(PrimTypeFactor_ factor, const GlobalAngularVelocity<PrimType_>& globalAngularVelocity) {
  return GlobalAngularVelocity<PrimType_>(globalAngularVelocity.toBase()*factor);
}

//! \brief 3D global angular velocity with primitive type double
typedef GlobalAngularVelocity<double>  GlobalAngularVelocityD;
//! \brief 3D global angular velocity with primitive type float
typedef GlobalAngularVelocity<float>  GlobalAngularVelocityF;

namespace internal {

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Conversion Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_, typename Rotation_>
class RotationDiffConversionTraits<GlobalAngularVelocity<PrimType_>, LocalAngularVelocity<PrimType_>, Rotation_> {
 public:
  inline static GlobalAngularVelocity<PrimType_> convert(const Rotation_& rotationLocalToGlobal, const LocalAngularVelocity<PrimType_>& localAngularVelocity) {
    return GlobalAngularVelocity<PrimType_>(rotationLocalToGlobal.rotate(localAngularVelocity.toImplementation()));
  }
};

template<typename PrimType_>
class RotationDiffConversionTraits<GlobalAngularVelocity<PrimType_>, RotationQuaternionDiff<PrimType_>, RotationQuaternion<PrimType_>> {
 public:
  inline static GlobalAngularVelocity<PrimType_> convert(const RotationQuaternion<PrimType_>& rquat, const RotationQuaternionDiff<PrimType_>& rquatdiff) {
    return GlobalAngularVelocity<PrimType_>(2.0*rquat.getGlobalQuaternionDiffMatrix()*rquatdiff.toQuaternion().vector());
  }
};

template<typename PrimType_>
class RotationDiffConversionTraits<GlobalAngularVelocity<PrimType_>, RotationMatrixDiff<PrimType_>, RotationMatrix<PrimType_>> {
 public:
  inline static GlobalAngularVelocity<PrimType_> convert(const RotationMatrix<PrimType_>& rotationMatrix, const RotationMatrixDiff<PrimType_>& rotationMatrixDiff) {
    return GlobalAngularVelocity<PrimType_>(getVectorFromSkewMatrix<PrimType_>(rotationMatrixDiff.matrix()*rotationMatrix.inverted().matrix()));
  }
};

template<typename PrimType_>
class RotationDiffConversionTraits<GlobalAngularVelocity<PrimType_>, EulerAnglesZyxDiff<PrimType_>, EulerAnglesZyx<PrimType_>> {
 public:
  inline static GlobalAngularVelocity<PrimType_> convert(const EulerAnglesZyx<PrimType_>& eulerAngles, const EulerAnglesZyxDiff<PrimType_>& eulerAnglesDiff) {
    return GlobalAngularVelocity<PrimType_>(eulerAngles.getMappingFromDiffToGlobalAngularVelocity()*eulerAnglesDiff.toImplementation());

  }
};

template<typename PrimType_>
class RotationDiffConversionTraits<GlobalAngularVelocity<PrimType_>, EulerAnglesXyzDiff<PrimType_>, EulerAnglesXyz<PrimType_>> {
 public:
  inline static GlobalAngularVelocity<PrimType_> convert(const EulerAnglesXyz<PrimType_>& eulerAngles, const EulerAnglesXyzDiff<PrimType_>& eulerAnglesDiff) {
    return GlobalAngularVelocity<PrimType_>(eulerAngles.getMappingFromDiffToGlobalAngularVelocity()*eulerAnglesDiff.toImplementation());

  }
};

} // namespace internal
} /* namespace kindr */


// ------------------------------------------------------------
// Copyright (c) Microsoft Corporation.  All rights reserved.
// Licensed under the MIT License (MIT). See License.txt in the repo root for license information.
// ------------------------------------------------------------

#include "stdafx.h"

#include <boost/test/unit_test.hpp>
#include "Common/boost-taef.h"
#include "Common/Parse.h"
#include <stdlib.h>

namespace Common
{
    BOOST_AUTO_TEST_SUITE2(ParseTests)

    BOOST_AUTO_TEST_CASE(Smoke0)
    {
        int value = 101;
        auto retval = StringUtility::TryFromWString(L"0", value);
        VERIFY_IS_TRUE(retval);
        VERIFY_ARE_EQUAL2(value, 0);

        uint uvalue = 101;
        retval = StringUtility::TryFromWString(L"0", uvalue);
        VERIFY_IS_TRUE(retval);
        VERIFY_ARE_EQUAL2(uvalue, 0U);
    }

    BOOST_AUTO_TEST_CASE(Smoke1)
    {
        __int32 i = 5;

        std::string s = "5";

        VERIFY_IS_TRUE(Int32_Parse(s) == i);
    }

    BOOST_AUTO_TEST_CASE(Smoke2)
    {
        VERIFY_IS_TRUE(Int32_Parse("    12348 ") == 12348);
    }

    BOOST_AUTO_TEST_CASE(Smoke3)
    {
        VERIFY_IS_TRUE(Int32_Parse("-42") == -42);
    }

    BOOST_AUTO_TEST_CASE(Smoke4)
    {
        VERIFY_IS_TRUE(Int32_Parse(L"-42   ") == -42);
    }

    BOOST_AUTO_TEST_CASE(Smoke5)
    {
        VERIFY_IS_TRUE(Int32_Parse(std::wstring(L"5")) == 5);
    }

    BOOST_AUTO_TEST_CASE(Smoke6)
    {
        // Expected format: [whitespace] [sign] [digits] [.digits] [ {e | E}[sign]digits]
        VERIFY_ARE_EQUAL2(Double_Parse(std::wstring(L"  3.14")), 3.14);
        VERIFY_ARE_EQUAL2(Double_Parse(std::string("-3.14")), -3.14);

        VERIFY_ARE_EQUAL2(Double_Parse(L"-3.14   "), -3.14);
        VERIFY_ARE_EQUAL2(Double_Parse("3.14 Followed by some string"), 3.14);

        VERIFY_ARE_EQUAL2(Double_Parse(L"3.14e-2"), 3.14e-2);
        VERIFY_ARE_EQUAL2(Double_Parse("3.14e8"), 3.14e8);
        VERIFY_ARE_EQUAL2(Double_Parse("-3.14e3"), -3.14e3);
        VERIFY_ARE_EQUAL2(Double_Parse("-3.14e8"), -3.14e8);
        VERIFY_ARE_EQUAL2(Double_Parse("0"), 0);
        VERIFY_ARE_EQUAL2(Double_Parse("-0"), 0);
        VERIFY_ARE_EQUAL2(Double_Parse("0.0"), 0);
        VERIFY_ARE_EQUAL2(Double_Parse("-0.0"), 0);

        {
            auto value = numeric_limits<double>::min();
            stringstream ss;
            ss << value;

            auto parsed_by_strtod = strtod(ss.str().c_str(), nullptr);
            auto delta = std::abs(value - parsed_by_strtod);
            printf("value = %e, strtod = %e, abs(delta) = %e\n", value, parsed_by_strtod, delta);

            double parsedValue = 0;
            auto success = TryParseDouble(ss.str(), parsedValue);
            VERIFY_IS_TRUE(success);
            delta = std::abs(value - parsedValue);
            printf("value = %e, parsed = %e, abs(delta) = %e\n", value, parsedValue, delta);
            VERIFY_IS_TRUE(delta <= 1e-300);
        }

        {
            auto value = numeric_limits<double>::lowest();
            stringstream ss;
            ss << value;

            auto parsed_by_strtod = strtod(ss.str().c_str(), nullptr);
            auto delta = std::abs(value - parsed_by_strtod);
            printf("value = %e, strtod = %e, abs(delta) = %e\n", value, parsed_by_strtod, delta);

            double parsedValue = 0;
            auto success = TryParseDouble(ss.str(), parsedValue);
            VERIFY_IS_TRUE(success);

            delta = std::abs(value - parsedValue);
            printf("value = %e, parsed = %e, abs(delta) = %e\n", value, parsedValue, delta);
            VERIFY_IS_TRUE(delta <= 0.00001*std::abs(value));

            VERIFY_ARE_EQUAL2(parsedValue, parsed_by_strtod);
        }

        {
            auto value =  numeric_limits<double>::max();
            stringstream ss;
            ss << value;

            auto parsed_by_strtod = strtod(ss.str().c_str(), nullptr);
            auto delta = std::abs(value - parsed_by_strtod);
            printf("value = %e, strtod = %e, abs(delta) = %e\n", value, parsed_by_strtod, delta);

            double parsedValue = 0;
            auto success = TryParseDouble(ss.str(), parsedValue);
            VERIFY_IS_TRUE(success);

            delta = std::abs(value - parsedValue);
            printf("value = %e, parsed = %e, abs(delta) = %e\n", value, parsedValue, delta);
            VERIFY_IS_TRUE(delta <= 0.00001*value);

            VERIFY_ARE_EQUAL2(parsedValue, parsed_by_strtod);
        }

        Random r;
        for (int i = 0; i < 10; ++i)
        {
            auto value = r.NextDouble() * r.Next(numeric_limits<int>::max());
            stringstream ss;
            ss << value;

            auto parsed_by_strtod = strtod(ss.str().c_str(), nullptr);
            auto delta = std::abs(value - parsed_by_strtod);
            printf("value = %e, strtod = %e, abs(delta) = %e\n", value, parsed_by_strtod, delta);

            double parsedValue = 0;
            auto success = TryParseDouble(ss.str(), parsedValue);
            VERIFY_IS_TRUE(success);

            delta = std::abs(value - parsedValue);
            printf("value = %e, parsed = %e, abs(delta) = %e\n", value, parsedValue, delta);
            VERIFY_IS_TRUE(delta <= 0.00001*std::abs(value));

            VERIFY_ARE_EQUAL2(parsedValue, parsed_by_strtod);
        }
    }

    BOOST_AUTO_TEST_CASE(Smoke7)
    {
        std::string sMin;
        StringWriterA(sMin).Write("{0}", (std::numeric_limits<__int64>::min()));

        std::string sMax;
        StringWriterA(sMax).Write("{0}", (std::numeric_limits<__int64>::max()));

        VERIFY_IS_TRUE(Int64_Parse(sMin) == std::numeric_limits<__int64>::min());
        VERIFY_IS_TRUE(Int64_Parse(sMax) == std::numeric_limits<__int64>::max());
        VERIFY_IS_TRUE(Int64_Parse("2767011611056432742") == 2767011611056432742);
        VERIFY_ARE_EQUAL2(Int64_Parse("0"), 0);
        VERIFY_ARE_EQUAL2(Int64_Parse("0x0"), 0);
        VERIFY_ARE_EQUAL2(Int64_Parse("0x123456789a"), 0x123456789a);
        VERIFY_ARE_EQUAL2(Int64_Parse("0x0123456789a"), 0x123456789a);
        VERIFY_ARE_EQUAL2(Int64_Parse("0x000123456789a"), 0x123456789a);

        int64 result = 0;
        auto success = TryParseInt64("123456", result, 16);
        VERIFY_IS_TRUE(success);
        VERIFY_ARE_EQUAL2(result, 0x123456);

        success = TryParseInt64("0xffffffffffffffff", result);
        VERIFY_IS_FALSE(success);

        uint64 uresult = 0;
        success = TryParseUInt64("0xffffffffffffffff", uresult);
        VERIFY_IS_TRUE(success);
        success = TryParseUInt64("ffffffffffffffff", uresult, 16);
        VERIFY_IS_TRUE(success);

        uint32 uint32Result = 0;
        success = StringUtility::TryFromWString(L"0xffffffff", uint32Result);
        VERIFY_IS_TRUE(success);
        success = StringUtility::TryFromWString(L"ffffffff", uint32Result, 16);
        VERIFY_IS_TRUE(success);
    }

    BOOST_AUTO_TEST_CASE(TryFromWStringError)
    {
        bool b = false;
        auto retval = StringUtility::TryFromWString(L"notbool", b);
        VERIFY_IS_FALSE(retval);

        int i = 0;
        retval = StringUtility::TryFromWString(L"notint", i);
        VERIFY_IS_FALSE(retval);

        retval = StringUtility::TryFromWString(L"0x333", i, 10);
        VERIFY_IS_FALSE(retval);

        unsigned short s = 0;
        retval = StringUtility::TryFromWString(L"notshort", s);
        VERIFY_IS_FALSE(retval);

        retval = StringUtility::TryFromWString(L"999999", s);
        VERIFY_IS_FALSE(retval);

        retval = StringUtility::TryFromWString(L"-1", s);
        VERIFY_IS_FALSE(retval);
    }

    BOOST_AUTO_TEST_CASE(Underflow_Int16)
    {
        std::string s;
        StringWriterA(s).Write("{0}", (std::numeric_limits<__int16>::min() - 1));

        Assert::DisableDebugBreakInThisScope disableDebugBreakInThisScope;
        VERIFY_THROWS(Int16_Parse(s), std::system_error);
    }

    BOOST_AUTO_TEST_CASE(Overflow_Int16)
    {
        std::string s;
        StringWriterA(s).Write("{0}", (std::numeric_limits<__int16>::max() + 1));

        Assert::DisableDebugBreakInThisScope disableDebugBreakInThisScope;
        VERIFY_THROWS(Int16_Parse(s), std::system_error);
    }

    BOOST_AUTO_TEST_CASE(Underflow_Int32)
    {
        std::string s;
        StringWriterA(s).Write("{0}1", (std::numeric_limits<__int32>::min()));

        Assert::DisableDebugBreakInThisScope disableDebugBreakInThisScope;
        VERIFY_THROWS(Int32_Parse(s), std::system_error);
    }

    BOOST_AUTO_TEST_CASE(Overflow_Int32)
    {
        std::string s;
        StringWriterA(s).Write("{0}1", (std::numeric_limits<__int32>::max()));

        Assert::DisableDebugBreakInThisScope disableDebugBreakInThisScope;
        VERIFY_THROWS(Int32_Parse(s), std::system_error);
    }

    BOOST_AUTO_TEST_CASE(Underflow_Int64)
    {
        std::string s;
        StringWriterA(s).Write("{0}1", std::numeric_limits<__int64>::min());

        Assert::DisableDebugBreakInThisScope disableDebugBreakInThisScope;
        VERIFY_THROWS(Int64_Parse(s), std::system_error);
    }

    BOOST_AUTO_TEST_CASE(Overflow_Int64)
    {
        std::string s = "27670116110564327422";  //0X26666,6666,6666,6666*0xa+2

        Assert::DisableDebugBreakInThisScope disableDebugBreakInThisScope;
        VERIFY_THROWS(Int64_Parse(s), std::system_error);
    }

    BOOST_AUTO_TEST_CASE(Underflow_Int16_W)
    {
        std::wstring s;
        StringWriter(s).Write("{0}", (std::numeric_limits<__int16>::min() - 1));

        Assert::DisableDebugBreakInThisScope disableDebugBreakInThisScope;
        VERIFY_THROWS(Int16_Parse(s), std::system_error);
    }

    BOOST_AUTO_TEST_CASE(Overflow_Int16_W)
    {
        std::wstring s;
        StringWriter(s).Write("{0}", (std::numeric_limits<__int16>::max() + 1));

        Assert::DisableDebugBreakInThisScope disableDebugBreakInThisScope;
        VERIFY_THROWS(Int16_Parse(s), std::system_error);
    }

    BOOST_AUTO_TEST_CASE(Underflow_Int32_W)
    {
        std::wstring s;
        StringWriter(s).Write("{0}1", (std::numeric_limits<__int32>::min()));

        Assert::DisableDebugBreakInThisScope disableDebugBreakInThisScope;
        VERIFY_THROWS(Int32_Parse(s), std::system_error);
    }

    BOOST_AUTO_TEST_CASE(Overflow_Int32_W)
    {
        std::wstring s;
        StringWriter(s).Write("{0}1", (std::numeric_limits<__int32>::max()));

        Assert::DisableDebugBreakInThisScope disableDebugBreakInThisScope;
        VERIFY_THROWS(Int32_Parse(s), std::system_error);
    }

    BOOST_AUTO_TEST_CASE(Underflow_Int64_W)
    {
        std::wstring s;
        StringWriter(s).Write("{0}1", std::numeric_limits<__int64>::min());

        Assert::DisableDebugBreakInThisScope disableDebugBreakInThisScope;
        VERIFY_THROWS(Int64_Parse(s), std::system_error);
    }

    BOOST_AUTO_TEST_CASE(Overflow_Int64_W)
    {
        std::wstring s = L"27670116110564327422";  //0X26666,6666,6666,6666*0xa+2

        Assert::DisableDebugBreakInThisScope disableDebugBreakInThisScope;
        VERIFY_THROWS(Int64_Parse(s), std::system_error);
    }

    BOOST_AUTO_TEST_SUITE_END()
};


/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

/*
 * File:   distrubuted_dbh_ingress.hpp
 * Author: Vincent
 *
 * Created on July 19, 2017, 5:56 PM
 */

#ifndef DISTRIBUTED_DBH_INGRESS_HPP
#define DISTRIBUTED_DBH_INGRESS_HPP

#include <boost/functional/hash.hpp>

#include <graphlab/rpc/buffered_exchange.hpp>
#include <graphlab/graph/graph_basic_types.hpp>
#include <graphlab/graph/ingress/distributed_ingress_base.hpp>
#include <graphlab/graph/distributed_graph.hpp>
#include <graphlab/logger/logger.hpp>
#include <vector>
#include <graphlab/graph/graph_hash.hpp>
#include <graphlab/rpc/distributed_event_log.hpp>

#include <graphlab/macros_def.hpp>
namespace graphlab{
  template<typename VertexData, typename EdgeData>
  class distributed_graph;

  /**
   * \brief Ingress object assigning edges using random hash function on
   * vertex with higher degree between source and target
   */
  template<typename VertexData, typename EdgeData>
  class distributed_dbh_ingress :
    public distributed_ingress_base<VertexData, EdgeData> {
  public:
    typedef distributed_graph<VertexData, EdgeData> graph_type;
    /// The type of the vertex data stored in the graph
    typedef VertexData vertex_data_type;
    /// The type of the edge data stored in the graph
    typedef EdgeData   edge_data_type;

    typedef distributed_ingress_base<VertexData, EdgeData> base_type;

    typedef typename base_type::edge_buffer_record edge_buffer_record;
    typedef typename buffered_exchange<edge_buffer_record>::buffer_type
        edge_buffer_type;

    graph_type& graph;

    typedef typename boost::unordered_map<vertex_id_type, size_t> degree_map_type;
    ///Variables for partition alg

    //store the degrees of each vertex
    degree_map_type degree_map;

    typedef typename std::pair<vertex_id_type, size_t> vertex_degree_pair;
    buffered_exchange<vertex_degree_pair> degree_exchange;
    typedef typename buffered_exchange<vertex_degree_pair>::buffer_type
        degree_buffer_type;

    //1 edge exchange for first pass, when reading the edges to examine degree
    //the first edge exchange arbitrarily decides proc to send
    //second edge exchange used to add edge to correct proc owner,
    //that is hashing the higher degree vertex between source and target
    buffered_exchange<edge_buffer_record> dbh_edge_exchange;

    //rpc for this class, allows communication between objects through various machines
    dc_dist_object<distributed_dbh_ingress> dbh_rpc;

  public:
    distributed_dbh_ingress(distributed_control& dc, graph_type& graph) :
         base_type(dc, graph),
         degree_exchange(dc),
         dbh_edge_exchange(dc), dbh_rpc(dc, this), graph(graph) {

             dbh_rpc.barrier();
    } // end of constructor

    ~distributed_dbh_ingress() { }

    /** Add an edge to the ingress object using random assignment.
     * This is the first pass for format SNAP, used only to count the degree of each vertex.
     * The procid for the edge is arbitrary in this function, so hashing the source will be used.
     *  */
    void add_edge(vertex_id_type source, vertex_id_type target,
                  const EdgeData& edata) {
      procid_t procid;
//    procid_t l_procid = dbh_rpc.procid();
//    std::cout << "Reading " << source << " to " << target << " on machine " << l_procid << std::endl;

      //increase degree of source
      //degrees are stored/accumulated on each machine locally, then sent out in finalize
      if(degree_map.find(source) != degree_map.end()) {
          ++degree_map[source];
      }
      else degree_map.emplace(source, 1);

/*
      typename degree_map_type::iterator degree_it = degree_map.insert(std::make_pair(source, 0)).first;
      degree_it->second = degree_it->second + 1;
  */
      procid = graph_hash::hash_vertex(source) % dbh_rpc.numprocs();
      const procid_t owning_proc = procid;
      const edge_buffer_record record(source, target, edata);
      dbh_edge_exchange.send(owning_proc, record);
    } // end of add edge

    /** Assign the edges from the first passthrough of add_edges to its rightful procid
     * Add an edge to the ingress object by hashing the source or target vid
     * chosen by taking the vertex with higher degree, ties broken by taking source
     *  */
    void assign_edges() {
        graphlab::timer ti;
        size_t nprocs = dbh_rpc.numprocs();
        procid_t l_procid = dbh_rpc.procid();

        edge_buffer_type edge_buffer;
        procid_t proc = -1;
        while(dbh_edge_exchange.recv(proc, edge_buffer)) {
            for(typename edge_buffer_type::iterator it = edge_buffer.begin(); it != edge_buffer.end(); ++it){
                procid_t procid;
                size_t source_degree = 0;
                size_t target_degree = 0;
                if(degree_map.find(it->source) != degree_map.end())
                    source_degree = degree_map.at(it->source);
                if(degree_map.find(it->target) != degree_map.end())
                    target_degree = degree_map.at(it->target);
                if(source_degree < target_degree)
                    procid = graph_hash::hash_vertex(it->source) % dbh_rpc.numprocs();
                else procid = graph_hash::hash_vertex(it->target) % dbh_rpc.numprocs();

                const procid_t owning_proc = procid;
                const edge_buffer_record record(it->source, it->target, it->edata);
                base_type::edge_exchange.send(owning_proc, record);

            }
        }
        std::cout << "Finished Assigning Edges" <<std::endl;
        dbh_rpc.barrier();
	dbh_edge_exchange.clear();
    } //end of assign_edges

    //finalize will need to combine the degree exchanges
    void finalize() {
      graphlab::timer ti;

      size_t nprocs = dbh_rpc.numprocs();
      procid_t l_procid = dbh_rpc.procid();

      dbh_rpc.full_barrier();

      if(l_procid == 0) {
        memory_info::log_usage("start finalizing");
        logstream(LOG_EMPH) << "DBH finalizing ..."
                            << " #vertices=" << graph.local_graph.num_vertices()
                            << " #edges=" << graph.local_graph.num_edges()
                            << std::endl;
      }
      /**************************************************************************/
      /*                                                                        */
      /*                        Flush additional data                           */
      /*                                                                        */
      /**************************************************************************/

      dbh_edge_exchange.flush();
      base_type::edge_exchange.flush();

      /**
       * Fast pass for redundant finalization with no graph changes.
       */
      {
        size_t changed_size = base_type::edge_exchange.size() + dbh_edge_exchange.size();
        dbh_rpc.all_reduce(changed_size);
        if (changed_size == 0) {
          logstream(LOG_INFO) << "Skipping Graph Finalization because no changes happened... (pass 1)" << std::endl;
          return;
        }
      }

      /**************************************************************************/
      /*                                                                        */
      /*                       Manage all degree values                        */
      /*                                                                        */
      /**************************************************************************/
      if(nprocs != 1) {
            if(l_procid == 0) logstream(LOG_INFO) << "Collecting Degree Counts" <<std::endl;
            std::cout << "sending degrees to main" << std::endl;
            //send degree_vector values to main machine
            if(l_procid != 0) {
                //send the degree_vector values into the exchange so main computer can handle
                for(typename degree_map_type::iterator it = degree_map.begin(); it != degree_map.end(); ++it){
                    degree_exchange.send(0, std::make_pair(it->first, it->second));
                }
            }
            dbh_rpc.full_barrier();
	    degree_exchange.flush();
//	    std::cout << "summing degrees on main" << std::endl;
            //sum degree values from other machines
            if(l_procid == 0) {
		std::cout << "summing degrees on main" <<std::endl;
                for(procid_t procid = 1; procid < nprocs; procid++) {
			std::cout <<"summing form procid: " << procid <<std::endl;
                    //for each procid, get the contents of its degree exchange
                    degree_buffer_type degree_accum;
                    while(degree_exchange.recv(procid, degree_accum)) {
                        for(typename degree_buffer_type::iterator it = degree_accum.begin(); it != degree_accum.end(); ++it) {
                            if(degree_map.find(it->first) == degree_map.end()) {
                                degree_map.emplace(it->first, it->second);
                            }
                            else degree_map.at(it->first) = degree_map.at(it->first) +  it->second;
                        }
                    }
                }
		degree_exchange.clear();
		std::cout << "send degrees from main" <<std::endl;
                //send the final degree vector to the other machines
		for(procid_t procid = 1; procid < nprocs; ++procid) {
			std::cout << "sending to procid: " << procid << std::endl;
			for(typename degree_map_type::iterator it = degree_map.begin(); it != degree_map.end(); ++it) {
				degree_exchange.send(procid, std::make_pair(it->first, it->second));
			}
		}
            }
            dbh_rpc.barrier();
	    degree_exchange.flush();

            //update degree map for sub machines
            if(l_procid != 0) {
		std::cout << "Updating degree on machine " << l_procid << std::endl;
                procid_t rec_proc = 0;
                degree_buffer_type rec_degree_map;
                while(degree_exchange.recv(rec_proc, rec_degree_map)) {
                    for(typename degree_buffer_type::iterator it = rec_degree_map.begin(); it != rec_degree_map.end(); ++it) {
                    	if(degree_map.find(it->first) == degree_map.end()) {
			      degree_map.emplace(it->first, it->second);
                   	}
                   	else degree_map.at(it->first) = it->second;
                    }
                }

            }
            dbh_rpc.barrier();
	    degree_exchange.clear();
      }

      /**************************************************************************/
      /*                                                                        */
      /*                       Assign Edges                                     */
      /*                                                                        */
      /**************************************************************************/
      assign_edges();
      if(l_procid == 0) {
        logstream(LOG_INFO) << "assign edges: "
                            << ti.current_time()
                            << " secs"
                            << std::endl;
      }

      dbh_rpc.barrier();
      base_type::finalize();

    }//end of finalize
  }; // end of distributed_dbh_ingress
}; // end of namespace graphlab
#include <graphlab/macros_undef.hpp>

#endif /* DISTRIBUTED_DBH_INGRESS_HPP */


#include "precompiled.h"

#define WIN32_LEAN_AND_MEAN
#include <windows.h>

#include <boost/format.hpp>

#include "NetworkReceiverApp.h"

NetworkReceiverApp::NetworkReceiverApp(void)
	:mAnimState2(NULL)
	,mThread(0)
	,mUdpSocket(0)
    ,mCurrentInterpolationTime(0)
    ,mIsMoving(false)
    ,mHasMoved(false)
    ,mCurrentStartPosition(Vector3::ZERO)
    ,mCurrentStartSpeed(Vector3::ZERO)
    ,mCurrentInterpolationStep(0)
{
    mTitle = "Receiver";
    mPathSpline.clear();

}
//------------------------------------------------------------------------------
NetworkReceiverApp::~NetworkReceiverApp(void)
{
	mRunning = false;
	//mThread->join();
}
//------------------------------------------------------------------------------
bool NetworkReceiverApp::frameStarted(const FrameEvent& evt)
{
	if (mAnimState2)
		mAnimState2->addTime(evt.timeSinceLastFrame);

    if (mIsMoving)
    {
        //Vector3 newPos = mPathSpline.interpolate(mCurrentInterpolationTime);
        Vector3 newPos = mPathSpline2.evaluate(mCurrentInterpolationTime);
        mBallNode->setPosition(newPos);
        if ((mCurrentInterpolationTime + (evt.timeSinceLastFrame / 0.5)) < 1.0)
        {
            mNetworkLog->logMessage(boost::str(boost::format("[%d] %f : (%.2f, %.2f, %.2f)")
                                                            % mCurrentInterpolationStep
                                                            % mCurrentInterpolationTime
                                                            % newPos.x % newPos.y % newPos.z));
            mCurrentInterpolationTime += (evt.timeSinceLastFrame / 0.5) ;
            mCurrentInterpolationStep++;
        }
    }

	return true;
}
//------------------------------------------------------------------------------
void NetworkReceiverApp::createScene()
{
	const RenderSystemCapabilities* caps = Root::getSingleton().getRenderSystem()->getCapabilities();
	if (!caps->hasCapability(RSC_VERTEX_PROGRAM) || !(caps->hasCapability(RSC_FRAGMENT_PROGRAM)))
	{
		OGRE_EXCEPT(Exception::ERR_NOT_IMPLEMENTED, "Your card does not support vertex and fragment programs, so cannot "
			"run this demo. Sorry!",
			"createScene");
	}

	Viewport *vp = mWindow->getViewport(0);
	vp->setBackgroundColour(ColourValue(0.7, 0.7, 0.7));

	mRoot->addFrameListener(this);

	mSceneMgr->setNormaliseNormalsOnScale(true);

	_createAxes(5);
	_createGrid(5);

	mCamera->setPosition(Vector3(100, 100, 100));
	mCamera->lookAt(Vector3::ZERO);

	_createLight();

	mBallNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("Ball Node");
	Entity *ent = mSceneMgr->createEntity("my ball", "sphere.mesh");
	ent->setMaterialName("Objects/Ball");

	ent->getSubEntity(0)->setMaterialName("Examples/CelShading");

	ent->getSubEntity(0)->setCustomParameter(0, Vector4(10.0f, 0.0f, 0.0f, 0.0f));
	ent->getSubEntity(0)->setCustomParameter(1, Vector4(0.0f, 0.5f, 0.0f, 1.0f));
	ent->getSubEntity(0)->setCustomParameter(2, Vector4(0.3f, 0.5f, 0.3f, 1.0f));

	float w = ent->getBoundingBox().getSize().x;
	float ws = 10.0 / w;

	mBallNode->attachObject(ent);
	mBallNode->setScale(ws, ws, ws);

    mCurrentPath = mSceneMgr->createManualObject("current spline path");
    mSceneMgr->getRootSceneNode()->attachObject(mCurrentPath);

    mControlPoints.resize(4);
    for(int i=0 ; i<4 ; i++)
    {
        Ogre::Entity *pointSphere;
        mControlPoints[i]= mSceneMgr->getRootSceneNode()->createChildSceneNode("Control Point "+Ogre::StringConverter::toString(i));
        pointSphere = mSceneMgr->createEntity("Control Point "+Ogre::StringConverter::toString(i), "geosphere4500.mesh");

        switch(i)
        {
        case 0:
            pointSphere->setMaterialName("Objects/StartPathPoint");
            break;
        case 1:
            pointSphere->setMaterialName("Objects/StartControlPoint");
            break;
        case 2:
            pointSphere->setMaterialName("Objects/EndPathPoint");
            break;
        case 3:
            pointSphere->setMaterialName("Objects/EndControlPoint");
            break;
        }
        float w = pointSphere->getBoundingBox().getSize().x;
        float ws = 1.0 / w;

        mControlPoints[i]->attachObject(pointSphere);
        mControlPoints[i]->setScale(ws, ws, ws);

    }

    _startThread();

}
//------------------------------------------------------------------------------
void NetworkReceiverApp::_createAxes(int _nUnits)
{
	mGridNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("WorldGrid Node");
	ManualObject *line = mSceneMgr->createManualObject("X Axis");
	int scale = 100;

	line->begin("WorldGrid/XAxis", RenderOperation::OT_LINE_LIST);
	line->position(-_nUnits, 0.0, 0.0);     line->normal(Vector3::UNIT_Y);      line->colour(0.1, 0.0, 0.0);
	line->position( _nUnits, 0.0, 0.0);     line->normal(Vector3::UNIT_Y);      line->colour(1.0, 0.0, 0.0);
	line->end();
	mGridNode->attachObject(line);

	line = mSceneMgr->createManualObject("Y Axis");
	line->begin("WorldGrid/YAxis", RenderOperation::OT_LINE_LIST);
	line->position(0.0, -_nUnits, 0.0);     line->normal(Vector3::UNIT_Y);      line->colour(0.0, 0.1, 0.0);
	line->position(0.0,  _nUnits, 0.0);     line->normal(Vector3::UNIT_Y);      line->colour(0.0, 1.0, 0.0);
	line->end();
	mGridNode->attachObject(line);

	line = mSceneMgr->createManualObject("Z Axis");
	line->begin("WorldGrid/ZAxis", RenderOperation::OT_LINE_LIST);
	line->position( 0.0, 0.0, -_nUnits);     line->normal(Vector3::UNIT_Y);      line->colour(0.0, 0.0, 0.1);
	line->position( 0.0, 0.0,  _nUnits);     line->normal(Vector3::UNIT_Y);      line->colour(0.0, 0.0, 1.0);
	line->end();
	mGridNode->attachObject(line);

	mGridNode->scale(scale, scale, scale);

}
//------------------------------------------------------------------------------
void NetworkReceiverApp::_createGrid(int _nUnits)
{
	float step = 0.1;
	int nUnits = _nUnits;

	ManualObject *grid = mSceneMgr->createManualObject("Grid Lines");
	grid->begin("WorldGrid/Lines", RenderOperation::OT_LINE_LIST);

	grid->colour(0.5, 0.5, 0.5);

	for(int i=1 ; i <= int(nUnits * (1.0/step)) ; i++)
	{
		float offset=i*step;

		// horizontal (parallel to X axis)

		grid->position( -nUnits, 0.0, offset);   grid->normal(Vector3::UNIT_Y);
		grid->position(  nUnits, 0.0, offset);   grid->normal(Vector3::UNIT_Y);

		grid->position( -nUnits, 0.0, -offset);   grid->normal(Vector3::UNIT_Y);
		grid->position(  nUnits, 0.0, -offset);   grid->normal(Vector3::UNIT_Y);

		// vertical (parallel to Z axis)
		grid->position( offset, 0.0, -nUnits);   grid->normal(Vector3::UNIT_Y);
		grid->position( offset, 0.0,  nUnits);   grid->normal(Vector3::UNIT_Y);

		grid->position( -offset, 0.0, -nUnits);   grid->normal(Vector3::UNIT_Y);
		grid->position( -offset, 0.0,  nUnits);   grid->normal(Vector3::UNIT_Y);

	}

	grid->end();

	mGridNode->attachObject(grid);

}
//------------------------------------------------------------------------------
void NetworkReceiverApp::_createLight()
{
	Light *light = mSceneMgr->createLight("Main Light");
	light->setType(Light::LT_POINT);
	light->setPosition(Vector3(0, 100, 0));
	light->setSpecularColour(ColourValue::White);
	light->setDiffuseColour(ColourValue::White);

	mLightNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("light node");
	mLightNode->attachObject(light);

	Animation *anim = mSceneMgr->createAnimation("light track", 4);
	anim->setDefaultInterpolationMode(Animation::IM_SPLINE);

	NodeAnimationTrack *track = anim->createNodeTrack(0, mLightNode);
	TransformKeyFrame *key = track->createNodeKeyFrame(0);
	key->setTranslate(Vector3(100, 100, 0));
	key = track->createNodeKeyFrame(1);
	key->setTranslate(Vector3(100, 100, 100));
	key = track->createNodeKeyFrame(2);
	key->setTranslate(Vector3(-100, 100, 0));
	key = track->createNodeKeyFrame(3);
	key->setTranslate(Vector3(-100, 100, -100));
	key = track->createNodeKeyFrame(4);
	key->setTranslate(Vector3(100, 100, 0));

	mAnimState2 = mSceneMgr->createAnimationState("light track");
	mAnimState2->setEnabled(1);
}
//------------------------------------------------------------------------------
void NetworkReceiverApp::_startThread()
{
	mThread = new boost::thread(boost::ref<NetworkReceiverApp>(*this));
}
//------------------------------------------------------------------------------
void NetworkReceiverApp::operator()()
{

	//Timer timer;
	mRunning = true;
	mUdpRemotePoint = udp::endpoint(udp::v4(), 8888);
	mUdpSocket = new udp::socket(mIOService, mUdpRemotePoint);

	mNetworkLog = LogManager::getSingleton().createLog("network_receiver.log");

	while(mRunning)
	{
        _readPosition();
	}
}
//------------------------------------------------------------------------------
void NetworkReceiverApp::_readPosition()
{
	boost::system::error_code ec;
	Vector3 position, speed;

    _readPdu(position, speed, ec);

    boost::format fmt("[received pdu] position (%.2f  %.2f  %.2f)   speed (%.2f  %.2f  %.2f)");
    fmt % position.x % position.y % position.z % speed.x % speed.y % speed.z;
    mNetworkLog->logMessage(fmt.str());

	if (ec)
		mConnected = false;
    else if(!mHasMoved)
    {
        mCurrentTargetPosition = position;
        mCurrentTargetSpeed = speed;
        mHasMoved = true;
    }
    else
    {
        mCurrentStartPosition = mCurrentTargetPosition;
        mCurrentStartSpeed = mCurrentTargetSpeed;
        mCurrentTargetPosition = position;
        mCurrentTargetSpeed = speed;

        //
        //boost::format fmt2("[new path] %s : (%.2f %.2f %.2f) (%.2f %.2f %.2f)");
        //fmt2 % "start" % mCurrentStartPosition.x % mCurrentStartPosition.y % mCurrentStartPosition.z
        //               % mCurrentStartSpeed.x    % mCurrentStartSpeed.y   % mCurrentStartSpeed.z;
        //mNetworkLog->logMessage(fmt2.str());
        //fmt2 % "stop" % mCurrentTargetPosition.x % mCurrentTargetPosition.y % mCurrentTargetPosition.z
        //               % mCurrentTargetSpeed.x    % mCurrentTargetSpeed.y   % mCurrentTargetSpeed.z;
        //mNetworkLog->logMessage(fmt2.str());

        mPathSpline2.setStartPoint(mCurrentStartPosition, mCurrentStartSpeed);
        mPathSpline2.setEndPoint(mCurrentTargetPosition, mCurrentTargetSpeed);
        mPathSpline2.calcControlPoints();

        _predictSplineControlPolygon(mCurrentStartPosition, mCurrentStartSpeed
                                    ,mCurrentTargetPosition, mCurrentTargetSpeed);

        //mPathSpline.clear();

        //mPathSpline.addPoint(mControlPolygon.p1);
        //mPathSpline.addPoint(mControlPolygon.p2);
        //mPathSpline.addPoint(mControlPolygon.p3);
        //mPathSpline.addPoint(mControlPolygon.p4);

        //mPathSpline.recalcTangents();

        mCurrentInterpolationTime = 0;
        mCurrentInterpolationStep=0;
        _redrawControlPolygon();
        mIsMoving = true;
    }
}
//------------------------------------------------------------------------------
void NetworkReceiverApp::_predictSplineControlPolygon(const Vector3 _startPos
                                                     ,const Vector3 _startSpeed
                                                     ,const Vector3 _targetPos
                                                     ,const Vector3 _targetSpeed)
{
    mControlPolygon.p1 = _startPos;
    mControlPolygon.p4 = _targetPos;

    mControlPolygon.p2 = _startPos  + _startSpeed;
    mControlPolygon.p3 = _targetPos - _targetSpeed;

}
//------------------------------------------------------------------------------
void NetworkReceiverApp::_redrawControlPolygon()
{
    //mCurrentPath->clear();
    mCurrentPath->begin("Objects/DynamicLine", Ogre::RenderOperation::OT_LINE_STRIP);
    mCurrentPath->position(mControlPolygon.p1);
    mCurrentPath->position(mControlPolygon.p2);
    mCurrentPath->position(mControlPolygon.p3);
    mCurrentPath->position(mControlPolygon.p4);
    mCurrentPath->end();

    mControlPoints[0]->setPosition(mControlPolygon.p1);
    mControlPoints[1]->setPosition(mControlPolygon.p2);
    mControlPoints[2]->setPosition(mControlPolygon.p3);
    mControlPoints[3]->setPosition(mControlPolygon.p4);

}
//------------------------------------------------------------------------------
void NetworkReceiverApp::_readPdu(Vector3& _oPos, Vector3& _oSpeed, boost::system::error_code &_error)
{
    boost::array<char, 6*sizeof(Real)> buf;
    mUdpSocket->receive_from(boost::asio::buffer(buf), mUdpRemotePoint, 0, _error);

    memcpy(_oPos.ptr(),     buf.c_array(), 3*sizeof(Real));
    memcpy(_oSpeed.ptr(),   buf.c_array()+(3*sizeof(Real)), 3*sizeof(Real));
}
//------------------------------------------------------------------------------
void NetworkReceiverApp::_readFloat(udp::socket&_socket, boost::system::error_code &_error, float &_val)
{
	boost::array<char, 4> buf;
	mUdpSocket->receive_from(boost::asio::buffer(buf), mUdpRemotePoint, 0, _error);

	memcpy(&_val, buf.c_array(), 4);
}
//------------------------------------------------------------------------------


#include <vector>
#include <random>
#include <numeric>
#include <algorithm>
#include <iostream>

#include <boost/mpi.hpp>

namespace mpi = boost::mpi;

namespace tce
{
  auto make_random(int min = 0, int max = 30) {
    std::random_device device{};
    std::mt19937       rand{device()};
    std::uniform_int_distribution<> dist{min, max};
    return dist(rand);
  }
  void print_potato(int potato, int dest)
  {
	std::cout << "Sending potato: " << potato
			  << " to processor #"  << dest
			  << '\n';
  }
}

int main() {
    mpi::environment env{};
    mpi::communicator world{};

	static const int size = world.size();
	static const int rank = world.rank();

    if(world.rank() == 0) {
		auto potato = tce::make_random(0, 100);
		auto dest = tce::make_random(0, size);
		tce::print_potato(potato, dest);
		world.send(dest, 0, potato);
		world.recv(mpi::any_source, 0, potato);
		std::cout << "Received potato on the 0 processor, killing everything\n";
		for(auto i = 1; i < size; ++i) {
			std::cout << "Sending kill message to " << i << '\n';
			world.send(i, 0, -1);
		}
    } else {
		int potato = 0;
		while(true) {
			world.recv(mpi::any_source, 0, potato);
			if(potato < 0) break;
			auto dest = tce::make_random(0, size - 1);
			tce::print_potato(potato, dest);
			world.send(dest, 0, potato);
		}
		std::cout << "Killing process " << rank << '\n';
    }
    return 0;
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::uint8_t, 4, 3> test_uchar_4_3(const Eigen::Matrix<boost::uint8_t, 4, 3> & M)
{
	return M;
}
void export_uchar_4_3()
{
	boost::python::def("test_uchar_4_3",test_uchar_4_3);
}