//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 integration toolbox - primitive"

#include <iostream>
#include <nt2/integration/primitive.hpp>
#include <nt2/include/functions/quad.hpp>
#include <nt2/integration/output.hpp>
#include <nt2/integration/options.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/bind.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/rowvect.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/log.hpp>
#include <nt2/include/functions/oneminus.hpp>
#include <nt2/include/functions/expm1.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/globalsum.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/mul_i.hpp>
#include <nt2/include/functions/linspace.hpp>
#include <nt2/include/functions/dist.hpp>
#include <nt2/include/functions/rec.hpp>
#include <nt2/include/functions/minusone.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/sqrteps.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/sqrteps.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/table.hpp>
struct f
{
  template < class X > inline
  X operator()(const X & x ) const
  {
    return x;
  }
};

NT2_TEST_CASE_TPL( primitive_cplx_inout2, NT2_REAL_TYPES )
{
  using nt2::primitive;
  using nt2::options;
  using nt2::integration::output;
//  primitive<nt2::tag::quad_>(f(), nt2::linspace(T(0), T(5), 10));

   BOOST_AUTO_TPL(res, (primitive<nt2::tag::quad_>(f(), nt2::linspace(T(0), T(5)))));
   std::cout << "Integrals:" << res.integrals << ") with error " << res.errors
             << " after " << res.eval_count <<  " evaluations\n";
   NT2_TEST_ULP_EQUAL(res.integrals(nt2::end_), T(12.5), 2);
}


#include <sparse.h>
#include <sparse_fill.h>
#include <prox_math_policy.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(prod_mass_vec);
BOOST_AUTO_TEST_CASE(mass_vec_prod_test_case)
{
  typedef prox::MathPolicy<float>           math_policy;
  typedef math_policy::value_traits         value_traits;
  typedef math_policy::diagonal_mass_type   matrix_type;
  typedef math_policy::vector6_type         vector_type;

  matrix_type M(1);
  sparse::fill(M(0,0), value_traits::zero() );// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'
  M(0,0)[0] = value_traits::numeric_cast(2.5);

  BOOST_CHECK_EQUAL( M(0,0)[0] , value_traits::numeric_cast(2.5) );
  BOOST_CHECK_EQUAL( M(0,0)[1] , value_traits::numeric_cast(1) );
  BOOST_CHECK_EQUAL( M(0,0)[2] , value_traits::numeric_cast(2) );
  BOOST_CHECK_EQUAL( M(0,0)[3] , value_traits::numeric_cast(3) );
  BOOST_CHECK_EQUAL( M(0,0)[4] , value_traits::numeric_cast(4) );
  BOOST_CHECK_EQUAL( M(0,0)[5] , value_traits::numeric_cast(5) );
  BOOST_CHECK_EQUAL( M(0,0)[6] , value_traits::numeric_cast(6) );

  vector_type u(1);
  sparse::fill(u(0), 1.0f);// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'

  BOOST_CHECK_EQUAL( u(0)(0) , value_traits::numeric_cast(1) );
  BOOST_CHECK_EQUAL( u(0)(1) , value_traits::numeric_cast(2) );
  BOOST_CHECK_EQUAL( u(0)(2) , value_traits::numeric_cast(3) );
  BOOST_CHECK_EQUAL( u(0)(3) , value_traits::numeric_cast(4) );
  BOOST_CHECK_EQUAL( u(0)(4) , value_traits::numeric_cast(5) );
  BOOST_CHECK_EQUAL( u(0)(5) , value_traits::numeric_cast(6) );

  vector_type r(1);
  sparse::prod(M, u, r);

  BOOST_CHECK_EQUAL( r(0)(0) , 2.5 );
  BOOST_CHECK_EQUAL( r(0)(1) , 5   );
  BOOST_CHECK_EQUAL( r(0)(2) , 7.5 );
  BOOST_CHECK_EQUAL( r(0)(3) , 32  );
  BOOST_CHECK_EQUAL( r(0)(4) , 58  );
  BOOST_CHECK_EQUAL( r(0)(5) , 73  );

  M.resize(2,2);
  sparse::fill(M(1,1), 0.0f);// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'
  M(1,1)[0] = 2.5f;
  u.resize(2);

  sparse::fill(u(1), 7.0f);// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'
  r.clear();
  r.resize(2);
  sparse::prod(M, u, r);

  BOOST_CHECK_EQUAL( r(0)(0) , 2.5 );
  BOOST_CHECK_EQUAL( r(0)(1) , 5   );
  BOOST_CHECK_EQUAL( r(0)(2) , 7.5 );
  BOOST_CHECK_EQUAL( r(0)(3) , 32  );
  BOOST_CHECK_EQUAL( r(0)(4) , 58  );
  BOOST_CHECK_EQUAL( r(0)(5) , 73  );
  BOOST_CHECK_EQUAL( r(1)(0) , 17.5);
  BOOST_CHECK_EQUAL( r(1)(1) , 20  );
  BOOST_CHECK_EQUAL( r(1)(2) , 22.5);
  BOOST_CHECK_EQUAL( r(1)(3) , 68  );
  BOOST_CHECK_EQUAL( r(1)(4) , 124 );
  BOOST_CHECK_EQUAL( r(1)(5) , 157 );

}
BOOST_AUTO_TEST_SUITE_END();


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

#include <graph_localizer/pose_rotation_factor.h>
#include <graph_localizer/rotation_factor_adder.h>
#include <localization_common/logger.h>
#include <localization_measurements/feature_point.h>

#include <gtsam/geometry/PinholePose.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PoseTranslationPrior.h>

#include <boost/make_shared.hpp>

#include <gtest/gtest.h>

namespace gl = graph_localizer;
namespace lm = localization_measurements;
namespace sym = gtsam::symbol_shorthand;
class RotationFactorAdderTester : public ::testing::Test {
 protected:
  virtual void SetUp() {
    gl::FeatureTrackerParams feature_tracker_params;
    feature_tracker_params.sliding_window_duration = 2.0;
    feature_tracker_.reset(new gl::FeatureTracker(feature_tracker_params));
    rotation_factor_adder_params_.enabled = true;
    rotation_factor_adder_params_.huber_k = 1.345;
    rotation_factor_adder_params_.min_avg_disparity = 0.2;
    rotation_factor_adder_params_.rotation_stddev = 0.1;
    rotation_factor_adder_params_.max_percent_outliers = 0.5;
    Eigen::Matrix<double, 4, 4> body_T_nav_cam;
    body_T_nav_cam << 0, 0, 1, 0.1177, 1, 0, 0, -0.0422, 0, 1, 0, -0.0826, 0, 0, 0, 1;
    rotation_factor_adder_params_.body_T_nav_cam = gtsam::Pose3(body_T_nav_cam.matrix());
    rotation_factor_adder_params_.nav_cam_intrinsics = gtsam::Cal3_S2(608.807, 607.614, 0, 0, 0);
    rotation_factor_adder_.reset(new gl::RotationFactorAdder(rotation_factor_adder_params_, feature_tracker_));
  }

  void AddMeasurementsForTwoFrames(const gtsam::Pose3& world_T_cam_1, const gtsam::Pose3& world_T_cam_2,
                                   const int num_points, const localization_common::Time time_diff = 1.0) {
    const gtsam::PinholePose<gtsam::Cal3_S2> cam_1(
      world_T_cam_1, boost::make_shared<gtsam::Cal3_S2>(rotation_factor_adder_params_.nav_cam_intrinsics));
    const gtsam::PinholePose<gtsam::Cal3_S2> cam_2(
      world_T_cam_2, boost::make_shared<gtsam::Cal3_S2>(rotation_factor_adder_params_.nav_cam_intrinsics));
    const double x_min = -1.0;
    const double x_max = 1.0;
    const double y_min = -1.0;
    const double y_max = 1.0;
    const double z_min = 1.0;
    const double z_max = 10.0;
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> x_dis(x_min, x_max);
    std::uniform_real_distribution<> y_dis(y_min, y_max);
    std::uniform_real_distribution<> z_dis(z_min, z_max);
    lm::FeaturePoints feature_points_1;
    lm::FeaturePoints feature_points_2;
    for (int i = 0; i < num_points; ++i) {
      const double x = x_dis(gen);
      const double y = y_dis(gen);
      const double z = z_dis(gen);
      const gtsam::Point3 cam_1_t_point(x, y, z);
      const gtsam::Point3 world_t_point = world_T_cam_1 * cam_1_t_point;
      const auto result_1 = cam_1.projectSafe(world_t_point);
      const auto result_2 = cam_2.projectSafe(world_t_point);
      if (!result_1.second || !result_2.second) {
        continue;
      }
      feature_points_1.emplace_back(result_1.first.x(), result_1.first.y(), 1, i, 0);
      feature_points_2.emplace_back(result_2.first.x(), result_2.first.y(), 2, i, 0.1);
    }
    feature_tracker_->UpdateFeatureTracks(feature_points_1);
    feature_tracker_->UpdateFeatureTracks(feature_points_2);
    ASSERT_EQ(feature_points_1.size(), feature_points_2.size());
    ASSERT_GT(feature_points_1.size(), 5);
  }

  gl::RotationFactorAdderParams rotation_factor_adder_params_;
  std::shared_ptr<gl::FeatureTracker> feature_tracker_;
  std::unique_ptr<gl::RotationFactorAdder> rotation_factor_adder_;
};

TEST_F(RotationFactorAdderTester, CorrectRotation) {
  const gtsam::Pose3 world_T_cam_1;
  const gtsam::Point3 cam_1_t_cam_2(0.1, 0.1, 0.1);
  const gtsam::Rot3 cam_1_R_cam_2(gtsam::Rot3::RzRyRx(3.0 * M_PI / 180.0, 2.0 * M_PI / 180.0, 1.0 * M_PI / 180.0));
  const gtsam::Pose3 cam_1_T_cam_2(cam_1_R_cam_2, cam_1_t_cam_2);
  const gtsam::Pose3 world_T_cam_2 = world_T_cam_1 * cam_1_T_cam_2;
  AddMeasurementsForTwoFrames(world_T_cam_1, world_T_cam_2, 30);
  const auto factors_to_add_vec = rotation_factor_adder_->AddFactors(lm::FeaturePointsMeasurement());
  const auto rotation_factor =
    dynamic_cast<gtsam::PoseRotationFactor*>(factors_to_add_vec.front().Get().front().factor.get());
  ASSERT_TRUE(rotation_factor);
  const auto rotation = rotation_factor->rotation();
  const auto world_T_body_1 = world_T_cam_1 * rotation_factor_adder_params_.body_T_nav_cam.inverse();
  const auto world_T_body_2 = world_T_cam_2 * rotation_factor_adder_params_.body_T_nav_cam.inverse();
  const auto body_1_R_body_2 = (world_T_body_1.inverse() * world_T_body_2).rotation();
  ASSERT_TRUE(rotation.equals(body_1_R_body_2, 1e-6));
}

TEST_F(RotationFactorAdderTester, CorrectOptimization) {
  const gtsam::Pose3 world_T_cam_1;
  const gtsam::Point3 cam_1_t_cam_2(0.1, 0.1, 0.1);
  const gtsam::Rot3 cam_1_R_cam_2(gtsam::Rot3::RzRyRx(3.0 * M_PI / 180.0, 2.0 * M_PI / 180.0, 1.0 * M_PI / 180.0));
  const gtsam::Pose3 cam_1_T_cam_2(cam_1_R_cam_2, cam_1_t_cam_2);
  const gtsam::Pose3 world_T_cam_2 = world_T_cam_1 * cam_1_T_cam_2;
  AddMeasurementsForTwoFrames(world_T_cam_1, world_T_cam_2, 30);
  const auto factors_to_add_vec = rotation_factor_adder_->AddFactors(lm::FeaturePointsMeasurement());
  auto rotation_factor =
    dynamic_cast<gtsam::PoseRotationFactor*>(factors_to_add_vec.front().Get().front().factor.get());
  ASSERT_TRUE(rotation_factor);
  const gtsam::Pose3 world_T_body_1 = world_T_cam_1 * rotation_factor_adder_params_.body_T_nav_cam.inverse();
  const gtsam::Pose3 world_T_body_2 = world_T_cam_2 * rotation_factor_adder_params_.body_T_nav_cam.inverse();
  gtsam::Values values_;
  values_.insert(sym::P(1), world_T_body_1);
  // Use cam_1_T_cam_2 as a pertubation and make sure the optimizer corrects this
  const gtsam::Pose3 world_T_perturbed_body_2 = world_T_body_2 * cam_1_T_cam_2;
  values_.insert(sym::P(2), world_T_perturbed_body_2);
  gtsam::NonlinearFactorGraph graph_;
  const auto noise_1 = gtsam::noiseModel::Unit::Create(6);
  const gtsam::PriorFactor<gtsam::Pose3> world_T_body_1_prior(sym::P(1), world_T_body_1, noise_1);
  graph_.push_back(world_T_body_1_prior);
  const auto noise_2 = gtsam::noiseModel::Unit::Create(3);
  gtsam::PoseTranslationPrior<gtsam::Pose3> pose_translation_prior(sym::P(2), world_T_body_2.translation(), noise_2);
  graph_.push_back(pose_translation_prior);
  rotation_factor->keys() = gtsam::KeyVector({sym::P(1), sym::P(2)});
  graph_.push_back(*rotation_factor);
  gtsam::LevenbergMarquardtOptimizer optimizer(graph_, values_);
  const auto optimized_values = optimizer.optimize();
  const auto optimized_world_T_body_1 = optimized_values.at<gtsam::Pose3>(sym::P(1));
  const auto optimized_world_T_body_2 = optimized_values.at<gtsam::Pose3>(sym::P(2));
  ASSERT_TRUE(optimized_world_T_body_1.equals(world_T_body_1, 1e-6));
  ASSERT_TRUE(optimized_world_T_body_2.equals(world_T_body_2, 1e-6));
}


#ifndef PYTHONIC_INCLUDE_NUMPY_EXPM1_HPP
#define PYTHONIC_INCLUDE_NUMPY_EXPM1_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/expm1.hpp>

namespace pythonic
{

  namespace numpy
  {
#define NUMPY_NARY_FUNC_NAME expm1
#define NUMPY_NARY_FUNC_SYM boost::simd::expm1
#include "pythonic/include/types/numpy_nary_expr.hpp"
  }
}

#endif


#include <boost/test/unit_test.hpp>
#include <vector>

#include "../interface/Analysers/Binned_variable_analyser.h"
#include "../interface/HistHelpers/HistogramManager.h"

using namespace BAT;

struct TestSetup {
	TestSetup() :
					histMan(new HistogramManager()),
					analyser(new Binned_variable_analyser(histMan, "TestBinnedVariableAnalyser")),
					zero_start_var(),
					non_zero_start_var(),
					fit_variable("fit_var", 20, 0.0, 100.),
					zero_var_bins(),
					non_zero_var_bins() {
		boost::array<bool, DataType::NUMBER_OF_DATA_TYPES> seenDataTypes;
		for (unsigned int i = 0; i < DataType::NUMBER_OF_DATA_TYPES; ++i)
			seenDataTypes.at(i) = false;
		seenDataTypes.at(DataType::SingleElectron_Rereco) = true;
		histMan->prepareForSeenDataTypes(seenDataTypes);
		histMan->setCurrentDataType(DataType::SingleElectron_Rereco);
		zero_var_bins.push_back(0);
		zero_var_bins.push_back(1);
		zero_var_bins.push_back(5);
		zero_var_bins.push_back(10);

		non_zero_var_bins.push_back(1);
		non_zero_var_bins.push_back(5);
		non_zero_var_bins.push_back(10);

		zero_start_var = Variable("zero_start_var", zero_var_bins);
		non_zero_start_var = Variable("non_zero_start_var", non_zero_var_bins);

	}

	~TestSetup() {

	}

	HistogramManagerPtr histMan;
	Binned_Variable_analyser_ptr analyser;
	Variable zero_start_var, non_zero_start_var, fit_variable;
	std::vector<double> zero_var_bins, non_zero_var_bins;
};
BOOST_AUTO_TEST_SUITE (TestBinnedVariableAnalyser)
BOOST_FIXTURE_TEST_CASE(test_zero_start_var, TestSetup) {
	TH1F::AddDirectory(false);
	analyser->set_variables(zero_start_var, fit_variable);
	analyser->createHistograms();
	BOOST_CHECK_EQUAL(histMan->size(), zero_var_bins.size());

	// this folder should not exist
	string folder = "TestBinnedVariableAnalyser/" + zero_start_var.name + "_bin_0-0";
	BOOST_CHECK_EQUAL(histMan->hasFolder(folder), false);
	// this should be the first valid bin
	folder = "TestBinnedVariableAnalyser/" + zero_start_var.name + "_bin_0-1";
	BOOST_CHECK_EQUAL(histMan->hasFolder(folder), true);

	// this should go into the first bin
	analyser->analyse(0.5, 3, 1.);
	histMan->setCurrentHistogramFolder(folder);
	// only check further if histograms are created
	BOOST_CHECK_EQUAL(histMan->H1D("fit_var")->Integral(), 1);
}

BOOST_FIXTURE_TEST_CASE(test_non_zero_start_var, TestSetup) {
	analyser->set_variables(non_zero_start_var, fit_variable);
	analyser->createHistograms();
	BOOST_CHECK_EQUAL(histMan->size(), non_zero_var_bins.size());

	// this folder should not exist
	string folder = "TestBinnedVariableAnalyser/" + non_zero_start_var.name + "_bin_0-1";
	BOOST_CHECK_EQUAL(histMan->hasFolder(folder), false);
	// this should be the first valid bin
	folder = "TestBinnedVariableAnalyser/" + non_zero_start_var.name + "_bin_1-5";
	BOOST_CHECK_EQUAL(histMan->hasFolder(folder), true);

	// this should not go anywhere
	analyser->analyse(0.5, 3, 1.);
	histMan->setCurrentHistogramFolder(folder);
	BOOST_CHECK_EQUAL(histMan->H1D("fit_var")->Integral(), 0);

	// but this should
	analyser->analyse(4, 3, 1.);
	histMan->setCurrentHistogramFolder(folder);
	BOOST_CHECK_EQUAL(histMan->H1D("fit_var")->Integral(), 1);
}

BOOST_AUTO_TEST_SUITE_END()


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  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 Willow Garage 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 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <image_geometry/pinhole_camera_model.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <boost/thread.hpp>
#include <depth_image_proc/depth_traits.h>

#include <sensor_msgs/point_cloud2_iterator.h>

namespace depth_image_proc {

    using namespace message_filters::sync_policies;
    namespace enc = sensor_msgs::image_encodings;
    typedef ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> SyncPolicy;

    class PointCloudXyziRadialNodelet : public nodelet::Nodelet
    {
	ros::NodeHandlePtr intensity_nh_;

	// Subscriptions
	boost::shared_ptr<image_transport::ImageTransport> intensity_it_, depth_it_;
	image_transport::SubscriberFilter sub_depth_, sub_intensity_;
	message_filters::Subscriber<sensor_msgs::CameraInfo> sub_info_;

	int queue_size_;

	// Publications
	boost::mutex connect_mutex_;
	typedef sensor_msgs::PointCloud2 PointCloud;
	ros::Publisher pub_point_cloud_;

	typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
	boost::shared_ptr<Synchronizer> sync_;

	std::vector<double> D_;
	boost::array<double, 9> K_;

	int width_;
	int height_;

	cv::Mat transform_;

	virtual void onInit();

	void connectCb();

	void imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
		     const sensor_msgs::ImageConstPtr& intensity_msg_in,
		     const sensor_msgs::CameraInfoConstPtr& info_msg);

	// Handles float or uint16 depths
	template<typename T>
	void convert_depth(const sensor_msgs::ImageConstPtr& depth_msg, PointCloud::Ptr& cloud_msg);

	template<typename T>
	void convert_intensity(const sensor_msgs::ImageConstPtr &inten_msg, PointCloud::Ptr& cloud_msg);

	cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial);

    };

    cv::Mat PointCloudXyziRadialNodelet::initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial)
    {
	int i,j;
	int totalsize = width*height;
	cv::Mat pixelVectors(1,totalsize,CV_32FC3);
	cv::Mat dst(1,totalsize,CV_32FC3);

	cv::Mat sensorPoints(cv::Size(height,width), CV_32FC2);
	cv::Mat undistortedSensorPoints(1,totalsize, CV_32FC2);

	std::vector<cv::Mat> ch;
	for(j = 0; j < height; j++)
	{
	    for(i = 0; i < width; i++)
	    {
		cv::Vec2f &p = sensorPoints.at<cv::Vec2f>(i,j);
		p[0] = i;
		p[1] = j;
	    }
	}

	sensorPoints = sensorPoints.reshape(2,1);

	cv::undistortPoints(sensorPoints, undistortedSensorPoints, cameraMatrix, distCoeffs);

	ch.push_back(undistortedSensorPoints);
	ch.push_back(cv::Mat::ones(1,totalsize,CV_32FC1));
	cv::merge(ch,pixelVectors);

	if(radial)
	{
	    for(i = 0; i < totalsize; i++)
	    {
		normalize(pixelVectors.at<cv::Vec3f>(i),
			  dst.at<cv::Vec3f>(i));
	    }
	    pixelVectors = dst;
	}
	return pixelVectors.reshape(3,width);
    }

    void PointCloudXyziRadialNodelet::onInit()
    {
	ros::NodeHandle& nh         = getNodeHandle();
	ros::NodeHandle& private_nh = getPrivateNodeHandle();

	intensity_nh_.reset( new ros::NodeHandle(nh, "intensity") );
	ros::NodeHandle depth_nh(nh, "depth");
	intensity_it_  .reset( new image_transport::ImageTransport(*intensity_nh_) );
	depth_it_.reset( new image_transport::ImageTransport(depth_nh) );

	// Read parameters
	private_nh.param("queue_size", queue_size_, 5);

	// Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
	sync_.reset( new Synchronizer(SyncPolicy(queue_size_), sub_depth_, sub_intensity_, sub_info_) );
	sync_->registerCallback(boost::bind(&PointCloudXyziRadialNodelet::imageCb, this, _1, _2, _3));

	// Monitor whether anyone is subscribed to the output
	ros::SubscriberStatusCallback connect_cb =
	    boost::bind(&PointCloudXyziRadialNodelet::connectCb, this);
	// Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
	boost::lock_guard<boost::mutex> lock(connect_mutex_);
	pub_point_cloud_ = nh.advertise<PointCloud>("points", 20, connect_cb, connect_cb);
    }

    // Handles (un)subscribing when clients (un)subscribe
    void PointCloudXyziRadialNodelet::connectCb()
    {
	boost::lock_guard<boost::mutex> lock(connect_mutex_);

	if (pub_point_cloud_.getNumSubscribers() == 0)
	{
	    sub_depth_.unsubscribe();
	    sub_intensity_.unsubscribe();
	    sub_info_.unsubscribe();
	}
	else if (!sub_depth_.getSubscriber())
	{
	    ros::NodeHandle& private_nh = getPrivateNodeHandle();
	    // parameter for depth_image_transport hint
	    std::string depth_image_transport_param = "depth_image_transport";

	    // depth image can use different transport.(e.g. compressedDepth)
	    image_transport::TransportHints depth_hints("raw",ros::TransportHints(), private_nh, depth_image_transport_param);
	    sub_depth_.subscribe(*depth_it_, "image_raw",       5, depth_hints);

	    // intensity uses normal ros transport hints.
	    image_transport::TransportHints hints("raw", ros::TransportHints(), private_nh);
	    sub_intensity_.subscribe(*intensity_it_,   "image_raw", 5, hints);
	    sub_info_.subscribe(*intensity_nh_,   "camera_info",      5);
	}
    }

    void PointCloudXyziRadialNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
					      const sensor_msgs::ImageConstPtr& intensity_msg,
					      const sensor_msgs::CameraInfoConstPtr& info_msg)
    {
	PointCloud::Ptr cloud_msg(new PointCloud);
	cloud_msg->header = depth_msg->header;
	cloud_msg->height = depth_msg->height;
	cloud_msg->width  = depth_msg->width;
	cloud_msg->is_dense = false;
	cloud_msg->is_bigendian = false;

	sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
	pcd_modifier.setPointCloud2Fields(4,
					  "x", 1, sensor_msgs::PointField::FLOAT32,
					  "y", 1, sensor_msgs::PointField::FLOAT32,
					  "z", 1, sensor_msgs::PointField::FLOAT32,
					  "intensity", 1, sensor_msgs::PointField::FLOAT32);

	if(info_msg->D != D_ || info_msg->K != K_ || width_ != info_msg->width ||
	   height_ != info_msg->height)
	{
	    D_ = info_msg->D;
	    K_ = info_msg->K;
	    width_ = info_msg->width;
	    height_ = info_msg->height;
	    transform_ = initMatrix(cv::Mat_<double>(3, 3, &K_[0]),cv::Mat(D_),width_,height_,true);
	}

	if (depth_msg->encoding == enc::TYPE_16UC1)
	{
	    convert_depth<uint16_t>(depth_msg, cloud_msg);
	}
	else if (depth_msg->encoding == enc::TYPE_32FC1)
	{
	    convert_depth<float>(depth_msg, cloud_msg);
	}
	else
	{
	    NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
	    return;
	}

	if(intensity_msg->encoding == enc::TYPE_16UC1)
	{
	    convert_intensity<uint16_t>(intensity_msg, cloud_msg);

	}
	else if(intensity_msg->encoding == enc::MONO8)
	{
	    convert_intensity<uint8_t>(intensity_msg, cloud_msg);
	}
	else if(intensity_msg->encoding == enc::TYPE_32FC1)
	{
	    convert_intensity<float>(intensity_msg, cloud_msg);
	}
	else
	{
	    NODELET_ERROR_THROTTLE(5, "Intensity image has unsupported encoding [%s]", intensity_msg->encoding.c_str());
	    return;
	}

	pub_point_cloud_.publish (cloud_msg);
    }

    template<typename T>
    void PointCloudXyziRadialNodelet::convert_depth(const sensor_msgs::ImageConstPtr& depth_msg,
						    PointCloud::Ptr& cloud_msg)
    {
	// Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
	double unit_scaling = DepthTraits<T>::toMeters( T(1) );
	float bad_point = std::numeric_limits<float>::quiet_NaN();

	sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
	sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
	sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
	const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);

	int row_step   = depth_msg->step / sizeof(T);
	for (int v = 0; v < (int)cloud_msg->height; ++v, depth_row += row_step)
	{
	    for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z)
	    {
		T depth = depth_row[u];

		// Missing points denoted by NaNs
		if (!DepthTraits<T>::valid(depth))
		{
		    *iter_x = *iter_y = *iter_z = bad_point;
		    continue;
		}
		const cv::Vec3f &cvPoint = transform_.at<cv::Vec3f>(u,v) * DepthTraits<T>::toMeters(depth);
		// Fill in XYZ
		*iter_x = cvPoint(0);
		*iter_y = cvPoint(1);
		*iter_z = cvPoint(2);
	    }
	}
    }

    template<typename T>
    void PointCloudXyziRadialNodelet::convert_intensity(const sensor_msgs::ImageConstPtr& intensity_msg,
							PointCloud::Ptr& cloud_msg)
    {
	sensor_msgs::PointCloud2Iterator<float> iter_i(*cloud_msg, "intensity");
	const T* inten_row = reinterpret_cast<const T*>(&intensity_msg->data[0]);

	const int i_row_step = intensity_msg->step/sizeof(T);
	for (int v = 0; v < (int)cloud_msg->height; ++v, inten_row += i_row_step)
	{
	    for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_i)
	    {
		*iter_i = inten_row[u];
	    }
	}
    }

} // namespace depth_image_proc

// Register as nodelet
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(depth_image_proc::PointCloudXyziRadialNodelet,nodelet::Nodelet);


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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.
 */

#include "reed_muller_synthesis.hpp"

#include <math.h>

#include <boost/assign/std/vector.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/algorithm/find_if.hpp>
#include <boost/range/algorithm/for_each.hpp>
#include <boost/range/irange.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/variant.hpp>

#include <core/functor.hpp>
#include <core/utils/timer.hpp>

#include <reversible/functions/add_gates.hpp>
#include <reversible/functions/clear_circuit.hpp>
#include <reversible/functions/copy_metadata.hpp>
#include <reversible/functions/fully_specified.hpp>
#include <reversible/io/print_circuit.hpp>

#include "synthesis_utils_p.hpp"

using namespace boost::assign;

namespace cirkit
{

  using spectra_t = std::vector<boost::dynamic_bitset<>>;

  struct to_value
  {
    using result_type = bool;

    bool operator()( const boost::optional<bool>& b ) const
    {
      return *b;
    }
  };

  template<typename CubeIterator>
  unsigned long cube_to_value( CubeIterator first, CubeIterator second )
  {
    boost::dynamic_bitset<> input;

    using boost::adaptors::transformed;
    boost::for_each( boost::make_iterator_range( first, second ) | transformed( to_value() ), [&input]( bool b ) { input.push_back( b ); } );
    return input.to_ulong();
  }

  void apply_cnot( spectra_t& f, unsigned c, unsigned t )
  {
    for ( auto& row : f )
    {
      row[t] = row[t] ^ row[c];
    }
  }

  boost::dynamic_bitset<> multiply_columns( const spectra_t& f, const std::vector<unsigned>& columns )
  {
    boost::dynamic_bitset<> m( f.size() );

    // Initial
    for ( unsigned r = 0u; r < f.size(); ++r )
    {
      m.set( r, f[r].test( columns.at( 0u ) ) );
    }

    for ( unsigned i = 1u; i < columns.size(); ++i )
    {
      boost::dynamic_bitset<> mnew( f.size(), 0 );

      for ( unsigned r = 0u; r < f.size(); ++r )
      {
        if ( m.test( r ) )
        {
          for ( unsigned r2 = 0u; r2 < f.size(); ++r2 )
          {
            if ( f[r2].test( columns.at( i ) ) )
            {
              mnew.flip( r | r2 );
            }
          }
        }
      }

      m = mnew;
    }

    return m;
  }

  void apply_toffoli( spectra_t& f, const std::vector<unsigned>& controls, unsigned t )
  {
    boost::dynamic_bitset<> c = multiply_columns( f, controls );

    for ( unsigned r = 0u; r < f.size(); ++r )
    {
      f[r][t] = f[r][t] ^ c[r];
    }
  }

  void apply_toffoli_front( spectra_t& f, const std::vector<unsigned>& controls, unsigned t )
  {
    if ( f.empty() ) return;

    // Control Mask
    unsigned cmask = 0u;
    for ( unsigned c : controls )
    {
      cmask |= 1u << c;
    }

    // Target Mask
    unsigned tmask = 1u << t;

    // for each column
    for ( unsigned j : boost::irange( 0u, (unsigned)f[0u].size() ) )
    {
      // for each row
      for ( unsigned r : boost::irange( 0u, (unsigned)f.size() ) )
      {
        // match?
        if ( ( r & tmask ) && f[r].test( j ) )
        {
          // Clear the bit
          unsigned mask = r & ~tmask;
          f[mask | cmask].flip( j );
        }
      }
    }
  }

  void apply_gate( circuit& circ, const std::vector<spectra_t*>& funcs, unsigned offset, unsigned& insert_at, const boost::variant<std::vector<unsigned>, unsigned>& controls, unsigned t )
  {
    std::vector<unsigned> _controls;

    if ( const std::vector<unsigned>* op = boost::get<std::vector<unsigned> >( &controls ) )
    {
      _controls.assign( op->begin(), op->end() );
    }
    else if ( const unsigned* op = boost::get<unsigned>( &controls ) )
    {
      _controls += *op;
    }

    switch ( _controls.size() )
    {
    case 0u:
      insert_not( circ, insert_at, t );
      insert_at += offset;
      funcs[offset]->at( 0u ).reset( t );
      apply_toffoli_front( *funcs[1u - offset], _controls, t );
      break;

    case 1u:
      insert_cnot( circ, insert_at, _controls.at( 0u ), t );
      insert_at += offset;
      apply_cnot( *funcs[offset], _controls.at( 0u ), t );
      apply_toffoli_front( *funcs[1u - offset], _controls, t );
      break;

    default:
      insert_toffoli( circ, insert_at, _controls, t );
      insert_at += offset;
      apply_toffoli( *funcs[offset], _controls, t );
      apply_toffoli_front( *funcs[1u - offset], _controls, t );
      break;
    }
  }

  void print_spectra( const spectra_t& f )
  {
    for ( unsigned i = 0u; i < f.size(); ++i )
    {
      std::cout << f[i] << std::endl;
    }
  }

  bool reed_muller_synthesis( circuit& circ, const binary_truth_table& spec, properties::ptr settings, properties::ptr statistics )
  {

    // Settings parsing
    const auto bidirectional = get( settings, "bidirectional", true );

    // Run-time measuring
    properties_timer t( statistics );

    // circuit has to be empty
    clear_circuit( circ );

    // truth table has to be fully specified
    if ( !fully_specified( spec ) )
    {
      set_error_message( statistics, "truth table `spec` is not fully specified." );
      return false;
    }

    // Determine Function Vectors from Specification
    unsigned n = spec.num_outputs();
    spectra_t func( 1u << n, boost::dynamic_bitset<>( n ) );
    spectra_t ifunc( 1u << n, boost::dynamic_bitset<>( n ) );

    for ( binary_truth_table::const_iterator it = spec.begin(); it != spec.end(); ++it )
    {
      unsigned long ipos = cube_to_value( it->first.first, it->first.second );
      binary_truth_table::cube_type output( it->second.first, it->second.second );

      for ( unsigned i = 0u; i < n; ++i )
      {
        func[ipos].set( i, *output.at( i ) );
      }

      if ( bidirectional )
      {
        unsigned long opos = cube_to_value( it->second.first, it->second.second );
        binary_truth_table::cube_type input( it->first.first, it->first.second );
        for ( unsigned i = 0u; i < n; ++i )
        {
          ifunc[opos].set( i, *input.at( i ) );
        }
      }
    }

    // Determine Reed Muller Spectra fom Function Vectors
    {
      unsigned i, j, k, m, p;
      for ( m = 1u; m < ( 1u << n ); m = 2 * m )
      {
        for ( i = 0u; i < ( 1u << n ); i = i + 2 * m )
        {
          for ( j = i, p = k = i + m; j < p; j = j + 1, k = k + 1 )
          {
            func[k] = func[k] ^ func[j];
            if ( bidirectional )
            {
              ifunc[k] = ifunc[k] ^ ifunc[j];
            }
          }
        }
      }
    }

    // Synthesis
    // copy metadata
    circ.set_lines( n );
    copy_metadata( spec, circ );

    std::vector<spectra_t*> funcs;
    funcs += &func,&ifunc;
    unsigned insert_at = 0u;

    // Step A (i = 0)
    for ( unsigned j = 0u; j < n; ++j )
    {
      unsigned offset = bidirectional && ( ifunc[0u].count() < func[0u].count() ) ? 1u : 0u;

      if ( funcs[offset]->at( 0u ).test( j ) )
      {
        apply_gate( circ, funcs, offset, insert_at, std::vector<unsigned>(), j );
      }
    }

    for ( unsigned i = 1u; i < ( 1u << n ) - 1; ++i )
    {
      // Step B (i = 2^(k-1), variable rows)
      if ( ( log( i ) / log( 2.0 ) ) == ceil( ( log( i ) / log( 2.0 ) ) ) )
      {
        unsigned k = (unsigned)ceil( ( log( i ) / log( 2.0 ) ) );
        unsigned offset = bidirectional && ( hamming_distance( i, ifunc[0u].to_ulong() ) < hamming_distance( i, func[0u].to_ulong() ) ) ? 1u : 0u;
        const boost::dynamic_bitset<>& func_offset = funcs[offset]->at( i );
        if ( !func_offset.test( k ) )
        {
          using boost::adaptors::reversed;

          unsigned s = *boost::find_if( boost::irange( 0u, n ) | reversed, [&func_offset]( unsigned j ) { return func_offset.test( j ); } );
          apply_gate( circ, funcs, offset, insert_at, s, k );
        }

        for ( unsigned j = 0u; j < n; ++j )
        {
          if ( j != k && func_offset.test( j ) )
          {
            apply_gate( circ, funcs, offset, insert_at, k, j );
          }
        }
      }
      // Step C (i != 2^(k-1), non-variable rows)
      else
      {
        unsigned offset = bidirectional && ( hamming_distance( i, ifunc[0u].to_ulong() ) < hamming_distance( i, func[0u].to_ulong() ) ) ? 1u : 0u;

        // already empty?
        if ( funcs[offset]->at( i ).none() ) continue;

        // Find s?
        using boost::adaptors::reversed;
        unsigned s = n;
        for ( unsigned j : boost::irange( 0u, n ) | reversed )
        {
          if ( funcs[offset]->at( i ).test( j ) && ( i & ( 1u << j ) ) == 0u )
          {
            s = j;
            break;
          }
        }

        assert( s != n );

        // Before CNOTs
        std::vector<unsigned> targets;
        for ( unsigned j = 0u; j < n; ++j )
        {
          if ( j != s && funcs[offset]->at( i ).test( j ) )
          {
            apply_gate( circ, funcs, offset, insert_at, s, j );
            targets += j;
          }
        }

        // Toffoli Gate
        std::vector<unsigned> controls;
        for ( unsigned j = 0u; j < n; ++j )
        {
          if ( i & ( 1u << j ) )
          {
            controls += j;
          }
        }
        apply_gate( circ, funcs, offset, insert_at, controls, s );

        // After CNOTs
        for ( unsigned j : targets )
        {
          apply_gate( circ, funcs, offset, insert_at, s, j );
        }
      }
    }

    return true;
  }

  truth_table_synthesis_func reed_muller_synthesis_func( properties::ptr settings, properties::ptr statistics )
  {
    truth_table_synthesis_func f = [&settings, &statistics]( circuit& circ, const binary_truth_table& spec ) {
      return reed_muller_synthesis( circ, spec, settings, statistics );
    };
    f.init( settings, statistics );
    return f;
  }

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE RecomputeTestPopxStandardCalls0

#include <boost/test/unit_test.hpp>
#include <iostream>
#include <vector>
#include <popart/testdevice.hpp>

#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/op.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/l1.hpp>
#include <popart/sgd.hpp>

#define private public
#define protected public
#include <popart/popx/devicex.hpp>
#include <popart/popx/irlowering.hpp>
#include <popart/session.hpp>
#undef private
#undef public

using namespace popart;

TensorId conv(Builder *b, TensorId act, ConstVoidData wdata) {
  auto aiOnnx  = b->aiOnnxOpset9();
  auto weights = b->addInitializedInputTensor(wdata);
  act = aiOnnx.conv({act, weights}, {1, 1}, 1, {}, {0, 0, 0, 0}, {1, 1});
  return act;
}

TensorId batchnormalization(Builder *b, TensorId act, ConstVoidData bndata) {
  auto aiOnnx = b->aiOnnxOpset9();
  auto scale  = b->addInitializedInputTensor(bndata);
  auto bias   = b->addInitializedInputTensor(bndata);
  auto mean   = b->addInitializedInputTensor(bndata);
  auto var    = b->addInitializedInputTensor(bndata);
  auto bn_out = aiOnnx.batchnormalization({act, scale, bias, mean, var}, 5);
  act         = bn_out.at(0);
  return act;
}

BOOST_AUTO_TEST_CASE(RecomputeTestPopxStandardCalls0) {

  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();
  TensorInfo input_info{"FLOAT", std::vector<int64_t>{1, 4, 32, 32}};

  // convolution,  4 channels -> 4 channels
  TensorInfo weights_info{"FLOAT", std::vector<int64_t>{4, 4, 1, 1}};
  float weight_vals[4 * 4 * 1 * 1] = {0};
  ConstVoidData weight_data        = {weight_vals, weights_info};

  // batch-normalization
  TensorInfo bn_info{"FLOAT", std::vector<int64_t>{4}};
  float bn_vals[4]      = {0};
  ConstVoidData bn_data = {bn_vals, bn_info};

  int nLayers  = 8;
  TensorId act = builder->addInputTensor(input_info);
  for (int i = 0; i < nLayers; ++i) {
    TensorId skip = act;
    // residual 1:
    act           = conv(builder.get(), skip, weight_data);
    act           = batchnormalization(builder.get(), act, bn_data);
    TensorId res1 = aiOnnx.relu({act});

    // residual 2 (flip relu and batchnorm)
    act           = conv(builder.get(), skip, weight_data);
    act           = aiOnnx.relu({act});
    TensorId res2 = batchnormalization(builder.get(), act, bn_data);

    act = aiOnnx.add({res1, res2});
    act = aiOnnx.add({act, skip});
    act = aiOnnx.sigmoid({act});
  }
  auto l1 = builder->aiGraphcoreOpset1().l1loss({act}, 0.1);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  auto art       = AnchorReturnType("All");
  auto dataFlow  = DataFlow(1, {{act, art}});
  auto optimizer = ConstSGD(0.01);

  auto device = popart::createTestDevice(TEST_TARGET);

  auto opts              = SessionOptions();
  opts.autoRecomputation = RecomputationType::Standard;
  opts.enableOutlining   = false;

  auto session = popart::TrainingSession::createFromOnnxModel(
      proto,
      dataFlow,
      l1,
      optimizer,
      device,
      InputShapeInfo(),
      opts,
      Patterns::create({"PostNRepl", "ConvDataGrad"})
          .enableRuntimeAsserts(false));
  session->prepareDevice();

  popart::popx::Devicex *devicex =
      dynamic_cast<popart::popx::Devicex *>(session->device_.get());

  // we count how many times each op appears
  std::map<Op *, int, POpCmp> counts =
      devicex->lowering().getMainGraphOpCounts();

  int nOnes = 0;
  int nTwos = 0;
  // for norm only, we expect all Norm ops to appear twice, all others once
  for (auto op_count : counts) {
    if (op_count.second == 2) {
      ++nTwos;
    }
    if (op_count.second == 1) {
      ++nOnes;
    }
  }

  BOOST_CHECK(nTwos > 0);
  // more non-recomputed than recomputed (including backwards pass)
  BOOST_CHECK(nOnes > nTwos);
}


#pragma once
#include <Eigen/Core>
#include <memory>
#include <vector>

#include "../PointCLoud/Geometry.hpp"
#include "KDTreeSearchParam.hpp"

namespace flann {
	template <typename T>
	class Matrix;
	template <typename T>
	struct L2;
	template <typename T>
	class Index;
}  // namespace flann

namespace pointcloudhandler
{

		class KDTreeFlann {
		public:
			KDTreeFlann();
			KDTreeFlann(const Eigen::MatrixXd& data);
			KDTreeFlann(const Geometry& geometry);
			~KDTreeFlann();
			KDTreeFlann(const KDTreeFlann& ) = delete;
			KDTreeFlann& operator=(const KDTreeFlann& ) = delete;

		public:
			bool SetMatrixData(const Eigen::MatrixXd& data);
			bool SetGeometry(const Geometry& geometry);

			template <typename T>
			int Search(const T& query, const KDTreeSearchParam& param,
				std::vector<int>& indices, std::vector<double>& distance2) const;

			template <typename T>
			int SearchKNN(const T& query, int knn,
				std::vector<int>& indices, std::vector<double>& distance2) const;

			template <typename T>
			int SearchRadius(const T& query, double radius,
				std::vector<int>& indices, std::vector<double>& distance2) const;

		private:
			bool SetRawData(const Eigen::Map<const Eigen::MatrixXd>& data);

		protected:
			std::vector<double> mData;
			std::unique_ptr<flann::Matrix<double>> mFlannDataset;
			std::unique_ptr<flann::Index<flann::L2<double>>> mFlannIndex;
			size_t mDimension = 0;
			size_t mDatasetSize = 0;
		};

}  // namespace pointcloudhandler


/*
 * protonReconstruct.cpp
 *
 *  Created on: Dec 22, 2011
 *      Author: u051747
 */
#include <iostream>
#include "parameterparser.h"
#include "cuNDArray.h"
#include "hoCuNDArray.h"

#include "cuNDArray.h"
#include "cuNDArray_math.h"
#include "cuNDArray_fileio.h"

#include "protonSubsetOperator.h"
#include "hoNDArray_fileio.h"
#include "check_CUDA.h"

#include "hoCuGPBBSolver.h"
#include "hoCuPartialDerivativeOperator.h"
#include "hoCuNDArray_blas.h"

#include "protonDROPSolver.h"
#include "hdf5_utils.h"
#include "BILBSolver.h"
#include "cuSolverUtils.h"

#include "encodingOperatorContainer.h"
#include "hoCuOperator.h"
#include "hoImageOperator.h"
#include "identityOperator.h"
#include <boost/program_options.hpp>
#include "vector_td_io.h"

#include "hoCuTvOperator.h"
#include "hoCuTvPicsOperator.h"
#include "projectionSpaceOperator.h"

using namespace std;
using namespace Gadgetron;
typedef float _real;

typedef solver<hoCuNDArray<_real>, hoCuNDArray<_real> > baseSolver;
namespace po = boost::program_options;
int main( int argc, char** argv)
{

	//
	// Parse command line
	//

	_real background =  0.00106;
	std::string dataName;
	std::string outputFile;
	vector_td<int,3> dimensions;
	vector_td<float,3> physical_dims;
	vector_td<float,3> origin;
	float beta,gamma;
	float huber;
	int iterations;
	int device;
	int subsets;
	bool use_hull,use_weights, use_non_negativity;
	float tv_weight;
	po::options_description desc("Allowed options");
	desc.add_options()
					("help", "produce help message")
					("data,D", po::value<std::string>(&dataName)->default_value("data.hdf5"), "HDF5 file containing projections and splines")
					("dimensions,d", po::value<vector_td<int,3> >(&dimensions)->default_value(vector_td<int,3>(512,512,1)), "Pixel dimensions of the image")
					("size,S", po::value<vector_td<float,3> >(&physical_dims)->default_value(vector_td<float,3>(20,20,5)), "Dimensions of the image")
					("center,c", po::value<vector_td<float,3> >(&origin)->default_value(vector_td<float,3>(0,0,0)), "Center of the reconstruction")
					("iterations,i", po::value<int>(&iterations)->default_value(10), "Dimensions of the image")
					("output,f", po::value<std::string>(&outputFile)->default_value("image.hdf5"), "Output filename")
					("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
					("TV,T",po::value<float>(&tv_weight)->default_value(0),"TV Weight ")
					("subsets,n", po::value<int>(&subsets)->default_value(10), "Number of subsets to use")
					("use_hull",po::value<bool>(&use_hull)->default_value(true),"Estimate convex hull of object")
					("use_weights",po::value<bool>(&use_weights)->default_value(false),"Use weights if available. ")
					("use_non_negativity",po::value<bool>(&use_non_negativity)->default_value(true),"Use non-negativity constraint. ")
					("dump","Will dump all iterations")

					;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		std::cout << it->first << ": ";
		if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
		else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
		else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
		else if (a.type() == typeid(bool)) std::cout << it->second.as<bool>();
		else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
		else std::cout << "Unknown type" << std::endl;
		std::cout << std::endl;
	}
	cudaSetDevice(device);
	//cudaDeviceReset();

	protonDROPSolver<cuNDArray > solver;
	solver.set_beta(1.9f);

	solver.set_non_negativity_constraint(use_non_negativity);
	solver.set_max_iterations(iterations);
	solver.set_dump(vm.count("dump") > 0);

	std::vector<size_t> rhs_dims(&dimensions[0],&dimensions[3]); //Quick and dirty vector_td to vector

	boost::shared_ptr<protonDataset<cuNDArray> >  data(new protonDataset<cuNDArray>(dataName,use_weights));

	data = protonDataset<cuNDArray>::shuffle_dataset(data,subsets);

	data->preprocess(rhs_dims,physical_dims,use_hull);

	std::cout << "Size: " << data->get_projections()->get_number_of_elements() << std::endl;

	boost::shared_ptr< protonSubsetOperator<cuNDArray> > E (new protonSubsetOperator<cuNDArray>(data->get_subsets(), physical_dims) );

	E->set_domain_dimensions(&rhs_dims);
	E->set_codomain_dimensions(data->get_projections()->get_dimensions().get());
  if (vm.count("TV")){
	  std::cout << "Total variation regularization in use" << std::endl;
	  boost::shared_ptr<cuTvOperator<float,3> > tv(new cuTvOperator<float,3>);
	  solver.set_reg_op(tv);
  }

	solver.set_encoding_operator(E);
	solver.set_output_mode(baseSolver::OUTPUT_VERBOSE);
	if (data->get_weights()) *data->get_projections() *= *data->get_weights();

	//fill(data->get_projections().get(),1.0f);
	boost::shared_ptr<cuNDArray<float> > result;
	{
		GPUTimer timer("Reconstruction time");
		result = solver.solve(data->get_projections().get());
	}

	boost::shared_ptr< hoNDArray<float> > host_result = result->to_host();
	//write_nd_array<_real>(result.get(), (char*)parms.get_parameter('f')->get_string_value());
	std::stringstream ss;
	for (int i = 0; i < argc; i++){
		ss << argv[i] << " ";
	}
	saveNDArray2HDF5<3>(host_result.get(),outputFile,physical_dims,origin,ss.str(), solver.get_max_iterations());
}


#include <gtest/gtest.h>
#include <boost/math/quaternion.hpp>

#include "test_util.h"

#include "ApproachCube.hpp"
#include "SwarmieSensors.hpp"
#include "Tag.hpp"

class ApproachCubeTest : public testing::Test
{
protected:
   SwarmieSensors sensors;
   ApproachCube approach;
   boost::math::quaternion<double> defaultOrientation;
   ApproachCubeTest() : defaultOrientation(1.2, 1.2, 1.2, 2.1) {
      approach.Update(sensors, SwarmieAction());
   }
};

TEST_F(ApproachCubeTest, noCubeNoMovement)
{
   EXPECT_FALSE(is_moving(approach.GetAction()));
}

TEST_F(ApproachCubeTest, nestTagOnlyNoMovement)
{
   Tag t = tag_top_left(Tag::NEST_TAG_ID);
   sensors.DetectedTag(t);
   approach.Update(sensors, SwarmieAction());
   for(int i = 0; i < 30 && !is_moving(approach.GetAction()); i++) {
      approach.Update(sensors, SwarmieAction());
   }
   EXPECT_FALSE(is_moving(approach.GetAction()));
}

TEST_F(ApproachCubeTest, nestTagAlignedNoMovement)
{
   Tag t(Tag::NEST_TAG_ID, -0.023, -0.12, 0.5, defaultOrientation);
   ASSERT_TRUE(fabs(t.Alignment()) < 0.01) << "Tag not aligned: " << t.Alignment() << std::endl;
   sensors.DetectedTag(t);
   approach.Update(sensors, SwarmieAction());
   for(int i = 0; i < 30 && !is_moving(approach.GetAction()); i++) {
      approach.Update(sensors, SwarmieAction());
   }
   EXPECT_FALSE(is_moving(approach.GetAction()));
}

TEST_F(ApproachCubeTest, tagMisalignedNoMovement)
{
   Tag t = tag_top_left(Tag::CUBE_TAG_ID);
   sensors.DetectedTag(t);
   approach.Update(sensors, SwarmieAction());
   for(int i = 0; i < 30 && !is_moving(approach.GetAction()); i++) {
      approach.Update(sensors, SwarmieAction());
   }
   EXPECT_FALSE(is_moving(approach.GetAction()));
}

TEST_F(ApproachCubeTest, tagAlignedMovement)
{
   Tag t(Tag::CUBE_TAG_ID, -0.022, -0.12, 0.4, defaultOrientation);
   ASSERT_TRUE(fabs(t.Alignment()) < 0.01) << "Tag not aligned: " << t.Alignment() << std::endl;
   sensors.DetectedTag(t);
   approach.Update(sensors, SwarmieAction());
   for(int i = 0; i < 30 && !is_moving(approach.GetAction()); i++) {
      approach.Update(sensors, SwarmieAction());
   }
   EXPECT_TRUE(is_moving(approach.GetAction()));
}

TEST_F(ApproachCubeTest, movementStopsWhenTagVanishes)
{
   Tag t(Tag::CUBE_TAG_ID, -0.022, -0.12, 0.4, defaultOrientation);
   sensors.DetectedTag(t);
   approach.Update(sensors, SwarmieAction());
   for(int i = 0; i < 30 && !is_moving(approach.GetAction()); i++) {
      approach.Update(sensors, SwarmieAction());
   }
   EXPECT_TRUE(is_moving(approach.GetAction()));
   sensors.ClearDetections();
   // Update many times to test that the integral term is no longer
   // having an effect.
   for(int i = 0; i < 1000; i++) {
      approach.Update(sensors, SwarmieAction());
   }
   EXPECT_FALSE(is_moving(approach.GetAction()));
}

// This is the actual position of cubes when this bug is triggered.
// Tag[0]{ alignment: -0.0582302 | position: (-0.0812302, -0.0454901, 0.426023) | Orientation: (0.954242,-0.0389091,0.160215,-0.24948)}n
// Tag[0]{ alignment: 0.00210978 | position: (-0.0208902, -0.0438522, 0.423104) | Orientation: (0.0378738,0.948888,0.2493,0.189805)}
TEST_F(ApproachCubeTest, approachWhenBackgroundTagMisalignedToLeft)
{
   Tag aligned(Tag::CUBE_TAG_ID, -0.0208902, -0.0438522, 0.423104,
               boost::math::quaternion<double>(0.0378738,0.948888,0.2493,0.189805));
   Tag background(Tag::CUBE_TAG_ID, -0.0812302, -0.0454901, 0.426023,
                  boost::math::quaternion<double>(0.954242,-0.0389091,0.160215,-0.24948));

   sensors.DetectedTag(background);
   sensors.DetectedTag(aligned);

   approach.Update(sensors, SwarmieAction());

   for(int i = 0; i < 30 && !is_moving(approach.GetAction()); i++) {
      approach.Update(sensors, SwarmieAction());
   }
   EXPECT_TRUE(is_moving(approach.GetAction()));

   sensors.ClearDetections();
   approach.Update(sensors, SwarmieAction());

   sensors.DetectedTag(aligned);
   sensors.DetectedTag(background);

   approach.Update(sensors, SwarmieAction());

   for(int i = 0; i < 30 && !is_moving(approach.GetAction()); i++) {
      approach.Update(sensors, SwarmieAction());
   }
   EXPECT_TRUE(is_moving(approach.GetAction()));
}

TEST_F(ApproachCubeTest, approachWhenBackgroundTagMisalignedToRight)
{
   Tag aligned(Tag::CUBE_TAG_ID, -0.0208902, -0.0438522, 0.423104,
               boost::math::quaternion<double>(0.0378738,0.948888,0.2493,0.189805));
   Tag background(Tag::CUBE_TAG_ID, 0.1012302, -0.0454901, 0.426023,
                  boost::math::quaternion<double>(0.954242,-0.0389091,0.160215,-0.24948));

   sensors.DetectedTag(background);
   sensors.DetectedTag(aligned);

   approach.Update(sensors, SwarmieAction());

   for(int i = 0; i < 30 && !is_moving(approach.GetAction()); i++) {
      approach.Update(sensors, SwarmieAction());
   }
   EXPECT_TRUE(is_moving(approach.GetAction()));

   sensors.ClearDetections();
   approach.Update(sensors, SwarmieAction());

   sensors.DetectedTag(aligned);
   sensors.DetectedTag(background);

   approach.Update(sensors, SwarmieAction());

   for(int i = 0; i < 30 && !is_moving(approach.GetAction()); i++) {
      approach.Update(sensors, SwarmieAction());
   }
   EXPECT_TRUE(is_moving(approach.GetAction()));
}


/*
 * IMath.hpp
 *
 *  Created on: May 18, 2014
 *      Author: igoraraujo
 */

#ifndef IMATH_HPP_
#define IMATH_HPP_

#include "IBlock.hpp"
#include "ISourceErrorException.hpp"
#include <vector>
#include <cstdlib>
#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>

/* Typedef para simplificação da definição do tipo tokenizer da biblioteca boost */
typedef boost::tokenizer<boost::char_separator<char>> Tokenizer;

/* Classe IMath, que analisa e verifica expressões matemáticas */
class IMath{
private:
	/* String que representa a expressão como um todo */
	std::string expression;

	/* Vetor de variáveis existentes na expressão */
	std::vector<std::string> varList;

	/* Variáveis de controle do tipo de saida do calculo */
	int mathType = TYPE_INTEGER;
	int outType = TYPE_REAL;

public:
	/* Possíveis tipos */
	const static int TYPE_INTEGER = 0;
	const static int TYPE_REAL = 1;
	const static int TYPE_STRING = 2;

public:

	/* Construtor do objeto IMath, a partir de uma expressão completa */
	IMath(std::string *newExpression){
		expression.clear();
		expression.push_back('(');
		expression.append(*newExpression);
		expression.push_back(')');
	}

	int getMathType(){
		return mathType;
	}

	void setOutType(std::string *type){
		if(*type == "inteiro"){
			outType = TYPE_INTEGER;
		}
	}

	/* Método estatico, de apoio que verifica se uma determinada string
	 * é um número ou não. Utiliza-se o método da biblioteca padrão
	 * stof(string, size_t)
	 */
	static bool isNumber(std::string numberStr){
		try{
			std::string::size_type sz;
			std::stof(numberStr, &sz);
			return true;
		}catch(std::invalid_argument& e){
			return false;
		}catch(std::out_of_range& ex){
			std::cout << "Erro ao avaliar string " << numberStr << std::endl;
			return false;
		}
	}

	/* Método estatico, de apoio que verifica se uma determinada string
	 * é um número INTEIRO ou não.
	 */
	static bool isInteger(std::string numberStr){
		if(isNumber(numberStr)){
			if(numberStr.find(".") != std::string::npos || numberStr.find("e") != std::string::npos ||
					numberStr.find("E") != std::string::npos)
				return false;

			return true;
		}

		return false;
	}

	/* Método que realiza a análise da expressão matemática em questão e retorna a expressão completa */
	std::string checkAndGetExpression(IBlock *ownerBlock, int myLine) {

		/* Utiliza-se um tokenizer que quebra a expressão matemática nos seus operadores */
		boost::char_separator<char> separator("+-*/() ");
		Tokenizer tokens(expression, separator);

		/* Para cada token, verificamos se exitem erros, como variáveis não declaradas
		 * ou valores String
		 */
		for(auto tokenIterator = tokens.begin(); tokenIterator != tokens.end(); tokenIterator++){
			/* Se o token atual NÃO for um NUMERO, deve ser uma variável */
			if(!isNumber(*tokenIterator)){
				/* Portanto obtemos a variavel */
				IData *data = ownerBlock->getLocalVariable(*tokenIterator);
				std::string errorMsg;

				/* Se não for uma variável, então é uma string ou a variável não foi declarada
				 * Então geramos o erro aproriado
				 */
				if(data == NULL){
					if(tokenIterator->find("\"") != std::string::npos)
						errorMsg = "Impossivel utilizar uma string em uma expressao matematica (valor " + *tokenIterator + ")";
					else
						errorMsg = "Variavel " + *tokenIterator + " nao declarada.";

					throw ISourceErrorException(errorMsg, myLine);
				}
				/* Se for uma variavel string, também geramos um erro.*/
				else if(data->getType() == IString::myType){
					errorMsg = "Impossivel usar uma string em uma expressao matematica (variavel " + *tokenIterator + ")" ;
					throw ISourceErrorException(errorMsg, myLine);
				}
				else if(data->getType() == IReal::myType)
					mathType = TYPE_REAL;
			}else if(!IMath::isInteger(*tokenIterator))
				mathType = TYPE_REAL;
		}

		if(outType == TYPE_INTEGER){
			expression.insert(0, "(int)");
		}

		return expression;
	}

};

#endif /* IMATH_HPP_ */


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

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

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

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

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_MULTILINESTRING_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_MULTILINESTRING_HPP

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/algorithms/expand.hpp>

#include <boost/geometry/algorithms/detail/envelope/linestring.hpp>
#include <boost/geometry/algorithms/detail/envelope/range.hpp>

#include <boost/geometry/algorithms/dispatch/envelope.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace envelope
{

struct envelope_range_expand_policy
{
    template <typename Box, typename Geometry>
    static inline void apply(Box& mbr, Geometry const& geometry)
    {
        Box mbr2;
        envelope_range<>::apply(geometry, mbr2);
        geometry::expand(mbr, mbr2);
    }
};

template <typename CSTag>
struct envelope_multilinestring
    : envelope_range<envelope_range_expand_policy>
{};

template <>
struct envelope_multilinestring<spherical_equatorial_tag>
    : envelope_linear_on_spheroid
{};

}} // namespace detail::envelope
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename MultiLinestring>
struct envelope<MultiLinestring, multi_linestring_tag>
    : detail::envelope::envelope_multilinestring
        <
            typename cs_tag<MultiLinestring>::type
        >
{};

} // namespace dispatch
#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_MULTILINESTRING_HPP


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include <catch.hpp>

#include <cmath>
#include <vector>

#include <Eigen/Core>

#include "bi_operators/CollocationIntegrator.hpp"
#include "green/DerivativeTypes.hpp"
#include "cavity/GePolCavity.hpp"
#include "utils/Molecule.hpp"
#include "green/Vacuum.hpp"
#include "green/UniformDielectric.hpp"
#include "solver/IEFSolver.hpp"
#include "TestingMolecules.hpp"

/*! \class IEFSolver
 *  \test \b C2H4GePolD2h tests IEFSolver using C2H4 with a GePol cavity in D2h symmetry
 */
TEST_CASE("Test solver for the IEFPCM and the C2H4 molecule in D2h symmetry", "[iefpcm][iefpcm_symmetry][iefpcm_gepol-C2H4_D2h]")
{
  Molecule molec = C2H4();
  double area = 0.2 / bohr2ToAngstrom2();
  double probeRadius = 1.385 / bohrToAngstrom();
  double minRadius = 100.0 / bohrToAngstrom();
  GePolCavity cavity = GePolCavity(molec, area, probeRadius, minRadius, "ief_d2h_noadd");

  double permittivity = 78.39;
  Vacuum<> gf_i;
  UniformDielectric<> gf_o(permittivity);
  bool symm = true;
  IEFSolver solver(symm);
  solver.buildSystemMatrix(cavity, gf_i, gf_o);

  double Ccharge = 6.0;
  double Hcharge = 1.0;
  int size = cavity.size();
  Eigen::VectorXd fake_mep = computeMEP(molec, cavity.elements());
  // The total ASC for a dielectric is -Q*(epsilon-1)/epsilon
  int irr_size = cavity.irreducible_size();
  Eigen::VectorXd fake_asc = Eigen::VectorXd::Zero(size);
  fake_asc = solver.computeCharge(fake_mep);

  for (int i = 0; i < size; ++i) {
    INFO("fake_mep(" << i << ") = " << fake_mep(i));
  }
  for (int i = 0; i < size; ++i) {
    INFO("fake_asc(" << i << ") = " << fake_asc(i));
  }

  double totalASC = - (2.0 * Ccharge + 4.0 * Hcharge) * (permittivity - 1) / permittivity;
  // Renormalize
  int nr_irrep = cavity.pointGroup().nrIrrep();
  double totalFakeASC = fake_asc.sum() * nr_irrep;
  CAPTURE(totalASC);
  CAPTURE(totalFakeASC);
  CAPTURE(totalASC - totalFakeASC);
  REQUIRE(totalASC == Approx(totalFakeASC).epsilon(1.0e-03));
}


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_DRIVER_GESDD_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_DRIVER_GESDD_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/detail/array.hpp>
#include <boost/numeric/bindings/is_column_major.hpp>
#include <boost/numeric/bindings/is_complex.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/is_real.hpp>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/utility/enable_if.hpp>

//
// The LAPACK-backend for gesdd is the netlib-compatible backend.
//
#include <boost/numeric/bindings/lapack/detail/lapack.h>
#include <boost/numeric/bindings/lapack/detail/lapack_option.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace lapack {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end LAPACK-routine.
//
namespace detail {

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * float value-type.
//
inline std::ptrdiff_t gesdd( const char jobz, const fortran_int_t m,
        const fortran_int_t n, float* a, const fortran_int_t lda, float* s,
        float* u, const fortran_int_t ldu, float* vt,
        const fortran_int_t ldvt, float* work, const fortran_int_t lwork,
        fortran_int_t* iwork ) {
    fortran_int_t info(0);
    LAPACK_SGESDD( &jobz, &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work,
            &lwork, iwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * double value-type.
//
inline std::ptrdiff_t gesdd( const char jobz, const fortran_int_t m,
        const fortran_int_t n, double* a, const fortran_int_t lda, double* s,
        double* u, const fortran_int_t ldu, double* vt,
        const fortran_int_t ldvt, double* work, const fortran_int_t lwork,
        fortran_int_t* iwork ) {
    fortran_int_t info(0);
    LAPACK_DGESDD( &jobz, &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work,
            &lwork, iwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<float> value-type.
//
inline std::ptrdiff_t gesdd( const char jobz, const fortran_int_t m,
        const fortran_int_t n, std::complex<float>* a,
        const fortran_int_t lda, float* s, std::complex<float>* u,
        const fortran_int_t ldu, std::complex<float>* vt,
        const fortran_int_t ldvt, std::complex<float>* work,
        const fortran_int_t lwork, float* rwork, fortran_int_t* iwork ) {
    fortran_int_t info(0);
    LAPACK_CGESDD( &jobz, &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work,
            &lwork, rwork, iwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<double> value-type.
//
inline std::ptrdiff_t gesdd( const char jobz, const fortran_int_t m,
        const fortran_int_t n, std::complex<double>* a,
        const fortran_int_t lda, double* s, std::complex<double>* u,
        const fortran_int_t ldu, std::complex<double>* vt,
        const fortran_int_t ldvt, std::complex<double>* work,
        const fortran_int_t lwork, double* rwork, fortran_int_t* iwork ) {
    fortran_int_t info(0);
    LAPACK_ZGESDD( &jobz, &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work,
            &lwork, rwork, iwork, &info );
    return info;
}

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to gesdd.
//
template< typename Value, typename Enable = void >
struct gesdd_impl {};

//
// This implementation is enabled if Value is a real type.
//
template< typename Value >
struct gesdd_impl< Value, typename boost::enable_if< is_real< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename VectorS, typename MatrixU,
            typename MatrixVT, typename WORK, typename IWORK >
    static std::ptrdiff_t invoke( const char jobz, MatrixA& a, VectorS& s,
            MatrixU& u, MatrixVT& vt, detail::workspace2< WORK,
            IWORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixU >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixVT >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorS >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixU >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixVT >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorS >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixU >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixVT >::value) );
        std::ptrdiff_t minmn = std::min< std::ptrdiff_t >( size_row(a),
                size_column(a) );
        BOOST_ASSERT( bindings::size(s) >= std::min<
                std::ptrdiff_t >(bindings::size_row(a),
                bindings::size_column(a)) );
        BOOST_ASSERT( bindings::size(work.select(fortran_int_t())) >=
                min_size_iwork( minmn ));
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_work( bindings::size_row(a),
                bindings::size_column(a), jobz, minmn ));
        BOOST_ASSERT( bindings::size_column(a) >= 0 );
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        BOOST_ASSERT( bindings::size_minor(u) == 1 ||
                bindings::stride_minor(u) == 1 );
        BOOST_ASSERT( bindings::size_minor(vt) == 1 ||
                bindings::stride_minor(vt) == 1 );
        BOOST_ASSERT( bindings::size_row(a) >= 0 );
        BOOST_ASSERT( bindings::stride_major(a) >= std::max< std::ptrdiff_t >(1,
                bindings::size_row(a)) );
        BOOST_ASSERT( jobz == 'A' || jobz == 'S' || jobz == 'O' ||
                jobz == 'N' );
        return detail::gesdd( jobz, bindings::size_row(a),
                bindings::size_column(a), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(s),
                bindings::begin_value(u), bindings::stride_major(u),
                bindings::begin_value(vt), bindings::stride_major(vt),
                bindings::begin_value(work.select(real_type())),
                bindings::size(work.select(real_type())),
                bindings::begin_value(work.select(fortran_int_t())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename MatrixA, typename VectorS, typename MatrixU,
            typename MatrixVT >
    static std::ptrdiff_t invoke( const char jobz, MatrixA& a, VectorS& s,
            MatrixU& u, MatrixVT& vt, minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        std::ptrdiff_t minmn = std::min< std::ptrdiff_t >( size_row(a),
                size_column(a) );
        bindings::detail::array< real_type > tmp_work( min_size_work(
                bindings::size_row(a), bindings::size_column(a), jobz,
                minmn ) );
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( minmn ) );
        return invoke( jobz, a, s, u, vt, workspace( tmp_work, tmp_iwork ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename MatrixA, typename VectorS, typename MatrixU,
            typename MatrixVT >
    static std::ptrdiff_t invoke( const char jobz, MatrixA& a, VectorS& s,
            MatrixU& u, MatrixVT& vt, optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        return invoke( jobz, a, s, u, vt, minimal_workspace() );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t m,
            const std::ptrdiff_t n, const char jobz,
            const std::ptrdiff_t minmn ) {
        if ( n == 0 ) return 1;
        if ( jobz == 'N' ) return 3*minmn + std::max<
                std::ptrdiff_t >( std::max< std::ptrdiff_t >(m,n), 7*minmn );
        if ( jobz == 'O' ) return 3*minmn*minmn + std::max<
                std::ptrdiff_t >( std::max< std::ptrdiff_t >( m,n ),
                5*minmn*minmn + 4*minmn );
        return 3*minmn*minmn + std::max< std::ptrdiff_t >( std::max<
                std::ptrdiff_t >( m,n ), 4*minmn*minmn + 4*minmn );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array iwork.
    //
    static std::ptrdiff_t min_size_iwork( const std::ptrdiff_t minmn ) {
            return 8*minmn;
    }
};

//
// This implementation is enabled if Value is a complex type.
//
template< typename Value >
struct gesdd_impl< Value, typename boost::enable_if< is_complex< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename VectorS, typename MatrixU,
            typename MatrixVT, typename WORK, typename RWORK, typename IWORK >
    static std::ptrdiff_t invoke( const char jobz, MatrixA& a, VectorS& s,
            MatrixU& u, MatrixVT& vt, detail::workspace3< WORK, RWORK,
            IWORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixU >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixVT >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixU >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixVT >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorS >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixU >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixVT >::value) );
        std::ptrdiff_t minmn = std::min< std::ptrdiff_t >( size_row(a),
                size_column(a) );
        BOOST_ASSERT( bindings::size(s) >= std::min<
                std::ptrdiff_t >(bindings::size_row(a),
                bindings::size_column(a)) );
        BOOST_ASSERT( bindings::size(work.select(fortran_int_t())) >=
                min_size_iwork( minmn ));
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_rwork( minmn, jobz ));
        BOOST_ASSERT( bindings::size(work.select(value_type())) >=
                min_size_work( bindings::size_row(a),
                bindings::size_column(a), jobz, minmn ));
        BOOST_ASSERT( bindings::size_column(a) >= 0 );
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        BOOST_ASSERT( bindings::size_minor(u) == 1 ||
                bindings::stride_minor(u) == 1 );
        BOOST_ASSERT( bindings::size_minor(vt) == 1 ||
                bindings::stride_minor(vt) == 1 );
        BOOST_ASSERT( bindings::size_row(a) >= 0 );
        BOOST_ASSERT( bindings::stride_major(a) >= std::max< std::ptrdiff_t >(1,
                bindings::size_row(a)) );
        BOOST_ASSERT( jobz == 'A' || jobz == 'S' || jobz == 'O' ||
                jobz == 'N' );
        return detail::gesdd( jobz, bindings::size_row(a),
                bindings::size_column(a), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(s),
                bindings::begin_value(u), bindings::stride_major(u),
                bindings::begin_value(vt), bindings::stride_major(vt),
                bindings::begin_value(work.select(value_type())),
                bindings::size(work.select(value_type())),
                bindings::begin_value(work.select(real_type())),
                bindings::begin_value(work.select(fortran_int_t())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename MatrixA, typename VectorS, typename MatrixU,
            typename MatrixVT >
    static std::ptrdiff_t invoke( const char jobz, MatrixA& a, VectorS& s,
            MatrixU& u, MatrixVT& vt, minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        std::ptrdiff_t minmn = std::min< std::ptrdiff_t >( size_row(a),
                size_column(a) );
        bindings::detail::array< value_type > tmp_work( min_size_work(
                bindings::size_row(a), bindings::size_column(a), jobz,
                minmn ) );
        bindings::detail::array< real_type > tmp_rwork( min_size_rwork( minmn,
                jobz ) );
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( minmn ) );
        return invoke( jobz, a, s, u, vt, workspace( tmp_work, tmp_rwork,
                tmp_iwork ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename MatrixA, typename VectorS, typename MatrixU,
            typename MatrixVT >
    static std::ptrdiff_t invoke( const char jobz, MatrixA& a, VectorS& s,
            MatrixU& u, MatrixVT& vt, optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        std::ptrdiff_t minmn = std::min< std::ptrdiff_t >( size_row(a),
                size_column(a) );
        value_type opt_size_work;
        bindings::detail::array< real_type > tmp_rwork( min_size_rwork( minmn,
                jobz ) );
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( minmn ) );
        detail::gesdd( jobz, bindings::size_row(a),
                bindings::size_column(a), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(s),
                bindings::begin_value(u), bindings::stride_major(u),
                bindings::begin_value(vt), bindings::stride_major(vt),
                &opt_size_work, -1, bindings::begin_value(tmp_rwork),
                bindings::begin_value(tmp_iwork) );
        bindings::detail::array< value_type > tmp_work(
                traits::detail::to_int( opt_size_work ) );
        return invoke( jobz, a, s, u, vt, workspace( tmp_work, tmp_rwork,
                tmp_iwork ) );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t m,
            const std::ptrdiff_t n, const char jobz,
            const std::ptrdiff_t minmn ) {
        if ( n == 0 ) return 1;
        if ( jobz == 'N' ) return 2*minmn + std::max< std::ptrdiff_t >( m,n );
        if ( jobz == 'O' ) return 2*(minmn*minmn + minmn) + std::max<
                std::ptrdiff_t >( m, n );
        return minmn*minmn + 2*minmn + std::max< std::ptrdiff_t >( m, n );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array rwork.
    //
    static std::ptrdiff_t min_size_rwork( const std::ptrdiff_t minmn,
            const char jobz ) {
        if ( jobz == 'N' ) return 5*minmn;
        return 5*minmn*minmn + 7*minmn;
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array iwork.
    //
    static std::ptrdiff_t min_size_iwork( const std::ptrdiff_t minmn ) {
            return 8*minmn;
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. In
// addition, if applicable, they are overloaded for user-defined workspaces.
// Calls to these functions are passed to the gesdd_impl classes. In the
// documentation, most overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for gesdd. Its overload differs for
// * User-defined workspace
//
template< typename MatrixA, typename VectorS, typename MatrixU,
        typename MatrixVT, typename Workspace >
inline typename boost::enable_if< detail::is_workspace< Workspace >,
        std::ptrdiff_t >::type
gesdd( const char jobz, MatrixA& a, VectorS& s, MatrixU& u, MatrixVT& vt,
        Workspace work ) {
    return gesdd_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( jobz, a, s, u, vt, work );
}

//
// Overloaded function for gesdd. Its overload differs for
// * Default workspace-type (optimal)
//
template< typename MatrixA, typename VectorS, typename MatrixU,
        typename MatrixVT >
inline typename boost::disable_if< detail::is_workspace< MatrixVT >,
        std::ptrdiff_t >::type
gesdd( const char jobz, MatrixA& a, VectorS& s, MatrixU& u,
        MatrixVT& vt ) {
    return gesdd_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( jobz, a, s, u, vt,
            optimal_workspace() );
}

} // namespace lapack
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif