#ifndef CBCBALPINTERFACE_HPP
#define CBCBALPINTERFACE_HPP

#include "stochasticInput.hpp"
#include "BALPSolverInterface.hpp"
#include "OsiClpSolverInterface.hpp"
#include "CbcModel.hpp"
#include "BA.hpp"

#include <boost/scoped_ptr.hpp>

class CbcBALPInterface : public BALPSolverInterface<CbcBALPInterface> {
public:

	CbcBALPInterface(stochasticInput &in, BAContext &);
	//ClpBALPInterface(const BAData &d, solveType t);
	~CbcBALPInterface() {}

	void writeStatus(const std::string &filebase);
	void loadStatus(const std::string &filebase);

	void go();

	void setPrimalTolerance(double val) { assert(0); }
	void setDualTolerance(double val) { assert(0); }
	void setDualObjectiveLimit(double val) { assert(0); }
	double getObjective() const { return cbcm->getObjValue(); }
	solverState getStatus() const;

	std::vector<double> getFirstStagePrimalColSolution() const;
	std::vector<double> getSecondStagePrimalColSolution(int scen) const;
	std::vector<double> getFirstStageDualColSolution() const;
	std::vector<double> getSecondStageDualColSolution(int scen) const;

	void setFirstStageColState(int idx,variableState);
	void setFirstStageRowState(int idx,variableState);
	void setSecondStageColState(int scen, int idx,variableState);
	void setSecondStageRowState(int scen, int idx,variableState);
	void commitStates() {}

	variableState getFirstStageColState(int idx) const;
	variableState getFirstStageRowState(int idx) const;
	variableState getSecondStageColState(int scen, int idx) const;
	variableState getSecondStageRowState(int scen, int idx) const;

	// add row that preserves block-angular structure
	// e.g. lb <= elts1*x + elts2*y_i <= ub
	void addRow(const std::vector<double>& elts1, const std::vector<double> &elts2, int scen, double lb = -COIN_DBL_MAX, double ub = COIN_DBL_MAX);

	void setFirstStageColLB(int idx, double newLb);
	void setFirstStageColUB(int idx, double newUb);

protected:
	OsiClpSolverInterface model;

	BADimensions dims;
	boost::scoped_ptr<CbcModel> cbcm;

	const BADimensions& getDims() const { return dims; }

};

#endif


/*
Copyright 2016 Australian Centre for Robotic Vision
*/

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>

#include <shape_msgs/Mesh.h>

#include <Eigen/Core>

#include <moveit/move_group_interface/move_group.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>

#include <cv_bridge/cv_bridge.h>
#include <pcl_ros/point_cloud.h>

#include <string>
#include <vector>
#include <iostream>

#include "geometric_shapes/shapes.h"
#include "geometric_shapes/mesh_operations.h"
#include "geometric_shapes/shape_operations.h"

static const double PUBLISH_UPDATE_FREQUENCY = 1.0;

int main(int argc, char** argv) {
    ros::init(argc, argv, "test_publish_stl");
    ros::NodeHandle nh;

    // ---------------- Shelf CAD Model ---------------------------------

    ros::Publisher collision_object_publisher =
        nh.advertise<moveit_msgs::CollisionObject>("/collision_object", 0);

    moveit_msgs::CollisionObject collision_object;

    collision_object.header.frame_id = "/kinect2_link";
    collision_object.header.stamp = ros::Time::now();
    collision_object.id = "shelf";
    shapes::Mesh* m = shapes::createMeshFromResource(
        "package://apc_3d_vision/models/pod_lowres.stl");

    shapes::ShapeMsg co_mesh_msg;
    shapes::constructMsgFromShape(m, co_mesh_msg);

    shape_msgs::Mesh co_mesh;
    co_mesh = boost::get<shape_msgs::Mesh>(co_mesh_msg);

    geometry_msgs::Pose shelf_pose;
    Eigen::Matrix3f shelf_rotation;
    shelf_rotation <<
          0.999291,  0.025606, 0.0275546,
        0.00483011, -0.813819,  0.581096,
          0.037304, -0.580552, -0.813366;
    Eigen::Quaternionf shelf_rotation_quaternion(shelf_rotation);
    shelf_pose.position.x = -0.10305;
    shelf_pose.position.y =  0.80056;
    shelf_pose.position.z =   2.1014;
    shelf_pose.orientation.x = shelf_rotation_quaternion.x();
    shelf_pose.orientation.y = shelf_rotation_quaternion.y();
    shelf_pose.orientation.z = shelf_rotation_quaternion.z();
    shelf_pose.orientation.w = shelf_rotation_quaternion.w();

    // shelf_rotation_quaternion = Eigen::Matrix3f(shelf_rotation);

   //   0.999291  0.025606 0.0275546 -0.103059
   // 0.00483011 -0.813819  0.581096  0.800566
   //   0.037304 -0.580552 -0.813366   2.10149
   //         0         0         0         1

    collision_object.meshes.push_back(co_mesh);
    collision_object.mesh_poses.push_back(shelf_pose);
    collision_object.operation = collision_object.ADD;

    // ------------------- BOX PRIMITIVE ----------------------------------

    // moveit_msgs::CollisionObject collision_object;
    // collision_object.header.frame_id = "/right_wrist";
    // collision_object.header.stamp = ros::Time::now();
    //
    // /* The id of the object is used to identify it. */
    // collision_object.id = "box1";
    //
    // /* Define a box to add to the world. */
    // shape_msgs::SolidPrimitive primitive;
    // primitive.type = primitive.BOX;
    // primitive.dimensions.resize(3);
    // primitive.dimensions[0] = 1.4;
    // primitive.dimensions[1] = 1.1;
    // primitive.dimensions[2] = 1.4;
    //
    // /* A pose for the box (specified relative to frame_id) */
    // geometry_msgs::Pose box_pose;
    // box_pose.orientation.w = 1.0;
    // box_pose.position.x =  0.6;
    // box_pose.position.y = -0.4;
    // box_pose.position.z =  1.2;
    //
    // collision_object.primitives.push_back(primitive);
    // collision_object.primitive_poses.push_back(box_pose);
    // collision_object.operation = collision_object.ADD;

    // ---------------- Adding the collision object into the planning scene
    // static const std::string ROBOT_DESCRIPTION = "robot_description";
    // planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_(
    //     new planning_scene_monitor::PlanningSceneMonitor(ROBOT_DESCRIPTION));
    // ros::spinOnce();
    // ros::Duration(1).sleep();
    // // static const std::string PLANNING_SCENE_TOPIC = "planning_scene";
    // static const std::string PLANNING_SCENE_TOPIC =
    //     "/move_group/monitored_planning_scene";
    // std::string planning_scene_topic_ = PLANNING_SCENE_TOPIC;
    //
    // if (planning_scene_monitor_->getPlanningScene()) {
    //     // Optional monitors to start:
    //     // planning_scene_monitor_->startWorldGeometryMonitor();
    //     // planning_scene_monitor_->startSceneMonitor(
    //     //     "/move_group/monitored_planning_scene");
    //     // planning_scene_monitor_->startStateMonitor(
    //     //     "/joint_states", "/attached_collision_object");
    //
    //     planning_scene_monitor_->startPublishingPlanningScene(
    //         planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
    //         planning_scene_topic_);
    //
    //     // planning_scene_monitor_->getPlanningScene()->setName(
    //     //     "visual_tools_scene");
    //     planning_scene_monitor::LockedPlanningSceneRW scene(
    //         planning_scene_monitor_);
    //
    //     // hack to prevent bad transforms
    //     scene->getCurrentStateNonConst().update();
    //     collision_object.header.stamp = ros::Time::now();
    //     scene->processCollisionObjectMsg(collision_object);
    //     ros::spinOnce();
    //     ros::Duration(1).sleep();
    //     planning_scene_monitor_->triggerSceneUpdateEvent(
    //         planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE);
    //     ros::spinOnce();
    //     std::cout << "Yoooooooo Good\n";
    //     // scene->setObjectColor(msg.id, getColor(color));
    // } else {
    //     std::cout << "Yooooooooo Fail\n";
    //     // ROS_ERROR_STREAM_NAMED(name_, "Planning scene not configured");
    // }

    // ----------------

    while (nh.ok()) {
        collision_object.header.stamp = ros::Time::now();
        collision_object_publisher.publish(collision_object);
        ros::spinOnce();
        ros::Duration(PUBLISH_UPDATE_FREQUENCY).sleep();
    }

    return 0;
}


// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
// 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 ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: Johannes L. Schoenberger (jsch at inf.ethz.ch)

#define TEST_NAME "base/triangulation"
#include "util/testing.h"

#include <Eigen/Core>

#include "base/similarity_transform.h"
#include "base/triangulation.h"

using namespace colmap;

BOOST_AUTO_TEST_CASE(TestTriangulatePoint) {
  std::vector<Eigen::Vector3d> points3D(6);
  points3D[0] = Eigen::Vector3d(0, 0.1, 0.1);
  points3D[1] = Eigen::Vector3d(0, 1, 3);
  points3D[2] = Eigen::Vector3d(0, 1, 2);
  points3D[3] = Eigen::Vector3d(0.01, 0.2, 3);
  points3D[4] = Eigen::Vector3d(-1, 0.1, 1);
  points3D[5] = Eigen::Vector3d(0.1, 0.1, 0.2);

  Eigen::Matrix3x4d proj_matrix1 = Eigen::MatrixXd::Identity(3, 4);

  for (double qz = 0; qz < 1; qz += 0.2) {
    for (double tx = 0; tx < 10; tx += 2) {
      SimilarityTransform3 tform(1, Eigen::Vector4d(0.2, 0.3, 0.4, qz),
                                 Eigen::Vector3d(tx, 2, 3));

      Eigen::Matrix3x4d proj_matrix2 = tform.Matrix().topLeftCorner<3, 4>();

      for (size_t i = 0; i < points3D.size(); ++i) {
        const Eigen::Vector3d& point3D = points3D[i];
        const Eigen::Vector4d point3D1(point3D(0), point3D(1), point3D(2), 1);
        Eigen::Vector3d point2D1 = proj_matrix1 * point3D1;
        Eigen::Vector3d point2D2 = proj_matrix2 * point3D1;
        point2D1 /= point2D1(2);
        point2D2 /= point2D2(2);

        const Eigen::Vector2d point2D1_N(point2D1(0), point2D1(1));
        const Eigen::Vector2d point2D2_N(point2D2(0), point2D2(1));

        const Eigen::Vector3d tri_point3D = TriangulatePoint(
            proj_matrix1, proj_matrix2, point2D1_N, point2D2_N);

        BOOST_CHECK((point3D - tri_point3D).norm() < 1e-10);
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(TestCalculateTriangulationAngle) {
  const Eigen::Vector3d tvec1(0, 0, 0);
  const Eigen::Vector3d tvec2(0, 1, 0);

  BOOST_CHECK_CLOSE(
      CalculateTriangulationAngle(tvec1, tvec2, Eigen::Vector3d(0, 0, 100)),
      0.009999666687, 1e-8);
  BOOST_CHECK_CLOSE(
      CalculateTriangulationAngle(tvec1, tvec2, Eigen::Vector3d(0, 0, 50)),
      0.019997333973, 1e-8);
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_MEMORY_FUNCTIONS_SIMD_VMX_ALTIVEC_SPLAT_HPP_INCLUDED
#define BOOST_SIMD_MEMORY_FUNCTIONS_SIMD_VMX_ALTIVEC_SPLAT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_VMX_SUPPORT

#include <boost/simd/memory/functions/splat.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( splat_
                                    , boost::simd::tag::vmx_, (A0)(A1)
                                    , (scalar_< fundamental_<A0> >)
                                      ((target_ < simd_ < arithmetic_<A1>
                                                        ,boost::simd::tag::vmx_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename A1::type result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1&) const
    {
      result_type v;
      v[0] = a0;
      return vec_splat(v(), 0);
    }
  };
} } }

#endif
#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2019 Aleksey Moskvin <zerg1996@yandex.ru>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_ACCUMULATORS_CODEC_HPP
#define CRYPTO3_ACCUMULATORS_CODEC_HPP

#include <boost/parameter/value_type.hpp>

#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>

#include <boost/container/static_vector.hpp>

#include <boost/range/algorithm.hpp>

#include <nil/crypto3/detail/make_array.hpp>
#include <nil/crypto3/detail/digest.hpp>

#include <nil/crypto3/codec/accumulators/parameters/bits.hpp>

namespace nil {
    namespace crypto3 {
        namespace accumulators {
            template<typename CodecMode>
            struct accumulator_mode {
                typedef CodecMode codec_mode_type;
            };

            template<typename CodecMode>
            struct preprocessing_accumulator_mode : public accumulator_mode<CodecMode> {
                typedef typename accumulator_mode<CodecMode>::codec_mode_type codec_mode_type;
            };

            template<typename CodecMode>
            struct postprocessing_accumulator_mode : public accumulator_mode<CodecMode> {
                typedef typename accumulator_mode<CodecMode>::codec_mode_type codec_mode_type;
            };

            namespace impl {
                template<typename CodecMode, typename AccumulatorMode>
                struct codec_impl : boost::accumulators::accumulator_base {
                    typedef CodecMode codec_mode_type;
                    typedef AccumulatorMode accumulator_mode_type;
                };

                template<typename CodecMode>
                struct codec_impl<CodecMode, preprocessing_accumulator_mode<CodecMode>>
                    : boost::accumulators::accumulator_base {
                protected:
                    typedef CodecMode codec_mode_type;
                    typedef preprocessing_accumulator_mode<CodecMode> accumulator_mode_type;

                    typedef typename codec_mode_type::finalizer_type finalizer_type;
                    typedef typename codec_mode_type::preprocessor_type preprocessor_type;

                    constexpr static const std::size_t input_block_bits = codec_mode_type::input_block_bits;
                    constexpr static const std::size_t input_block_values = codec_mode_type::input_block_values;
                    typedef typename codec_mode_type::input_block_type input_block_type;

                    constexpr static const std::size_t input_value_bits = codec_mode_type::input_value_bits;
                    typedef typename input_block_type::value_type input_value_type;

                    constexpr static const std::size_t output_block_bits = codec_mode_type::output_block_bits;
                    constexpr static const std::size_t output_block_values = codec_mode_type::output_block_values;
                    typedef typename codec_mode_type::output_block_type output_block_type;

                    constexpr static const std::size_t output_value_bits = codec_mode_type::output_value_bits;
                    typedef typename output_block_type::value_type output_value_type;

                    typedef boost::container::static_vector<input_value_type, input_block_values> cache_type;

                public:
                    typedef digest<output_block_bits> result_type;

                    codec_impl(boost::accumulators::dont_care) : seen(0) {
                    }

                    template<typename ArgumentPack>
                    inline void operator()(const ArgumentPack &args) {
                        resolve_type(args[boost::accumulators::sample], args[bits | std::size_t()]);
                    }

                    inline result_type result(boost::accumulators::dont_care) const {
                        result_type res = dgst;

                        if (!cache.empty()) {
                            input_block_type ib = {0};
                            std::move(cache.begin(), cache.end(), ib.begin());
                            output_block_type ob = codec_mode_type::process_block(ib);
                            std::move(ob.begin(), ob.end(), std::inserter(res, res.end()));
                        }

                        if (seen % input_block_bits) {
                            finalizer_type(input_block_bits - seen % input_block_bits)(res);
                        } else {
                            finalizer_type(0)(res);
                        }

                        return res;
                    }

                protected:
                    inline void resolve_type(const input_value_type &value, std::size_t bits) {
                        if (bits == std::size_t()) {
                            process(value, input_value_bits);
                        } else {
                            process(value, bits);
                        }
                    }

                    inline void resolve_type(const input_block_type &value, std::size_t bits) {
                        if (bits == std::size_t()) {
                            process(value, input_block_bits);
                        } else {
                            process(value, bits);
                        }
                    }

                    inline void process(const input_value_type &value, std::size_t) {
                        if (cache.size() == cache.max_size()) {
                            input_block_type ib = {0};
                            std::move(cache.begin(), cache.end(), ib.begin());
                            output_block_type ob = codec_mode_type::process_block(ib);
                            std::move(ob.begin(), ob.end(), std::inserter(dgst, dgst.end()));

                            cache.clear();
                        }

                        cache.push_back(value);
                        seen += input_value_bits;
                    }

                    inline void process(const input_block_type &block, std::size_t) {
                        output_block_type ob;
                        if (cache.empty()) {
                            ob = codec_mode_type::process_block(block);
                        } else {
                            input_block_type b = make_array<input_block_values>(cache.begin(), cache.end());
                            typename input_block_type::const_iterator itr =
                                block.begin() + (cache.max_size() - cache.size());

                            std::move(block.begin(), itr, b.end());

                            ob = codec_mode_type::process_block(b);

                            cache.clear();
                            cache.insert(cache.end(), itr, block.end());
                        }

                        std::move(ob.begin(), ob.end(), std::inserter(dgst, dgst.end()));
                        seen += input_block_bits;
                    }

                    std::size_t seen;
                    cache_type cache;
                    result_type dgst;
                };

                template<typename CodecMode>
                struct codec_impl<CodecMode, postprocessing_accumulator_mode<CodecMode>>
                    : boost::accumulators::accumulator_base {
                protected:
                    typedef CodecMode codec_mode_type;
                    typedef postprocessing_accumulator_mode<CodecMode> accumulator_mode_type;

                    typedef typename codec_mode_type::finalizer_type finalizer_type;
                    typedef typename codec_mode_type::preprocessor_type preprocessor_type;

                    constexpr static const std::size_t input_block_bits = codec_mode_type::input_block_bits;
                    constexpr static const std::size_t input_block_values = codec_mode_type::input_block_values;
                    typedef typename codec_mode_type::input_block_type input_block_type;

                    constexpr static const std::size_t input_value_bits = codec_mode_type::input_value_bits;
                    typedef typename input_block_type::value_type input_value_type;

                    constexpr static const std::size_t output_block_bits = codec_mode_type::output_block_bits;
                    constexpr static const std::size_t output_block_values = codec_mode_type::output_block_values;
                    typedef typename codec_mode_type::output_block_type output_block_type;

                    constexpr static const std::size_t output_value_bits = codec_mode_type::output_value_bits;
                    typedef typename output_block_type::value_type output_value_type;

                public:
                    typedef digest<output_block_bits> result_type;

                    codec_impl(boost::accumulators::dont_care) : leading_zeros(0) {
                    }

                    template<typename ArgumentPack>
                    inline void operator()(const ArgumentPack &args) {
                        preprocessor_type preprocessor;
                        const input_block_type block =
                            args[boost::accumulators::sample];    // TODO: I think it must be user type block like
                                                                  // dgst
                        if (input.empty()) {
                            preprocessor(block);
                            leading_zeros = preprocessor.leading_zeros;
                        }
                        std::move(block.begin(), block.end(), std::back_inserter(input));
                    }

                    inline result_type result(boost::accumulators::dont_care) const {
                        result_type res;
                        output_block_type ob = codec_mode_type::process_block(input);
                        std::move(ob.begin(), ob.end(), std::inserter(res, res.end()));
                        if (leading_zeros) {
                            finalizer_type fin(leading_zeros);
                            fin(res);
                        }
                        std::reverse(res.begin(), res.end());
                        return res;
                    }

                protected:
                    std::size_t leading_zeros;
                    input_block_type input;
                };    // namespace impl
            }         // namespace impl

            namespace tag {
                template<typename ProcessingMode>
                struct codec : boost::accumulators::depends_on<> {
                    typedef ProcessingMode mode_type;

                    /// INTERNAL ONLY
                    ///

                    typedef boost::mpl::always<accumulators::impl::codec_impl<
                        mode_type, typename mode_type::codec_type::template accumulator_mode_type<ProcessingMode>>>
                        impl;
                };
            }    // namespace tag

            namespace extract {
                template<typename Mode, typename AccumulatorSet>
                typename boost::mpl::apply<AccumulatorSet, tag::codec<Mode>>::type::result_type
                    codec(const AccumulatorSet &acc) {
                    return boost::accumulators::extract_result<tag::codec<Mode>>(acc);
                }

                template<typename Codec, typename AccumulatorSet>
                typename boost::mpl::apply<AccumulatorSet,
                                           tag::codec<typename Codec::stream_encoder_type>>::type::result_type
                    encode(const AccumulatorSet &acc) {
                    return boost::accumulators::extract_result<tag::codec<typename Codec::stream_encoder_type>>(acc);
                }

                template<typename Codec, typename AccumulatorSet>
                typename boost::mpl::apply<AccumulatorSet,
                                           tag::codec<typename Codec::stream_decoder_type>>::type::result_type
                    decode(const AccumulatorSet &acc) {
                    return boost::accumulators::extract_result<tag::codec<typename Codec::stream_decoder_type>>(acc);
                }
            }    // namespace extract
        }        // namespace accumulators
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_ACCUMULATORS_CODEC_HPP


/**
 * @file methods/ann/brnn_impl.hpp
 * @author Saksham Bansal
 *
 * Definition of the BRNN class, which implements bidirectional recurrent
 * neural networks.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_ANN_BRNN_IMPL_HPP
#define MLPACK_METHODS_ANN_BRNN_IMPL_HPP

// In case it hasn't been included yet.
#include "brnn.hpp"

#include "visitor/load_output_parameter_visitor.hpp"
#include "visitor/save_output_parameter_visitor.hpp"
#include "visitor/forward_visitor.hpp"
#include "visitor/backward_visitor.hpp"
#include "visitor/reset_cell_visitor.hpp"
#include "visitor/gradient_set_visitor.hpp"
#include "visitor/gradient_visitor.hpp"
#include "visitor/weight_set_visitor.hpp"
#include "visitor/run_set_visitor.hpp"

#include <boost/serialization/variant.hpp>

namespace mlpack {
namespace ann /** Artificial Neural Network. */ {

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::BRNN(
    const size_t rho,
    const bool single,
    OutputLayerType outputLayer,
    MergeLayerType* mergeLayer,
    MergeOutputType* mergeOutput,
    InitializationRuleType initializeRule) :
    rho(rho),
    outputLayer(std::move(outputLayer)),
    mergeLayer(mergeLayer),
    mergeOutput(mergeOutput),
    initializeRule(std::move(initializeRule)),
    inputSize(0),
    outputSize(0),
    targetSize(0),
    reset(false),
    single(single),
    numFunctions(0),
    deterministic(true),
    forwardRNN(rho, single, outputLayer, initializeRule),
    backwardRNN(rho, single, outputLayer, initializeRule)
{
  /* Nothing to do here. */
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::~BRNN()
{
  // Remove the last layers from the forward and backward RNNs, as they are held
  // in mergeLayer.  So, when we use DeleteVisitor with mergeLayer, those two
  // layers will be properly (and not doubly) freed.
  forwardRNN.network.pop_back();
  backwardRNN.network.pop_back();

  // Clean up layers that we allocated.
  boost::apply_visitor(DeleteVisitor(), mergeLayer);
  boost::apply_visitor(DeleteVisitor(), mergeOutput);
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename OptimizerType>
typename std::enable_if<
      HasMaxIterations<OptimizerType, size_t&(OptimizerType::*)()>
      ::value, void>::type
BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::WarnMessageMaxIterations
(OptimizerType& optimizer, size_t samples) const
{
  if (optimizer.MaxIterations() < samples &&
      optimizer.MaxIterations() != 0)
  {
    Log::Warn << "The optimizer's maximum number of iterations "
              << "is less than the size of the dataset; the "
              << "optimizer will not pass over the entire "
              << "dataset. To fix this, modify the maximum "
              << "number of iterations to be at least equal "
              << "to the number of points of your dataset "
              << "(" << samples << ")." << std::endl;
  }
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename OptimizerType>
typename std::enable_if<
      !HasMaxIterations<OptimizerType, size_t&(OptimizerType::*)()>
      ::value, void>::type
BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::WarnMessageMaxIterations
(OptimizerType& optimizer, size_t samples) const
{
  return;
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename OptimizerType>
double BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::Train(
    arma::cube predictors,
    arma::cube responses,
    OptimizerType& optimizer)
{
  numFunctions = responses.n_cols;

  this->predictors = std::move(predictors);
  this->responses = std::move(responses);

  this->deterministic = true;
  ResetDeterministic();

  if (!reset)
  {
    ResetParameters();
  }

  WarnMessageMaxIterations<OptimizerType>(optimizer, this->predictors.n_cols);

  // Train the model.
  Timer::Start("BRNN_optimization");
  const double out = optimizer.Optimize(*this, parameter);
  Timer::Stop("BRNN_optimization");

  Log::Info << "BRNN::BRNN(): final objective of trained model is " << out
      << "." << std::endl;
  return out;
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename OptimizerType>
double BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::Train(
    arma::cube predictors,
    arma::cube responses)
{
  numFunctions = responses.n_cols;

  this->predictors = std::move(predictors);
  this->responses = std::move(responses);

  this->deterministic = true;
  ResetDeterministic();

  if (!reset)
  {
    ResetParameters();
  }

  OptimizerType optimizer;

  WarnMessageMaxIterations<OptimizerType>(optimizer, this->predictors.n_cols);

  // Train the model.
  const double out = optimizer.Optimize(*this, parameter);

  Log::Info << "BRNN::BRNN(): final objective of trained model is " << out
      << "." << std::endl;
  return out;
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
void BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::Predict(
    arma::cube predictors, arma::cube& results, const size_t batchSize)
{
  forwardRNN.rho = backwardRNN.rho = rho;

  forwardRNN.ResetCells();
  backwardRNN.ResetCells();

  if (!deterministic)
  {
    deterministic = true;
    ResetDeterministic();
  }
  if (parameter.is_empty())
  {
    ResetParameters();
  }

  if (std::is_same<MergeLayerType, Concat<>>::value)
  {
    results = arma::zeros<arma::cube>(outputSize * 2, predictors.n_cols, rho);
  }
  else
  {
    results = arma::zeros<arma::cube>(outputSize, predictors.n_cols, rho);
  }

  std::vector<arma::mat> results1, results2;
  arma::mat input;

  // Forward both RNN's from opposite directions.
  for (size_t begin = 0; begin < predictors.n_cols; begin += batchSize)
  {
    const size_t effectiveBatchSize = std::min(batchSize,
        size_t(predictors.n_cols - begin));
    for (size_t seqNum = 0; seqNum < rho; ++seqNum)
    {
      forwardRNN.Forward(arma::mat(
          predictors.slice(seqNum).colptr(begin),
          predictors.n_rows, effectiveBatchSize, false, true));
      backwardRNN.Forward(std::move(arma::mat(
          predictors.slice(rho - seqNum - 1).colptr(begin),
          predictors.n_rows, effectiveBatchSize, false, true)));

      boost::apply_visitor(SaveOutputParameterVisitor(results1),
          forwardRNN.network.back());
      boost::apply_visitor(SaveOutputParameterVisitor(results2),
          backwardRNN.network.back());
    }
    reverse(results1.begin(), results1.end());

    // Forward outputs from both RNN's through merge layer for each time step.
    for (size_t seqNum = 0; seqNum < rho; ++seqNum)
    {
      boost::apply_visitor(LoadOutputParameterVisitor(results1),
          forwardRNN.network.back());
      boost::apply_visitor(LoadOutputParameterVisitor(results2),
          backwardRNN.network.back());

      boost::apply_visitor(ForwardVisitor(input,
          boost::apply_visitor(outputParameterVisitor, mergeLayer)),
          mergeLayer);
      boost::apply_visitor(ForwardVisitor(
          boost::apply_visitor(outputParameterVisitor, mergeLayer),
          boost::apply_visitor(outputParameterVisitor, mergeOutput)),
          mergeOutput);
      results.slice(seqNum).submat(0, begin, results.n_rows - 1, begin +
          effectiveBatchSize - 1) =
          boost::apply_visitor(outputParameterVisitor, mergeOutput);
    }
  }
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
double BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::Evaluate(
    const arma::mat& /* parameters */,
    const size_t begin,
    const size_t batchSize,
    const bool deterministic)
{
  forwardRNN.rho = backwardRNN.rho = rho;
  if (parameter.is_empty())
  {
    ResetParameters();
  }

  if (deterministic != this->deterministic)
  {
    this->deterministic = deterministic;
    ResetDeterministic();
  }

  if (!inputSize)
  {
    inputSize = predictors.n_rows;
    targetSize = responses.n_rows;
  }
  else if (targetSize == 0)
  {
    targetSize = responses.n_rows;
  }

  forwardRNN.ResetCells();
  backwardRNN.ResetCells();

  double performance = 0;
  size_t responseSeq = 0;

  std::vector<arma::mat> results1, results2;
  for (size_t seqNum = 0; seqNum < rho; ++seqNum)
  {
    forwardRNN.Forward(arma::mat(
        predictors.slice(seqNum).colptr(begin),
        predictors.n_rows, batchSize, false, true));
    backwardRNN.Forward(arma::mat(
        predictors.slice(rho - seqNum - 1).colptr(begin),
        predictors.n_rows, batchSize, false, true));

    boost::apply_visitor(SaveOutputParameterVisitor(results1),
        forwardRNN.network.back());
    boost::apply_visitor(SaveOutputParameterVisitor(results2),
        backwardRNN.network.back());
  }
  if (outputSize == 0)
  {
    outputSize = boost::apply_visitor(outputParameterVisitor,
        forwardRNN.network.back()).n_elem / batchSize;
    forwardRNN.outputSize = backwardRNN.outputSize = outputSize;
  }
  reverse(results1.begin(), results1.end());

  // Performance calculation after forwarding through merge layer.
  arma::mat input;
  for (size_t seqNum = 0; seqNum < rho; ++seqNum)
  {
    if (!single)
    {
      responseSeq = seqNum;
    }
    boost::apply_visitor(LoadOutputParameterVisitor(results1),
        forwardRNN.network.back());
    boost::apply_visitor(LoadOutputParameterVisitor(results2),
        backwardRNN.network.back());

    boost::apply_visitor(ForwardVisitor(input,
        boost::apply_visitor(outputParameterVisitor, mergeLayer)),
        mergeLayer);
    boost::apply_visitor(ForwardVisitor(
        boost::apply_visitor(outputParameterVisitor, mergeLayer),
        boost::apply_visitor(outputParameterVisitor, mergeOutput)),
        mergeOutput);
    performance += outputLayer.Forward(
        boost::apply_visitor(outputParameterVisitor, mergeOutput),
        arma::mat(responses.slice(responseSeq).colptr(begin),
        responses.n_rows, batchSize, false, true));
  }
  return performance;
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
double BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::Evaluate(
    const arma::mat& parameters,
    const size_t begin,
    const size_t batchSize)
{
  return Evaluate(parameters, begin, batchSize, true);
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename GradType>
double BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::
EvaluateWithGradient(const arma::mat& /* parameters */,
                     const size_t begin,
                     GradType& gradient,
                     const size_t batchSize)
{
  forwardRNN.rho = backwardRNN.rho = rho;
  if (gradient.is_empty())
  {
    if (parameter.is_empty())
    {
      ResetParameters();
    }
    gradient = arma::zeros<arma::mat>(parameter.n_rows, parameter.n_cols);
  }
  else
  {
    gradient.zeros();
  }

  if (backwardGradient.is_empty())
  {
    backwardGradient = arma::zeros<arma::mat>(
        parameter.n_rows/ 2,
        parameter.n_cols);
    forwardGradient = arma::zeros<arma::mat>(
        parameter.n_rows/ 2,
        parameter.n_cols);
  }
  if (this->deterministic)
  {
    this->deterministic = false;
    ResetDeterministic();
  }

  if (!inputSize)
  {
    inputSize = predictors.n_rows;
    targetSize = responses.n_rows;
  }
  else if (targetSize == 0)
  {
    targetSize = responses.n_rows;
  }

  forwardRNN.ResetCells();
  backwardRNN.ResetCells();
  size_t networkSize = backwardRNN.network.size();

  // Forward propogation from both directions.
  std::vector<arma::mat> results1, results2;
  for (size_t seqNum = 0; seqNum < rho; ++seqNum)
  {
    forwardRNN.Forward(arma::mat(
        predictors.slice(seqNum).colptr(begin),
        predictors.n_rows, batchSize, false, true));
    backwardRNN.Forward(arma::mat(
        predictors.slice(rho - seqNum - 1).colptr(begin),
        predictors.n_rows, batchSize, false, true));

    for (size_t l = 0; l < networkSize; ++l)
    {
      boost::apply_visitor(SaveOutputParameterVisitor(
          forwardRNNOutputParameter), forwardRNN.network[l]);
      boost::apply_visitor(SaveOutputParameterVisitor(
          backwardRNNOutputParameter), backwardRNN.network[l]);
    }
    boost::apply_visitor(SaveOutputParameterVisitor(results1),
        forwardRNN.network.back());
    boost::apply_visitor(SaveOutputParameterVisitor(results2),
        backwardRNN.network.back());
  }
  if (outputSize == 0)
  {
    outputSize = boost::apply_visitor(outputParameterVisitor,
        forwardRNN.network.back()).n_elem / batchSize;
    forwardRNN.outputSize = backwardRNN.outputSize = outputSize;
  }

  arma::cube results;
  if (std::is_same<MergeLayerType, Concat<>>::value)
  {
    results = arma::zeros<arma::cube>(outputSize * 2, batchSize, rho);
  }
  else
  {
    results = arma::zeros<arma::cube>(outputSize, batchSize, rho);
  }

  double performance = 0;
  size_t responseSeq = 0;
  arma::mat input;

  reverse(results1.begin(), results1.end());
  // Performance calculation here.
  for (size_t seqNum = 0; seqNum < rho; ++seqNum)
  {
    if (!single)
    {
      responseSeq = seqNum;
    }
    boost::apply_visitor(LoadOutputParameterVisitor(
          results1), forwardRNN.network.back());
    boost::apply_visitor(LoadOutputParameterVisitor(
          results2), backwardRNN.network.back());
    boost::apply_visitor(ForwardVisitor(input,
        boost::apply_visitor(outputParameterVisitor, mergeLayer)),
        mergeLayer);
    boost::apply_visitor(ForwardVisitor(
        boost::apply_visitor(outputParameterVisitor, mergeLayer),
        results.slice(seqNum)), mergeOutput);
    performance += outputLayer.Forward(results.slice(seqNum),
        arma::mat(responses.slice(responseSeq).colptr(begin),
        responses.n_rows, batchSize, false, true));
  }

  // Calculate and storing delta parameters from output for t = 1 to T.
  arma::mat delta;
  std::vector<arma::mat> allDelta;

  for (size_t seqNum = 0; seqNum < rho; ++seqNum)
  {
    if (single && seqNum > 0)
    {
      error.zeros();
    }
    else if (single && seqNum == 0)
    {
      outputLayer.Backward(results.slice(seqNum),
          arma::mat(responses.slice(0).colptr(begin),
          responses.n_rows, batchSize, false, true), error);
    }
    else
    {
      outputLayer.Backward(results.slice(seqNum),
          arma::mat(responses.slice(seqNum).colptr(begin),
          responses.n_rows, batchSize, false, true), error);
    }

    boost::apply_visitor(BackwardVisitor(results.slice(seqNum), error, delta),
        mergeOutput);
    allDelta.push_back(arma::mat(delta));
  }

  // BPTT ForwardRNN from t = T to 1.
  totalGradient = arma::mat(gradient.memptr(),
      parameter.n_elem / 2, 1, false, false);

  forwardGradient.zeros();
  forwardRNN.ResetGradients(forwardGradient);
  backwardGradient.zeros();
  backwardRNN.ResetGradients(backwardGradient);

  for (size_t seqNum = 0; seqNum < rho; ++seqNum)
  {
    forwardGradient.zeros();
    for (size_t l = 0; l < networkSize; ++l)
    {
      boost::apply_visitor(LoadOutputParameterVisitor(
          forwardRNNOutputParameter),
          forwardRNN.network[networkSize - 1 - l]);
    }
    boost::apply_visitor(BackwardVisitor(boost::apply_visitor(
        outputParameterVisitor, forwardRNN.network.back()),
        allDelta[rho - seqNum - 1], delta, 0),
        mergeLayer);

    for (size_t i = 2; i < networkSize; ++i)
    {
      boost::apply_visitor(BackwardVisitor(
          boost::apply_visitor(outputParameterVisitor,
          forwardRNN.network[networkSize - i]),
          boost::apply_visitor(deltaVisitor,
          forwardRNN.network[networkSize - i + 1]),
          boost::apply_visitor(deltaVisitor,
          forwardRNN.network[networkSize - i])),
          forwardRNN.network[networkSize - i]);
    }
    forwardRNN.Gradient(
        arma::mat(predictors.slice(rho - seqNum - 1).colptr(begin),
        predictors.n_rows, batchSize, false, true));
    boost::apply_visitor(GradientVisitor(
        boost::apply_visitor(outputParameterVisitor,
        forwardRNN.network[networkSize - 2]),
        allDelta[rho - seqNum - 1], 0), mergeLayer);
    totalGradient += forwardGradient;
  }

  // BPTT BackwardRNN from t = 1 to T.
  totalGradient = arma::mat(gradient.memptr() + parameter.n_elem/2,
      parameter.n_elem/2, 1, false, false);

  for (size_t seqNum = 0; seqNum < rho; ++seqNum)
  {
    backwardGradient.zeros();
    for (size_t l = 0; l < networkSize; ++l)
    {
      boost::apply_visitor(LoadOutputParameterVisitor(
          backwardRNNOutputParameter),
          backwardRNN.network[networkSize - 1 - l]);
    }
    boost::apply_visitor(BackwardVisitor(
        boost::apply_visitor(outputParameterVisitor,
        backwardRNN.network.back()),
        allDelta[seqNum], delta, 1), mergeLayer);
    for (size_t i = 2; i < networkSize; ++i)
    {
      boost::apply_visitor(BackwardVisitor(
        boost::apply_visitor(outputParameterVisitor,
        backwardRNN.network[networkSize - i]), boost::apply_visitor(
        deltaVisitor, backwardRNN.network[networkSize - i + 1]),
        boost::apply_visitor(deltaVisitor,
        backwardRNN.network[networkSize - i])),
        backwardRNN.network[networkSize - i]);
    }

    backwardRNN.Gradient(
        arma::mat(predictors.slice(seqNum).colptr(begin),
        predictors.n_rows, batchSize, false, true));
    boost::apply_visitor(GradientVisitor(
        std::move(boost::apply_visitor(outputParameterVisitor,
        backwardRNN.network[networkSize - 2])),
        allDelta[seqNum], 1), mergeLayer);
    totalGradient += backwardGradient;
  }
  return performance;
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
void BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::Gradient(
    const arma::mat& parameters,
    const size_t begin,
    arma::mat& gradient,
    const size_t batchSize)
{
  this->EvaluateWithGradient(parameters, begin, gradient, batchSize);
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
void BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::Shuffle()
{
  arma::cube newPredictors, newResponses;
  math::ShuffleData(predictors, responses, newPredictors, newResponses);

  predictors = std::move(newPredictors);
  responses = std::move(newResponses);
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
template <class LayerType, class... Args>
void BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::Add(Args... args)
{
  forwardRNN.network.push_back(new LayerType(args...));
  backwardRNN.network.push_back(new LayerType(args...));
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
void BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::
Add(LayerTypes<CustomLayers...> layer)
{
  forwardRNN.network.push_back(layer);
  backwardRNN.network.push_back(boost::apply_visitor(copyVisitor, layer));
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
void BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::ResetParameters()
{
  if (!reset)
  {
    // TODO: what if we call ResetParameters() multiple times?  Do we have to
    // remove any existing mergeLayer?
    boost::apply_visitor(AddVisitor<CustomLayers...>(
        forwardRNN.network.back()), mergeLayer);
    boost::apply_visitor(AddVisitor<CustomLayers...>(
        backwardRNN.network.back()), mergeLayer);
    boost::apply_visitor(RunSetVisitor(false), mergeLayer);
  }

  ResetDeterministic();

  // Reset the network parameter with the given initialization rule.
  NetworkInitialization<InitializationRuleType,
                        CustomLayers...> networkInit(initializeRule);
  size_t rnnWeights = 0;
  for (size_t i = 0; i < forwardRNN.network.size(); ++i)
  {
    rnnWeights += boost::apply_visitor(weightSizeVisitor,
        forwardRNN.network[i]);
  }

  parameter.set_size(2 * rnnWeights, 1);

  forwardRNN.Parameters() = arma::mat(parameter.memptr(),
      rnnWeights, 1, false, false);
  backwardRNN.Parameters() = arma::mat(parameter.memptr() + rnnWeights,
      rnnWeights, 1, false, false);

  // Initialize the forward RNN parameters
  networkInit.Initialize(forwardRNN.network, parameter);

  // Initialize the backward RNN parameters
  networkInit.Initialize(backwardRNN.network, parameter, rnnWeights);

  reset = forwardRNN.reset = backwardRNN.reset = true;
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
void BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::Reset()
{
  ResetParameters();
  forwardRNN.ResetCells();
  backwardRNN.ResetCells();
  forwardGradient.zeros();
  forwardRNN.ResetGradients(forwardGradient);
  backwardGradient.zeros();
  backwardRNN.ResetGradients(backwardGradient);
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
void BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::ResetDeterministic()
{
  forwardRNN.deterministic = this->deterministic;
  backwardRNN.deterministic = this->deterministic;
  forwardRNN.ResetDeterministic();
  backwardRNN.ResetDeterministic();
}

template<typename OutputLayerType, typename MergeLayerType,
         typename MergeOutputType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename Archive>
void BRNN<OutputLayerType, MergeLayerType, MergeOutputType,
    InitializationRuleType, CustomLayers...>::serialize(
    Archive& ar, const unsigned int version)
{
  ar & BOOST_SERIALIZATION_NVP(parameter);
  ar & BOOST_SERIALIZATION_NVP(backwardRNN);
  ar & BOOST_SERIALIZATION_NVP(forwardRNN);

  // TODO: are there more parameters to be serialized?
}

} // namespace ann
} // namespace mlpack

#endif


#include <iostream>
#include <math.h>
#include <mutex>
#include <time.h>

//PCL
/*
//This solves the problem of compiling pcl search with C++11
#include <pcl/search/impl/search.hpp>
#ifndef PCL_NO_PRECOMPILE
#include <pcl/impl/instantiate.hpp>
#include <pcl/point_types.h>
PCL_INSTANTIATE(Search, PCL_POINT_TYPES)
#endif // PCL_NO_PRECOMPILE
*/
#include <pcl/io/pcd_io.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl_ros/transforms.h>

//PCL filters
#include <pcl/filters/filter_indices.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/voxel_grid_covariance.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/conditional_removal.h>
//#include <pcl/filters/radius_outlier_removal.h>
//the shadow filter requires to compute the normals previously
#include <pcl/filters/shadowpoints.h>

//ROS
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>

//Eigen
#include <Eigen/Geometry>
#include <Eigen/Dense>

//Filters
pcl::VoxelGridCovariance<pcl::PointXYZ>* vgc;
bool voxelgridcov = false;
pcl::VoxelGrid<pcl::PointXYZ>* vg;
bool voxelgrid = false;
pcl::StatisticalOutlierRemoval<pcl::PointXYZ>* sor;
bool statistical = false;
pcl::PassThrough<pcl::PointXYZ>* pass;
bool passthrough = false;
float pass_cx = 0.0;
float pass_cy = 0.0;
float pass_cz = 0.0;
float pass_rx = 100.0;
float pass_ry = 100.0;
float pass_rz = 100.0;
pcl::CropBox<pcl::PointXYZ>* box;
bool cropbox = false;

std::string pc_frame;
bool use_robot_pose = false;
bool filter_ceiling = false;
float local_grid_radius = 3.0;
float max_height_allowed = 2.0;
std::string robot_pose_topic;
std::string odom_frame;
std::string sensor_frame;
geometry_msgs::PoseStamped robot_pose;
std::mutex mutex;

bool print_arrows = false;

double pitch_max = M_PI/2.0;
double roll_max = M_PI/2.0;
double roughness = 0.03;

double pitch_low;
double pitch_high;
double roll_low;
double roll_high;

bool assign_costs = true;

ros::Publisher pc_pub;
ros::Publisher arrow_pub;

tf::TransformListener *listener;

bool isQuaternionValid(const geometry_msgs::Quaternion q){
    //first we need to check if the quaternion has nan's or infs
    if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){
		ROS_ERROR("Quaternion has infs!!!!");
		return false;
    }
    if(std::isnan(q.x) || std::isnan(q.y) || std::isnan(q.z) || std::isnan(q.w)) {
		ROS_ERROR("Quaternion has nans !!!");
		return false;
	}

	if(std::fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1) > 0.01) {
		ROS_ERROR("Quaternion malformed, magnitude: %.3f should be 1.0", (q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w));
		return false;
	}

    tf::Quaternion tf_q(q.x, q.y, q.z, q.w);

    //next, we need to check if the length of the quaternion is close to zero
    if(tf_q.length2() < 1e-6){
      ROS_ERROR("Quaternion has length close to zero... discarding.");
      return false;
    }

    //next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
    /*tf_q.normalize();

    tf::Vector3 up(0, 0, 1);

    double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));

    if(fabs(dot - 1) > 1e-3){
      ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
      return false;
    }*/

    return true;
}

geometry_msgs::PoseStamped transformPoseTo(geometry_msgs::PoseStamped pose_in, std::string frame_out, bool usetime)
{
	geometry_msgs::PoseStamped in = pose_in;
	if(!usetime)
		in.header.stamp = ros::Time(); //ros::Time::now(); //ros::Time();

	geometry_msgs::PoseStamped pose_out;

	geometry_msgs::Quaternion q = in.pose.orientation;
	if(!isQuaternionValid(q))
	{
		ROS_WARN("pcl_filters. transformPoseTo. Quaternion no valid. Creating new quaternion with yaw=0.0");
		in.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
	}
	try {
		listener->transformPose(frame_out.c_str(), in, pose_out);
	}catch (tf::TransformException ex){
		ROS_WARN("pcl_filters. TransformException in method transformPoseTo. TargetFrame: %s : %s", frame_out.c_str(), ex.what());
	}
	//printf("Tranform pose. frame_in: %s, x:%.2f, y:%.2f, frame_out: %s, x:%.2f, y:%.2f\n", in.header.frame_id.c_str(), in.pose.position.x, in.pose.position.y, frame_out.c_str(), pose_out.pose.position.x, pose_out.pose.position.y);
	return pose_out;
}

pcl::PointCloud<pcl::PointXYZ> removeWallsAndCeiling(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{

	visualization_msgs::MarkerArray marray;
	pcl::PointCloud<pcl::PointXYZ> filtered;
	filtered.header = cloud->header;
	filtered.is_dense = cloud->is_dense;
	filtered.height = cloud->height;
	filtered.sensor_origin_ = cloud->sensor_origin_;
	filtered.sensor_orientation_ = cloud->sensor_orientation_;

	const std::map< size_t, pcl::VoxelGridCovariance<pcl::PointXYZ>::Leaf >* leaves = &(vgc->getLeaves());

	if(!cloud->isOrganized()) //The height value must be different than 1 for a dataset to be organized
	{
		//the index of the width should match with the leaf index???
		pcl::IndicesPtr indices(new std::vector<int>); //typedef boost::shared_ptr<std::vector<int> > pcl::IndicesPtr
		//std::vector<int> indices2;

		int ind=0;
		for(const auto& it : *leaves) //const
		{
			const pcl::VoxelGridCovariance<pcl::PointXYZ>::Leaf *l = &(it.second);

			Eigen::Vector3d mean = l->getMean();
			Eigen::Matrix3d evecs = l->getEvecs();
			Eigen::Vector3d evals = l->getEvals();

			pcl::PointXYZ p;
			p.x = mean[0];
			p.y = mean[1];
			p.z = mean[2];

			Eigen::Quaternion<double> q(evecs);

			auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2); //0->yaw, 1->pitch, 2->roll
			double yaw = euler[0];
			double pitch = euler[1];
			double roll = euler[2];

			//Identify the lowest eigenvalue
			//TODO: the eigen values are ordered.
			//The smallest is the last one. CHECK THIS!
			int i=0;
			if(evals(0) < evals(1) && evals(0) < evals(2))
				i = 0;
			else if(evals(1) < evals(0) && evals(1) < evals(2))
				i = 1;
			else
				i = 2;

			float nx = (float)evecs(0,i);
			float ny = (float)evecs(1,i);
			float nz = (float)evecs(2,i);

			//create a pose
			//geometry_msgs::PoseStamped pose;
			//pose.header.frame_id = odom_frame;
			//pose.header.stamp = ros::Time::now();
			tf::Quaternion aux(q.x(), q.y(), q.z(), q.w());
			aux.normalize();
			//pose.pose.position.x = mean[0];
			//pose.pose.position.y = mean[1];
			//pose.pose.position.z = mean[2];

			//pose.pose.orientation.x = aux.x();
			//pose.pose.orientation.y = aux.y();
			//pose.pose.orientation.z = aux.z();
			//pose.pose.orientation.w = aux.w();
			//Transform pose to odom frame
			//geometry_msgs::PoseStamped meanpose = transformPoseTo(pose, sensor_frame, false);

			mutex.lock();
			geometry_msgs::PoseStamped rpose = robot_pose;
			mutex.unlock();

			//WATCH OUT!!!
			//This is a very poor filter of the ceiling
			//I compare the height of the point with the height of the robot
			if(fabs(rpose.pose.position.z - p.z) > max_height_allowed)
				continue;

			/*

			//geometry_msgs::PoseStamped rpose = transformPoseTo(aux_pose, odom_frame, false);

			// correct if n . (pv - Pi) > 0
			// n  -> evec with the lowest eigenvalue (= normal vector)
			// pv -> (0,0,0)
			// pi -> mean
			// See if we need to flip any plane normals

			// Dot product between the (viewpoint - point) and the plane normal
			float cos_theta = ((rpose.pose.position.x-mean[0]) * nx + (rpose.pose.position.y-mean[1]) * ny + (rpose.pose.position.z-mean[2]) * nz);
			// Flip the plane normal
			if (cos_theta < 0)
			{

				pcl::flipNormalTowardsViewpoint(p, rpose.pose.position.x, rpose.pose.position.y, rpose.pose.position.z, nx, ny, nz);

				Eigen::Vector3d norm(nx, ny, nz);
				norm = norm.normalized();
				//evecs(0,i) = nx;
				//evecs(1,i) = ny;
				//evecs(2,i) = nz;
				evecs.col(i) =	norm;
				//evecs = -1.0*evecs;

				Eigen::Vector3d norm2;
				Eigen::Vector3d norm3;
				if ((fabs((double)norm(0)) > 0.001) || (fabs((double)norm(1)) > 0.001)) {
    				norm2 << -norm(1), norm(0), 0;
				} else {
    				norm2 << 0, norm(2), -norm(1);
				}
				norm2.normalize();

				norm3 = norm.cross(norm2);

				Eigen::Matrix3d R;  // Rotation matrix defining orientation
				R.col(0) = norm;
				R.col(1) = norm2;
				R.col(2) = norm3;

				//Eigen::Quaternion<double> q2(evecs); //evecs_trans
				Eigen::Quaternion<double> q2(R);
				tf::Quaternion aux2(q2.x(), q2.y(), q2.z(), q2.w());
				aux2.normalize();
				aux = aux2;

				tf::Matrix3x3 m(aux2);
     			//double roll2, pitch2, yaw2;
      			m.getRPY(roll, pitch, yaw);

				//pose.pose.orientation.x = aux2.x();
				//pose.pose.orientation.y = aux2.y();
				//pose.pose.orientation.z = aux2.z();
				//pose.pose.orientation.w = aux2.w();
			}*/

			//if(fabs(pitch) < pitch_low || fabs(pitch) > pitch_high && fabs(yaw) < roll_low || fabs(yaw) > roll_high)
			//if(pitch < pitch_high && pitch > pitch_low && yaw < roll_high && yaw > roll_low) // && fabs(evals(i)) < roughness))
			if(fabs(pitch) < pitch_high && fabs(pitch) > pitch_low && fabs(roll) < roll_high && fabs(roll) > roll_low) //ok
			//if(-pitch < pitch_high && -pitch > pitch_low && -roll < roll_high && -roll > roll_low)
			{
				//printf("pose3d VALID. pitch:%.2f, roll:%.2f, upper:%.2f, lower:%.2f\n", fabs(pitch), fabs(roll), pitch_high, pitch_low);
				indices->push_back(ind);
				//indices2.push_back(ind);

				filtered.push_back(p);

				if(print_arrows)
				{
					visualization_msgs::Marker marker;
					marker.header.frame_id = odom_frame; //frame_out;
					marker.header.stamp = ros::Time();
					marker.ns = "normal_arrows";
					marker.id = ind; //key;
					marker.type = visualization_msgs::Marker::ARROW;
					marker.action = visualization_msgs::Marker::ADD;
					marker.pose.position.x = mean[0];  //m[0]
					marker.pose.position.y = mean[1];	//m[1]
					marker.pose.position.z = mean[2];  //m[2]
					//marker.pose.position = meanpose.pose.position;
					//tf::Quaternion aux(q.x(), q.y(), q.z(), q.w());
					//aux.normalize();
					marker.pose.orientation.x = aux.x();
					marker.pose.orientation.y = aux.y();
					marker.pose.orientation.z = aux.z();
					marker.pose.orientation.w = aux.w();
					//marker.pose.orientation = meanpose.pose.orientation;

					marker.scale.x = 9.0*evals(i);
					marker.scale.y = 0.03; //2.0*evals(1);
					marker.scale.z = 0.03; //2.0*evals(2);

					marker.color.a = 1.0;
					marker.color.r = 0.0;
					marker.color.g = 0.0;
					marker.color.b = 1.0;

					marker.lifetime = ros::Duration(2.0);

					marray.markers.push_back(marker);

				}

			} else {
				//printf("pose3d INVALID. pitch:%.2f, roll:%.2f, upper:%.2f, lower:%.2f\n", fabs(pitch), fabs(roll), pitch_high, pitch_low);
			}
			ind++;

		}

		pcl::ExtractIndices<pcl::PointXYZ> eifilter(false); // Initializing with true will allow us to extract the removed indices
		eifilter.setInputCloud(cloud);
		eifilter.setIndices(indices);
		//eifilter.filter (*cloud_out);
		eifilter.filterDirectly(cloud);
		//eifilter.filter(indices2);

		arrow_pub.publish(marray);
		return filtered;
	} else {
		printf("pcl_filters. The point cloud is ordered!!!\n");
	}

/*

En setIndices, indices_in es un pcl::IndicesPtr,
que es un puntero a vector: typedef boost::shared_ptr<std::vector<int> >

pcl::ExtractIndices<PointType> eifilter (true); // Initializing with true will allow us to extract the removed indices
eifilter.setInputCloud (cloud_in);
eifilter.setIndices (indices_in);
eifilter.filter (*cloud_out);
Alternativamente a filter, podemos usar:
eifilter.filterDirectly (cloud_in);
This will directly modify cloud_in instead of creating a copy of the cloud

*/

}

pcl::PointCloud<pcl::PointXYZRGB> assignCosts(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{

	visualization_msgs::MarkerArray marray;
	pcl::PointCloud<pcl::PointXYZRGB> coloured;
	coloured.header = cloud->header;
	coloured.is_dense = cloud->is_dense;
	coloured.height = cloud->height;
	coloured.sensor_origin_ = cloud->sensor_origin_;
	coloured.sensor_orientation_ = cloud->sensor_orientation_;

	std::vector<float> weights;

	const std::map< size_t, pcl::VoxelGridCovariance<pcl::PointXYZ>::Leaf >* leaves = &(vgc->getLeaves());

	if(!cloud->isOrganized()) //The height value must be different than 1 for a dataset to be organized
	{
		//the index of the width should match with the leaf index???
		//pcl::IndicesPtr indices(new std::vector<int>); //typedef boost::shared_ptr<std::vector<int> > pcl::IndicesPtr

		int ind=0;
		for(const auto& it : *leaves) //const
		{
			const pcl::VoxelGridCovariance<pcl::PointXYZ>::Leaf *l = &(it.second);

			Eigen::Vector3d mean = l->getMean();
			Eigen::Matrix3d evecs = l->getEvecs();
			Eigen::Vector3d evals = l->getEvals();

			float num_points = (float)l->getPointCount();
			int points = l->getPointCount();

			pcl::PointXYZRGB p;
			p.x = mean[0];
			p.y = mean[1];
			p.z = mean[2];

			Eigen::Quaternion<double> q(evecs);

			auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2); //0->yaw, 1->pitch, 2->roll
			double yaw = euler[0];
			double pitch = euler[1];
			double roll = euler[2];

			//Identify the lowest eigenvalue
			//TODO: the eigen values are ordered.
			//The smallest is the last one. CHECK THIS!
			int i=0;
			if(evals(0) < evals(1) && evals(0) < evals(2))
				i = 0;
			else if(evals(1) < evals(0) && evals(1) < evals(2))
				i = 1;
			else
				i = 2;

			float nx = (float)evecs(0,i);
			float ny = (float)evecs(1,i);
			float nz = (float)evecs(2,i);

			//create a pose
			//geometry_msgs::PoseStamped pose;
			//pose.header.frame_id = odom_frame;
			//pose.header.stamp = ros::Time::now();
			tf::Quaternion aux(q.x(), q.y(), q.z(), q.w());
			aux.normalize();
			//pose.pose.position.x = mean[0];
			//pose.pose.position.y = mean[1];
			//pose.pose.position.z = mean[2];

			//pose.pose.orientation.x = aux.x();
			//pose.pose.orientation.y = aux.y();
			//pose.pose.orientation.z = aux.z();
			//pose.pose.orientation.w = aux.w();
			//Transform pose to odom frame
			//geometry_msgs::PoseStamped meanpose = transformPoseTo(pose, sensor_frame, false);

			mutex.lock();
			geometry_msgs::PoseStamped rpose = robot_pose;
			mutex.unlock();

			if(filter_ceiling)
			{
				//WATCH OUT!!!
				//This is a very poor filter of the ceiling
				//I compare the height of the point with the height of the robot
				if(fabs(rpose.pose.position.z - p.z) > max_height_allowed)
					continue;

				if(!(fabs(pitch) < pitch_high && fabs(pitch) > pitch_low && fabs(roll) < roll_high && fabs(roll) > roll_low)) //!ok
					continue;

			}

			std::vector<float> features;

			//Normalize pitch and yaw
			float zero = M_PI/2.0;
			pitch = fabs(zero - fabs(pitch));
			pitch = pitch/zero;
			roll = fabs(zero - fabs(roll));
			roll = roll/zero;

			if (pitch > 0.55)
				pitch = 1.0;
			if(roll > 0.55)
				roll = 1.0;

			features.push_back(pitch);
			features.push_back(roll);

			//Normalize roughness
			//Maximum value of roughness?????
			float rough = fabs(evals(2));
			if (rough > roughness) rough = roughness;
			rough = rough/roughness;

			features.push_back(rough);

			//normalize point dist. max dist = robot_radius_
			//point_dist = point_dist / robot_radius_;

			//Normalize stddev
			//stddev = 1.0 - (stddev / max_stddev);

			//Normalize num_points
			float max_points = 50.0;
			if(num_points > max_points) num_points = max_points;
			num_points = 1.0 - (num_points/max_points);

			features.push_back(num_points);
			std::vector<float> weights;
			weights.assign(4, 0.25);

			float cost = 0.0;
			for(unsigned int i=0; i<features.size(); i++)
				cost += features[i] * weights[i];

			//Filter walls, high ceiling and high roughness
			//if(pitch > 0.6 || roll > 0.6 || rough > 0.6 || (fabs(rpose.pose.position.z - p.z) > max_height_allowed) ) {
			if(!filter_ceiling && fabs(rpose.pose.position.z - p.z) > max_height_allowed) {
				cost = 1.0;
			}

			if(pitch == 1.0 || roll == 1.0 || rough > 0.75)
				cost = cost>0.8? 0.8 : cost;

			//Convert scale [0-1] of the cost to scale [0-255];
			int c = (int)round(cost * 255);

			//printf("cost:%.3f, color:%i, p:%.2f, r:%.2f, roug:%.2f, np:%i, pc:%.2f\n", cost, c, pitch, roll, rough, points, num_points);

			p.r = c;
			p.g = 255 - c;
			p.b = 0;

			coloured.push_back(p);
			ind++;

			/*

			//geometry_msgs::PoseStamped rpose = transformPoseTo(aux_pose, odom_frame, false);

			// correct if n . (pv - Pi) > 0
			// n  -> evec with the lowest eigenvalue (= normal vector)
			// pv -> (0,0,0)
			// pi -> mean
			// See if we need to flip any plane normals

			// Dot product between the (viewpoint - point) and the plane normal
			float cos_theta = ((rpose.pose.position.x-mean[0]) * nx + (rpose.pose.position.y-mean[1]) * ny + (rpose.pose.position.z-mean[2]) * nz);
			// Flip the plane normal
			if (cos_theta < 0)
			{

				pcl::flipNormalTowardsViewpoint(p, rpose.pose.position.x, rpose.pose.position.y, rpose.pose.position.z, nx, ny, nz);

				Eigen::Vector3d norm(nx, ny, nz);
				norm = norm.normalized();
				//evecs(0,i) = nx;
				//evecs(1,i) = ny;
				//evecs(2,i) = nz;
				evecs.col(i) =	norm;
				//evecs = -1.0*evecs;

				Eigen::Vector3d norm2;
				Eigen::Vector3d norm3;
				if ((fabs((double)norm(0)) > 0.001) || (fabs((double)norm(1)) > 0.001)) {
    				norm2 << -norm(1), norm(0), 0;
				} else {
    				norm2 << 0, norm(2), -norm(1);
				}
				norm2.normalize();

				norm3 = norm.cross(norm2);

				Eigen::Matrix3d R;  // Rotation matrix defining orientation
				R.col(0) = norm;
				R.col(1) = norm2;
				R.col(2) = norm3;

				//Eigen::Quaternion<double> q2(evecs); //evecs_trans
				Eigen::Quaternion<double> q2(R);
				tf::Quaternion aux2(q2.x(), q2.y(), q2.z(), q2.w());
				aux2.normalize();
				aux = aux2;

				tf::Matrix3x3 m(aux2);
     			//double roll2, pitch2, yaw2;
      			m.getRPY(roll, pitch, yaw);

				//pose.pose.orientation.x = aux2.x();
				//pose.pose.orientation.y = aux2.y();
				//pose.pose.orientation.z = aux2.z();
				//pose.pose.orientation.w = aux2.w();
			}*/

		}
		coloured.width = ind;
		return coloured;

	} else {
		ROS_ERROR("pcl_filters. The point cloud is ordered!!!\n");
		return coloured;
	}

}

void pcCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
{

	//Transform the coordinates of the pointcloud
	sensor_msgs::PointCloud2 local;
	pcl_ros::transformPointCloud(odom_frame, *msg, local, *listener);

	//pc_frame = msg->header.frame_id;

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    //pcl::fromROSMsg(*msg, *cloud);
	pcl::fromROSMsg(local, *cloud);

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_fil(new pcl::PointCloud<pcl::PointXYZ>);

	//pcl::PointCloud<pcl::PointXYZ> cloud_out = *cloud;

	sensor_msgs::PointCloud2 pc_out;

	if(passthrough)
	{
		//TODO: this is not the most efficient way to do it. Try to use indices

		//TODO: Try conditional removal
		/*
		pcl::ConditionOr<PointT>::Ptr range_cond (new pcl::ConditionOr<PointT> ());
		range_cond->addComparison (pcl::FieldComparison<PointT>::Ptr (new pcl::FieldComparison<PointT>("x", pcl::ComparisonOps::GT, minX)));
		range_cond->addComparison (pcl::FieldComparison<PointT>::Ptr (new pcl::FieldComparison<PointT>("x", pcl::ComparisonOps::LT, maxX)));
		range_cond->addComparison (pcl::FieldComparison<PointT>::Ptr (new pcl::FieldComparison<PointT>("y", pcl::ComparisonOps::GT, minY)));
		range_cond->addComparison (pcl::FieldComparison<PointT>::Ptr (new pcl::FieldComparison<PointT>("y", pcl::ComparisonOps::LT, maxY)));
		range_cond->addComparison (pcl::FieldComparison<PointT>::Ptr (new pcl::FieldComparison<PointT>("z", pcl::ComparisonOps::GT, minZ)));
		range_cond->addComparison (pcl::FieldComparison<PointT>::Ptr (new pcl::FieldComparison<PointT>("z", pcl::ComparisonOps::LT, maxZ)));

		pcl::ConditionalRemoval<PointT> range_filt;
		range_filt.setInputCloud(body);
		range_filt.setCondition (range_cond);
		range_filt.filter(*bodyFiltered);
		*/

		pass->setInputCloud(cloud);

		//filter X axis
		pass->setFilterFieldName ("x");
		pass->setFilterLimits((pass_cx - pass_rx), (pass_cx + pass_rx));
		//std::vector<int> indices_x;
		//pcl::IndicesPtr indices_x;
		//ROS_INFO("before filter X");
		//pass->filter(*indices_x);
		pass->filter(*cloud_fil);
		*cloud = *cloud_fil;

		//filter Y axis
		//pass->setIndices(indices_x);
		pass->setInputCloud(cloud);
		pass->setFilterFieldName ("y");
		pass->setFilterLimits ((pass_cy - pass_ry), (pass_cy + pass_ry));
		//pass->setNegative(true);
		//std::vector<int>* indices_xy;
		//pcl::IndicesPtr indices_xy;
		pass->filter(*cloud_fil);
		*cloud = *cloud_fil;

		//filter Z axis
		//pass->setIndices(indices_xy);
		pass->setInputCloud(cloud);
		pass->setFilterFieldName ("z");
		pass->setFilterLimits ((pass_cz - pass_rz), (pass_cz + pass_rz));
		//pass->setNegative(true);
		pass->filter(*cloud_fil);
		*cloud = *cloud_fil;
	}

	if(cropbox)
	{
		box->setInputCloud(cloud);
		if(use_robot_pose)
		{
			mutex.lock();
			geometry_msgs::PoseStamped p = robot_pose;
			mutex.unlock();

			//geometry_msgs::PoseStamped p;
			//p = transformPoseTo(pose, odom_frame, true);

			float min_x = p.pose.position.x - local_grid_radius;
			float max_x = p.pose.position.x + local_grid_radius;
			float min_y = p.pose.position.y - local_grid_radius;
			float max_y = p.pose.position.y + local_grid_radius;
			float min_z = p.pose.position.z - local_grid_radius;
			float max_z = p.pose.position.z + local_grid_radius;

			box->setMin(Eigen::Vector4f(min_x, min_y, min_z, 1.0));
			box->setMax(Eigen::Vector4f(max_x, max_y, max_z, 1.0));
		}
		box->filter(*cloud_fil);
		*cloud = *cloud_fil;
	}

	if(statistical)
	{
		sor->setInputCloud(cloud);
		sor->filter(*cloud_fil);
		*cloud = *cloud_fil;
	}
	if(voxelgrid)
	{
		vg->setInputCloud(cloud);
		vg->filter(*cloud_fil);
		*cloud = *cloud_fil;

	}

	pcl::PointCloud<pcl::PointXYZRGB> coloured;
	if(voxelgridcov && !voxelgrid)
	{
		vgc->setInputCloud(cloud);
		vgc->filter(*cloud_fil, true);
		*cloud = *cloud_fil;

		//if(filter_ceiling)
		//{
		//	filtered = removeWallsAndCeiling(cloud);
		//	*cloud = filtered;
		//}

		if(assign_costs)
		{
			coloured = assignCosts(cloud);
			pcl::toROSMsg(coloured, pc_out);
			pc_pub.publish(pc_out);
			return;
		}

	}

	pcl::toROSMsg(*cloud, pc_out);

	//Transform the coordinates of the pointcloud
	//sensor_msgs::PointCloud2 local;
	//pcl_ros::transformPointCloud("/indires_rover/base_link", pc_out, local, *listener);

	pc_pub.publish(pc_out);
	//pc_pub.publish(local);

}

void poseCallback(const nav_msgs::OdometryConstPtr &msg)
{
	geometry_msgs::PoseStamped pose;

	pose.header = msg->header;
	pose.pose = msg->pose.pose;

	geometry_msgs::PoseStamped pose_out;
	//if(!pc_frame.empty())
	pose_out = transformPoseTo(pose, odom_frame, false);

	mutex.lock();
	robot_pose = pose_out;
	mutex.unlock();

}

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

	ros::init(argc, argv, "pcl_costmap");
	ROS_INFO("---PCL COSTMAP---");
    ros::NodeHandle n;
	ros::NodeHandle nh("~");

	listener = new tf::TransformListener(ros::Duration(10));

	std::string pointcloud_topic = "indires_rover/front_rgbd_camera/front_rgbd_camera/depth/points";
	//camera_tf = "indires_rover/front_rgbd_camera/depth_frame";
	nh.getParam("pointcloud_topic", pointcloud_topic);
	std::string output_topic = "PointCloud_filtered";
	nh.getParam("output_topic", output_topic);

	nh.getParam("use_robot_pose", use_robot_pose);
	nh.getParam("robot_pose_topic", robot_pose_topic);
	nh.getParam("local_grid_radius", local_grid_radius);

	nh.getParam("max_height_allowed", max_height_allowed);
	nh.getParam("odom_frame", odom_frame);
	nh.getParam("sensor_frame", sensor_frame);

	//nh.getParam("show_normal_arrows", print_arrows);

	nh.getParam("filter_ceiling_and_walls", filter_ceiling);
	nh.getParam("pitch_max_inclination", pitch_max);
	nh.getParam("roll_max_inclination", roll_max);
	nh.getParam("max_roughness", roughness);

	//pitch_low = -M_PI/2.0 - pitch_max;
	//pitch_high = -M_PI/2.0 + pitch_max;
	//roll_low = -M_PI/2.0 - roll_max;
	//roll_high = -M_PI/2.0 + roll_max;

	//pitch_low = pitch_max;
	//pitch_high = M_PI - pitch_max;
	//roll_low = roll_max;
	//roll_high = M_PI - roll_max;

	pitch_low = M_PI/2.0 - pitch_max;
	pitch_high = M_PI/2.0 + pitch_max;
	roll_low = M_PI/2.0 - roll_max;
	roll_high = M_PI/2.0 + roll_max;

	//sleep a bit in order to fill the TF buffer
	sleep(6.0);

	if(nh.hasParam("PassThroughFilter"))
	{
		ros::NodeHandle n("~PassThroughFilter");
		n.getParam("enable", passthrough);
		if(passthrough) {
			ROS_INFO("Using PassThrough filter:");
			n.getParam("center_x", pass_cx);
			n.getParam("center_y", pass_cy);
			n.getParam("center_z", pass_cz);
			ROS_INFO("\tcx: %.2f, cy: %.2f, cz: %.2f", pass_cx, pass_cy, pass_cz);
			n.getParam("rad_x", pass_rx);
			n.getParam("rad_y", pass_ry);
			n.getParam("rad_z", pass_rz);
			ROS_INFO("\trx: %.2f, ry: %.2f, rz: %.2f", pass_rx, pass_ry, pass_rz);
			pass = new pcl::PassThrough<pcl::PointXYZ>(false);
		}
	}

	if(nh.hasParam("CropBoxFilter"))
	{
		ros::NodeHandle n("~CropBoxFilter");
		n.getParam("enable", cropbox);
		if(cropbox) {
			ROS_INFO("Using CropBox filter:");
			float min_x = -10.0;
			float min_y = -10.0;
			float min_z = -10.0;
   			float max_x = 10.0;
			float max_y = 10.0;
			float max_z = 10.0;
			n.getParam("cb_min_x", min_x);
			n.getParam("cb_min_y", min_y);
			n.getParam("cb_min_z", min_z);
			n.getParam("cb_max_x", max_x);
			n.getParam("cb_max_y", max_y);
			n.getParam("cb_max_z", max_z);
			ROS_INFO("\tminx: %.2f, miny: %.2f, minz: %.2f", min_x, min_y, min_z);
			ROS_INFO("\tmaxx: %.2f, maxy: %.2f, maxz: %.2f", max_x, max_y, max_z);
			box = new pcl::CropBox<pcl::PointXYZ>();
			box->setMin(Eigen::Vector4f(min_x, min_y, min_z, 1.0));
			box->setMax(Eigen::Vector4f(max_x, max_y, max_z, 1.0));

		}
	}

	if(nh.hasParam("StatisticalOutlierFilter"))
	{
		ros::NodeHandle n("~StatisticalOutlierFilter");
		n.getParam("enable", statistical);
		if(statistical) {
			ROS_INFO("Using StatisticalOutlierRemoval filter:");
			int kneighbors = 80;
			n.getParam("mean_k_neighbors", kneighbors);
			ROS_INFO("\tMean_k_neighbors: %i", kneighbors);
			float stddev = 2.0;
			n.getParam("std_dev", stddev);
			ROS_INFO("\tStd_dev: %.2f", stddev);

			sor = new pcl::StatisticalOutlierRemoval<pcl::PointXYZ>();
			sor->setMeanK (kneighbors);
  			sor->setStddevMulThresh (stddev);
		}
	}

	if(nh.hasParam("VoxelGridFilter"))
	{
		ros::NodeHandle n("~VoxelGridFilter");
		n.getParam("enable", voxelgrid);
		if(voxelgrid) {
			ROS_INFO("Using VoxelGrid filter:");
			float leaf_size = 0.20;
			n.getParam("leaf_size", leaf_size);
			ROS_INFO("\tLeaf_size: %.2f", leaf_size);
			int min_point_per_voxel = 10;
			n.getParam("leaf_size", leaf_size);
			n.getParam("min_point_per_voxel", min_point_per_voxel);
			ROS_INFO("\tmin_point_per_voxel: %i", min_point_per_voxel);
			vg = new pcl::VoxelGrid<pcl::PointXYZ>();
			vg->setLeafSize(leaf_size, leaf_size, leaf_size);
			//vg->setMinPointPerVoxel(min_point_per_voxel);
			vg->setMinimumPointsNumberPerVoxel(min_point_per_voxel);
		}
	}

	if(nh.hasParam("VoxelGridCovarianceFilter") && !voxelgrid)
	{
		ros::NodeHandle n("~VoxelGridCovarianceFilter");
		n.getParam("enable", voxelgridcov);
		if(voxelgridcov) {
			ROS_INFO("Using VoxelGridCovariance filter:");
			float leaf_size = 0.20;
			n.getParam("leaf_size", leaf_size);
			ROS_INFO("\tLeaf_size: %.2f", leaf_size);
			int min_point_per_voxel = 10;
			n.getParam("leaf_size", leaf_size);
			n.getParam("min_point_per_voxel", min_point_per_voxel);
			vgc = new pcl::VoxelGridCovariance<pcl::PointXYZ>();
			vgc->setLeafSize(leaf_size, leaf_size, leaf_size); //0.01f, 0.01f, 0.01f	//inherited from pcl::VoxelGrid
			vgc->setMinPointPerVoxel(min_point_per_voxel);
		}
	}

	arrow_pub = nh.advertise<visualization_msgs::MarkerArray>( "pcl_filter_arrows", 0);
	//filter_pub = nh.advertise<visualization_msgs::MarkerArray>( "NDTMAP_TRANS", 0);
	pc_pub = nh.advertise<sensor_msgs::PointCloud2>( output_topic, 0);

	ros::Subscriber sub;
	sub  = n.subscribe(pointcloud_topic, 1, pcCallback);

	ros::Subscriber sub2;
	if(use_robot_pose)
		sub2 = n.subscribe(robot_pose_topic, 1, poseCallback);

	ros::spin();

}