// The MIT License
// (c) 2019 Daniel Williams

#ifndef _QUAD_TREE_HPP_
#define _QUAD_TREE_HPP_

#define QUAD_TREE_VERSION "0.0.1"

#include <cmath>
#include <cstdlib>
#include <iostream>
#include <memory>
#include <string>

#include <Eigen/Geometry>

#include <nlohmann/json/json_fwd.hpp>

#include "geometry/cell_value.hpp"
#include "geometry/layout.hpp"
#include "geometry/sample.hpp"

#include "quadtree/node.hpp"

using std::unique_ptr;

using namespace terrain::geometry;

namespace terrain::quadtree {

/**
 * Datastructure: A point Quad Tree for representing 2D data. Each
 * region has the same ratio as the bounds for the tree.
 *
 * The implementation currently requires pre-determined bounds for data as it
 * can not rebalance itself to that degree.
 */
class Tree {
public:
    /**
     * Constructs a new quad tree, centered at 0,0 and 1024 units wide, square
     *
     * Use this constructor if the tree will be loaded from a config file, and the initialization values do not matter.
     */
    Tree();

    /**
     * Constructs a new quad tree.
     *
     * @param x, y coordinates of tree's center point.
     * @param tree width.  Tree is square. (i.e. height === width)
     */
    Tree(const Layout& _layout);

    /**
     *  Releases all memory associated with this quad tree.
     */
    ~Tree();

    static size_t calculate_complete_tree(const size_t height);

    /**
     * Returns true if the point at (x, y) exists in the tree.
     *
     * @param {double} x The x-coordinate.
     * @param {double} y The y-coordinate.
     * @return {bool} Whether the tree contains a point at (x, y).
     */
    bool contains(const Eigen::Vector2d& p) const;

    /**
     * retrieve the value of the requested point `p`.
     *
     * @param {double} x The x-coordinate.
     * @param {double} y The y-coordinate.
     * @param {cell_value_t} opt_default The default value to return if the node doesn't
     *                 exist.
     * @return {cell_value_t} The value of the node, if available; or the default value.
     */
    cell_value_t classify(const Eigen::Vector2d& p) const;

    /**
     * Draws a simple debug representation of this tree to the given
     * output stream.
     *
     */
    void debug_tree(const bool show_pointer=false) const;

    /**
     * Gets the value of the point at (x, y).  If the point is not close to the center of a node, this function interpolates or extrapolates an appropriate value.
     *
     * @param {double} x The x-coordinate.
     * @param {double} y The y-coordinate.
     * @return {cell_value_t} The resultant value
     */
    cell_value_t interp(const Eigen::Vector2d& at) const;

    ///! \brief sets all leaf nodes to the given value
    ///! \param fill_value - value to write
    void fill(const cell_value_t fill_value);

    cell_value_t operator()(const double x, const double y);

    ///! \brief Get the overall layout of this tree
    ///!
    ///! \return layout object describing the tree's overall bounds.
    ///!
    inline const Layout& get_layout() const { return layout; }

    size_t get_height() const;

    double get_load_factor() const;

    size_t get_memory_usage() const;

    /**
     * Loads the vector of points as a CCW polygon.
     *
     * @param {std::istream} input stream containing the serialization text
     */
    void load_polygon(const std::vector<Eigen::Vector2d>& source);

    /**
     * Loads a representation of a tree from the data source.  The the form source is assumed to
     * contain a serialization of a tree, as represented in valid json.  That is, in the same
     * format as Tree::serialize(...).
     *
     * @param tree - input json object containing a tree structure
     */
    bool load_tree(const nlohmann::json& tree);

    void prune();

    void reset();

    ///! \brief resets _the tree_ to fully populate the bounds at the given precision
    ///!
    ///! \param bounds - new bounds to describe
    ///! \param precision - describe the bounds to at least this precision
    void reset(const Layout& new_layout);

    ///! \brief Classify what value the requested point `p` has.
    ///!
    ///! This method is designed for use with an interpolation algorithm.
    ///!
    ///! \param location to sample near
    ///! @return the point-value-pair _actually_ contained in the tree.
    Sample sample(const Eigen::Vector2d& p) const;

    size_t size() const;

    ///! \brief store a value in the tree, at point `p`
    ///!
    ///! This is the primary method to populate a useable tree.
    ///!
    ///! \param p - the x,y coordinates to write to
    ///! \param new_value - the value to write at point 'p'
    ///! \return success - fails if out-of-bounds.
    bool store(const Eigen::Vector2d& p, const cell_value_t new_value);

    ///! \brief generates a json structure, describing the tree itself
    nlohmann::json to_json_tree() const;

    bool write_png(const std::string filename) const;

private:
    ///! the data layout this tree represents
    geometry::Layout layout;

    unique_ptr<terrain::quadtree::Node> root;

private:
    friend class QuadTreeTest_ConstructDefault_Test;
    friend class QuadTreeTest_LoadValidTree_Test;
    friend class QuadTreeTest_CalculateLoadFactor_Test;
    friend class QuadTreeTest_LoadGridFromJSON_Test;
    friend class QuadTreeTest_WriteLoadCycle_Test;
    friend class QuadTreeTest_SearchExplicitTree_Test;
    friend class QuadTreeTest_InterpolateTree_Test;

};
} // namespace terrain::quadtree

#endif // _QUADTREE_QUAD_TREE_HPP_


/*
 * test_HDF5_loader.cpp
 *
 *  Created on: 14 Jul 2015
 *      Author: pqm78245
 */

#include "percival_processing.h"
#include <boost/test/unit_test.hpp>
#include <iostream>

const int TEST_FRAME_HEIGHT 	= 	32;
const int TEST_FRAME_WIDTH		=	512;
class fixture_test_percival_ADU_to_electron_correction{
	public:
		percival_frame<float> src_frame;
		percival_frame<float> output_frame;
		percival_calib_params calib;
		fixture_test_percival_ADU_to_electron_correction(){
			src_frame.set_frame_size(TEST_FRAME_HEIGHT, TEST_FRAME_WIDTH);
			output_frame.set_frame_size(TEST_FRAME_HEIGHT, TEST_FRAME_WIDTH);
		}
};

BOOST_FIXTURE_TEST_SUITE(percival_ADU_to_electron_correctionn_test, fixture_test_percival_ADU_to_electron_correction)

BOOST_AUTO_TEST_CASE ( output_frame_dimension_should_be_the_same_as_input ){
	int frame_width = 10;
	int frame_height = 5;
	src_frame.set_frame_size(frame_height, frame_width);
	calib.ADU_to_electrons_conversion.set_frame_size(frame_height, frame_width);
	BOOST_REQUIRE_NO_THROW(percival_ADU_to_electron_correction(src_frame, output_frame, calib));
	BOOST_REQUIRE_EQUAL(output_frame.width, frame_width);
	BOOST_REQUIRE_EQUAL(output_frame.height, frame_height);
}

BOOST_AUTO_TEST_CASE ( should_throw_if_calibration_data_and_input_data_dimension_mismatch ){
	int frame_width = 10;
	int frame_height = 5;
	src_frame.set_frame_size(frame_height, frame_width);
	BOOST_REQUIRE_NO_THROW(percival_ADU_to_electron_correction(src_frame, output_frame, calib));
	BOOST_REQUIRE_EQUAL(output_frame.width, frame_width);
	BOOST_REQUIRE_EQUAL(output_frame.height, frame_height);
}

BOOST_AUTO_TEST_CASE ( should_give_sensible_result ){
	src_frame.set_frame_size(TEST_FRAME_HEIGHT, TEST_FRAME_WIDTH);
	calib.ADU_to_electrons_conversion.set_frame_size(TEST_FRAME_HEIGHT, TEST_FRAME_WIDTH);
	output_frame.set_frame_size(TEST_FRAME_HEIGHT, TEST_FRAME_WIDTH);

	float test1S = 3.1;
	float test2S = 4.1;
	float test3S = 5.9;
	float test4S = 2.6;

	float test1R = 3.2;
	float test2R = 413;
	float test3R = 5.302;
	float test4R = 2.9;

	*(src_frame.data)												= test1S;
	*(src_frame.data + TEST_FRAME_HEIGHT * TEST_FRAME_WIDTH - 1)	= test2S;
	*(src_frame.data + 					 TEST_FRAME_WIDTH - 1)		= test3S;
	*(src_frame.data + TEST_FRAME_WIDTH * (TEST_FRAME_HEIGHT - 1) )	= test4S;

	*(calib.ADU_to_electrons_conversion.data)												= test1R;
	*(calib.ADU_to_electrons_conversion.data + TEST_FRAME_HEIGHT * TEST_FRAME_WIDTH - 1)	= test2R;
	*(calib.ADU_to_electrons_conversion.data + 					 TEST_FRAME_WIDTH - 1)		= test3R;
	*(calib.ADU_to_electrons_conversion.data + TEST_FRAME_WIDTH * (TEST_FRAME_HEIGHT - 1) )	= test4R;

	BOOST_REQUIRE_NO_THROW(percival_ADU_to_electron_correction(src_frame, output_frame, calib));
	//four corners
	BOOST_REQUIRE_CLOSE_FRACTION(*(output_frame.data), test1S / test1R, 0.01);												//upper left
	BOOST_REQUIRE_CLOSE_FRACTION(*(output_frame.data + TEST_FRAME_HEIGHT * TEST_FRAME_WIDTH - 1), test2S / test2R, 0.01);	//Lower right
	BOOST_REQUIRE_CLOSE_FRACTION(*(output_frame.data + 					  TEST_FRAME_WIDTH - 1), test3S / test3R, 0.01);	//upper right
	BOOST_REQUIRE_CLOSE_FRACTION(*(output_frame.data + TEST_FRAME_WIDTH  * (TEST_FRAME_HEIGHT - 1) ), test4S / test4R, 0.01);	//lower left
}

BOOST_AUTO_TEST_SUITE_END()


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/config.hpp>
#include <boost/simd/detail/nsm.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    Generate the smallest floating point exponent.

    @par Semantic:

    @code
    T r = Minexponent<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r =  -1022;
    else if T is float
      r = -126;
    @endcode

    @return The Minexponent constant for the proper type
  */

namespace boost { namespace simd
{
  namespace tag
  {
    namespace tt = nsm::type_traits;

    struct minexponent_ : boost::dispatch::constant_value_<minexponent_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,minexponent_,boost::dispatch::constant_value_<minexponent_>);

      struct value_map
      {
        template<typename X>
        static tt::integral_constant<X,0> value(boost::dispatch::integer_<X> const&);

        template<typename X>
        static tt::integral_constant<std::int32_t,-126> value(boost::dispatch::single_<X> const&);

        template<typename X>
       static tt::integral_constant<std::int64_t,-1022> value(boost::dispatch::double_<X> const&);
      };
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, minexponent_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::minexponent_,minexponent);
  }

  template<typename T> BOOST_FORCEINLINE auto Minexponent()
  BOOST_NOEXCEPT_DECLTYPE(detail::minexponent( boost::dispatch::as_<T>{}))
  {
    return detail::minexponent( boost::dispatch::as_<T>{} );
  }
} }

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


//
// Created by clementriu on 10/6/20.
//

#ifndef MAGSAC_EXPERIENCE_UTILS_HPP
#define MAGSAC_EXPERIENCE_UTILS_HPP

#include <fstream>
#include <iostream>
#include <vector>
#include <Eigen/Eigen>

#include <opencv2/core.hpp>

/// Save matching position between two points.
struct Match {
    Match() {}

    Match(float ix1, float iy1, float ix2, float iy2)
            : x1(ix1), y1(iy1), x2(ix2), y2(iy2) {}

    float x1, y1, x2, y2;

//    /**
//    * Transform into matrix where each column is (x1 y1 x2 y2)^T.
//    * \param m The matches that we transform.
//    * \return Matrix 4xn where n is the size of vector \a m.
//    */
//    static libNumerics::matrix<double> toMat(const std::vector<Match> &m) {
//        libNumerics::matrix<double> M(4, static_cast<int>(m.size()));
//        std::vector<Match>::const_iterator it = m.begin();
//        for (int j = 0; it != m.end(); ++it, ++j) {
//            M(0, j) = it->x1;
//            M(1, j) = it->y1;
//            M(2, j) = it->x2;
//            M(3, j) = it->y2;
//        }
//        return M;
//    }

    /**
    * Load the corresponding matches from file.
    * \param nameFile   The file where matches were saved.
    * \param vec_match  The loaded corresponding points.
    * \return bool      True if everything was ok, otherwise false.
    */
    static bool loadMatch(const char *nameFile, std::vector<Match> &vec_match) {
        vec_match.clear();
        std::ifstream f(nameFile);
        while (f.good()) {
            std::string str;
            std::getline(f, str);
            if (f.good()) {
                std::istringstream s(str);
                Match m;
                s >> m;
                if (!s.fail())
                    vec_match.push_back(m);
            }
        }
        return !vec_match.empty();
    }

    friend std::ostream &operator<<(std::ostream &os, const Match &m) {
        return os << m.x1 << " " << m.y1 << " "
                  << m.x2 << " " << m.y2 << std::endl;
    }

    friend std::istream &operator>>(std::istream &in, Match &m) {
        return in >> m.x1 >> m.y1 >> m.x2 >> m.y2;
    }
};

bool ReadPoints(const char *fileInliers, const char *fileOutliers, int nGen,
                std::vector<cv::Mat> &pointsAll, std::vector<std::vector<int>> &groundTruthLabelsAll,
                bool readOutliers = true);

template<typename T, typename U>
void
randomDataFusionWithMemory(const std::vector<T> &vect0, const std::vector<T> &vect1, std::vector<T> &mixedVector,
                           const U label0, const U label1, std::vector<U> &labels) {
    typename std::vector<T>::const_iterator it0 = vect0.begin();
    typename std::vector<T>::const_iterator it1 = vect1.begin();
    size_t size0 = vect0.size();
    size_t size1 = vect1.size();
    while (mixedVector.size() < size0 + size1) {
        if (it0 != vect0.end() && it1 != vect1.end()) {
            int chosenDataSet = std::rand() % 2;
            if (chosenDataSet == 0) {
                mixedVector.push_back(*it0);
                labels.push_back(label0);
                it0++;
            } else {
                mixedVector.push_back(*it1);
                labels.push_back(label1);
                it1++;
            }
        } else {
            if (it0 == vect0.end() && it1 != vect1.end()) {
                mixedVector.push_back(*it1);
                labels.push_back(label1);
                it1++;
            } else {
                if (it1 == vect1.end() && it0 != vect0.end()) {
                    mixedVector.push_back(*it0);
                    labels.push_back(label0);
                    it0++;
                } else {
                    std::cerr << "PROBLEM during fusion of dataset." << std::endl;
                    assert(false); // TODO: Raise exception rather than assert error.
                }
            }
        }
    }

}

template<typename T>
void printVector(const std::vector<T> vectToPrint) {
    typename std::vector<T>::const_iterator itVectToPrint = vectToPrint.begin();
    for (; itVectToPrint != vectToPrint.end(); ++itVectToPrint) {
        std::cout << *itVectToPrint << " ";
    }
    std::cout << std::endl;
}

bool
saveExpInfo(const char *nameFile, unsigned int seed, int nGen, int nRun, const char *pathToIn1, const char *pathToIn2,
            double AlgoPrecMean, double AlgoPrecStd, const std::vector<double> &AlgoPrecVect,
            double AlgoRecMean, double AlgoRecStd, const std::vector<double> &AlgoRecVect,
            double AlgoRuntimeMean, double AlgoRuntimeStd, const std::vector<double> &AlgoRuntimeVect,
            bool fullSave = false);

template<typename T>
bool saveVectOfVect(const char *nameFile, const std::vector<std::vector<T>> &vectOfVect) {
    std::ofstream f(nameFile);
    if (f.is_open()) {
        typename std::vector<std::vector<T>>::const_iterator itVectOfVect = vectOfVect.begin();
        for (; itVectOfVect != vectOfVect.end(); ++itVectOfVect) {
            typename std::vector<T>::const_iterator itVect = (*itVectOfVect).begin();
            for (; itVect != (*itVectOfVect).end(); ++itVect) {
                f << *itVect << ", ";
            }
            f << "\n";
        }
    }
    return f.is_open();
}

bool loadCalibration(const char *path, Eigen::Matrix<double, 3, 3> &K1, Eigen::Matrix<double, 3, 3> &K2);

#endif //MAGSAC_EXPERIENCE_UTILS_HPP


#ifndef CONTINUOUSFUSION_HPP
#define CONTINUOUSFUSION_HPP

#include <vector>
#include <sensor_msgs/Image.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <image_transport/image_transport.h>
#include <message_filters/synchronizer.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <Eigen/Dense>

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> KittiSyncPolicy;

class ContinuousFusion
{
    private:
        cv::Mat _PRT;
        ros::NodeHandle _nh;
        image_transport::ImageTransport _xport;
        image_transport::Publisher _publisher;
        message_filters::Subscriber<sensor_msgs::Image> _imageSub;
        message_filters::Subscriber<sensor_msgs::PointCloud2> _veloSub;
        message_filters::Synchronizer<KittiSyncPolicy> _sync;
        Eigen::MatrixXf pcl2ToEigen(pcl::PCLPointCloud2 cloud);
        pcl::PCLPointCloud2 rosMsgToPcl2(const sensor_msgs::PointCloud2ConstPtr& cloudPtr);
        cv::Mat rosMsgToCvMat(const sensor_msgs::PointCloud2ConstPtr& cloudPtr);
        void publishBevImage(cv::Mat bevImage);

    public:
        ContinuousFusion(cv::Mat PRT);
        void callback(
            const sensor_msgs::ImageConstPtr& imageIn,
            const sensor_msgs::PointCloud2ConstPtr& veloIn);
};

#endif


//==============================================================================
//         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
//==============================================================================
//////////////////////////////////////////////////////////////////////////////
// cover test behavior of arithmetic components in scalar mode
//////////////////////////////////////////////////////////////////////////////

#include <nt2/arithmetic/include/functions/hypot.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <vector>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/hundred.hpp>
#include <nt2/include/functions/hypot.hpp>
#include <nt2/sdk/meta/as_floating.hpp>

#include <nt2/sdk/unit/tests/cover.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL ( hypot_real__1_0_1,  NT2_REAL_TYPES)
{

  using nt2::hypot;
  using nt2::tag::hypot_;
  typedef typename nt2::meta::call<hypot_(T, T)>::type r_t;

  nt2::uint32_t NR = NT2_NB_RANDOM_TEST;
  std::vector<T> in1(NR), in2(NR);
  std::vector<r_t> ref(NR);
  nt2::roll(in1, nt2::Zero<T>()/2, nt2::Hundred<T>()/2);
  nt2::roll(in2, nt2::Zero<T>()/2, nt2::Hundred<T>()/2);
  for(nt2::uint32_t i=0; i < NR ; ++i)
  {
    ref[i] = ::hypot( in1[i], in2[i]);
  }

 NT2_COVER_ULP_EQUAL(hypot_, ((T, in1))((T, in2)), ref, 2);

}


#include <gtest/gtest.h>

#include "scheme/util/meta/InstanceMap.hh"
#include "scheme/util/meta/print_type.hh"
#include <boost/mpl/quote.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/fusion/include/for_each.hpp>
#include <boost/fusion/include/io.hpp>

#include "scheme/util/SimpleArray.hh"
#include "scheme/io/cache.hh"

namespace scheme {
namespace util {
namespace meta {

using std::cout;
using std::endl;

namespace bf = boost::fusion;
namespace mpl = boost::mpl;

namespace dummy {
	struct T {};
	struct U {};
	struct V {};
}
TEST(NumericInstanceMap,basic_test){
	using namespace dummy;
	typedef NumericInstanceMap<m::vector<T,U,V>, m::always<double> > NMAP;
	NMAP a(0,0,0);
	ASSERT_EQ( a.sum(), 0 );

	ASSERT_EQ( NMAP(1,2,3), NMAP(1,2,3) );
	ASSERT_NE( NMAP(1,2,3), NMAP(0,2,3) );
	ASSERT_NE( NMAP(1,2,3), NMAP(1,0,3) );
	ASSERT_NE( NMAP(1,2,3), NMAP(1,2,0) );

	NMAP x;
	x.get<T>() = 1;
	x.get<U>() = 2;
	x.get<V>() = 3;
	ASSERT_EQ( NMAP(1,2,3), x );

	typedef NumericInstanceMap<m::vector<U,V,T>, m::always<double> > NMAP2;
	NMAP2 y;
	y.get<U>() = 1;
	y.get<V>() = 2;
	y.get<T>() = 3;
	ASSERT_EQ( NMAP2(1,2,3), y );

}

TEST(NumericInstanceMap,serialization){
	#ifdef CEREAL
		using namespace dummy;
		typedef NumericInstanceMap<m::vector<T,U,V>, m::always<double> > NMAP;
		NMAP imap(1,2,3);
		ASSERT_EQ( imap, io::test_serialization(imap) );
	#endif
}

}
}
}


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

#include <amgcl/adapter/zero_copy.hpp>
#include <amgcl/solver/skyline_lu.hpp>
#include <amgcl/backend/builtin.hpp>
#include <amgcl/profiler.hpp>
#include "sample_problem.hpp"

namespace amgcl {
    profiler<> prof;
}

BOOST_AUTO_TEST_SUITE( test_skyline_lu )

BOOST_AUTO_TEST_CASE(skyline_lu)
{
    std::vector<ptrdiff_t> ptr;
    std::vector<ptrdiff_t> col;
    std::vector<double>    val;
    std::vector<double>    rhs;

    size_t n = sample_problem(16, val, col, ptr, rhs);

    auto A = amgcl::adapter::zero_copy(n, ptr.data(), col.data(), val.data());

    amgcl::solver::skyline_lu<double> solve(*A);

    std::vector<double> x(n);
    std::vector<double> r(n);

    solve(rhs, x);

    amgcl::backend::residual(rhs, *A, x, r);

    BOOST_CHECK_SMALL(sqrt(amgcl::backend::inner_product(r, r)), 1e-8);
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <boost/algorithm/minmax_element.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/container/vector.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>

#include "part_two.h"

part_two::part_two(const boost::container::string& input) : abstract_part(input)
{
}

part_two::~part_two()
{
}

int part_two::sum_of_divisible_pair(boost::container::vector<int> numbers)
{
	for (size_t a = 0; a < numbers.size(); ++a)
	{
		for (size_t b = 0; b < numbers.size(); ++b)
		{
			if (a == b)
				continue;

			if (numbers[a] % numbers[b] == 0)
				return numbers[a] / numbers[b];
		}
	}

	return 0;
}

void part_two::run()
{
	boost::filesystem::ifstream ifs;

	ifs.open(input_path, boost::filesystem::ifstream::in);

	try
	{
		std::string line;
		int sum = 0;

		while (std::getline(ifs, line))
		{
			boost::container::vector<std::string> tokens;
			boost::container::vector<int> numbers;

			split(tokens, line, boost::is_any_of("\t"));
			numbers.resize(tokens.size());

			std::transform(tokens.begin(), tokens.end(), numbers.begin(), boost::lexical_cast<int, std::string>);

			sum += sum_of_divisible_pair(numbers);
		}

		std::cout << "sum: " << sum << std::endl;
	}
	catch (const std::ios_base::failure& e)
	{
		std::cout << e.what() << std::endl;
	}
}


/*
 *  RandomWORSelection.cpp
 *  MWorksCore
 *
 *  Created by labuser on 9/25/07.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "RandomWORSelection.h"

#include <boost/random.hpp>
#include "Clock.h"

BEGIN_NAMESPACE_MW

RandomWORSelection::RandomWORSelection(int _n_draws, bool _autoreset) : Selection(_n_draws, _autoreset){
	shared_ptr <Clock> clock = Clock::instance();
	rng.seed((int)clock->getSystemTimeNS());
	reset();
}

void RandomWORSelection::reset() {

	draw_list = std::list<int>();

	if(getNItems() != 0){

		int nper = ceil((float)n_draws / (float)getNItems());

		for(int i = 0; i < getNItems(); i++) {
			for(int j = 0; j < nper; j++){
				draw_list.push_back(i);
			}
		}
	}

	Selection::reset();
}

shared_ptr <Selection> RandomWORSelection::clone(){
	shared_ptr <RandomWORSelection> newsel = shared_ptr<RandomWORSelection>(new RandomWORSelection(*this));
	return boost::dynamic_pointer_cast<Selection, RandomWORSelection>(newsel);
}

int RandomWORSelection::draw() {

	if(done_so_far >= n_draws){
        if(autoreset){
            mwarning(M_PARADIGM_MESSAGE_DOMAIN, "Autoresetting selection object");
            reset();
        } else {
            SelectionOffEdgeException e;
            throw e;
        }
	}

	if(draw_list.size() <= 0){
		InvalidSelectionException e;
		throw e;
	}

	int returnval;

	int the_size = draw_list.size();

	// Make a random selection
	boost::uniform_int<> our_range(0, draw_list.size()-1);
	boost::variate_generator<random_generator&, boost::uniform_int<> > dice(rng, our_range);

	int random = dice();

	// has to be a better way
	std::list<int>::iterator i = draw_list.begin();
	for(int k=0; k < random; k++){
		i++;
	}

	returnval = *i;
//	cerr << "Selected: " << returnval << endl;

	// Remove it from further use
	draw_list.erase(i);

	the_size = draw_list.size();
	// Save it on the tentative list
	tentative_selections.push_back(returnval);

	done_so_far++;
	return returnval;
}

void RandomWORSelection::rejectSelections(){

	std::vector<int>::iterator i;
	for(i = tentative_selections.begin(); i != tentative_selections.end(); i++){
		draw_list.push_back(*i); // put it back
		done_so_far--;
	}

	tentative_selections = std::vector<int>();
}

END_NAMESPACE_MW


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, 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.
 *********************************************************************/

//
// Sparse Pose Adjustment classes and functions, 2D version
//
#include <nav2d_karto/spa2d.h>

#include <stdio.h>
#include <Eigen/Cholesky>

using namespace Eigen;
using namespace std;

#include <iostream>
#include <iomanip>
#include <fstream>
#include <sys/time.h>

// elapsed time in microseconds
static long long utime()
{
  timeval tv;
  gettimeofday(&tv,NULL);
  long long ts = tv.tv_sec;
  ts *= 1000000;
  ts += tv.tv_usec;
  return ts;
}

  // R = [[c -s][s c]]
  // [R' | R't]
  void Node2d::setTransform()
    {
      w2n(0,0) = w2n(1,1) = cos(arot);
      w2n(0,1) = sin(arot);
      w2n(1,0) = -w2n(0,1);
      w2n.col(2).setZero();
      w2n.col(2) = -w2n*trans;
    }

  //
  // sets angle derivatives dR'/dth
  //
  void Node2d::setDr()
  {
    dRdx(0,0) = dRdx(1,1) = w2n(1,0); // -sin(th)
    dRdx(0,1) = w2n(0,0);       // cos(th)
    dRdx(1,0) = -w2n(0,0);      // -cos(th)
  }

  // set up Jacobians

  void Con2dP2::setJacobians(std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &nodes)
  {
    // node references
    Node2d &nr = nodes[ndr];
    Matrix<double,3,1> &tr = nr.trans;
    Node2d &n1 = nodes[nd1];
    Matrix<double,3,1> &t1 = n1.trans;

    // first get the second frame in first frame coords
    Eigen::Matrix<double,2,1> pc = nr.w2n * t1;

    // Jacobians wrt first frame parameters

    // translational part of 0p1 wrt translational vars of p0
    // this is just -R0'  [from 0t1 = R0'(t1 - t0)]
    J0.block<2,2>(0,0) = -nr.w2n.block<2,2>(0,0);

    // translational part of 0p1 wrt rotational vars of p0
    // dR'/dq * [pw - t]
    Eigen::Matrix<double,2,1> pwt;
    pwt = (t1-tr).head(2);   // transform translations

    // dx
    Eigen::Matrix<double,2,1> dp = nr.dRdx * pwt; // dR'/dq * [pw - t]
    J0.block<2,1>(0,2) = dp;

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J0.block<1,2>(2,0).setZero();

    // rotational part of 0p1 wrt rotational vars of p0
    // from 0q1 = (q1-q0)
    J0(2,2) = -1.0;
    J0t = J0.transpose();

    //  cout << endl << "J0 " << ndr << endl << J0 << endl;

    // Jacobians wrt second frame parameters
    // translational part of 0p1 wrt translational vars of p1
    // this is just R0'  [from 0t1 = R0'(t1 - t0)]
    J1.block<2,2>(0,0) = nr.w2n.block<2,2>(0,0);

    // translational part of 0p1 wrt rotational vars of p1: zero
    J1.block<2,1>(0,2).setZero();

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J1.block<1,2>(2,0).setZero();

    // rotational part of 0p1 wrt rotational vars of p0
    // from 0q1 = (q1-q0)
    J1(2,2) = 1.0;
    J1t = J1.transpose();

    //  cout << endl << "J1 " << nd1 << endl << J1 << endl;

  };

  // error function
  // NOTE: this is h(x) - z, not z - h(x)
  inline double Con2dP2::calcErr(const Node2d &nd0, const Node2d &nd1)
    {
      err.block<2,1>(0,0) = nd0.w2n * nd1.trans - tmean;
      double aerr = (nd1.arot - nd0.arot) - amean;
      if (aerr > M_PI) aerr -= 2.0*M_PI;
      if (aerr < -M_PI) aerr += 2.0*M_PI;
      err(2) = aerr;

      //      cout << err.transpose() << endl;

      return err.dot(prec * err);
    }

  // error function for distance cost
  double Con2dP2::calcErrDist(const Node2d &nd0, const Node2d &nd1)
    {
      Vector2d derr = nd0.w2n * nd1.trans - tmean;
      return derr.dot(derr);
    }

  // error measure, squared
  // assumes node transforms have already been calculated
  // <tcost> is true if we just want the distance offsets
  double SysSPA2d::calcCost(bool tcost)
  {
    double cost = 0.0;

    // do distance offset
    if (tcost)
      {
        for(size_t i=0; i<p2cons.size(); i++)
          {
            Con2dP2 &con = p2cons[i];
            double err = con.calcErrDist(nodes[con.ndr],nodes[con.nd1]);
            cost += err;
          }
      }

    // full cost
    else
      {
        for(size_t i=0; i<p2cons.size(); i++)
          {
            Con2dP2 &con = p2cons[i];
            double err = con.calcErr(nodes[con.ndr],nodes[con.nd1]);
            cost += err;
          }
        errcost = cost;
      }

    return cost;
  }

  // add a node at a pose
  // <pos> is x,y,th, with th in radians
  // returns node position
  int SysSPA2d::addNode(const Vector3d &pos, int id)
  {
    Node2d nd;
    nd.nodeId = id;

    nd.arot = pos(2);
    nd.trans.head(2) = pos.head(2);
    nd.trans(2) = 1.0;

    // add in to system
    nd.setTransform();          // set up world2node transform
    nd.setDr();
    int ndi = nodes.size();
    nodes.push_back(nd);
    return ndi;
  }

  // add a constraint
  // <nd0>, <nd1> are node id's
  // <mean> is x,y,th, with th in radians
  // <prec> is a 3x3 precision matrix (inverse covariance
  // returns true if nodes are found
  // TODO: make node lookup more efficient
  bool SysSPA2d::addConstraint(int ndi0, int ndi1, const Vector3d &mean,
                                 const Matrix3d &prec)
  {
    int ni0 = -1, ni1 = -1;
    for (int i=0; i<(int)nodes.size(); i++)
      {
        if (nodes[i].nodeId == ndi0)
          ni0 = i;
        if (nodes[i].nodeId == ndi1)
          ni1 = i;
      }
    if (ni0 < 0 || ni1 < 0) return false;

    Con2dP2 con;
    con.ndr = ni0;
    con.nd1 = ni1;

    con.tmean = mean.head(2);
    con.amean = mean(2);
    con.prec = prec;
    p2cons.push_back(con);
    return true;
  }

  // Set up linear system
  // Use dense matrices

  void SysSPA2d::setupSys(double sLambda)
  {
    // set matrix sizes and clear
    // assumes scales vars are all free
    int nFree = nodes.size() - nFixed;
    A.setZero(3*nFree,3*nFree);
    B.setZero(3*nFree);
    VectorXi dcnt(nFree);
    dcnt.setZero(nFree);

    // lambda augmentation
    double lam = 1.0 + sLambda;

    // loop over P2 constraints
    for(size_t pi=0; pi<p2cons.size(); pi++)
      {
        Con2dP2 &con = p2cons[pi];
        con.setJacobians(nodes);

        // add in 4 blocks of A
        int i0 = 3*(con.ndr-nFixed); // will be negative if fixed
        int i1 = 3*(con.nd1-nFixed); // will be negative if fixed

        if (i0>=0)
          {
            A.block<3,3>(i0,i0) += con.J0t * con.prec * con.J0;
            dcnt(con.ndr - nFixed)++;
          }
        if (i1>=0)
          {
            dcnt(con.nd1 - nFixed)++;
            Matrix<double,3,3> tp = con.prec * con.J1;
            A.block<3,3>(i1,i1) += con.J1t * tp;
            if (i0>=0)
              {
                A.block<3,3>(i0,i1) += con.J0t * con.prec * con.J1;
                A.block<3,3>(i1,i0) += con.J1t * con.prec * con.J0;
              }
          }

        // add in 2 blocks of B
        if (i0>=0)
          B.block<3,1>(i0,0) -= con.J0t * con.prec * con.err;
        if (i1>=0)
          B.block<3,1>(i1,0) -= con.J1t * con.prec * con.err;
      } // finish P2 constraints

    // augment diagonal
    A.diagonal() *= lam;

    // check the matrix and vector
    for (int i=0; i<3*nFree; i++)
      for (int j=0; j<3*nFree; j++)
        if (isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }

    for (int j=0; j<3*nFree; j++)
      if (isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }

    int ndc = 0;
    for (int i=0; i<nFree; i++)
      if (dcnt(i) == 0) ndc++;

    if (ndc > 0)
      cout << "[SetupSys] " << ndc << " disconnected nodes" << endl;
  }

  // Set up sparse linear system; see setupSys for algorithm.
  // Currently doesn't work with scale variables
  void SysSPA2d::setupSparseSys(double sLambda, int iter, int sparseType)
  {
    // set matrix sizes and clear
    // assumes scales vars are all free
    int nFree = nodes.size() - nFixed;
	if(nFree < 0) nFree = 0;

    long long t0, t1, t2, t3;
    t0 = utime();

    if (iter == 0)
	{
      csp.setupBlockStructure(nFree); // initialize CSparse structures
	}
//    else
//      csp.setupBlockStructure(0); // zero out CSparse structures

    t1 = utime();

    VectorXi dcnt(nFree);
    dcnt.setZero(nFree);

    // lambda augmentation
    double lam = 1.0 + sLambda;

    // loop over P2 constraints
    for(size_t pi=0; pi<p2cons.size(); pi++)
      {
        Con2dP2 &con = p2cons[pi];
        con.setJacobians(nodes);

        // add in 4 blocks of A; actually just need upper triangular
        int i0 = con.ndr-nFixed; // will be negative if fixed
        int i1 = con.nd1-nFixed; // will be negative if fixed

        if (i0>=0)
          {
           Matrix<double,3,3> m = con.J0t*con.prec*con.J0;
            csp.addDiagBlock(m,i0);
            dcnt(con.ndr - nFixed)++;
          }
        if (i1>=0)
          {
            dcnt(con.nd1 - nFixed)++;
            Matrix<double,3,3> tp = con.prec * con.J1;
            Matrix<double,3,3> m = con.J1t * tp;
            csp.addDiagBlock(m,i1);
            if (i0>=0)
              {
                Matrix<double,3,3> m2 = con.J0t * tp;
                if (i1 < i0)
                  {
                    m = m2.transpose();
                    csp.addOffdiagBlock(m,i1,i0);
                  }
                else
                  {
                    csp.addOffdiagBlock(m2,i0,i1);
                  }
              }
          }

        // add in 2 blocks of B
        if (i0>=0)
		{
          csp.B.block<3,1>(i0*3,0) -= con.J0t * con.prec * con.err;

		}
        if (i1>=0)
		{
			csp.B.block<3,1>(i1*3,0) -= con.J1t * con.prec * con.err;
		}

      } // finish P2 constraints
    t2 = utime();

    // set up sparse matrix structure from blocks
    if (sparseType == SBA_BLOCK_JACOBIAN_PCG)
      csp.incDiagBlocks(lam);   // increment diagonal block
    else
      csp.setupCSstructure(lam,iter==0);
    t3 = utime();

    if (verbose)
      printf("\n[SetupSparseSys] Block: %0.1f   Cons: %0.1f  CS: %0.1f\n",
           (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);

    int ndc = 0;
    for (int i=0; i<nFree; i++)
      if (dcnt(i) == 0) ndc++;

    if (ndc > 0)
      cout << "[SetupSparseSys] " << ndc << " disconnected nodes" << endl;
  }

  /// Run the LM algorithm that computes a nonlinear SPA estimate.
  /// <niter> is the max number of iterations to perform; returns the
  /// number actually performed.
  /// <lambda> is the diagonal augmentation for LM.
  /// <useCSparse> = 0 for dense Cholesky, 1 for sparse system,
  ///                2 for gradient system, 3 for block jacobian PCG
  /// <initTol> is the initial tolerance for CG
  /// <maxCGiters> is max # of iterations in BPCG

  int SysSPA2d::doSPA(int niter, double sLambda, int useCSparse, double initTol,
                      int maxCGiters)
  {
    // number of nodes
    int ncams = nodes.size();

    // set number of constraints
    int ncons = p2cons.size();

	if(ncams <= 0 || ncons <= 0)
	{
		return 0;
	}

    // check for fixed frames
    if (nFixed <= 0)
      {
        cout << "[doSPA2d] No fixed frames" << endl;
        return 0;
      }
    for (int i=0; i<ncams; i++)
      {
        Node2d &nd = nodes[i];
        if (i >= nFixed)
          nd.isFixed = false;
        else
          nd.isFixed = true;
        nd.setTransform();      // set up world-to-node transform for cost calculation
        nd.setDr();         // always use local angles
      }

    // initialize vars
    if (sLambda > 0.0)          // do we initialize lambda?
      lambda = sLambda;

    double laminc = 2.0;        // how much to increment lambda if we fail
    double lamdec = 0.5;        // how much to decrement lambda if we succeed
    int iter = 0;               // iterations
    sqMinDelta = 1e-8 * 1e-8;
    double cost = calcCost();
    if (verbose)
      cout << iter << " Initial squared cost: " << cost << " which is "
           << sqrt(cost/ncons) << " rms error" << endl;

    int good_iter = 0;
    double cumTime = 0;
    for (; iter<niter; iter++)  // loop at most <niter> times
      {
	    if(verbose)
	    {
		    printf(" --- Begin of iteration %d ---\n", iter);
	    }
        // set up and solve linear system
        // NOTE: shouldn't need to redo all calcs in setupSys if we
        //   got here from a bad update

        long long t0, t1, t2, t3;
        t0 = utime();
        if (useCSparse)
		{
          setupSparseSys(lambda,iter,useCSparse); // set up sparse linear system
		}
        else
		{
          setupSys(lambda);     // set up linear system
		}
        //        cout << "[SPA] Solving...";
        t1 = utime();

        // use appropriate linear solver
        if (useCSparse == SBA_BLOCK_JACOBIAN_PCG)
          {/*
            if (csp.B.rows() != 0)
              {
                int iters = csp.doBPCG(maxCGiters,initTol,iter);
                if (verbose)
                  cout << "[Block PCG] " << iters << " iterations" << endl;
              } */
          }
#ifdef SBA_DSIF
        // PCG with incomplete Cholesky
        else if (useCSparse == 3)
          {
            int res = csp.doPCG(maxCGiters);
            if (res > 1)
              cout << "[DoSPA] Sparse PCG failed with error " << res << endl;
          }
#endif
        else if (useCSparse > 0)
          {
            if (csp.B.rows() != 0)
              {
                bool ok = csp.doChol();
                if (!ok)
                  cout << "[DoSBA] Sparse Cholesky failed!" << endl;
              }
          }

        // Dense direct Cholesky
        else
          A.ldlt().solveInPlace(B); // Cholesky decomposition and solution

        t2 = utime();
        //        cout << "solved" << endl;

        // get correct result vector
        VectorXd &BB = useCSparse ? csp.B : B;

        // check for convergence
        // this is a pretty crummy convergence measure...
        double sqDiff = BB.squaredNorm();
        if (sqDiff < sqMinDelta) // converged, done...
          {
            if (verbose)
              cout << "Converged with delta: " << sqrt(sqDiff) << endl;
            break;
          }

        // update the frames
        int ci = 0;
        for(int i=0; i < ncams; i++)
          {
            Node2d &nd = nodes[i];
            if (nd.isFixed) continue; // not to be updated
            nd.oldtrans = nd.trans; // save in case we don't improve the cost
            nd.oldarot = nd.arot;
            nd.trans.head<2>() += BB.segment<2>(ci);

            nd.arot += BB(ci+2);
            nd.normArot();
            nd.setTransform();  // set up projection matrix for cost calculation
            nd.setDr();         // set rotational derivatives
            ci += 3;            // advance B index
          }

        // new cost
        double newcost = calcCost();
        if (verbose)
          cout << iter << " Updated squared cost: " << newcost << " which is "
               << sqrt(newcost/ncons) << " rms error" << endl;

        // check if we did good
        if (newcost < cost) // && iter != 0) // NOTE: iter==0 case is for checking
          {
            cost = newcost;
            lambda *= lamdec;   // decrease lambda
            //      laminc = 2.0;       // reset bad lambda factor; not sure if this is a good idea...
            good_iter++;
          }
        else
          {
            lambda *= laminc;   // increase lambda
            laminc *= 2.0;      // increase the increment

            // reset nodes
            for(int i=0; i<ncams; i++)
              {
                Node2d &nd = nodes[i];
                if (nd.isFixed) continue; // not to be updated
                nd.trans = nd.oldtrans;
                nd.arot = nd.oldarot;
                nd.setTransform(); // set up projection matrix for cost calculation
                nd.setDr();
              }

            cost = calcCost();  // need to reset errors
            if (verbose)
              cout << iter << " Downdated cost: " << cost << endl;
            // NOTE: shouldn't need to redo all calcs in setupSys
          }

        t3 = utime();
        if (iter == 0 && verbose)
          {
            printf("[SPA] Setup: %0.2f ms  Solve: %0.2f ms  Update: %0.2f ms\n",
                   0.001*(double)(t1-t0),
                   0.001*(double)(t2-t1),
                   0.001*(double)(t3-t2));
          }

        double dt=1e-6*(double)(t3-t0);
        cumTime+=dt;
        if (print_iros_stats){
          cerr << "iteration= " << iter
               << "\t chi2= " << cost
               << "\t time= " << dt
               << "\t cumTime= " << cumTime
               << "\t kurtChi2= " << cost
               << endl;
        }

      }

    // return number of iterations performed
    return good_iter;

  }

  /// Run the LM algorithm that computes a nonlinear SPA estimate.
  /// <window> is the number of nodes in the window, the last nodes added
  /// <niter> is the max number of iterations to perform; returns the
  ///    number actually performed.
  /// <lambda> is the diagonal augmentation for LM.
  /// <useCSParse> = 0 for dense Cholesky, 1 for sparse Cholesky, 2 for sparse PCG

  static inline int getind(std::map<int,int> &m, int ind)
  {
    std::map<int,int>::iterator it;
    it = m.find(ind);
    if (it == m.end())
      return -1;
    else
      return it->second;
  }

  int SysSPA2d::doSPAwindowed(int window, int niter, double sLambda, int useCSparse)
  {
    // number of nodes
    int nnodes = nodes.size();
    if (nnodes < 2) return 0;

    int nlow = nnodes - window;
    if (nlow < 1) nlow = 1;     // always have one fixed node

    if (verbose)
      cout << "[SPA Window] From " << nlow << " to " << nnodes << endl;

    // number of constraints
    int ncons = p2cons.size();

    // set up SPA
    SysSPA2d spa;
    spa.verbose = verbose;

    // node, constraint vectors and index mapping
    std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &wnodes = spa.nodes;
    std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> > &wp2cons = spa.p2cons;
    std::map<int,int> inds;
    std::vector<int> rinds;     // reverse indices

    // loop through all constraints and set up fixed nodes and constraints
    for (int i=0; i<ncons; i++)
      {
        Con2dP2 &con = p2cons[i];
        if (con.ndr >= nlow || con.nd1 >= nlow)
            wp2cons.push_back(con);

        if (con.ndr >= nlow && con.nd1 < nlow) // have a winner
          {
            int j = getind(inds,con.nd1); // corresponding index
            if (j < 0)      // not present, add it in
              {
                inds.insert(std::pair<int,int>(con.nd1,wnodes.size()));
                wnodes.push_back(nodes[con.nd1]);
              }
            rinds.push_back(con.nd1);
          }
        else if (con.nd1 >= nlow && con.ndr < nlow)
          {
            int j = getind(inds,con.ndr); // corresponding index
            if (j < 0)      // not present, add it in
              {
                inds.insert(std::pair<int,int>(con.ndr,wnodes.size()));
                wnodes.push_back(nodes[con.ndr]);
              }
            rinds.push_back(con.ndr);
          }
      }

    spa.nFixed = wnodes.size();
    if (verbose)
      cout << "[SPA Window] Fixed node count: " << spa.nFixed << endl;

    // add in variable nodes
    for (int i=0; i<(int)wp2cons.size(); i++)
      {
        Con2dP2 &con = wp2cons[i];
        if (con.nd1 >= nlow && con.ndr >= nlow) // have a winner
          {
            int n0 = getind(inds,con.ndr);
            if (n0 < 0)
              {
                inds.insert(std::pair<int,int>(con.ndr,wnodes.size()));
                wnodes.push_back(nodes[con.ndr]);
                rinds.push_back(con.ndr);
              }
            int n1 = getind(inds,con.nd1);
            if (n1 < 0)
              {
                inds.insert(std::pair<int,int>(con.nd1,wnodes.size()));
                wnodes.push_back(nodes[con.nd1]);
                rinds.push_back(con.nd1);
              }
          }
      }

    if (verbose)
      {
        cout << "[SPA Window] Variable node count: " << spa.nodes.size() - spa.nFixed << endl;
        cout << "[SPA Window] Constraint count: " << spa.p2cons.size() << endl;
      }

    // new constraint indices
    for (int i=0; i<(int)wp2cons.size(); i++)
      {
        Con2dP2 &con = wp2cons[i];
        con.ndr = getind(inds,con.ndr);
        con.nd1 = getind(inds,con.nd1);
      }

    // run spa
    niter = spa.doSPA(niter,sLambda,useCSparse);

    // reset constraint indices
    for (int i=0; i<(int)wp2cons.size(); i++)
      {
        Con2dP2 &con = wp2cons[i];
        con.ndr = rinds[con.ndr];
        con.nd1 = rinds[con.nd1];
      }
    return niter;
  }

#ifdef SBA_DSIF
  ///
  /// Delayed Sparse Info Filter (DSIF)
  ///

  // Set up sparse linear system for Delayed Sparse Information Filter
  void SysSPA2d::setupSparseDSIF(int newnode)
  {
    // set matrix sizes and clear
    // assumes scales vars are all free
    int nFree = nodes.size() - nFixed;

    //    long long t0, t1, t2, t3;
    //    t0 = utime();

    // don't erase old stuff here, the delayed filter just grows in size
    csp.setupBlockStructure(nFree,false); // initialize CSparse structures

    //    t1 = utime();

    // loop over P2 constraints
    for(size_t pi=0; pi<p2cons.size(); pi++)
      {
        Con2dP2 &con = p2cons[pi];

        // don't consider old constraints
        if (con.ndr < newnode && con.nd1 < newnode)
          continue;

        con.setJacobians(nodes);

        // add in 4 blocks of A; actually just need upper triangular
        int i0 = con.ndr-nFixed; // will be negative if fixed
        int i1 = con.nd1-nFixed; // will be negative if fixed

        // DSIF will not diverge on standard datasets unless
        //   we reduce the precision of the constraints
        double fact = 1.0;
        if (i0 != i1-1) fact = 0.99; // what should we use????

        if (i0>=0)
          {
            Matrix<double,3,3> m = con.J0t*con.prec*con.J0;
            csp.addDiagBlock(m,i0);
          }
        if (i1>=0)
          {
            Matrix<double,3,3> m = con.J1t*con.prec*con.J1;
            csp.addDiagBlock(m,i1);

            if (i0>=0)
              {
                Matrix<double,3,3> m2 = con.J0t*con.prec*con.J1;
                m2 = m2 * fact * fact;
                if (i1 < i0)
                  {
                    m = m2.transpose();
                    csp.addOffdiagBlock(m,i1,i0);
                  }
                else
                  {
                    csp.addOffdiagBlock(m2,i0,i1);
                  }
              }
          }

        // add in 2 blocks of B
        if (i0>=0)
          csp.B.block<3,1>(i0*3,0) -= con.J0t * con.prec * con.err;
        if (i1>=0)
          csp.B.block<3,1>(i1*3,0) -= con.J1t * con.prec * con.err;

      } // finish P2 constraints

    //    t2 = utime();

    csp.Bprev = csp.B;          // save for next iteration

    // set up sparse matrix structure from blocks
    csp.setupCSstructure(1.0,true);

    //    t3 = utime();

    //    printf("\n[SetupSparseSys] Block: %0.1f   Cons: %0.1f  CS: %0.1f\n",
    //           (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);

  }

  /// Run the Delayed Sparse Information Filter (Eustice et al.)
  /// <newnode> is the index of the first new node added since the last iteration
  void SysSPA2d::doDSIF(int newnode)
  {
    // number of nodes
    int nnodes = nodes.size();

    // set number of constraints
    int ncons = p2cons.size();

    // check for fixed frames
    if (nFixed <= 0)
      {
        cout << "[doDSIF] No fixed frames" << endl;
        return;
      }

    // check for newnode being ok
    if (newnode >= nnodes)
      {
        cout << "[doDSIF] no new nodes to add" << endl;
        return;
      }
    else                        // set up saved mean value of pose
      {
        for (int i=newnode; i<nnodes; i++)
          {
            nodes[i].oldtrans = nodes[i].trans;
            nodes[i].oldarot = nodes[i].arot;
          }
      }

    for (int i=0; i<nnodes; i++)
      {
        Node2d &nd = nodes[i];
        if (i >= nFixed)
          nd.isFixed = false;
        else
          nd.isFixed = true;
        nd.setTransform();      // set up world-to-node transform for cost calculation
        nd.setDr();             // always use local angles
      }

    // initialize vars
    double cost = calcCost();
    if (verbose)
      cout << " Initial squared cost: " << cost << " which is "
           << sqrt(cost/ncons) << " rms error" << endl;

    // set up and solve linear system
    long long t0, t1, t2, t3;
    t0 = utime();
    setupSparseDSIF(newnode); // set up sparse linear system

#if 0
    cout << "[doDSIF] B = " << csp.B.transpose() << endl;
    csp.uncompress(A);
    cout << "[doDSIF] A = " << endl << A << endl;
#endif

    //        cout << "[SPA] Solving...";
    t1 = utime();
    bool ok = csp.doChol();
    if (!ok)
      cout << "[doDSIF] Sparse Cholesky failed!" << endl;
    t2 = utime();
    //        cout << "solved" << endl;

    // get correct result vector
    VectorXd &BB = csp.B;

    //    cout << "[doDSIF] RES  = " << BB.transpose() << endl;

    // update the frames
    int ci = 0;
    for(int i=0; i < nnodes; i++)
      {
        Node2d &nd = nodes[i];
        if (nd.isFixed) continue; // not to be updated
        nd.trans.head(2) = nd.oldtrans.head(2)+BB.segment<2>(ci);
        nd.arot = nd.oldarot+BB(ci+2);
        nd.normArot();
        nd.setTransform();  // set up projection matrix for cost calculation
        nd.setDr();         // set rotational derivatives
        ci += 3;            // advance B index
      }

    // new cost
    double newcost = calcCost();
    if (verbose)
      cout << " Updated squared cost: " << newcost << " which is "
           << sqrt(newcost/ncons) << " rms error" << endl;

    t3 = utime();
  }

  // write out the precision matrix for CSparse
  void SysSPA2d::writeSparseA(char *fname, bool useCSparse)
  {
    ofstream ofs(fname);
    if (ofs == NULL)
      {
        cout << "Can't open file " << fname << endl;
        return;
      }

    // cameras
    if (useCSparse)
      {
        setupSparseSys(0.0,0);

        int *Ai = csp.A->i;
        int *Ap = csp.A->p;
        double *Ax = csp.A->x;

        for (int i=0; i<csp.csize; i++)
          for (int j=Ap[i]; j<Ap[i+1]; j++)
            if (Ai[j] <= i)
              ofs << Ai[j] << " " << i << setprecision(16) << " " << Ax[j] << endl;
      }
    else
      {
        Eigen::IOFormat pfmt(16);

        int nrows = A.rows();
        int ncols = A.cols();

        for (int i=0; i<nrows; i++)
          for (int j=i; j<ncols; j++)
            {
              double a = A(i,j);
              if (A(i,j) != 0.0)
                ofs << i << " " << j << setprecision(16) << " " << a << endl;
            }
      }

    ofs.close();
  }
#endif

  // return vector of connections
  // 4 entries for each connection: x,y,x',y'
  void SysSPA2d::getGraph(std::vector<float> &graph)
  {
    for (int i=0; i<(int)p2cons.size(); i++)
      {
        Con2dP2 &con = p2cons[i];
        Node2d &nd0 = nodes[con.ndr];
        Node2d &nd1 = nodes[con.nd1];
        graph.push_back(nd0.trans(0));
        graph.push_back(nd0.trans(1));
        graph.push_back(nd1.trans(0));
        graph.push_back(nd1.trans(1));
      }
  }


/*
 *  Copyright 2011-2015 Maxim Milakov
 *
 *  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 "distort_2d_data_transformer.h"

#include "neural_network_exception.h"
#include "data_transformer_util.h"

#include <opencv2/core/core.hpp>
#include <boost/format.hpp>

namespace nnforge
{
	distort_2d_data_transformer::distort_2d_data_transformer(
		float max_absolute_rotation_angle_in_degrees,
		float max_scale_factor,
		float min_shift_right_x,
		float max_shift_right_x,
		float min_shift_down_y,
		float max_shift_down_y,
		bool flip_around_x_axis_allowed,
		bool flip_around_y_axis_allowed,
		float max_stretch_factor,
		float min_perspective_distance,
		unsigned char border_value)
		: border_value(border_value)
	{
		generator = rnd::get_random_generator();

		rotate_angle_distribution = nnforge_uniform_real_distribution<float>(-max_absolute_rotation_angle_in_degrees, max_absolute_rotation_angle_in_degrees);
		scale_distribution = nnforge_uniform_real_distribution<float>(1.0F / max_scale_factor, max_scale_factor);
		shift_x_distribution = nnforge_uniform_real_distribution<float>(min_shift_right_x, max_shift_right_x);
		shift_y_distribution = nnforge_uniform_real_distribution<float>(min_shift_down_y, max_shift_down_y);
		flip_around_x_distribution = nnforge_uniform_int_distribution<int>(0, flip_around_x_axis_allowed ? 1 : 0);
		flip_around_y_distribution = nnforge_uniform_int_distribution<int>(0, flip_around_y_axis_allowed ? 1 : 0);
		stretch_distribution = nnforge_uniform_real_distribution<float>(1.0F / max_stretch_factor, max_stretch_factor);
		stretch_angle_distribution = nnforge_uniform_real_distribution<float>(-180.0F, 180.0F);
		perspective_reverse_distance_distribution = nnforge_uniform_real_distribution<float>(0.0F, (min_perspective_distance == std::numeric_limits<float>::max()) ? 0.0F : 1.0F / min_perspective_distance);
		perspective_angle_distribution = nnforge_uniform_real_distribution<float>(-180.0F, 180.0F);
	}

	distort_2d_data_transformer::~distort_2d_data_transformer()
	{
	}

	void distort_2d_data_transformer::transform(
		const void * data,
		void * data_transformed,
		neuron_data_type::input_type type,
		const layer_configuration_specific& original_config,
		unsigned int sample_id)
	{
		if (type != neuron_data_type::type_byte)
			throw neural_network_exception("distort_2d_data_transformer is implemented for data stored as bytes only");

		if (original_config.dimension_sizes.size() < 2)
			throw neural_network_exception((boost::format("distort_2d_data_transformer is processing at least 2d data, data is passed with number of dimensions %1%") % original_config.dimension_sizes.size()).str());

		float rotation_angle = rotate_angle_distribution.min();
		if (rotate_angle_distribution.max() > rotate_angle_distribution.min())
			rotation_angle = rotate_angle_distribution(generator);
		float scale = scale_distribution.min();
		if (scale_distribution.max() > scale_distribution.min())
			scale = scale_distribution(generator);
		float shift_x = shift_x_distribution.min();
		if (shift_x_distribution.max() > shift_x_distribution.min())
			shift_x = shift_x_distribution(generator);
		float shift_y = shift_y_distribution.min();
		if (shift_y_distribution.max() > shift_y_distribution.min())
			shift_y = shift_y_distribution(generator);
		bool flip_around_x_axis = (flip_around_x_distribution.min() == 1);
		if (flip_around_x_distribution.max() > flip_around_x_distribution.min())
			flip_around_x_axis = (flip_around_x_distribution(generator) == 1);
		bool flip_around_y_axis = (flip_around_y_distribution.min() == 1);
		if (flip_around_y_distribution.max() > flip_around_y_distribution.min())
			flip_around_y_axis = (flip_around_y_distribution(generator) == 1);
		float stretch = stretch_distribution.min();
		if (stretch_distribution.max() > stretch_distribution.min())
			stretch = stretch_distribution(generator);
		float stretch_angle = stretch_angle_distribution.min();
		if (stretch_angle_distribution.max() > stretch_angle_distribution.min())
			stretch_angle = stretch_angle_distribution(generator);
		float perspective_reverse_distance = perspective_reverse_distance_distribution.min();
		if (perspective_reverse_distance_distribution.max() > perspective_reverse_distance_distribution.min())
			perspective_reverse_distance = perspective_reverse_distance_distribution(generator);
		float perspective_distance = std::numeric_limits<float>::max();
		if (perspective_reverse_distance > 0.0F)
			perspective_distance = 1.0F / perspective_reverse_distance;
		float perspective_angle = perspective_angle_distribution.min();
		if (perspective_angle_distribution.max() > perspective_angle_distribution.min())
			perspective_angle = perspective_angle_distribution(generator);

		unsigned int neuron_count_per_image = original_config.dimension_sizes[0] * original_config.dimension_sizes[1];
		unsigned int image_count = original_config.get_neuron_count() / neuron_count_per_image;
		for(unsigned int image_id = 0; image_id < image_count; ++image_id)
		{
			cv::Mat1b image(static_cast<int>(original_config.dimension_sizes[1]), static_cast<int>(original_config.dimension_sizes[0]), static_cast<unsigned char *>(data_transformed) + (image_id * neuron_count_per_image));

			if ((rotation_angle != 0.0F) || (scale != 1.0F) || (shift_x != 0.0F) || (shift_y != 0.0F) || (stretch != 1.0F) || (perspective_distance != std::numeric_limits<float>::max()))
			{
				data_transformer_util::stretch_rotate_scale_shift_perspective(
					image,
					cv::Point2f(static_cast<float>(image.cols) * 0.5F, static_cast<float>(image.rows) * 0.5F),
					rotation_angle,
					scale,
					shift_x,
					shift_y,
					stretch,
					stretch_angle,
					perspective_distance,
					perspective_angle,
					border_value);
			}

			data_transformer_util::flip(
				image,
				flip_around_x_axis,
				flip_around_y_axis);
		}
	}

 	bool distort_2d_data_transformer::is_deterministic() const
	{
		return false;
	}
}


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/LU>

namespace py = pybind11;

// convenient matrix indexing comes for free
double get(Eigen::MatrixXd xs, int i, int j) {
    return xs(i, j);
}

// takes numpy array as input and returns double
double det(Eigen::MatrixXd xs) {
    return xs.determinant();
}

// takes numpy array as input and returns another numpy array
Eigen::MatrixXd inv(Eigen::MatrixXd xs) {
    return xs.inverse();
}

PYBIND11_MODULE(ex8, m) {
    m.doc() = "auto-compiled c++ extension";
    m.def("inv", &inv);
    m.def("det", &det);
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;

template <int DataLayout>
static void test_simple_broadcasting()
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  tensor.setRandom();
  array<ptrdiff_t, 4> broadcasts;
  broadcasts[0] = 1;
  broadcasts[1] = 1;
  broadcasts[2] = 1;
  broadcasts[3] = 1;

  Tensor<float, 4, DataLayout> no_broadcast;
  no_broadcast = tensor.broadcast(broadcasts);

  VERIFY_IS_EQUAL(no_broadcast.dimension(0), 2);
  VERIFY_IS_EQUAL(no_broadcast.dimension(1), 3);
  VERIFY_IS_EQUAL(no_broadcast.dimension(2), 5);
  VERIFY_IS_EQUAL(no_broadcast.dimension(3), 7);

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 5; ++k) {
        for (int l = 0; l < 7; ++l) {
          VERIFY_IS_EQUAL(tensor(i,j,k,l), no_broadcast(i,j,k,l));
        }
      }
    }
  }

  broadcasts[0] = 2;
  broadcasts[1] = 3;
  broadcasts[2] = 1;
  broadcasts[3] = 4;
  Tensor<float, 4, DataLayout> broadcast;
  broadcast = tensor.broadcast(broadcasts);

  VERIFY_IS_EQUAL(broadcast.dimension(0), 4);
  VERIFY_IS_EQUAL(broadcast.dimension(1), 9);
  VERIFY_IS_EQUAL(broadcast.dimension(2), 5);
  VERIFY_IS_EQUAL(broadcast.dimension(3), 28);

  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 9; ++j) {
      for (int k = 0; k < 5; ++k) {
        for (int l = 0; l < 28; ++l) {
          VERIFY_IS_EQUAL(tensor(i%2,j%3,k%5,l%7), broadcast(i,j,k,l));
        }
      }
    }
  }
}

template <int DataLayout>
static void test_vectorized_broadcasting()
{
  Tensor<float, 3, DataLayout> tensor(8,3,5);
  tensor.setRandom();
  array<ptrdiff_t, 3> broadcasts;
  broadcasts[0] = 2;
  broadcasts[1] = 3;
  broadcasts[2] = 4;

  Tensor<float, 3, DataLayout> broadcast;
  broadcast = tensor.broadcast(broadcasts);

  VERIFY_IS_EQUAL(broadcast.dimension(0), 16);
  VERIFY_IS_EQUAL(broadcast.dimension(1), 9);
  VERIFY_IS_EQUAL(broadcast.dimension(2), 20);

  for (int i = 0; i < 16; ++i) {
    for (int j = 0; j < 9; ++j) {
      for (int k = 0; k < 20; ++k) {
        VERIFY_IS_EQUAL(tensor(i%8,j%3,k%5), broadcast(i,j,k));
      }
    }
  }

  tensor.resize(11,3,5);
  tensor.setRandom();
  broadcast = tensor.broadcast(broadcasts);

  VERIFY_IS_EQUAL(broadcast.dimension(0), 22);
  VERIFY_IS_EQUAL(broadcast.dimension(1), 9);
  VERIFY_IS_EQUAL(broadcast.dimension(2), 20);

  for (int i = 0; i < 22; ++i) {
    for (int j = 0; j < 9; ++j) {
      for (int k = 0; k < 20; ++k) {
        VERIFY_IS_EQUAL(tensor(i%11,j%3,k%5), broadcast(i,j,k));
      }
    }
  }
}

template <int DataLayout>
static void test_static_broadcasting()
{
  Tensor<float, 3, DataLayout> tensor(8,3,5);
  tensor.setRandom();

#if defined(EIGEN_HAS_INDEX_LIST)
  Eigen::IndexList<Eigen::type2index<2>, Eigen::type2index<3>, Eigen::type2index<4>> broadcasts;
#else
  Eigen::array<int, 3> broadcasts;
  broadcasts[0] = 2;
  broadcasts[1] = 3;
  broadcasts[2] = 4;
#endif

  Tensor<float, 3, DataLayout> broadcast;
  broadcast = tensor.broadcast(broadcasts);

  VERIFY_IS_EQUAL(broadcast.dimension(0), 16);
  VERIFY_IS_EQUAL(broadcast.dimension(1), 9);
  VERIFY_IS_EQUAL(broadcast.dimension(2), 20);

  for (int i = 0; i < 16; ++i) {
    for (int j = 0; j < 9; ++j) {
      for (int k = 0; k < 20; ++k) {
        VERIFY_IS_EQUAL(tensor(i%8,j%3,k%5), broadcast(i,j,k));
      }
    }
  }

  tensor.resize(11,3,5);
  tensor.setRandom();
  broadcast = tensor.broadcast(broadcasts);

  VERIFY_IS_EQUAL(broadcast.dimension(0), 22);
  VERIFY_IS_EQUAL(broadcast.dimension(1), 9);
  VERIFY_IS_EQUAL(broadcast.dimension(2), 20);

  for (int i = 0; i < 22; ++i) {
    for (int j = 0; j < 9; ++j) {
      for (int k = 0; k < 20; ++k) {
        VERIFY_IS_EQUAL(tensor(i%11,j%3,k%5), broadcast(i,j,k));
      }
    }
  }
}

template <int DataLayout>
static void test_fixed_size_broadcasting()
{
  // Need to add a [] operator to the Size class for this to work
#if 0
  Tensor<float, 1, DataLayout> t1(10);
  t1.setRandom();
  TensorFixedSize<float, Sizes<1>, DataLayout> t2;
  t2 = t2.constant(20.0f);

  Tensor<float, 1, DataLayout> t3 = t1 + t2.broadcast(Eigen::array<int, 1>{{10}});
  for (int i = 0; i < 10; ++i) {
    VERIFY_IS_APPROX(t3(i), t1(i) + t2(0));
  }

  TensorMap<TensorFixedSize<float, Sizes<1>, DataLayout> > t4(t2.data(), {{1}});
  Tensor<float, 1, DataLayout> t5 = t1 + t4.broadcast(Eigen::array<int, 1>{{10}});
  for (int i = 0; i < 10; ++i) {
    VERIFY_IS_APPROX(t5(i), t1(i) + t2(0));
  }
#endif
}

template <int DataLayout>
static void test_simple_broadcasting_one_by_n()
{
  Tensor<float, 4, DataLayout> tensor(1,13,5,7);
  tensor.setRandom();
  array<ptrdiff_t, 4> broadcasts;
  broadcasts[0] = 9;
  broadcasts[1] = 1;
  broadcasts[2] = 1;
  broadcasts[3] = 1;
  Tensor<float, 4, DataLayout> broadcast;
  broadcast = tensor.broadcast(broadcasts);

  VERIFY_IS_EQUAL(broadcast.dimension(0), 9);
  VERIFY_IS_EQUAL(broadcast.dimension(1), 13);
  VERIFY_IS_EQUAL(broadcast.dimension(2), 5);
  VERIFY_IS_EQUAL(broadcast.dimension(3), 7);

  for (int i = 0; i < 9; ++i) {
    for (int j = 0; j < 13; ++j) {
      for (int k = 0; k < 5; ++k) {
        for (int l = 0; l < 7; ++l) {
          VERIFY_IS_EQUAL(tensor(i%1,j%13,k%5,l%7), broadcast(i,j,k,l));
        }
      }
    }
  }
}

template <int DataLayout>
static void test_simple_broadcasting_n_by_one()
{
  Tensor<float, 4, DataLayout> tensor(7,3,5,1);
  tensor.setRandom();
  array<ptrdiff_t, 4> broadcasts;
  broadcasts[0] = 1;
  broadcasts[1] = 1;
  broadcasts[2] = 1;
  broadcasts[3] = 19;
  Tensor<float, 4, DataLayout> broadcast;
  broadcast = tensor.broadcast(broadcasts);

  VERIFY_IS_EQUAL(broadcast.dimension(0), 7);
  VERIFY_IS_EQUAL(broadcast.dimension(1), 3);
  VERIFY_IS_EQUAL(broadcast.dimension(2), 5);
  VERIFY_IS_EQUAL(broadcast.dimension(3), 19);

  for (int i = 0; i < 7; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 5; ++k) {
        for (int l = 0; l < 19; ++l) {
          VERIFY_IS_EQUAL(tensor(i%7,j%3,k%5,l%1), broadcast(i,j,k,l));
        }
      }
    }
  }
}

template <int DataLayout>
static void test_simple_broadcasting_one_by_n_by_one_1d()
{
  Tensor<float, 3, DataLayout> tensor(1,7,1);
  tensor.setRandom();
  array<ptrdiff_t, 3> broadcasts;
  broadcasts[0] = 5;
  broadcasts[1] = 1;
  broadcasts[2] = 13;
  Tensor<float, 3, DataLayout> broadcasted;
  broadcasted = tensor.broadcast(broadcasts);

  VERIFY_IS_EQUAL(broadcasted.dimension(0), 5);
  VERIFY_IS_EQUAL(broadcasted.dimension(1), 7);
  VERIFY_IS_EQUAL(broadcasted.dimension(2), 13);

  for (int i = 0; i < 5; ++i) {
    for (int j = 0; j < 7; ++j) {
      for (int k = 0; k < 13; ++k) {
        VERIFY_IS_EQUAL(tensor(0,j%7,0), broadcasted(i,j,k));
      }
    }
  }
}

template <int DataLayout>
static void test_simple_broadcasting_one_by_n_by_one_2d()
{
  Tensor<float, 4, DataLayout> tensor(1,7,13,1);
  tensor.setRandom();
  array<ptrdiff_t, 4> broadcasts;
  broadcasts[0] = 5;
  broadcasts[1] = 1;
  broadcasts[2] = 1;
  broadcasts[3] = 19;
  Tensor<float, 4, DataLayout> broadcast;
  broadcast = tensor.broadcast(broadcasts);

  VERIFY_IS_EQUAL(broadcast.dimension(0), 5);
  VERIFY_IS_EQUAL(broadcast.dimension(1), 7);
  VERIFY_IS_EQUAL(broadcast.dimension(2), 13);
  VERIFY_IS_EQUAL(broadcast.dimension(3), 19);

  for (int i = 0; i < 5; ++i) {
    for (int j = 0; j < 7; ++j) {
      for (int k = 0; k < 13; ++k) {
        for (int l = 0; l < 19; ++l) {
          VERIFY_IS_EQUAL(tensor(0,j%7,k%13,0), broadcast(i,j,k,l));
        }
      }
    }
  }
}

EIGEN_DECLARE_TEST(cxx11_tensor_broadcasting)
{
  CALL_SUBTEST(test_simple_broadcasting<ColMajor>());
  CALL_SUBTEST(test_simple_broadcasting<RowMajor>());
  CALL_SUBTEST(test_vectorized_broadcasting<ColMajor>());
  CALL_SUBTEST(test_vectorized_broadcasting<RowMajor>());
  CALL_SUBTEST(test_static_broadcasting<ColMajor>());
  CALL_SUBTEST(test_static_broadcasting<RowMajor>());
  CALL_SUBTEST(test_fixed_size_broadcasting<ColMajor>());
  CALL_SUBTEST(test_fixed_size_broadcasting<RowMajor>());
  CALL_SUBTEST(test_simple_broadcasting_one_by_n<RowMajor>());
  CALL_SUBTEST(test_simple_broadcasting_n_by_one<RowMajor>());
  CALL_SUBTEST(test_simple_broadcasting_one_by_n<ColMajor>());
  CALL_SUBTEST(test_simple_broadcasting_n_by_one<ColMajor>());
  CALL_SUBTEST(test_simple_broadcasting_one_by_n_by_one_1d<ColMajor>());
  CALL_SUBTEST(test_simple_broadcasting_one_by_n_by_one_2d<ColMajor>());
  CALL_SUBTEST(test_simple_broadcasting_one_by_n_by_one_1d<RowMajor>());
  CALL_SUBTEST(test_simple_broadcasting_one_by_n_by_one_2d<RowMajor>());
}


#include <boost/unordered/unordered_map.hpp>
#include <queue>
#include <deque>

#include "observables/ShapeObservable.h"
#include "Vertex.h"
#include "Triangle.h"
#include <utility>

ShapeObservable::ShapeObservable(unsigned long writeFrequency) :
Observable(writeFrequency, 0, true),
filename(createFilename("shape")),
file(filename.c_str()) {
}

ShapeObservable::~ShapeObservable() {
}

bool ShapeObservable::checkNonContractibility(
        const boost::unordered_set<std::pair<unsigned int, unsigned int> >& edge) {
    unsigned int t, s;
    t = (*edge.begin()).first;
    s = (*edge.begin()).second;

    std::queue<unsigned int> queueA, queueB;
    std::vector<char> visitedA(dualNeighbours.size()),
            visitedB(dualNeighbours.size());
    unsigned int curA, curB;

    queueA.push(s);
    visitedA[s] = true;
    queueB.push(t);
    visitedB[t] = true;

    while (!queueA.empty() && !queueB.empty()) {
        curA = queueA.front();
        queueA.pop();

        curB = queueB.front();
        queueB.pop();

        for (int i = 0; i < 3; i++) {
            unsigned int nA = dualNeighbours[curA][i];
            unsigned int nB = dualNeighbours[curB][i];

            if (!visitedA[nA] &&
                    edge.find(std::make_pair(curA, nA)) == edge.end()) {
                queueA.push(nA);
                visitedA[nA] = true;

                if (visitedB[nA]) {
                    return true;
                }
            }

            if (!visitedB[nB] &&
                    edge.find(std::make_pair(curB, nB)) == edge.end()) {
                queueB.push(nB);
                 visitedB[nB] = true;

                if (visitedA[nB]) {
                    return true;
                }
            }
        }
    }

    return false;
}

void ShapeObservable::findNonContractibleLoop(const std::vector<Vertex*>& state,
        unsigned int start) {
    unsigned int cur;
    std::vector<unsigned int> prev(neighbours.size());
    std::queue<unsigned int> queue;
    queue.push(start);

    for (unsigned int i = 0; i < prev.size(); i++) {
        prev[i] = prev.size(); // prev.size() => no prev
    }

    while (!queue.empty()) {
        cur = queue.front();
        queue.pop();

        for (unsigned int k = 0; k < neighbours[cur].size(); k++) {
            unsigned int n = neighbours[cur][k];
            if (prev[n] == prev.size()) {
                queue.push(n);
                prev[n] = cur;
            } else { // loop found
                // loops in the same triangle are contractible
                if (prev[n] == prev[cur]) {
                    continue;
                }

                // TODO: eliminate obvious contractible loops
                unsigned int s = cur, t = n;
                boost::unordered_set<unsigned int> visited;
                std::deque<unsigned int> edge;

                while (true) {
                    if (s != prev.size()) {
                        visited.insert(s);
                        edge.insert(edge.begin(), s);
                        s = prev[s];
                    }

                    if (t != prev.size()) {
                        visited.insert(t);
                        edge.push_back(t);
                        t = prev[t];
                    }

                    if (s == t) {
                        visited.insert(s);
                        edge.insert(edge.begin(), s);
                        break;
                    }

                    if (visited.find(t) != visited.end()) {
                        // remove leading part from s branch
                        for (unsigned int i = 1; i < edge.size(); i++) {
                            if (edge[i] == t) {
                                edge.erase(edge.begin(), edge.begin() + i - 1);
                                break;
                            }
                        }
                        break;
                    }

                    if (visited.find(s) != visited.end()) {
                        // remove trailing part from t branch
                        for (unsigned int i = edge.size() - 2; i >= 0; i--) {
                            if (edge[i] == s) {
                                edge.erase(edge.begin() + i + 1, edge.end());
                                break;
                            }
                        }
                        break;
                    }
                }

                if (edge.size() < 3) { // disallow pathological case
                    continue;
                }

                // std::cout << "edge size: " << edge.size() << std::endl;
                /*  for (int i = 0; i < edge.size(); i++) {
                      VertSet a = state[edge[i]]->getNeighbouringVertices();
                      BOOST_ASSERT(a.find(state[edge[(i + 1) % edge.size()]]) != a.end());
                  }*/

                boost::unordered_set<std::pair<unsigned int, unsigned int> >
                        edgeVertices;
                Triangle* q, *r;
                for (unsigned int i = 0; i < edge.size(); i++) {
                    Vertex::getAdjacentTriangles(state[edge[i]],
                            state[edge[(i + 1) % edge.size()]], &q, &r);

                    edgeVertices.insert(std::make_pair(triangleIds[q], triangleIds[r]));
                    edgeVertices.insert(std::make_pair(triangleIds[r], triangleIds[q]));
                }

                if (checkNonContractibility(edgeVertices)) {
                    size = edgeVertices.size() / 2;
                    //  std::cout << "Found non-contractible loop of length " <<
                    //          edgeVertices.size() / 2 << std::endl;

                    return;
                }
            }
        }
    }

    std::cout << "No non-contractible loops found..." << std::endl;
    BOOST_ASSERT(false);
}

void ShapeObservable::process(const std::vector<Vertex*>& state) {
    neighbours = buildLatticeConnectivity(state);
    triangleIds = createTriangleIds(state);
    dualNeighbours = buildDualLatticeConnectivity(triangleIds);

    for (unsigned int i = 0; i < state.size(); i += state.size() / 10) {
        findNonContractibleLoop(state, i);
        file << size << " "; // FIXME, move to printToFile
    }
    file << std::endl;
}

void ShapeObservable::printToScreen() {
    //std::cout << "Found non-contractible loop of length " <<
    //        size << std::endl;
}

void ShapeObservable::printToFile() {
    //file << size << "\n";
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2017 Sebastian Koch <s.koch@tu-berlin.de> and Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
//#include <Eigen/Geometry>
//#include <Eigen/Dense>
//#include <Eigen/Sparse>

#include "../../python_shared.h"

#include <igl/copyleft/marching_cubes.h>
#include <igl/copyleft/swept_volume.h>

void python_export_igl_copyleft(py::module &me) {

  py::module m = me.def_submodule(
    "copyleft", "Wrappers for libigl functions that are copyleft");

  #include "../../py_igl/copyleft/py_marching_cubes.cpp"
  #include "../../py_igl/copyleft/py_swept_volume.cpp"

}


/// @file  dists.hpp
/// @brief Generating distributions for synthetic datasets.

#pragma once
#ifndef OGT_UTIL_DISTS_HPP
#define OGT_UTIL_DISTS_HPP

#include <ogt/config.hpp>
#include <array>
#include <Eigen/Dense>
#include <json.hpp>
#include <memory>
#include <string>
#include <vector>

namespace OGT_NAMESPACE {
namespace util {

/// The type thrown when invalid methods are reqested.
struct InvalidPointGeneratorErr : public std::runtime_error {

	/// Build an InvalidPointGeneratorErr with the specified message.
	InvalidPointGeneratorErr(std::string message)
		: std::runtime_error("ogt::InvalidPointGeneratorErr: " + message) {
	}
};

/// An abstract generator of datasets.
struct PointGenerator {
	virtual ~PointGenerator() = default;

	/// Create a generator using the specified method, specified in JSON. Throws
	/// InvalidPointGeneratorErr if method is invalid.
	static std::shared_ptr<PointGenerator> Create(nlohmann::json method);

	/// Generate a batch of points.
	virtual Eigen::MatrixXd generate(size_t nPoints) = 0;
};

/// Generate points uniformly from a box.
struct UnifBoxDist : PointGenerator {
	virtual ~UnifBoxDist() = default;

	/// Initialize with the specified box boundaries
	UnifBoxDist(std::vector<std::array<double,2>> boundaries)
		: boundaries(boundaries) {}

	/// Generate a batch of points.
	Eigen::MatrixXd generate(size_t nPoints) override;

	/// The box boundaries.
	std::vector<std::array<double,2>> boundaries;
};

/// Generate points uniformly from a ball.
struct BallDist : PointGenerator {
	virtual ~BallDist() = default;

	/// Initialize with the center and radius.
	BallDist(Eigen::VectorXd center, double radius)
		: center(center), radius(radius) {}

	/// Generate a batch of points.
	Eigen::MatrixXd generate(size_t nPoints) override;

	/// The ball center.
	Eigen::VectorXd center;

	/// The ball radius.
	double radius;
};

/// Generate points uniformly from a sphere (the boundary of a ball).
struct SphereDist : PointGenerator {
	virtual ~SphereDist() = default;

	/// Initialize with the center and radius.
	SphereDist(Eigen::VectorXd center, double radius)
		: center(center), radius(radius) {}

	/// Generate a batch of points.
	Eigen::MatrixXd generate(size_t nPoints) override;

	/// The sphere center.
	Eigen::VectorXd center;

	/// The sphere radius.
	double radius;
};

/// Generate points uniformly from a simplex (d-dimensional triangle).
///
/// Algorithm based on:
/// http://www.mathworks.com/matlabcentral/newsreader/view_thread/170480
struct SimplexDist : PointGenerator {
	virtual ~SimplexDist() = default;

	/// Initialize to the unit simplex in nDim dimensions.
	SimplexDist(size_t nDim);

	/// Initialize to a simplex with the given vertices. We assume the vertices
	/// you provide are affinely-independent.
	/// Each row of vertices is a vertex position.
	SimplexDist(Eigen::MatrixXd vertices) : vertices(vertices) {}

	/// Generate a batch of points.
	Eigen::MatrixXd generate(size_t nPoints) override;

	/// The simplex vertices.
	Eigen::MatrixXd vertices;
};

/// Generate points from a Multivariate Normal distribution.
struct MVNDist : PointGenerator {
	virtual ~MVNDist() = default;

	/// Initialize using the specified mean and covariance.
	MVNDist(Eigen::VectorXd mu, Eigen::MatrixXd cov) : mu(mu), cov(cov) {}

	/// Generate a batch of points.
	Eigen::MatrixXd generate(size_t nPoints) override;

	/// The mean.
	Eigen::VectorXd mu;

	/// The covariance matrix.
	Eigen::MatrixXd cov;
};

/// Generate points from a mixture model.
struct MixtureModel : PointGenerator {
	virtual ~MixtureModel() = default;

	/// Initialize using the specified components and mixture probabilites.
	MixtureModel(std::vector<std::shared_ptr<PointGenerator>> components,
		Eigen::VectorXd mixture)
		: components(components), mixture(mixture) {}

	/// Generate a batch of points.
	Eigen::MatrixXd generate(size_t nPoints) override;

	/// The mixture components.
	std::vector<std::shared_ptr<PointGenerator>> components;

	/// The mixture probaiblities.
	Eigen::VectorXd mixture;
};

/// Generate points from a Swiss Roll distribution.
/// This is a 2D manifold in the shape of a spiral inside a 3D background space.
struct SwissRollGenerator : PointGenerator {
	virtual ~SwissRollGenerator() = default;

	/// Generate a batch of points.
	Eigen::MatrixXd generate(size_t nPoints) override;
};

/// Generate points from a (possibly) scale-free process.
/// This process tends to create dense regions and sparse regions, where new
/// points are more likely to be added to already-dense regions.
/// We generate points by first deciding whether to randomly add a new point
/// uniformly within the bounding box of the set, with probability lambda.
/// The bounding box is initially the cube with range [-1, +1] in each
/// dimension. It is expanded as needed to contain all generated points.
///
/// If not, then (with probability 1-lambda) we select an existing "reference"
/// point uniformly at random and add a point close to the reference point. The
/// new point's position is chosen one axis at a time by selecting a random sign
/// and then drawing an offset from the reference point's position from an
/// exponential distribution with mean mu.
struct ScaleFreeGenerator : PointGenerator {
	virtual ~ScaleFreeGenerator() = default;

	/// Construct with exploration rate lambda and expected distance mu.
	ScaleFreeGenerator(size_t nDim, double lambda, double mu)
		: boundingBox(std::vector<std::array<double,2>>(nDim, {-1.0, +1.0}))
		, lambda(lambda)
		, mu(mu)
	{}

	/// Generate a batch of points.
	Eigen::MatrixXd generate(size_t nPoints) override;

	/// The distribution over the bounding box. The box will be expanded as
	/// needed when points are drawn outside its bounds.
	UnifBoxDist boundingBox;

	/// The probability of generating a point at random over the bounding box.
	double lambda;

	/// The average distance from a reference point along each axis.
	double mu;
};

} // end namespace util
} // end namespace OGT_NAMESPACE
#endif /* OGT_UTIL_DISTS_HPP */


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the Bayesian Network
    inference utilities found in the dlib C++ library.  In this example
    we load a saved Bayesian Network from disk.
*/

#include <dlib/bayes_utils.h>
#include <dlib/graph_utils.h>
#include <dlib/graph.h>
#include <dlib/directed_graph.h>
#include <iostream>
#include <fstream>

using namespace dlib;
using namespace std;

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

int main(int argc, char** argv)
{
    try
    {
        // This statement declares a bayesian network called bn.  Note that a bayesian network
        // in the dlib world is just a directed_graph object that contains a special kind
        // of node called a bayes_node.
        directed_graph<bayes_node>::kernel_1a_c bn;

        if (argc != 2)
        {
            cout << "You must supply a file name on the command line.  The file should "
                 << "contain a serialized Bayesian Network" << endl;
            return 1;
        }

        ifstream fin(argv[1],ios::binary);

        // Note that the saved networks produced by the bayes_net_gui_ex.cpp example can be deserialized
        // into a network.  So you can make your networks using that GUI if you like.
        cout << "Loading the network from disk..." << endl;
        deserialize(bn, fin);

        cout << "Number of nodes in the network: " << bn.number_of_nodes() << endl;

        // Let's compute some probability values using the loaded network using the join tree (aka. Junction
        // Tree) algorithm.

        // First we need to create an undirected graph which contains set objects at each node and
        // edge.  This long declaration does the trick.
        typedef graph<dlib::set<unsigned long>::compare_1b_c, dlib::set<unsigned long>::compare_1b_c>::kernel_1a_c join_tree_type;
        join_tree_type join_tree;

        // Now we need to populate the join_tree with data from our bayesian network.  The next two
        // function calls do this.  Explaining exactly what they do is outside the scope of this
        // example.  Just think of them as filling join_tree with information that is useful
        // later on for dealing with our bayesian network.
        create_moral_graph(bn, join_tree);
        create_join_tree(join_tree, join_tree);

        // Now we have a proper join_tree we can use it to obtain a solution to our
        // bayesian network.  Doing this is as simple as declaring an instance of
        // the bayesian_network_join_tree object as follows:
        bayesian_network_join_tree solution(bn, join_tree);

        // now print out the probabilities for each node
        cout << "Using the join tree algorithm:\n";
        for (unsigned long i = 0; i < bn.number_of_nodes(); ++i)
        {
            // print out the probability distribution for node i.
            cout << "p(node " << i <<") = " << solution.probability(i);
        }
    }
    catch (exception& e)
    {
        cout << "exception thrown: " << e.what() << endl;
        return 1;
    }
}


/**
 * Copyright (c) 2018-present, Facebook, Inc.
 *
 * 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 "caffe2/operators/pow_op.h"
#include "caffe2/utils/math.h"
// definition of NumericTypes and SameTypeAsInput is in below header file
//#include "caffe2/operators/elementwise_op.h"
#include <Eigen/Core>

namespace caffe2 {

#define EIGEN_POW(x, y) (x.pow(y))

struct EigenPowFunctor {
  template <int b_is_scalar, typename T1, typename T2, typename R>
  inline void
  Run(size_t n, const T1* a, const T2* b, T2 e, R* out, CPUContext*) {
    if (b == NULL) {
      EigenVectorArrayMap<R>(out, n) =
          EIGEN_POW((ConstEigenVectorArrayMap<T1>(a, n)), (e));
    } else {
      if (b_is_scalar) {
        EigenVectorArrayMap<R>(out, n) =
            EIGEN_POW((ConstEigenVectorArrayMap<T1>(a, n)), (b[0]));
      } else {
        EigenVectorArrayMap<R>(out, n) = EIGEN_POW(
            (ConstEigenVectorArrayMap<T1>(a, n)),
            (ConstEigenVectorArrayMap<T2>(b, n)));
      }
    }
  }
  template <typename T1, typename T2, typename R>
  void RunWithBroadcast(
      const T1* a,
      const T2* b,
      R* out,
      size_t pre,
      size_t n,
      CPUContext*) {
    EigenArrayMap<R>(out, n, pre) = EIGEN_POW(
        (ConstEigenArrayMap<T1>(a, n, pre)),
        (ConstEigenVectorArrayMap<T2>(b, n)).rowwise().replicate(pre));
    /*
    //below code only allows elementary ops, such as +, -, * and /,
    //and does not allow operations, such as pow, exp and log
    EIGEN_POW(
       (ConstEigenArrayMap<T>(a, n, pre).colwise()),
       (ConstEigenVectorArrayMap<T>(b, n)));
     */
  }
  template <typename T1, typename T2, typename R>
  void RunWithBroadcast2(
      const T1* a,
      const T2* b,
      R* out,
      size_t pre,
      size_t n,
      size_t post,
      CPUContext*) {
    for (int i = 0; i < pre; ++i) {
      EigenArrayMap<R>(out + i * n * post, post, n) = EIGEN_POW(
          (ConstEigenArrayMap<T1>(a + i * n * post, post, n)),
          (Eigen::Map<const Eigen::Array<T2, 1, Eigen::Dynamic>>(b, n))
              .colwise()
              .replicate(post));
      /*
      //below code only allows elementary ops, such as +, -, * and /,
      //and does not allow for operations, such as pow, exp and log
      EIEGN_POW(
        (ConstEigenArrayMap<T>(a + i * n * post, post, n).rowwise()),
        (Eigen::Map<const Eigen::Array<T, 1, Eigen::Dynamic>>(b, n)));
      */
    }
  }
};

REGISTER_CPU_OPERATOR(
    Pow,
    PowOp<
        TensorTypes<float> /*NumericTypes*/,
        CPUContext,
        EigenPowFunctor,
        SameTypeAsInput>)

OPERATOR_SCHEMA(Pow)
    .NumInputs(1, 2)
    .NumOutputs(1)
    .Arg("exponent", "The exponent of the power function.")
    .AllowInplace({{0, 0}, {1, 0}})
    .IdenticalTypeAndShapeOfInput(0)
    .SetDoc(R"DOC(
Pow takes input data (Tensor<T>) and an argument exponent, which can be a
scalar or another tensor. It produces one output data (Tensor<T>), where
the function `f(x) = x^exponent` is applied to the data tensor elementwise.
)DOC")
    .Input(0, "X", "Input tensor of any shape")
    .Input(1, "exponent", "The exponent of the power function.")
    .Output(0, "Y", "Output tensor (same size as X)");

class GetPowGradient : public GradientMakerBase {
  using GradientMakerBase::GradientMakerBase;
  vector<OperatorDef> GetGradientDefs() override {
    ArgumentHelper arg_helper(def_);
    if (arg_helper.HasArgument("exponent")) { // second input is a scalar
      // function f(w,a) = w^a
      // gradient operator with respect to first input tensor
      // df/dw = a * w^(a-1) (all operations are component-wise)
      float exponent = arg_helper.GetSingleArgument<float>("exponent", 0.0);
      Argument scale_arg;
      scale_arg.set_name("scale");
      scale_arg.set_f(exponent);
      Argument pow_arg;
      pow_arg.set_name("exponent");
      if (I(0) != O(0)) {
        pow_arg.set_f(exponent - 1);
      } else {
        LOG(WARNING) << "In-place Pow gradient, possible loss of precision";
        constexpr float kEps = 1e-12f;
        CAFFE_ENFORCE(std::fabs(exponent) > kEps);
        pow_arg.set_f((exponent - 1) / exponent);
      }
      return vector<OperatorDef>{CreateOperatorDef(
                                     "Pow",
                                     "",
                                     std::vector<string>{I(0)},
                                     std::vector<string>{GI(0)},
                                     std::vector<Argument>{pow_arg}),
                                 CreateOperatorDef(
                                     "Mul",
                                     "",
                                     std::vector<string>{GI(0), GO(0)},
                                     std::vector<string>{GI(0)}),
                                 CreateOperatorDef(
                                     "Scale",
                                     "",
                                     std::vector<string>{GI(0)},
                                     std::vector<string>{GI(0)},
                                     std::vector<Argument>{scale_arg})};
      /*
      // Alternative gradient computation
      return vector<OperatorDef>{CreateOperatorDef(
                                     "Div",
                                     "",
                                     std::vector<string>{O(0), I(0)},
                                     std::vector<string>{GI(0)}),
                                 CreateOperatorDef(
                                     "Mul",
                                     "",
                                     std::vector<string>{GI(0), GO(0)},
                                     std::vector<string>{GI(0)}),
                                 CreateOperatorDef(
                                     "Scale",
                                     "",
                                     std::vector<string>{GI(0)},
                                     std::vector<string>{GI(0)},
                                     std::vector<Argument>{scale_arg})};
      */
    } else { // second input is a tensor
      CAFFE_ENFORCE(
          Def().input(0) != Def().output(0) &&
              Def().input(1) != Def().output(0),
          "Gradient computation cannot be carried out if Pow uses in-place "
          "computation: ",
          ProtoDebugString(Def()));
      vector<OperatorDef> grad_ops;
      Argument one_arg;
      one_arg.set_name("value");
      one_arg.set_f(1);
      Argument broadcast, axis, axis_str, order;
      bool bflag = ArgumentHelper::HasArgument(Def(), "broadcast");

      if (bflag) {
        if (ArgumentHelper::HasArgument(Def(), "broadcast")) {
          broadcast = GetArgument(Def(), "broadcast");
        } else {
          broadcast = MakeArgument<int>("broadcast", 0);
        }
        if (ArgumentHelper::HasArgument(Def(), "axis")) {
          axis = GetArgument(Def(), "axis");
        } else {
          axis = MakeArgument<int>("axis", -1);
        }
        if (ArgumentHelper::HasArgument(Def(), "axis_str")) {
          axis_str = GetArgument(Def(), "axis_str");
        } else {
          axis_str = MakeArgument<string>("axis_str", "");
        }
        if (ArgumentHelper::HasArgument(Def(), "order")) {
          order = GetArgument(Def(), "order");
        } else {
          order = MakeArgument<string>("order", "NCHW");
        }
      }

      // function f(w,a) = w^a
      // gradient operator with respect to first input tensor
      // df/dw = a * w^(a-1) (all operations are component-wise)
      grad_ops.push_back(CreateOperatorDef(
          "ConstantFill",
          "",
          std::vector<string>{I(1)},
          std::vector<string>{GI(1)},
          std::vector<Argument>{one_arg}));
      grad_ops.push_back(CreateOperatorDef(
          "Sub",
          "",
          std::vector<string>{I(1), GI(1)},
          std::vector<string>{GI(1)}));
      if (bflag) {
        grad_ops.push_back(CreateOperatorDef(
            "Pow",
            "",
            std::vector<string>{I(0), GI(1)},
            std::vector<string>{GI(0)},
            vector<Argument>{broadcast, axis, axis_str, order}));
      } else {
        grad_ops.push_back(CreateOperatorDef(
            "Pow",
            "",
            std::vector<string>{I(0), GI(1)},
            std::vector<string>{GI(0)}));
      }

      grad_ops.push_back(CreateOperatorDef(
          "Mul",
          "",
          std::vector<string>{GI(0), GO(0)},
          std::vector<string>{GI(0)}));
      if (bflag) {
        grad_ops.push_back(CreateOperatorDef(
            "Mul",
            "",
            std::vector<string>{GI(0), I(1)},
            std::vector<string>{GI(0)},
            vector<Argument>{broadcast, axis, axis_str, order}));
      } else {
        grad_ops.push_back(CreateOperatorDef(
            "Mul",
            "",
            std::vector<string>{GI(0), I(1)},
            std::vector<string>{GI(0)}));
      }
      /*
      // Alternative gradient computation (no broadcast support)
      grad_ops.push_back(CreateOperatorDef(
                           "Div",
                           "",
                           std::vector<string>{O(0), I(0)},
                           std::vector<string>{GI(0)}));
      grad_ops.push_back(CreateOperatorDef(
                           "Mul",
                           "",
                           std::vector<string>{GI(0), GO(0)},
                           std::vector<string>{GI(0)}));
      grad_ops.push_back(CreateOperatorDef(
                           "Mul",
                           "",
                           std::vector<string>{GI(0), I(1)},
                           std::vector<string>{GI(0)}));
      */
      // gradient operator for with respect to second input tensor
      // df/da =  w^a * ln w (all operations are component-wise)
      /*
      // reset GI(1) to zero
      Argument zero_arg;
      zero_arg.set_name("value");
      zero_arg.set_f(0);
      grad_ops.push_back(CreateOperatorDef(
          "ConstantFill",
          "",
          std::vector<string>{I(1)},
          std::vector<string>{GI(1)},
          std::vector<Argument>{zero_arg}));
      */
      grad_ops.push_back(CreateOperatorDef(
          "Log",
          "",
          std::vector<string>{I(0)},
          std::vector<string>{GI(1) + "_autogen_pre_red"}));
      grad_ops.push_back(CreateOperatorDef(
          "Mul",
          "",
          std::vector<string>{GI(1) + "_autogen_pre_red", O(0)},
          std::vector<string>{GI(1) + "_autogen_pre_red"}));
      if (bflag) {
        grad_ops.push_back(CreateOperatorDef(
            "Mul",
            "",
            std::vector<string>{GI(1) + "_autogen_pre_red", GO(0)},
            std::vector<string>{GI(1) + "_autogen_pre_red"}));
        grad_ops.push_back(CreateOperatorDef(
            "SumReduceLike",
            "",
            vector<string>{GI(1) + "_autogen_pre_red", I(1)},
            vector<string>{GI(1)},
            vector<Argument>{axis, axis_str, order}));
      } else {
        grad_ops.push_back(CreateOperatorDef(
            "Mul",
            "",
            std::vector<string>{GI(1) + "_autogen_pre_red", GO(0)},
            std::vector<string>{GI(1)}));
      }

      return grad_ops;
    }
  }

  // Argument `shape` is no longer needed in backprop.
  bool CopyArguments() const override {
    return false;
  }
};

REGISTER_GRADIENT(Pow, GetPowGradient);

} // namespace caffe2