//---------------------------------------------------------------------------//
// Copyright (c) 2014 Roshan <thisisroshansmail@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

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

#include <vector>

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/count_if.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/random/default_random_engine.hpp>
#include <boost/compute/random/discrete_distribution.hpp>
#include <boost/compute/lambda.hpp>

#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(discrete_distribution_doctest)
{
    using boost::compute::uint_;
    using boost::compute::lambda::_1;

    boost::compute::vector<uint_> vec(100, context);

    //! [generate]
    // initialize the default random engine
    boost::compute::default_random_engine engine(queue);

    // initialize weights
    int weights[] = {2, 2};

    // setup the discrete distribution to produce integers 0 and 1
    // with equal weights
    boost::compute::discrete_distribution<uint_> distribution(weights, weights + 2);

    // generate the random values and store them to 'vec'
    distribution.generate(vec.begin(), vec.end(), engine, queue);
    // ! [generate]

    BOOST_CHECK_EQUAL(
        boost::compute::count_if(
            vec.begin(), vec.end(), _1 > 1, queue),
        size_t(0));
}

BOOST_AUTO_TEST_CASE(discrete_distribution)
{
    using boost::compute::uint_;
    using boost::compute::lambda::_1;

    size_t size = 100;
    boost::compute::vector<uint_> vec(size, context);

    // initialize the default random engine
    boost::compute::default_random_engine engine(queue);

    // initialize weights
    int weights[] = {10, 40, 40, 10};

    // setup the discrete distribution
    boost::compute::discrete_distribution<uint_> distribution(
        weights, weights + 4);

    std::vector<double, mi_stl_allocator<double>> p = distribution.probabilities();
    BOOST_CHECK_CLOSE(p[0], double(0.1), 0.001);
    BOOST_CHECK_CLOSE(p[1], double(0.4), 0.001);
    BOOST_CHECK_CLOSE(p[2], double(0.4), 0.001);
    BOOST_CHECK_CLOSE(p[3], double(0.1), 0.001);

    BOOST_CHECK_EQUAL((distribution.min)(), uint_(0));
    BOOST_CHECK_EQUAL((distribution.max)(), uint_(3));

    // generate the random values and store them to 'vec'
    distribution.generate(vec.begin(), vec.end(), engine, queue);

    BOOST_CHECK_EQUAL(
        boost::compute::count_if(
            vec.begin(), vec.end(), _1 < 4, queue),
        size);
}

BOOST_AUTO_TEST_CASE(discrete_distribution_default_ctor)
{
    using boost::compute::uint_;
    using boost::compute::lambda::_1;

    size_t size = 100;
    boost::compute::vector<uint_> vec(size, context);

    // initialize the default random engine
    boost::compute::default_random_engine engine(queue);

    // call default constructor
    boost::compute::discrete_distribution<uint_> distribution;

    std::vector<double, mi_stl_allocator<double>> p = distribution.probabilities();
    BOOST_CHECK_CLOSE(p[0], double(1), 0.001);

    // generate the random values and store them to 'vec'
    distribution.generate(vec.begin(), vec.end(), engine, queue);

    BOOST_CHECK_EQUAL(
        boost::compute::count_if(
            vec.begin(), vec.end(), _1 == 0, queue),
        size);
}

BOOST_AUTO_TEST_CASE(discrete_distribution_one_weight)
{
    using boost::compute::uint_;
    using boost::compute::lambda::_1;

    size_t size = 100;
    boost::compute::vector<uint_> vec(size, context);

    // initialize the default random engine
    boost::compute::default_random_engine engine(queue);

    std::vector<int, mi_stl_allocator<int>> weights(1, 1);
    // call default constructor
    boost::compute::discrete_distribution<uint_> distribution(
        weights.begin(), weights.end());

    std::vector<double, mi_stl_allocator<double>> p = distribution.probabilities();
    BOOST_CHECK_CLOSE(p[0], double(1), 0.001);

    BOOST_CHECK_EQUAL((distribution.min)(), uint_(0));
    BOOST_CHECK_EQUAL((distribution.max)(), uint_(0));

    // generate the random values and store them to 'vec'
    distribution.generate(vec.begin(), vec.end(), engine, queue);

    BOOST_CHECK_EQUAL(
        boost::compute::count_if(
            vec.begin(), vec.end(), _1 == 0, queue),
        size);
}

BOOST_AUTO_TEST_CASE(discrete_distribution_empty_weights)
{
    using boost::compute::uint_;
    using boost::compute::lambda::_1;

    size_t size = 100;
    boost::compute::vector<uint_> vec(size, context);

    // initialize the default random engine
    boost::compute::default_random_engine engine(queue);

    std::vector<int, mi_stl_allocator<int>> weights;
    // weights.begin() == weights.end()
    boost::compute::discrete_distribution<uint_> distribution(
        weights.begin(), weights.end());

    std::vector<double, mi_stl_allocator<double>> p = distribution.probabilities();
    BOOST_CHECK_CLOSE(p[0], double(1), 0.001);

    BOOST_CHECK_EQUAL((distribution.min)(), uint_(0));
    BOOST_CHECK_EQUAL((distribution.max)(), uint_(0));

    // generate the random values and store them to 'vec'
    distribution.generate(vec.begin(), vec.end(), engine, queue);

    BOOST_CHECK_EQUAL(
        boost::compute::count_if(
            vec.begin(), vec.end(), _1 == 0, queue),
        size);
}

BOOST_AUTO_TEST_CASE(discrete_distribution_uchar)
{
    using boost::compute::uchar_;
    using boost::compute::uint_;
    using boost::compute::lambda::_1;

    size_t size = 100;
    boost::compute::vector<uchar_> uchar_vec(size, context);
    boost::compute::vector<uint_> uint_vec(size, context);

    // initialize the default random engine
    boost::compute::default_random_engine engine(queue);

    // initialize weights
    std::vector<int, mi_stl_allocator<int>> weights(258, 0);
    weights[257] = 1;

    // setup the discrete distribution
    boost::compute::discrete_distribution<uchar_> distribution(
        weights.begin(), weights.end());

    BOOST_CHECK_EQUAL((distribution.min)(), uchar_(0));
    BOOST_CHECK_EQUAL((distribution.max)(), uchar_(255));

    // generate the random uchar_ values to the uchar_ vector
    distribution.generate(uchar_vec.begin(), uchar_vec.end(), engine, queue);

    BOOST_CHECK_EQUAL(
        boost::compute::count_if(
            uchar_vec.begin(), uchar_vec.end(), _1 == uchar_(1), queue),
        size);

    // generate the random uchar_ values to the uint_ vector
    distribution.generate(uint_vec.begin(), uint_vec.end(), engine, queue);

    BOOST_CHECK_EQUAL(
        boost::compute::count_if(
            uint_vec.begin(), uint_vec.end(), _1 == uint_(1), queue),
        size);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <cstdint>
#include <functional>
#include <limits>
#include <stdexcept>
#include <vector>

#include <Eigen/Geometry>

namespace common_robotics_utilities
{
namespace simple_dtw
{
template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
class SimpleDTW
{
protected:
  void InitializeMatrix(const ssize_t first_sequence_size,
                        const ssize_t second_sequence_size)
  {
    const ssize_t rows = first_sequence_size + 1;
    const ssize_t cols = second_sequence_size + 1;
    if (dtw_matrix_.rows() < rows || dtw_matrix_.cols() < cols)
    {
      dtw_matrix_ = Eigen::MatrixXd::Zero(rows, cols);
      if (rows > 1 && cols > 1)
      {
        for (ssize_t row = 1; row < rows; row++)
        {
          dtw_matrix_(row, 0) = std::numeric_limits<double>::infinity();
        }
        for (ssize_t col = 1; col < cols; col++)
        {
          dtw_matrix_(0, col) = std::numeric_limits<double>::infinity();
        }
      }
    }
  }

  Eigen::MatrixXd dtw_matrix_;

public:
  SimpleDTW()
  {
    InitializeMatrix(0, 0);
  }

  SimpleDTW(const ssize_t first_sequence_size,
            const ssize_t second_sequence_size)
  {
    InitializeMatrix(first_sequence_size, second_sequence_size);
  }

  double EvaluateWarpingCost(
      const FirstContainer& first_sequence,
      const SecondContainer& second_sequence,
      const std::function<double(const FirstDatatype&,
                                 const SecondDatatype&)>& distance_fn)
  {
    if (first_sequence.empty())
    {
      throw std::invalid_argument("first_sequence is empty");
    }
    if (second_sequence.empty())
    {
      throw std::invalid_argument("second_sequence is empty");
    }
    const ssize_t first_sequence_size
        = static_cast<ssize_t>(first_sequence.size());
    const ssize_t second_sequence_size
        = static_cast<ssize_t>(second_sequence.size());
    InitializeMatrix(first_sequence_size, second_sequence_size);
    //Compute DTW cost for the two sequences
    for (ssize_t i = 1; i <= first_sequence_size; i++)
    {
      const FirstDatatype& first_item
          = first_sequence[static_cast<size_t>(i) - 1];
      for (ssize_t j = 1; j <= second_sequence_size; j++)
      {
        const SecondDatatype& second_item
            = second_sequence[static_cast<size_t>(j) - 1];
        const double index_cost = distance_fn(first_item, second_item);
        double prev_cost = 0.0;
        // Get the next neighboring values from the matrix to use for the update
        double im1j = dtw_matrix_(i - 1, j);
        double im1jm1 = dtw_matrix_(i - 1, j - 1);
        double ijm1 = dtw_matrix_(i, j - 1);
        // Start the update step
        if (im1j < im1jm1 && im1j < ijm1)
        {
          prev_cost = im1j;
        }
        else if (ijm1 < im1j && ijm1 < im1jm1)
        {
          prev_cost = ijm1;
        }
        else
        {
          prev_cost = im1jm1;
        }
        // Update the value in the matrix
        const double new_cost = index_cost + prev_cost;
        dtw_matrix_(i, j) = new_cost;
      }
    }
    //Return total path cost
    const double warping_cost
        = dtw_matrix_(first_sequence_size, second_sequence_size);
    return warping_cost;
  }
};

template<typename FirstDatatype, typename SecondDatatype,
         typename FirstContainer=std::vector<FirstDatatype>,
         typename SecondContainer=std::vector<SecondDatatype>>
inline double EvaluateWarpingCost(
    const FirstContainer& first_sequence,
    const SecondContainer& second_sequence,
    const std::function<double(const FirstDatatype&,
                               const SecondDatatype&)>& distance_fn)
{
  SimpleDTW<FirstDatatype, SecondDatatype,
            FirstContainer, SecondContainer> dtw_evaluator;
  return dtw_evaluator.EvaluateWarpingCost(
      first_sequence, second_sequence, distance_fn);
}
}  // namespace simple_dtw
}  // namespace common_robotics_utilities


#include "neuralnet/magnitude.h"
#include "streams.h"

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

// -----------------------------------------------------------------------------
// Magnitude
// -----------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(Magnitude)

BOOST_AUTO_TEST_CASE(it_initializes_to_zero_magnitude)
{
    const NN::Magnitude mag = NN::Magnitude::Zero();

    BOOST_CHECK_EQUAL(mag.Scaled(), 0);
}

BOOST_AUTO_TEST_CASE(it_initializes_to_zero_when_rounding_invalid_magnitudes)
{
    // Negative
    const NN::Magnitude negative = NN::Magnitude::RoundFrom(-1.234);
    BOOST_CHECK_EQUAL(negative.Scaled(), 0);

    // Rounds-down to zero
    const NN::Magnitude effectivly_zero = NN::Magnitude::RoundFrom(0.005);
    BOOST_CHECK_EQUAL(effectivly_zero.Scaled(), 0);

    // Exceeded maximum
    const NN::Magnitude overflow = NN::Magnitude::RoundFrom(32767.123);
    BOOST_CHECK_EQUAL(overflow.Scaled(), 0);
}

BOOST_AUTO_TEST_CASE(it_rounds_a_small_magnitude_to_two_places)
{
    const NN::Magnitude min = NN::Magnitude::RoundFrom(0.0051);
    BOOST_CHECK_EQUAL(min.Scaled(), 1);

    const NN::Magnitude max = NN::Magnitude::RoundFrom(0.994);
    BOOST_CHECK_EQUAL(max.Scaled(), 99);
}

BOOST_AUTO_TEST_CASE(it_rounds_a_medium_magnitude_to_one_place)
{
    const NN::Magnitude min = NN::Magnitude::RoundFrom(0.995);
    BOOST_CHECK_EQUAL(min.Scaled(), 100);

    const NN::Magnitude max = NN::Magnitude::RoundFrom(9.94);
    BOOST_CHECK_EQUAL(max.Scaled(), 990);
}

BOOST_AUTO_TEST_CASE(it_rounds_a_large_magnitude_to_a_whole)
{
    const NN::Magnitude min = NN::Magnitude::RoundFrom(9.95);
    BOOST_CHECK_EQUAL(min.Scaled(), 1000);

    const NN::Magnitude max = NN::Magnitude::RoundFrom(32767.0);
    BOOST_CHECK_EQUAL(max.Scaled(), 3276700);
}

BOOST_AUTO_TEST_CASE(it_compares_another_magnitude_for_equality)
{
    const NN::Magnitude magnitude1 = NN::Magnitude::RoundFrom(1.23);
    const NN::Magnitude magnitude2 = NN::Magnitude::RoundFrom(1.23);

    BOOST_CHECK(magnitude1 == magnitude2);
    BOOST_CHECK(magnitude1 != NN::Magnitude::Zero());
}

BOOST_AUTO_TEST_CASE(it_compares_an_integer_for_equality)
{
    const NN::Magnitude magnitude = NN::Magnitude::RoundFrom(123);

    BOOST_CHECK(magnitude == 123);
    BOOST_CHECK(magnitude != 999);
}

BOOST_AUTO_TEST_CASE(it_compares_a_floating_point_number_for_equality)
{
    const NN::Magnitude magnitude = NN::Magnitude::RoundFrom(1.23);

    // Floating-point comparisons round the other value according to the size
    // for the precision tiers before checking equality:
    BOOST_CHECK(magnitude == 1.23);
    BOOST_CHECK(magnitude == 1.2);
    BOOST_CHECK(magnitude == 1.25);
    BOOST_CHECK(magnitude != 1.26);
}

BOOST_AUTO_TEST_CASE(it_reports_its_size_category)
{
    using Kind = NN::Magnitude::Kind;

    const NN::Magnitude zero = NN::Magnitude::Zero();
    BOOST_CHECK(zero.Which() == Kind::ZERO);

    const NN::Magnitude effectively_zero = NN::Magnitude::RoundFrom(0.005);
    BOOST_CHECK(effectively_zero.Which() == Kind::ZERO);

    const NN::Magnitude min_small = NN::Magnitude::RoundFrom(0.0051);
    BOOST_CHECK(min_small.Which() == Kind::SMALL);

    const NN::Magnitude max_small = NN::Magnitude::RoundFrom(0.994);
    BOOST_CHECK(max_small.Which() == Kind::SMALL);

    const NN::Magnitude min_medium = NN::Magnitude::RoundFrom(0.995);
    BOOST_CHECK(min_medium.Which() == Kind::MEDIUM);

    const NN::Magnitude max_medium = NN::Magnitude::RoundFrom(9.94);
    BOOST_CHECK(max_medium.Which() == Kind::MEDIUM);

    const NN::Magnitude min_large = NN::Magnitude::RoundFrom(9.95);
    BOOST_CHECK(min_large.Which() == Kind::LARGE);

    const NN::Magnitude max_large = NN::Magnitude::RoundFrom(32767.0);
    BOOST_CHECK(max_large.Which() == Kind::LARGE);
}

BOOST_AUTO_TEST_CASE(it_presents_the_scaled_magnitude_representation)
{
    const NN::Magnitude small = NN::Magnitude::RoundFrom(0.11);
    BOOST_CHECK_EQUAL(small.Scaled(), 11);

    const NN::Magnitude medium = NN::Magnitude::RoundFrom(1.1);
    BOOST_CHECK_EQUAL(medium.Scaled(), 110);

    const NN::Magnitude large = NN::Magnitude::RoundFrom(11.0);
    BOOST_CHECK_EQUAL(large.Scaled(), 1100);
}

BOOST_AUTO_TEST_CASE(it_presents_the_compact_magnitude_representation)
{
    const NN::Magnitude small = NN::Magnitude::RoundFrom(0.11);
    BOOST_CHECK_EQUAL(small.Compact(), 11);

    const NN::Magnitude medium = NN::Magnitude::RoundFrom(1.1);
    BOOST_CHECK_EQUAL(medium.Compact(), 11);

    const NN::Magnitude large = NN::Magnitude::RoundFrom(11.0);
    BOOST_CHECK_EQUAL(large.Compact(), 11);
}

BOOST_AUTO_TEST_CASE(it_presents_the_floatng_point_magnitude_representation)
{
    const NN::Magnitude small = NN::Magnitude::RoundFrom(0.11);
    BOOST_CHECK_EQUAL(small.Floating(), 0.11);

    const NN::Magnitude medium = NN::Magnitude::RoundFrom(1.1);
    BOOST_CHECK_EQUAL(medium.Floating(), 1.1);

    const NN::Magnitude large = NN::Magnitude::RoundFrom(11.0);
    BOOST_CHECK_EQUAL(large.Floating(), 11.0);
}

BOOST_AUTO_TEST_CASE(it_represents_itself_as_a_string)
{
    const NN::Magnitude zero = NN::Magnitude::Zero();
    BOOST_CHECK_EQUAL(zero.ToString(), "0");

    const NN::Magnitude small = NN::Magnitude::RoundFrom(0.11);
    BOOST_CHECK_EQUAL(small.ToString(), "0.11");

    const NN::Magnitude medium = NN::Magnitude::RoundFrom(1.1);
    BOOST_CHECK_EQUAL(medium.ToString(), "1.1");

    const NN::Magnitude large = NN::Magnitude::RoundFrom(11.0);
    BOOST_CHECK_EQUAL(large.ToString(), "11");

    const NN::Magnitude max = NN::Magnitude::RoundFrom(32767.0);
    BOOST_CHECK_EQUAL(max.ToString(), "32767");
}

BOOST_AUTO_TEST_SUITE_END()


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

#include <boost/graph/max_cardinality_matching.hpp>
#include <boost/graph/maximum_weighted_matching.hpp>

#include <iostream>
#include <fstream>
#include <sstream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/core/lightweight_test.hpp>

using namespace boost;

typedef property< edge_weight_t, float, property< edge_index_t, int > >
    EdgeProperty;

typedef adjacency_list< vecS, vecS, undirectedS,
    property< vertex_index_t, int >, EdgeProperty >
    undirected_graph;
typedef adjacency_list< listS, listS, undirectedS,
    property< vertex_index_t, int >, EdgeProperty >
    undirected_list_graph;
typedef adjacency_matrix< undirectedS, property< vertex_index_t, int >,
    EdgeProperty >
    undirected_adjacency_matrix_graph;

template < typename Graph > struct vertex_index_installer
{
    static void install(Graph&) {}
};

template <> struct vertex_index_installer< undirected_list_graph >
{
    static void install(undirected_list_graph& g)
    {
        typedef graph_traits< undirected_list_graph >::vertex_iterator
            vertex_iterator_t;
        typedef graph_traits< undirected_list_graph >::vertices_size_type
            v_size_t;

        vertex_iterator_t vi, vi_end;
        v_size_t i = 0;
        for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi, ++i)
            put(vertex_index, g, *vi, i);
    }
};

template < typename Graph > void print_graph(const Graph& g)
{
    typedef typename graph_traits< Graph >::edge_iterator edge_iterator_t;
    edge_iterator_t ei, ei_end;
    std::cout << std::endl << "The graph is: " << std::endl;
    for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
        std::cout << "add_edge(" << source(*ei, g) << ", " << target(*ei, g)
                  << ", EdgeProperty(" << get(edge_weight, g, *ei) << "), );"
                  << std::endl;
}

template < typename Graph >
void weighted_matching_test(const Graph& g,
    typename property_traits< typename property_map< Graph,
        edge_weight_t >::type >::value_type answer)
{
    typedef
        typename property_map< Graph, vertex_index_t >::type vertex_index_map_t;
    typedef vector_property_map<
        typename graph_traits< Graph >::vertex_descriptor, vertex_index_map_t >
        mate_t;
    mate_t mate(num_vertices(g));
    maximum_weighted_matching(g, mate);
    bool same_result = (matching_weight_sum(g, mate) == answer);
    BOOST_TEST(same_result);
    if (!same_result)
    {
        mate_t max_mate(num_vertices(g));
        brute_force_maximum_weighted_matching(g, max_mate);

        std::cout << std::endl
                  << "Found a weighted matching of weight sum "
                  << matching_weight_sum(g, mate) << std::endl
                  << "While brute-force search found a weighted matching of "
                     "weight sum "
                  << matching_weight_sum(g, max_mate) << std::endl;

        typedef
            typename graph_traits< Graph >::vertex_iterator vertex_iterator_t;
        vertex_iterator_t vi, vi_end;

        print_graph(g);

        std::cout << std::endl << "The algorithmic matching is:" << std::endl;
        for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
            if (mate[*vi] != graph_traits< Graph >::null_vertex()
                && *vi < mate[*vi])
                std::cout << "{" << *vi << ", " << mate[*vi] << "}"
                          << std::endl;

        std::cout << std::endl << "The brute-force matching is:" << std::endl;
        for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
            if (max_mate[*vi] != graph_traits< Graph >::null_vertex()
                && *vi < max_mate[*vi])
                std::cout << "{" << *vi << ", " << max_mate[*vi] << "}"
                          << std::endl;

        std::cout << std::endl;
    }
}

template < typename Graph >
Graph make_graph(typename graph_traits< Graph >::vertices_size_type num_v,
    typename graph_traits< Graph >::edges_size_type num_e,
    std::deque< std::size_t > input_edges)
{
    Graph g(num_v);
    vertex_index_installer< Graph >::install(g);
    for (std::size_t i = 0; i < num_e; ++i)
    {
        std::size_t src_v, tgt_v, edge_weight;
        src_v = input_edges.front();
        input_edges.pop_front();
        tgt_v = input_edges.front();
        input_edges.pop_front();
        edge_weight = input_edges.front();
        input_edges.pop_front();
        add_edge(
            vertex(src_v, g), vertex(tgt_v, g), EdgeProperty(edge_weight), g);
    }
    return g;
}

int main(int, char*[])
{
    std::ifstream in_file("weighted_matching.dat");
    std::string line;
    while (std::getline(in_file, line))
    {
        std::istringstream in_graph(line);
        std::size_t answer, num_v, num_e;
        in_graph >> answer >> num_v >> num_e;

        std::deque< std::size_t > input_edges;
        std::size_t i;
        while (in_graph >> i)
            input_edges.push_back(i);

        weighted_matching_test(
            make_graph< undirected_graph >(num_v, num_e, input_edges), answer);
        weighted_matching_test(
            make_graph< undirected_list_graph >(num_v, num_e, input_edges),
            answer);
        weighted_matching_test(make_graph< undirected_adjacency_matrix_graph >(
                                   num_v, num_e, input_edges),
            answer);
    }
    return boost::report_errors();
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/big/big_types.h>

#include <OpenTissue/core/math/optimization/optimization_compute_index_reordering.h>
#include <OpenTissue/core/math/optimization/optimization_partition_vector.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_math_big_partition_vector);

BOOST_AUTO_TEST_CASE(test_case)
{
  typedef ublas::vector<size_t>   idx_vector_type;

  idx_vector_type bitmask;

  bitmask.resize(10,false);

  bitmask(2) = OpenTissue::math::optimization::IN_ACTIVE;
  bitmask(4) = OpenTissue::math::optimization::IN_ACTIVE;
  bitmask(6) = OpenTissue::math::optimization::IN_ACTIVE;
  bitmask(9) = OpenTissue::math::optimization::IN_ACTIVE;
  bitmask(1) = OpenTissue::math::optimization::IN_LOWER;
  bitmask(3) = OpenTissue::math::optimization::IN_LOWER;
  bitmask(8) = OpenTissue::math::optimization::IN_LOWER;
  bitmask(0) = OpenTissue::math::optimization::IN_UPPER;
  bitmask(5) = OpenTissue::math::optimization::IN_UPPER;
  bitmask(7) = OpenTissue::math::optimization::IN_UPPER;

  idx_vector_type old2new;
  idx_vector_type new2old;

  OpenTissue::math::optimization::compute_index_reordering( bitmask, old2new, new2old );

  ublas::vector<double> rhs;
  rhs.resize(10,false);

  rhs(0) = 1.0;
  rhs(1) = 2.0;
  rhs(2) = 3.0;
  rhs(3) = 4.0;
  rhs(4) = 5.0;
  rhs(5) = 6.0;
  rhs(6) = 7.0;
  rhs(7) = 8.0;
  rhs(8) = 9.0;
  rhs(9) = 10.0;

  ublas::vector<double> rhs_a;
  ublas::vector<double> rhs_b;
  OpenTissue::math::optimization::partition_vector( rhs, bitmask, old2new, 4, 6, rhs_a, rhs_b );

  BOOST_CHECK( rhs_a.size() == 4);
  BOOST_CHECK( rhs_b.size() == 6);

  double tol =0.01;
  BOOST_CHECK_CLOSE( double( rhs_a( 0 ) ), double( rhs( new2old(0) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 1 ) ), double( rhs( new2old(1) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 2 ) ), double( rhs( new2old(2) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 3 ) ), double( rhs( new2old(3) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 0 ) ), double( rhs( new2old(0+4) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 1 ) ), double( rhs( new2old(1+4) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 2 ) ), double( rhs( new2old(2+4) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 3 ) ), double( rhs( new2old(3+4) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 4 ) ), double( rhs( new2old(4+4) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 5 ) ), double( rhs( new2old(5+4) ) ), tol );
}

BOOST_AUTO_TEST_SUITE_END();


#include "points_untangler/points_untangler.h"

#include <string>
#include <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Dense>
#include <igl/list_to_matrix.h>
#include <igl/write_triangle_mesh.h>

bool load(const std::string & path, Eigen::MatrixXd &res)
{
	std::fstream file;
	file.open(path.c_str());

	if (!file.good())
	{
		std::cerr << "Failed to open file : " << path << std::endl;
		file.close();
		return false;
	}

	std::string s;
	std::vector<std::vector<double>> matrix;

	while (getline(file, s))
	{
		std::stringstream input(s);
		double temp;
		matrix.emplace_back();

		std::vector<double> &currentLine = matrix.back();

		while (input >> temp)
			currentLine.push_back(temp);
	}

	if (!igl::list_to_matrix(matrix, res))
	{
		std::cerr << "list to matrix error" << std::endl;
		file.close();
		return false;
	}
	return true;
}

int main(int argc, char** argv) {
	Eigen::MatrixXd pts;
	Eigen::MatrixXi F;
	Eigen::MatrixXd newPts;

	const std::string root = DATA_DIR;

	load("/Users/teseo/GDrive/Cellogram/Images/img12/vDetected.xyz", pts);
	if(pts.cols() == 2){
		pts.conservativeResize(pts.rows(), 3);
		pts.col(2).setZero();
	}

	std::vector<int> dropped;

	cellogram::PointsUntangler::pointsUntangler(pts, F, dropped, newPts);

	Eigen::MatrixXd total(pts.rows()+newPts.rows(), pts.cols());
	// total.setZero();

	total.block(0, 0, pts.rows(), pts.cols()) = pts;
	total.block(pts.rows(), 0, newPts.rows(), newPts.cols()) = newPts;

	igl::write_triangle_mesh("test.obj", total, F);

	return 0;
}


#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <random>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <Eigen/LU>
#include <time.h>

// this is an example in how to use a class in one executable using full namespaces

// file that defines the class you want to use
#include "calibration/Calibration.h"

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

std::ofstream myfile;
myfile.open ("example.txt");
myfile << "iPos\ti_den\ti_times\t\terrorR\t\t\terrorT \n";

std::vector<double> comp_time_vec, vect1, vect2;
//int i_time = 0;

for (int i_5times = 1; i_5times <= 5; i_5times++) {

int i_pos = 1280;

	std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> dis(-1, 1);

	// AX = ZB
	Eigen::Matrix4f Z = Eigen::Matrix4f::Identity();
	Eigen::Vector4f qz_use = Eigen::Vector4f(dis(gen), dis(gen), dis(gen), dis(gen) );
	Eigen::Quaternionf qz( qz_use[0], qz_use[1], qz_use[2], qz_use[3] );
	qz.normalize();
	Eigen::Vector3f tz = Eigen::Vector3f( dis(gen), dis(gen), dis(gen) );
	Z.block<3,3>(0,0) = qz.toRotationMatrix();
	Z.block<3,1>(0,3) = tz;
	//std::cout << "Z: " << std::endl << Z << std::endl;

 	Eigen::Matrix4f X = Eigen::Matrix4f::Identity();;
	Eigen::Vector4f qx_use = Eigen::Vector4f(dis(gen), dis(gen), dis(gen), dis(gen) );
	Eigen::Quaternionf qx( qx_use[0], qx_use[1], qx_use[2], qx_use[3] );
	qx.normalize();
	Eigen::Vector3f tx = Eigen::Vector3f(dis(gen),dis(gen), dis(gen));
	X.block<3,3>(0,0) = qx.toRotationMatrix();
	X.block<3,1>(0,3) = tx;
	//std::cout << "X: " << std::endl << X << std::endl;

 	int NPoses = i_pos;

	std::vector<Matrix4f> B, A;

	Eigen::Matrix4f a = Eigen::Matrix4f::Identity();

	Eigen::Vector4f q_use = Eigen::Vector4f::Random();
	Eigen::Quaternionf q( q_use[0], q_use[1], q_use[2], q_use[3]);
	q.normalize();

	Eigen::Vector3f t = Eigen::Vector3f( dis(gen), dis(gen), dis(gen) );

	a.block<3,3>(0,0) = q.toRotationMatrix();
	a.block<3,1>(0,3) = t;

	int den_test = 1024; // grid resolution

	for (int i = 0; i < NPoses; i++)
	{

		// test data
		Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
		b = Z.inverse()*a*X;

		Eigen::Matrix4f d = Eigen::Matrix4f::Identity();

		Eigen::Vector3f t = 0.1*(1/den_test)*Eigen::Vector3f( dis(gen),dis(gen), dis(gen) );

		float roll = dis(gen)*M_PI/den_test;
		float pitch = dis(gen)*M_PI/den_test;
		float yaw = dis(gen)*M_PI/den_test;
		Eigen::Matrix3f R;
		R = AngleAxisf(roll, Vector3f::UnitX())
			*AngleAxisf(pitch, Vector3f::UnitY())
  			*AngleAxisf(yaw, Vector3f::UnitZ());
		d.block<3,3>(0,0) = R;
		d.block<3,1>(0,3) = t;

		a = d*a;

		A.push_back(a);
		B.push_back(b);

	}

	//i_den it's the sampling fraction: pick one champion every i_den
	for (int i_den = 1; i_den <= 32; i_den *= 2) {

	std::cout << std::endl << "i_pos: " << i_pos << ", " << "i_den: " << i_den << ", " << std::endl << "i_5times: " << i_5times << std::endl;

	std::vector<Matrix4f> Asampled, Bsampled; //A, B sampled
	Asampled.clear();
	Bsampled.clear();

	//fulfilling matrices A,B with choosen champions, limit per matrix sampled is NPoses (total available) over the sampling fraction i_den

	for (int i_sam = 0; i_sam < NPoses/i_den; i_sam +=i_den){
		Asampled.push_back(A[i_sam]);
		Bsampled.push_back(B[i_sam]);
	}

	// first, create an object of your class
	calibration::Calibration mycalib;

	mycalib.setInput(Asampled, Bsampled);

	clock_t t1,t2;
    t1=clock();
    vect1.push_back((float(t1)*1000)/CLOCKS_PER_SEC);

	mycalib.computeClosedForm();

	t2=clock();
	vect2.push_back((float(t2)*1000)/CLOCKS_PER_SEC);
    float diff ((float)t2-(float)t1);
    comp_time_vec.push_back( (diff*10000) / CLOCKS_PER_SEC );
    //i_time++;
	//sleep(2);

	mycalib.computeNonLinOpt();

	Eigen::Matrix4f Tx, Tz;
	float Er, Et;
	Tx = Eigen::Matrix4f::Identity();
	Tz = Eigen::Matrix4f::Identity();
	mycalib.getFullOutput(Tz, Tx, Er, Et);

	myfile << i_pos << "\t\t" << i_den << "\t\t" << i_5times << "\t\t" << Er << "\t\t" <<  Et << "\t\t" << "\n";

	// print out the result

	std::cout << "combo: " << i_pos << " / " << i_den << " / " << i_5times << std::endl;

	std::cout << "Input Z: " << std::endl << Z << std::endl;
	std::cout << "Estimated Z:" << std::endl << Tz << std::endl;
	std::cout << "Error over Z estimation: " << std::endl << Z-Tz << std::endl << std::endl;

	std::cout << "Input X: " << std::endl << X << std::endl;
	std::cout << "Estimated X:" << std::endl << Tx << std::endl;
	std::cout << "Error over X estimation: " << std::endl << X-Tx << std::endl;

// compute Rotation and translation errors over Non Linear results

	/*Eigen::Matrix3f RxOK_NL, RzOK_NL;
	Eigen::Vector3f txOK_NL, tzOK_NL;

	RxOK_NL = Tx.block<3,3>(0,0);
	RzOK_NL = Tz.block<3,3>(0,0);
	txOK_NL = Tx.block<3,1>(0,3);
	tzOK_NL = Tz.block<3,1>(0,3);

	float Er_NL = 0;

	float EtNum_NL = 0, EtDen_NL = 1;

	for (int i = 0; i < NPoses; i++)
	{

		Er_NL += Matrix3f( A[i].block<3,3>(0,0)*RxOK_NL - RzOK_NL*B[i].block<3,3>(0,0) ).squaredNorm();

		EtDen_NL += Vector3f( A[i].block<3,3>(0,0)*txOK_NL - A[i].block<3,1>(0,3) ).squaredNorm();

		EtNum_NL += Vector3f( A[i].block<3,3>(0,0)*txOK_NL + A[i].block<3,1>(0,3) - RzOK_NL*B[i].block<3,1>(0,3) - tzOK_NL ).squaredNorm();

	}

	float Et_NL = sqrt(EtNum_NL/EtDen_NL);

	std::cout << "Rotation error on Non Linear form is " << Er_NL << std::endl;
	std::cout << "Translation error on Non Linear form is " << Et_NL << std::endl;
	std::cout << "combo " << NPoses << "/ " << den_test << "/ " << i_5times << std::endl;*/

}
}
// chiusura dei tre for
std::cout << "closed form time computation in microseconds :"<< std::endl;
  for (unsigned i=0; i<comp_time_vec.size(); i++)
    std::cout << "i " << i<< " diff " << comp_time_vec[i] << " t1 " << vect1[i] <<" t2 " << vect2[i] << std::endl;
  std::cout << '\n';
//std::cout<< "closed form time computation in milliseconds " << std::endl << comp_time_vec << std::endl;

myfile.close();

return 0;

}


/* Boost libs/numeric/odeint/performance/openmp/osc_chain_1d.cpp

 Copyright 2013 Karsten Ahnert
 Copyright 2013 Mario Mulansky
 Copyright 2013 Pascal Germroth

 stronlgy nonlinear hamiltonian lattice in 2d

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

#include <iostream>
#include <vector>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/mpi/mpi.hpp>

#include <boost/program_options.hpp>
#include <boost/random.hpp>
#include <boost/timer/timer.hpp>
#include <boost/foreach.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include "osc_chain_1d_system.hpp"

using namespace std;
using namespace boost::numeric::odeint;
using namespace boost::accumulators;
using namespace boost::program_options;

using boost::timer::cpu_timer;

const double p_kappa = 3.3;
const double p_lambda = 4.7;

int main( int argc , char* argv[] )
{
    boost::mpi::environment env(argc, argv);
    boost::mpi::communicator world;

    size_t N, steps, repeat;
    bool dump;
    options_description desc("Options");
    desc.add_options()
        ("help,h", "show this help")
        ("length", value(&N)->default_value(1024), "length of chain")
        ("steps", value(&steps)->default_value(100), "simulation steps")
        ("repeat", value(&repeat)->default_value(25), "repeat runs")
        ("dump", bool_switch(&dump), "dump final state to stderr (on node 0)")
        ;
    variables_map vm;
    store(command_line_parser(argc, argv).options(desc).run(), vm);
    notify(vm);
    if(vm.count("help"))
    {
        if(world.rank() == 0)
            cerr << desc << endl;
        return EXIT_FAILURE;
    }
    cout << "length\tsteps\tthreads\ttime" << endl;

    accumulator_set< double, stats<tag::mean, tag::median> > acc_time;

    vector<double> p( N ), q( N, 0 );
    if(world.rank() == 0) {
        boost::random::uniform_real_distribution<double> distribution;
        boost::random::mt19937 engine( 0 );
        generate( p.begin() , p.end() , boost::bind( distribution , engine ) );
    }

    typedef vector<double> inner_state_type;
    typedef mpi_state< inner_state_type > state_type;
    typedef symplectic_rkn_sb3a_mclachlan<
              state_type , state_type , double
            > stepper_type;
    state_type p_split( world ), q_split( world );
    split(p, p_split);
    split(q, q_split);

    for(size_t n_run = 0 ; n_run != repeat ; n_run++) {
        cpu_timer timer;
        world.barrier();
        integrate_n_steps( stepper_type() , osc_chain( p_kappa , p_lambda ) ,
                           make_pair( boost::ref(q_split) , boost::ref(p_split) ) ,
                           0.0 , 0.01 , steps );
        world.barrier();
        if(world.rank() == 0) {
            double run_time = static_cast<double>(timer.elapsed().wall) * 1.0e-9;
            acc_time(run_time);
            cout << N << '\t' << steps << '\t' << world.size() << '\t' << run_time << endl;
        }
    }

    if(dump) {
        unsplit(p_split, p);
        if(world.rank() == 0) {
            copy(p.begin(), p.end(), ostream_iterator<double>(cerr, "\t"));
            cerr << endl;
        }
    }

    if(world.rank() == 0)
        cout << "# mean=" << mean(acc_time)
             << " median=" << median(acc_time) << endl;

    return 0;
}


#include <iostream>
#include <autodiff/forward/real.hpp>
#include <autodiff/forward/real/eigen.hpp>
#include <Eigen/Dense>
#include <Eigen/Core>

#include "rotation.hpp"

// using namespace autodiff;
using namespace Eigen;

class Object {
    public:
        Object();
        virtual autodiff::real U(const autodiff::ArrayXreal& pos);
        VectorXd DLT1(const autodiff::ArrayXreal& q1, const autodiff::ArrayXreal& q2, double h);
    private:

    protected:
        double m = 1;  // mass (kg)
        double g = 1;  // gravity accel (m/s^2)
        MatrixXd J;  // (3x3) Inertia Tensor (kg*m^2)

};


#pragma once

#include <skynet/config.hpp>
#include <skynet/ublas.hpp>
#include <skynet/utility/tag.hpp>

#include <Eigen/Core>

namespace skynet{namespace statistics{

	template <typename M>
	class metric;

	template <>
	class metric<Mahalanobis> : public unary_function<ublas::vector<double>, double>{
	public:
		typedef ublas::matrix<double>					matrix_type;
		typedef ublas::vector<double>					vector_type;

		metric(){}

		metric(const metric &rhs)
			: _mean(rhs._mean), _covariance(rhs._covariance), _inv_cov(rhs._inv_cov){}

		void attach(matrix_type  feature_matrix){
			_mean = ublas::zero_vector<double>(feature_matrix.size1());
			for (int i = 0; i < feature_matrix.size2(); ++i){
				_mean += ublas::column(feature_matrix, i);
			}

			_mean /= feature_matrix.size2();

			for (int i = 0; i < feature_matrix.size2(); ++i){
				ublas::column(feature_matrix, i) -= _mean;
			}

			_covariance = ublas::prod(feature_matrix, ublas::trans(feature_matrix));
			_covariance /= feature_matrix.size2();
			_inv_cov =
		}

		result_type	 operator()(const argument_type &x) const{
			auto x_sub_mean = x - _mean;
			auto dis = ublas::trans(x_sub_mean) * _inv_cov * x_sub_mean;
			return dis;
		}

		void mean(const vector_type &mean)		{ _mean = mean; }
		vector_type mean() const				{ return _mean; }

		template <typename Archive>
		void serialize(Archive &ar, const unsigned int &){
			ar & boost::serialization::make_nvp("mean_feature", _mean);
			ar & boost::serialization::make_nvp("covariance", _covariance);
			ar & boost::serialization::make_nvp("inv_covariance", _inv_cov);
		}

	private:
		vector_type						_mean;
		matrix_type						_covariance;
		matrix_type						_inv_cov;
	};

}}


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2012 Scientific Computing and Imaging Institute,
   University of Utah.

   License for the specific language governing rights and limitations under
   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Algorithms/Math/SolveLinearSystemWithEigen.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Eigen/Sparse>

using namespace SCIRun::Core::Algorithms::Math;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core;

namespace
{
  class SolveLinearSystemAlgorithmEigenCGImpl
  {
  public:
    SolveLinearSystemAlgorithmEigenCGImpl(const DenseColumnMatrix& rhs, double tolerance, int maxIterations) :
        rhs_(rhs), tolerance_(tolerance), maxIterations_(maxIterations) {}

    template <class MatrixType>
    DenseColumnMatrix::EigenBase solveWithEigen(const MatrixType& lhs)
    {
      Eigen::ConjugateGradient<typename MatrixType::EigenBase> cg;
      cg.compute(lhs);

      if (cg.info() != Eigen::Success)
        BOOST_THROW_EXCEPTION(AlgorithmInputException()
          << LinearAlgebraErrorMessage("Conjugate gradient initialization was unsuccessful")
          << EigenComputationInfo(cg.info()));

      cg.setTolerance(tolerance_);
      cg.setMaxIterations(maxIterations_);
      auto solution = cg.solve(rhs_).eval();
      tolerance_ = cg.error();
      maxIterations_ = cg.iterations();
      return solution;
    }

    double tolerance_;
    int maxIterations_;
  private:
    const DenseColumnMatrix& rhs_;
  };
}

SolveLinearSystemAlgorithm::Outputs SolveLinearSystemAlgorithm::run(const Inputs& input, const Parameters& params) const
{
  auto A = input.get<0>();
  ENSURE_ALGORITHM_INPUT_NOT_NULL(A, "Null input matrix");

  auto b = input.get<1>();
  ENSURE_ALGORITHM_INPUT_NOT_NULL(b, "Null rhs vector");

  double tolerance = params.get<0>();
  ENSURE_POSITIVE_DOUBLE(tolerance, "Tolerance out of range!");

  int maxIterations = params.get<1>();
  ENSURE_POSITIVE_INT(maxIterations, "Max iterations out of range!");

  SolveLinearSystemAlgorithmEigenCGImpl impl(*b, tolerance, maxIterations);
  DenseColumnMatrix x;
  if (matrix_is::dense(A))
  {
    x = impl.solveWithEigen(*matrix_cast::as_dense(A));
  }
  else if (matrix_is::sparse(A))
  {
    x = impl.solveWithEigen(*matrix_cast::as_sparse(A));
  }
  else
    BOOST_THROW_EXCEPTION(AlgorithmProcessingException() << ErrorMessage("solveWithEigen can only handle dense and sparse matrices."));

  if (x.size() != 0)
  {
    /// @todo: move ctor
    DenseColumnMatrixHandle solution(boost::make_shared<DenseColumnMatrix>(x));
    return SolveLinearSystemAlgorithm::Outputs(solution, impl.tolerance_, impl.maxIterations_);
  }
  else
    BOOST_THROW_EXCEPTION(AlgorithmProcessingException() << ErrorMessage("solveWithEigen produced an empty solution."));
}

AlgorithmOutput SolveLinearSystemAlgorithm::run_generic(const AlgorithmInput& input) const
{
  throw 2;
}


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace std;
using namespace boost::numeric::ublas;

int main () {
    matrix<complex<double>> m(3, 3);
    for (unsigned i = 0; i < m.size1(); ++ i)
        for (unsigned j = 0; j < m.size2(); ++ j)
            m(i, j) = complex<double> (3 * i + j, 3 * i + j);

    cout << - m << endl;
    cout << conj (m) << endl;
    cout << real (m) << endl;
    cout << imag (m) << endl;
    cout << trans (m) << endl;
    cout << herm (m) << endl;
}


/* -------------------------------------------------------------------------
 *  A repertory of multi primitive-to-primitive (MP2) ICP algorithms in C++
 * Copyright (C) 2018-2019 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
/**
 * @file   optimal_tf_gauss_newton.cpp
 * @brief  Simple non-linear optimizer to find the SE(3) optimal transformation
 * @author Jose Luis Blanco Claraco
 * @date   Jun 16, 2019
 */

#include <mp2p_icp/optimal_tf_gauss_newton.h>
#include <mrpt/poses/Lie/SE.h>
#include <Eigen/Dense>
#include <iostream>

using namespace mp2p_icp;

void mp2p_icp::optimal_tf_gauss_newton(
    const Pairings_GaussNewton& in, OptimalTF_Result& result)
{
    using std::size_t;

    MRPT_START

    // Run Gauss-Newton steps, using SE(3) relinearization at the current
    // solution:
    result.optimal_pose = in.initial_guess;

    const auto nPt2Pt = in.paired_points.size();
    const auto nPt2Pl = in.paired_pt2pl.size();
    const auto nPl2Pl = in.paired_planes.size();

    const auto nErrorTerms = (nPt2Pt + nPl2Pl) * 3 + nPt2Pl;

    Eigen::VectorXd                          err(nErrorTerms);
    Eigen::Matrix<double, Eigen::Dynamic, 6> J(nErrorTerms, 6);

    double       w_pt = in.weight_point2point;
    const double w_pl = in.weight_point2plane, w_pl2pl = in.weight_plane2plane;

    const bool  has_per_pt_weight       = !in.point_weights.empty();
    auto        cur_point_block_weights = in.point_weights.begin();
    std::size_t cur_point_block_start   = 0;

    MRPT_TODO("Implement robust Kernel in this solver");

    for (size_t iter = 0; iter < in.max_iterations; iter++)
    {
        // (12x6 Jacobian)
        const auto dDexpe_de =
            mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(result.optimal_pose);

        // Point-to-point:
        for (size_t idx_pt = 0; idx_pt < nPt2Pt; idx_pt++)
        {
            // Error:
            const auto&  p  = in.paired_points[idx_pt];
            const double lx = p.other_x, ly = p.other_y, lz = p.other_z;
            double       gx, gy, gz;
            result.optimal_pose.composePoint(lx, ly, lz, gx, gy, gz);
            err[idx_pt * 3 + 0] = gx - p.this_x;
            err[idx_pt * 3 + 1] = gy - p.this_y;
            err[idx_pt * 3 + 2] = gz - p.this_z;

            // Eval Jacobian:
            // clang-format off
            const Eigen::Matrix<double, 3, 12> J1 =
                (Eigen::Matrix<double, 3, 12>() <<
                   lx,  0,  0,  ly,  0,  0, lz,  0,  0,  1,  0,  0,
                    0, lx,  0,  0,  ly,  0,  0, lz,  0,  0,  1,  0,
                    0,  0, lx,  0,  0,  ly,  0,  0, lz,  0,  0,  1
                 ).finished();
            // clang-format on

            // Get weight:
            if (has_per_pt_weight)
            {
                if (idx_pt >=
                    cur_point_block_start + cur_point_block_weights->first)
                {
                    ASSERT_(cur_point_block_weights != in.point_weights.end());
                    ++cur_point_block_weights;  // move to next block
                    cur_point_block_start = idx_pt;
                }
                w_pt = cur_point_block_weights->second;
            }

            // Build Jacobian:
            J.block<3, 6>(idx_pt * 3, 0) = w_pt * J1 * dDexpe_de.asEigen();
        }

        // Point-to-plane:
        auto base_idx = nPt2Pt * 3;
        for (size_t idx_pl = 0; idx_pl < nPt2Pl; idx_pl++)
        {
            // Error:
            const auto& p = in.paired_pt2pl[idx_pl];

            const double lx = p.pt_other.x, ly = p.pt_other.y,
                         lz = p.pt_other.z;
            mrpt::math::TPoint3D g;
            result.optimal_pose.composePoint(lx, ly, lz, g.x, g.y, g.z);

            err(idx_pl + base_idx) = p.pl_this.plane.evaluatePoint(g);

            // Eval Jacobian:
            // clang-format off
            const Eigen::Matrix<double, 3, 12> J1 =
                (Eigen::Matrix<double, 3, 12>() <<
                   lx,  0,  0,  ly,  0,  0, lz,  0,  0,  1,  0,  0,
                    0, lx,  0,  0,  ly,  0,  0, lz,  0,  0,  1,  0,
                    0,  0, lx,  0,  0,  ly,  0,  0, lz,  0,  0,  1
                 ).finished();
            // clang-format on

            const Eigen::Matrix<double, 1, 3> Jpl =
                (Eigen::Matrix<double, 1, 3>() << p.pl_this.plane.coefs[0],
                 p.pl_this.plane.coefs[1], p.pl_this.plane.coefs[2])
                    .finished();

            const Eigen::Matrix<double, 1, 6> Jb =
                Jpl * J1 * dDexpe_de.asEigen();

            J.block<1, 6>(idx_pl + base_idx, 0) = w_pl * Jb;
        }

        // Plane-to-plane (only direction of normal vectors):
        base_idx += nPt2Pl * 1;
        for (size_t idx_pl = 0; idx_pl < nPl2Pl; idx_pl++)
        {
            // Error term:
            const auto& p = in.paired_planes[idx_pl];

            const auto nl = p.p_other.plane.getNormalVector();
            const auto ng = p.p_this.plane.getNormalVector();

            const auto p_oplus_nl = result.optimal_pose.rotateVector(nl);

            for (int i = 0; i < 3; i++)
                err(i + idx_pl * 3 + base_idx) = ng[i] - p_oplus_nl[i];

            // Eval Jacobian:

            // df_oplus(A,p)/d_A. Section 7.3.2 tech. report:
            // "A tutorial on SE(3) transformation parameterizations and
            // on-manifold optimization"
            // Modified, to discard the last I_3 block, since this particular
            // cost function is insensible to translations.

            // clang-format off
            const Eigen::Matrix<double, 3, 12> J1 =
                (Eigen::Matrix<double, 3, 12>() <<
                   nl.x,  0,  0,  nl.y,  0,  0, nl.z,  0,  0,  0,  0,  0,
                    0, nl.x,  0,  0,  nl.y,  0,  0, nl.z,  0,  0,  0,  0,
                    0,  0, nl.x,  0,  0,  nl.y,  0,  0, nl.z,  0,  0,  0
                 ).finished();
            // clang-format on

            J.block<3, 6>(3 * idx_pl + base_idx, 0) =
                w_pl2pl * J1 * dDexpe_de.asEigen();
        }

        // 3) Solve Gauss-Newton:
        const Eigen::VectorXd             g = J.transpose() * err;
        const Eigen::Matrix<double, 6, 6> H = J.transpose() * J;
        const Eigen::Matrix<double, 6, 1> delta =
            -H.colPivHouseholderQr().solve(g);

        // 4) add SE(3) increment:
        const auto dE = mrpt::poses::Lie::SE<3>::exp(
            mrpt::math::CVectorFixed<double, 6>(delta));

        result.optimal_pose = result.optimal_pose + dE;

        if (in.verbose)
        {
            std::cout << "[P2P GN] iter:" << iter << " err:" << err.norm()
                      << " delta:" << delta.transpose() << "\n";
        }

        // Simple convergence test:
        if (delta.norm() < in.min_delta) break;

    }  // for each iteration
    MRPT_END
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

#include <iostream>
#include <Eigen/Eigen>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  std::string sep = "\n----------------------------------------\n";
Matrix3d m1;
m1 << 1.111111, 2, 3.33333, 4, 5, 6, 7, 8.888888, 9;

IOFormat CommaInitFmt(StreamPrecision, DontAlignCols, ", ", ", ", "", "", " << ", ";");
IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
IOFormat OctaveFmt(StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
IOFormat HeavyFmt(FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");

std::cout << m1 << sep;
std::cout << m1.format(CommaInitFmt) << sep;
std::cout << m1.format(CleanFmt) << sep;
std::cout << m1.format(OctaveFmt) << sep;
std::cout << m1.format(HeavyFmt) << sep;

  return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2010 Gael Guennebaud <g.gael@free.fr>
//
// 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/SparseExtra>
#include "unsupported/Eigen/src/SparseExtra/BlockSparseQR.h"

template<typename Scalar, typename BlockSolverLeft, typename BlockSolverRight>
void block_sparse_qr(int nRows, int nCols, int blockCols)
{
    std::cout << "block_sparse_qr< " << eigen_test_nice_typename<BlockSolverLeft>() << ", " << eigen_test_nice_typename<BlockSolverRight>() << ">[" << nRows << "x" << blockCols << " | " << nRows << "x" << (nCols - blockCols) << "]\n";
    typedef Eigen::Matrix<Scalar, Dynamic, Dynamic> DenseMatrix;

    // Generate random sparse matrix
    SparseMatrix<Scalar> mat;
    mat.resize(nRows, nCols);
    std::vector< Eigen::Triplet<Scalar> > triplets;
    const double occupancy = 0.5;
    for(int i=0; i<nRows; i++) {
        for(int j=0; j<nCols; j++) {
            if( Eigen::internal::random<double>(0., 1.) > occupancy )
                triplets.push_back( Eigen::Triplet<Scalar>(i, j, Eigen::internal::random<Scalar>() ) );
        }
    }
    mat.setFromTriplets(triplets.begin(), triplets.end());
    mat.makeCompressed();

    // solve using BlockSparseQR, using provided Left and Right solvers
    BlockSparseQR<SparseMatrix<Scalar>, BlockSolverLeft, BlockSolverRight> solver;
    solver.setBlockParams(blockCols);
    solver.compute(mat);

    // check result
    auto Q = solver.matrixQ();
    auto R = solver.matrixR();

    DenseMatrix I = DenseMatrix::Identity(nRows, nRows);

    DenseMatrix sQ = Q * I;

    // check A*P = Q*R
    DenseMatrix  Q_dot_R = Q*R;
    DenseMatrix AP = mat.toDense();
    solver.colsPermutation().applyThisOnTheRight(AP);
    VERIFY_IS_APPROX( Q_dot_R, AP );

    // check Qt*Q = I
    DenseMatrix  QtQ = Q.transpose()*sQ;
    VERIFY_IS_APPROX( QtQ, I );

    // check R = upper triangular
    for(int i=0; i<R.rows(); i++)
        for(int j=0; j<R.cols() && j<i; j++)
            eigen_assert( fabs(R.coeff(i,j)) < 0.00001  );

}

void test_block_sparse_qr()
{
  for(int i = 0; i < g_repeat; i++) {

    typedef double Scalar;
    typedef SparseQR<SparseMatrix<Scalar>, COLAMDOrdering<int> > BlockSparseSolver;
    typedef ColPivHouseholderQR<Matrix<Scalar,Dynamic,Dynamic> > BlockDenseSolver;

    CALL_SUBTEST((block_sparse_qr<Scalar, BlockSparseSolver, BlockDenseSolver>(20, 31, 13)));
    CALL_SUBTEST((block_sparse_qr<Scalar, BlockSparseSolver, BlockSparseSolver>(20, 31, 13)));
    CALL_SUBTEST((block_sparse_qr<Scalar, BlockSparseSolver, BlockDenseSolver>(10, 6, 2)));
    CALL_SUBTEST((block_sparse_qr<Scalar, BlockSparseSolver, BlockDenseSolver>(6, 10, 2)));
    CALL_SUBTEST(( block_sparse_qr<Scalar, BlockSparseSolver, BlockDenseSolver>( 100, 50, 20 ) ));
    CALL_SUBTEST(( block_sparse_qr<Scalar, BlockSparseSolver, BlockDenseSolver>( 9, 8, 2 ) ));

  }
}


#ifndef OPENGV2_BSPLINEREAL_HPP
#define OPENGV2_BSPLINEREAL_HPP

#include <Eigen/Eigen>
#include <Eigen/StdVector>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <cmath>
#include <algorithm>
#include <numeric>
#include <iostream>

namespace opengv2 {
    template<int dim>
    class BsplineReal {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        explicit BsplineReal(int p = dim,
                             const std::vector<Eigen::Matrix<double, dim, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, dim, 1>>> &Q = std::vector<Eigen::Matrix<double, dim, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, dim, 1>>>(),
                             int controlPointsNum = -1,
                             const std::vector<double> &u = std::vector<double>(),
                             const std::vector<Eigen::Matrix<double, dim, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, dim, 1>>> &dQ = std::vector<Eigen::Matrix<double, dim, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, dim, 1>>>(),
                         bool derWithMagnitude = false, double lambda = 1) : degree(p), lambda(lambda) {
            // if dataPoints given, then do approximation
            if (!Q.empty()) {
                dataPoints = Q;

                // Data registration
                if (!u.empty()) {
                    correspondingUs = u;
                } else {
                    // Data registration using chord length
                    // Range: [0,1]
                    correspondingUs.resize(dataPoints.size());

                    double d = 0;
                    for (int i = 1; i < dataPoints.size(); ++i) {
                        d += (dataPoints[i] - dataPoints[i - 1]).norm();
                    }

                    correspondingUs.front() = 0;
                    correspondingUs.back() = 1;
                    for (int i = 1; i < dataPoints.size() - 1; ++i) {
                        correspondingUs[i] =
                                correspondingUs[i - 1] + (dataPoints[i] - dataPoints[i - 1]).norm() / d;
                    }
                }

                // derivative
                if (!dQ.empty()) {
                    derivatives = dQ;

                    if (!derWithMagnitude) {
                        // derivatives is unit direction, magnitude estimation by speed estimation
                        for (int i = 1; i <= dataPoints.size() - 2; ++i) {
                            double speed = (dataPoints[i] - dataPoints[i - 1]).norm() /
                                           (correspondingUs[i] - correspondingUs[i - 1]) +
                                           (dataPoints[i + 1] - dataPoints[i]).norm() /
                                           (correspondingUs[i + 1] - correspondingUs[i]);
                            derivatives[i] *= speed / 2;
                        }
                        derivatives.front() *=
                                (dataPoints[1] - dataPoints[0]).norm() / (correspondingUs[1] - correspondingUs[0]);
                        derivatives.back() *=
                                (dataPoints[dataPoints.size() - 1] - dataPoints[dataPoints.size() - 2]).norm() /
                                (correspondingUs[dataPoints.size() - 1] - correspondingUs[dataPoints.size() - 2]);
                    }
                }

                // approximation
                if (controlPointsNum < 0)
                    controlPointsNum = std::max(int(Q.size() / 3), degree + 1);
                if (controlPointsNum <= degree) {
                    std::cerr << "Constructor: control points number should greater than degree!" << std::endl;
                    return;
                }
                approximation(controlPointsNum);
            }
        };

        /*
         * Approximation
         * return: status: failed(-1), success(0)
         */
        int approximation(int controlPointsNum) {
            /*** knot initialization (9.68) in NURBS book ***/
            knotVector.resize(controlPointsNum + degree + 1);
            std::fill_n(knotVector.begin(), degree + 1, correspondingUs.front());
            std::fill_n(knotVector.rbegin(), degree + 1, correspondingUs.back());
            double d = dataPoints.size() / double(controlPointsNum - degree);
            for (int j = 1; j <= controlPointsNum - 1 - degree; j++) {
                int i = floor(j * d);
                double alpha = j * d - i;
                knotVector[degree + j] = (1 - alpha) * correspondingUs[i - 1] + alpha * correspondingUs[i];
            }

            return optimization();
        };

        /*
         * Compute nonzero basis functions and their derivatives up to derivativeLimit.
         * N_{i-p,p}(u),...,N_{i,p}(u)
         * Output: two-dimensional array, ders. ders[k][j] is the kth derivative of the function N_{i-p+j,p}, where 0<=k<=n and 0<=j<=p.
         */
        int dersBasisFuns(double u, size_t spanIdx, int derivativeLimit, std::vector<std::vector<double>> &ders) const {
            // check
            if (knotVector.empty()) {
                std::cerr << "Function dersBasisFuns: knotVector is empty!" << std::endl;
                return -1;
            }

            ders.resize(derivativeLimit + 1);
            for (auto &it:ders) {
                it.assign(degree + 1, 0);
            }

            double ndu[degree + 1][degree + 1]; // store the basis functions and knot differences

            // store (in an alternating fashion) the two most recently computed rows a_{k,j} and a_{k-1,j}
            double a[2][degree + 1];

            std::vector<double> left, right;
            left.resize(degree + 1);
            right.resize(degree + 1);

            ndu[0][0] = 1;
            for (int j = 1; j <= degree; j++) {
                left[j] = u - knotVector[spanIdx + 1 - j];
                right[j] = knotVector[spanIdx + j] - u;
                double saved = 0.0;
                for (int r = 0; r < j; ++r) {
                    ndu[j][r] = right[r + 1] + left[j - r];
                    double temp = ndu[r][j - 1] / ndu[j][r];

                    ndu[r][j] = saved + right[r + 1] * temp;
                    saved = left[j - r] * temp;
                }
                ndu[j][j] = saved;
            }

            /* Load the basis functions */
            for (int j = 0; j <= degree; j++)
                ders[0][j] = ndu[j][degree];

            if (derivativeLimit > 0) {
                /*** This section computes the derivatives (Eq.[2.9]) ***/
                /* Load over function index */
                for (int r = 0; r <= degree; r++) {
                    int s1 = 0, s2 = 1; // Alternate rows in array a
                    a[0][0] = 1;

                    // loop to compute kth derivative
                    for (int k = 1; k <= derivativeLimit; k++) {
                        double d = 0;
                        int rk = r - k, pk = degree - k;
                        if (r >= k) {
                            a[s2][0] = a[s1][0] / ndu[pk + 1][rk];
                            d = a[s2][0] * ndu[rk][pk];
                        }

                        int j1, j2;
                        if (rk >= -1)
                            j1 = 1;
                        else
                            j1 = -rk;

                        if (r - 1 <= pk)
                            j2 = k - 1;
                        else
                            j2 = degree - r;

                        for (int j = j1; j <= j2; j++) {
                            a[s2][j] = (a[s1][j] - a[s1][j - 1]) / ndu[pk + 1][rk + j];
                            d += a[s2][j] * ndu[rk + j][pk];
                        }

                        if (r <= pk) {
                            a[s2][k] = -a[s1][k - 1] / ndu[pk + 1][r];
                            d += a[s2][k] * ndu[r][pk];
                        }

                        ders[k][r] = d;

                        // switch rows
                        std::swap(s1, s2);
                    }
                }

                /* Multiply through by correct factors (Eq. [2.9]) */
                int r = degree;
                for (int k = 1; k <= derivativeLimit; ++k) {
                    for (int j = 0; j <= degree; j++)
                        ders[k][j] *= r;

                    r *= (degree - k);
                }
            }

            return 0;
        }

        /*
         * Determine the knot span index i \in [p,n], whole is [0,n+p+1]
         * u \in [u_{i}, u_{i+1}) , special at u==u_{n+1}
         */
        size_t findSpan(double u) const {
            // assume it's totally p-smooth, which means there is only one knot for each boundary
            // Notes: the size of knot vector is n+p+2
            size_t n = knotVector.size() - 2 - degree;

            // special case
            if (u == knotVector[n + 1])
                return n;

            // binary search
            size_t low = degree;
            size_t high = n + 1;
            size_t mid = (low + high) / 2;
            while (u < knotVector[mid] || u >= knotVector[mid + 1]) {
                if (u < knotVector[mid])
                    high = mid;
                else
                    low = mid;

                mid = (low + high) / 2;
            }

            return mid;
        };

        // shape won't change
        int insertKnot(double u) {
            if (knotVector.empty()) {
                std::cerr << "Function insertKnot: knotVector is empty!" << std::endl;
                return -1;
            }
            if (controlPoints.empty()) {
                std::cerr << "Function insertKnot: controlPoints is empty!" << std::endl;
                return -1;
            }

            size_t spanIdx = findSpan(u);
            std::vector<Eigen::Matrix<double, dim, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, dim,
                    1 >>> Q(controlPoints.size() + 1);

            std::copy(controlPoints.begin(), controlPoints.begin() + spanIdx - degree + 1, Q.begin());
            std::copy(controlPoints.begin() + spanIdx, controlPoints.end(), Q.begin() + spanIdx + 1);
            for (size_t i = spanIdx - degree + 1; i <= spanIdx; i++) {
                double alpha = (u - knotVector[i]) / (knotVector[i + degree] - knotVector[i]);
                Q[i] = alpha * controlPoints[i] + (1 - alpha) * controlPoints[i - 1];
            }

            //update knotVector
            knotVector.insert(knotVector.begin() + spanIdx + 1, u);
            //update controlPoints
            controlPoints = std::move(Q);

            return 0;
        };

        // assume X: ascending sorted
        int refineKnotVect(std::vector<double> &X) {
            if (knotVector.empty()) {
                std::cerr << "Function refineKnotVect: knotVector is empty!" << std::endl;
                return -1;
            }
            if (controlPoints.empty()) {
                std::cerr << "Function refineKnotVect: controlPoints is empty!" << std::endl;
                return -1;
            }

            std::sort(X.begin(), X.end());

            std::vector<Eigen::Matrix<double, dim, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, dim, 1>>> Q(
                    controlPoints.size() + X.size());
            std::vector<double> Ubar(knotVector.size() + X.size());

            size_t n = knotVector.size() - 2 - degree;
            size_t r = X.size() - 1;
            size_t m = knotVector.size() - 1;
            size_t a = findSpan(X.front());
            size_t b = findSpan(X.back());
            b++;

            for (size_t j = 0; j <= a - degree; j++)
                Q[j] = controlPoints[j];
            for (size_t j = b - 1; j <= n; j++)
                Q[j + X.size()] = controlPoints[j];
            for (size_t j = 0; j <= a; j++)
                Ubar[j] = knotVector[j];
            for (size_t j = b + degree; j <= m; j++)
                Ubar[j + X.size()] = knotVector[j];

            int i = b + degree - 1;
            int k = b + degree + r;
            for (int j = r; j >= 0; j--) {
                while (X[j] <= knotVector[i] && i > a) {
                    Q[k - degree - 1] = controlPoints[i - degree - 1];
                    Ubar[k] = knotVector[i];
                    k--;
                    i--;
                }

                Q[k - degree - 1] = Q[k - degree];
                for (int l = 1; l <= degree; l++) {
                    int idx = k - degree + l;
                    double alpha = Ubar[k + l] - X[j];
                    if (std::abs(alpha) == 0.0)
                        Q[idx - 1] = Q[idx];
                    else {
                        alpha /= (Ubar[k + l] - knotVector[i - degree + l]);
                        Q[idx - 1] = alpha * Q[idx - 1] + (1 - alpha) * Q[idx];
                    }
                }
                Ubar[k] = X[j];
                k--;
            }

            //update knotVector
            knotVector = std::move(Ubar);
            //update controlPoints
            controlPoints = std::move(Q);
            return 0;
        };

        // optimize control points w.r.t distance error after knot vector change
        int optimization() {
            /*** check ***/
            if (dataPoints.empty()) {
                std::cerr << "Function optimize: dataPoints is empty!" << std::endl;
                return -1;
            }
            if (dataPoints.size() != derivatives.size() && derivatives.size() != 0) {
                std::cerr
                        << "Function optimization: dataPoints ans derivatives should be same size (NAN for unknow) OR derivatives should be empty!"
                        << std::endl;
                return -1;
            }
            if (knotVector.empty()) {
                std::cerr << "Function optimization: knotVector is empty!" << std::endl;
                return -1;
            }

            int controlPointsNum = knotVector.size() - degree - 1;

            // initialize controlPoints
            controlPoints.resize(controlPointsNum);
            controlPoints.front() = dataPoints.front();
            controlPoints.back() = dataPoints.back();

            // support for derivative lacking
            int derivativesCounter = 0;
            std::vector<int> map(derivatives.size(), -1);
            for (size_t k = 0; k < derivatives.size(); k++) {
                if (!std::isnan(derivatives[k][0])) {
                    map[k] = derivativesCounter++;
                }
            }

            /*** Problem ***/
            Eigen::SparseMatrix<double> N(dataPoints.size(), controlPointsNum);
            Eigen::SparseMatrix<double> M(dataPoints.size(), controlPointsNum);
            std::vector<std::vector<double>> ders;
            for (int k = 0; k < dataPoints.size(); ++k) {
                size_t spanIdx = findSpan(correspondingUs[k]);
                dersBasisFuns(correspondingUs[k], spanIdx, derivativesCounter > 0 ? 1 : 0, ders);

                for (int j = 0; j <= degree; ++j) {
                    if (ders[0][j] != 0)
                        N.insert(k, spanIdx - degree + j) = ders[0][j]; // N_{i-p+j,p}^{0}(\bar{u}_k)

                    if (derivativesCounter > 0) {
                        if (ders[1][j] != 0)
                            M.insert(k, spanIdx - degree + j) = ders[1][j]; // N_{i-p+j,p}^{1}(\bar{u}_k)
                    }
                }
            }

            // formulating B
            std::vector<Eigen::Matrix<double, dim, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, dim, 1>>> B;
            B.assign(controlPointsNum - 2, Eigen::Matrix<double, dim, 1>::Zero());
            for (int l = 1; l <= controlPointsNum - 2; ++l) {
                // for l-th col of N
                for (Eigen::SparseMatrix<double>::InnerIterator it(N, l); it; ++it) {
                    // N_{l,p}(\bar{u}_k), valid for column-major
                    int k = it.row();
                    if (k != 0 && k != dataPoints.size() - 1) {
                        B[l - 1] += it.value() * (dataPoints[k] - N.coeff(k, 0) * dataPoints.front() -
                                                  N.coeff(k, controlPointsNum - 1) * dataPoints.back());
                    }
                }

                if (derivativesCounter > 0) {
                    // for l-th col of M
                    for (Eigen::SparseMatrix<double>::InnerIterator it(M, l); it; ++it) {
                        // N_{l,p}^{1}(\bar{u}_k), valid for column-major
                        int k = it.row();
                        if (map[k] > -1) {
                            B[l - 1] += lambda * it.value() * (derivatives[k] - M.coeff(k, 0) * dataPoints.front() -
                                                               M.coeff(k, controlPointsNum - 1) *
                                                               dataPoints.back());
                        }
                    }
                }
            }

            // formulating M,N
            Eigen::SparseMatrix<double> Nc = N.block(1, 1, dataPoints.size() - 2, controlPointsNum - 2);
            Eigen::SparseMatrix<double> A = Nc.transpose() * Nc;

            if (derivativesCounter > 0) {
                Eigen::SparseMatrix<double> Mc(derivativesCounter, controlPointsNum - 2);
                for (int i = 1; i <= controlPointsNum - 2; ++i) {
                    for (Eigen::SparseMatrix<double>::InnerIterator it(M, i); it; ++it) {// i-th col
                        if (map[it.row()] > -1) {
                            Mc.insert(map[it.row()], it.col() - 1) = it.value();
                        }
                    }
                }
                A += lambda * Mc.transpose() * Mc;
            }

            // solving
            A.makeCompressed();
            Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
            solver.compute(A);
            if (solver.info() != Eigen::Success) {
                std::cerr << "Function optimization: decomposition failed!" << std::endl;
                return -1;
            }
            Eigen::VectorXd b(controlPointsNum - 2);
            for (int i = 0; i < dim; i++) {
                for (int j = 0; j < B.size(); ++j) {
                    b[j] = B[j][i];
                }
                Eigen::VectorXd x = solver.solve(b);
                if (solver.info() != Eigen::Success) {
                    std::cerr << "Function optimization: solving failed!" << std::endl;
                    return -1;
                }
                for (int j = 1; j <= controlPointsNum - 2; ++j) {
                    controlPoints[j][i] = x[j - 1];
                }
            }

            return 0;
        };

        /*
         * Evaluate up to k-th derivative at u
         */
        int evaluate(double u, int derivativeLimit,
                     std::vector<Eigen::Matrix<double, dim, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, dim, 1>>> &Ders) const {
            if (controlPoints.empty()) {
                std::cerr << "Function evaluate: controlPoints is empty!" << std::endl;
                return -1;
            }

            Ders.assign(derivativeLimit + 1, Eigen::Matrix<double, dim, 1>::Zero());
            std::vector<std::vector<double>> ders;
            size_t spanIdx = findSpan(u);

            dersBasisFuns(u, spanIdx, derivativeLimit, ders);

            for (size_t i = 0; i < ders.size(); i++) {
                for (size_t j = 0; j < ders[i].size(); ++j) {
                    Ders[i] += ders[i][j] * controlPoints[spanIdx - degree + j];
                }
            }

            return 0;
        }

        inline std::vector<Eigen::Matrix<double, dim, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, dim, 1>>> &
        getCP() {
            return controlPoints;
        };

        inline const std::vector<double> &getCorrespondingUs() {
            return correspondingUs;
        }

        inline const std::vector<double> &getKnotVector() {
            return knotVector;
        }

    protected:
        int degree;

        std::vector<double> knotVector;
        std::vector<Eigen::Matrix<double, dim, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, dim, 1>>> controlPoints;

        // optional
        std::vector<Eigen::Matrix<double, dim, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, dim, 1>>> dataPoints;
        std::vector<double> correspondingUs; //corresponding u for each data point
        std::vector<Eigen::Matrix<double, dim, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, dim, 1>>> derivatives; //corresponding derivatives for each data point
        double lambda;
    };
}

#endif //OPENGV2_BSPLINEREAL_HPP


#ifndef PROCON_28_GEO_HPP
#define PROCON_28_GEO_HPP

#include <iostream>
#include <vector>

#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/geometry.hpp>

namespace bg = boost::geometry;
namespace trans = bg::strategy::transform;

namespace procon28 {
using Point_Type = long double;                     // x, y座標に使用する型
using Point = bg::model::d2::point_xy<Point_Type>;  // 座標
using Polygon = bg::model::polygon<Point>;          // 図形
using Segment = bg::model::segment<Point>;          // 線分
using Ring = typename Polygon::ring_type;
using InnerContainer = typename Polygon::inner_container_type;
const long double EPS = 1e-10;
const long double PI = std::acos(-1);

std::istream& operator>>(std::istream&, Polygon&);
std::ostream& operator<<(std::ostream&, Polygon&);

const Point operator+(const Point&, const Point&);
const Point operator-(const Point&, const Point&);

Point get_point(const Ring&, int);
Point get_point(const Polygon&, int);
Point get_point(const Segment&, int);
Segment inv_segment(const Segment&);
Segment get_segment(const Ring&, int, int);
Segment get_segment(const Polygon&, int, int);
Segment get_segment(const Ring&, int);
Segment get_segment(const Polygon&, int);
long double get_angle(const Segment&);
long double get_angle(const Segment&, const Segment&);
long double get_corner(const Ring&, int);
long double get_corner(const Polygon&, int);
Point to_vec(const Segment&);
Polygon to_frame(const std::vector<Polygon>&);
std::vector<Polygon> to_piece(const Polygon&);
Polygon translate(const Polygon&, Point_Type, Point_Type);
Polygon translate(const Polygon&, Point);
Polygon rotate(const Polygon&, long double);
Polygon rotate(const Polygon&, long double, Point);
Polygon inverse(const Polygon&);
Polygon scale(const Polygon&, double);
void printFrame(const Polygon&, std::ostream&);

}  // namespace procon28

#endif


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <stdint.h>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <unsupported/Eigen/MatrixFunctions>

// CUDA runtime
#include <cuda_runtime.h>
// Utilities and system includes
//#include <helper_functions.h>
#include <nvidia/helper_cuda.h>

//#include <mmf/defines.h>
#include <mmf/sphereSimple.hpp>
#include <mmf/optimizationSO3.hpp>
#include <manifold/SO3.h>
#include <jsCore/vmf.h>

using namespace Eigen;

extern void MMFvMFCostFctAssignmentGPU(float *h_cost, float *d_cost,
  uint32_t *h_W, uint32_t *d_W, float *d_x, float* d_weights,
  uint32_t *d_z, float *d_mu, float*d_pi, int N, int K);

namespace mmf{

// closed form solution for the vMF cost function.
class OptSO3MMFvMF : public OptSO3
{
  public:
  OptSO3MMFvMF(uint32_t K, float *d_weights =NULL):
    OptSO3(1.,1.,0.1,d_weights),
    Rs_(K, Eigen::Matrix3f::Identity()),
    pi_(K*6), taus_(Eigen::VectorXf::Ones(K*6)),
    estimateTau_(false)
  {
    // overwrite cld
    cld_ = jsc::ClDataGpu<float>(3,6*K);
    if(d_cost) checkCudaErrors(cudaFree(d_cost));
    if(d_mu_)  checkCudaErrors(cudaFree(d_mu_));
    if(d_N_) checkCudaErrors(cudaFree(d_N_));
    for (uint32_t k=0; k<K; ++k)
      Rs_[k] = SO3f::Random().matrix();
    init();
  };

  virtual ~OptSO3MMFvMF() { };
  uint32_t K() {return Rs_.size();};
  virtual std::vector<Eigen::Matrix3f> GetRs() { return Rs_; };

protected:
  std::vector<Eigen::Matrix3f> Rs_;
  jsc::GpuMatrix<float> pi_;
  Eigen::VectorXf taus_;
  bool estimateTau_;

  virtual float computeAssignment(uint32_t& N);

  virtual float conjugateGradientCUDA_impl(Matrix3f& R, float res0,
    uint32_t N, uint32_t maxIter);
  virtual void conjugateGradientPostparation_impl(Matrix3f& R);
  virtual float conjugateGradientPreparation_impl(Matrix3f& R, uint32_t& N);
  /* evaluate cost function for a given assignment of npormals to axes */
  virtual float evalCostFunction(Matrix3f& R);
  /* compute Jacobian */
  virtual void computeJacobian(Matrix3f&J, Matrix3f& R, float N);
  virtual void init();

  /* copy rotation to device */
  void Rot2Device();
};

}


#include <vector>

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

#include <cmath>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/layers/st_layer.hpp"
#include "caffe/util/math_functions.hpp"

namespace caffe {

template <typename Dtype>
void SpatialTransformerLayer<Dtype>::LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {

	string prefix = "\t\tSpatial Transformer Layer:: LayerSetUp: \t";

	if(this->layer_param_.st_param().transform_type() == "affine") {
		transform_type_ = "affine";
	} else {
		CHECK(false) << prefix << "Transformation type only supports affine now!" << std::endl;
	}

	if(this->layer_param_.st_param().sampler_type() == "bilinear") {
		sampler_type_ = "bilinear";
	} else {
		CHECK(false) << prefix << "Sampler type only supports bilinear now!" << std::endl;
	}

	if(this->layer_param_.st_param().to_compute_du()) {
		to_compute_dU_ = true;
	}

	std::cout<<prefix<<"Getting output_H_ and output_W_"<<std::endl;

	output_H_ = bottom[0]->shape(2);
	if(this->layer_param_.st_param().has_output_h()) {
		output_H_ = this->layer_param_.st_param().output_h();
	}
	output_W_ = bottom[0]->shape(3);
	if(this->layer_param_.st_param().has_output_w()) {
		output_W_ = this->layer_param_.st_param().output_w();
	}

	std::cout<<prefix<<"output_H_ = "<<output_H_<<", output_W_ = "<<output_W_<<std::endl;

	std::cout<<prefix<<"Getting pre-defined parameters"<<std::endl;

	is_pre_defined_theta[0] = false;
	if(this->layer_param_.st_param().has_theta_1_1()) {
		is_pre_defined_theta[0] = true;
		++ pre_defined_count;
		pre_defined_theta[0] = this->layer_param_.st_param().theta_1_1();
		std::cout<<prefix<<"Getting pre-defined theta[1][1] = "<<pre_defined_theta[0]<<std::endl;
	}

	is_pre_defined_theta[1] = false;
	if(this->layer_param_.st_param().has_theta_1_2()) {
		is_pre_defined_theta[1] = true;
		++ pre_defined_count;
		pre_defined_theta[1] = this->layer_param_.st_param().theta_1_2();
		std::cout<<prefix<<"Getting pre-defined theta[1][2] = "<<pre_defined_theta[1]<<std::endl;
	}

	is_pre_defined_theta[2] = false;
	if(this->layer_param_.st_param().has_theta_1_3()) {
		is_pre_defined_theta[2] = true;
		++ pre_defined_count;
		pre_defined_theta[2] = this->layer_param_.st_param().theta_1_3();
		std::cout<<prefix<<"Getting pre-defined theta[1][3] = "<<pre_defined_theta[2]<<std::endl;
	}

	is_pre_defined_theta[3] = false;
	if(this->layer_param_.st_param().has_theta_2_1()) {
		is_pre_defined_theta[3] = true;
		++ pre_defined_count;
		pre_defined_theta[3] = this->layer_param_.st_param().theta_2_1();
		std::cout<<prefix<<"Getting pre-defined theta[2][1] = "<<pre_defined_theta[3]<<std::endl;
	}

	is_pre_defined_theta[4] = false;
	if(this->layer_param_.st_param().has_theta_2_2()) {
		is_pre_defined_theta[4] = true;
		++ pre_defined_count;
		pre_defined_theta[4] = this->layer_param_.st_param().theta_2_2();
		std::cout<<prefix<<"Getting pre-defined theta[2][2] = "<<pre_defined_theta[4]<<std::endl;
	}

	is_pre_defined_theta[5] = false;
	if(this->layer_param_.st_param().has_theta_2_3()) {
		is_pre_defined_theta[5] = true;
		++ pre_defined_count;
		pre_defined_theta[5] = this->layer_param_.st_param().theta_2_3();
		std::cout<<prefix<<"Getting pre-defined theta[2][3] = "<<pre_defined_theta[5]<<std::endl;
	}

	// check the validation for the parameter theta
	CHECK(bottom[1]->count(1) + pre_defined_count == 6) << "The dimension of theta is not six!"
			<< " Only " << bottom[1]->count(1) << " + " << pre_defined_count << std::endl;
	CHECK(bottom[1]->shape(0) == bottom[0]->shape(0)) << "The first dimension of theta and " <<
			"U should be the same" << std::endl;

	// initialize the matrix for output grid
	std::cout<<prefix<<"Initializing the matrix for output grid"<<std::endl;

	vector<int> shape_output(2);
	shape_output[0] = output_H_ * output_W_; shape_output[1] = 3;
	output_grid.Reshape(shape_output);

	Dtype* data = output_grid.mutable_cpu_data();
	for(int i=0; i<output_H_ * output_W_; ++i) {
		data[3 * i] = (i / output_W_) * 1.0 / output_H_ * 2 - 1;
		data[3 * i + 1] = (i % output_W_) * 1.0 / output_W_ * 2 - 1;
		data[3 * i + 2] = 1;
	}

	// initialize the matrix for input grid
	std::cout<<prefix<<"Initializing the matrix for input grid"<<std::endl;

	vector<int> shape_input(3);
	shape_input[0] = bottom[1]->shape(0); shape_input[1] = output_H_ * output_W_; shape_input[2] = 2;
	input_grid.Reshape(shape_input);

	std::cout<<prefix<<"Initialization finished."<<std::endl;
}

template <typename Dtype>
void SpatialTransformerLayer<Dtype>::Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {

	string prefix = "\t\tSpatial Transformer Layer:: Reshape: \t";

	if(global_debug) std::cout<<prefix<<"Starting!"<<std::endl;

	N = bottom[0]->shape(0);
	C = bottom[0]->shape(1);
	H = bottom[0]->shape(2);
	W = bottom[0]->shape(3);

	// reshape V
	vector<int> shape(4);

	shape[0] = N;
	shape[1] = C;
	shape[2] = output_H_;
	shape[3] = output_W_;

	top[0]->Reshape(shape);

	// reshape dTheta_tmp
	vector<int> dTheta_tmp_shape(4);

	dTheta_tmp_shape[0] = N;
	dTheta_tmp_shape[1] = 2;
	dTheta_tmp_shape[2] = 3;
	dTheta_tmp_shape[3] = output_H_ * output_W_ * C;

	dTheta_tmp.Reshape(dTheta_tmp_shape);

	// init all_ones_2
	vector<int> all_ones_2_shape(1);
	all_ones_2_shape[0] = output_H_ * output_W_ * C;
	all_ones_2.Reshape(all_ones_2_shape);

	// reshape full_theta
	vector<int> full_theta_shape(2);
	full_theta_shape[0] = N;
	full_theta_shape[1] = 6;
	full_theta.Reshape(full_theta_shape);

	if(global_debug) std::cout<<prefix<<"Finished."<<std::endl;
}

template <typename Dtype>
Dtype SpatialTransformerLayer<Dtype>::transform_forward_cpu(const Dtype* pic, Dtype px, Dtype py) {

	bool debug = false;

	string prefix = "\t\tSpatial Transformer Layer:: transform_forward_cpu: \t";

	if(debug) std::cout<<prefix<<"Starting!\t"<<std::endl;
	if(debug) std::cout<<prefix<<"(px, py) = ("<<px<<", "<<py<<")"<<std::endl;

	Dtype res = (Dtype)0.;

	Dtype x = (px + 1) / 2 * H; Dtype y = (py + 1) / 2 * W;

	if(debug) std::cout<<prefix<<"(x, y) = ("<<x<<", "<<y<<")"<<std::endl;

	int m, n; Dtype w;

	m = floor(x); n = floor(y); w = 0;
	if(debug) std::cout<<prefix<<"1: (m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));
		res += w * pic[m * W + n];
		if(debug) std::cout<<prefix<<"w = "<<w<<", pic[m, n] = "<<pic[m * W + n]<<std::endl;
	}

	m = floor(x) + 1; n = floor(y); w = 0;
	if(debug) std::cout<<prefix<<"2: (m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));
		res += w * pic[m * W + n];
		if(debug) std::cout<<prefix<<"w = "<<w<<", pic[m, n] = "<<pic[m * W + n]<<std::endl;
	}

	m = floor(x); n = floor(y) + 1; w = 0;
	if(debug) std::cout<<prefix<<"3: (m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));
		res += w * pic[m * W + n];
		if(debug) std::cout<<prefix<<"w = "<<w<<", pic[m, n] = "<<pic[m * W + n]<<std::endl;
	}

	m = floor(x) + 1; n = floor(y) + 1; w = 0;
	if(debug) std::cout<<prefix<<"4: (m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));
		res += w * pic[m * W + n];
		if(debug) std::cout<<prefix<<"w = "<<w<<", pic[m, n] = "<<pic[m * W + n]<<std::endl;
	}

	if(debug) std::cout<<prefix<<"Finished. \tres = "<<res<<std::endl;

	return res;
}

template <typename Dtype>
void SpatialTransformerLayer<Dtype>::Forward_cpu(const vector<Blob<Dtype>*>& bottom,
    const vector<Blob<Dtype>*>& top) {

	string prefix = "\t\tSpatial Transformer Layer:: Forward_cpu: \t";

	// CHECK(false) << "Don't use the CPU implementation! If you really want to, delete the" <<
	//		" CHECK in st_layer.cpp file. Line number: 240-241." << std::endl;

	if(global_debug) std::cout<<prefix<<"Starting!"<<std::endl;

	const Dtype* U = bottom[0]->cpu_data();
	const Dtype* theta = bottom[1]->cpu_data();
	const Dtype* output_grid_data = output_grid.cpu_data();

	Dtype* input_grid_data = input_grid.mutable_cpu_data();
	Dtype* V = top[0]->mutable_cpu_data();

	caffe_set(input_grid.count(), (Dtype)0, input_grid_data);
	caffe_set(top[0]->count(), (Dtype)0, V);

	// for each input
	for(int i = 0; i < N; ++i) {

		Dtype* coordinates = input_grid_data + (output_H_ * output_W_ * 2) * i;

		caffe_cpu_gemm<Dtype>(CblasNoTrans, CblasTrans, output_H_ * output_W_, 2, 3, (Dtype)1.,
		      output_grid_data, theta + 6 * i, (Dtype)0., coordinates);

		int row_idx; Dtype px, py;

		for(int j = 0; j < C; ++j)
			for(int s = 0; s < output_H_; ++s)
				for(int t = 0; t < output_W_; ++t) {

					row_idx = output_W_ * s + t;

					px = coordinates[row_idx * 2];
					py = coordinates[row_idx * 2 + 1];

					V[top[0]->offset(i, j, s, t)] = transform_forward_cpu(
							U + bottom[0]->offset(i, j, 0, 0), px, py);
				}
	}

	if(global_debug) std::cout<<prefix<<"Finished."<<std::endl;
}

template <typename Dtype>
void SpatialTransformerLayer<Dtype>::transform_backward_cpu(Dtype dV, const Dtype* U, const Dtype px,
		const Dtype py, Dtype* dU, Dtype& dpx, Dtype& dpy) {

	bool debug = false;

	string prefix = "\t\tSpatial Transformer Layer:: transform_backward_cpu: \t";

	if(debug) std::cout<<prefix<<"Starting!"<<std::endl;

	Dtype x = (px + 1) / 2 * H; Dtype y = (py + 1) / 2 * W;
	if(debug) std::cout<<prefix<<"(x, y) = ("<<x<<", "<<y<<")"<<std::endl;

	int m, n; Dtype w;

	m = floor(x); n = floor(y); w = 0;
	if(debug) std::cout<<prefix<<"(m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));

		dU[m * W + n] += w * dV;

		if(abs(x - m) < 1) {
			if(m >= x) {
				dpx += max(0, 1 - abs(y - n)) * U[m * W + n] * dV * H / 2;
				if(debug) std::cout<<prefix<<"dpx += "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			} else {
				dpx -= max(0, 1 - abs(y - n)) * U[m * W + n] * dV * H / 2;
				if(debug) std::cout<<prefix<<"dpx -= "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			}
		}

		if(abs(y - n) < 1) {
			if(n >= y) {
				dpy += max(0, 1 - abs(x - m)) * U[m * W + n] * dV * W / 2;
				if(debug) std::cout<<prefix<<"dpy += "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			} else {
				dpy -= max(0, 1 - abs(x - m)) * U[m * W + n] * dV * W / 2;
				if(debug) std::cout<<prefix<<"dpy -= "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			}
		}
	}

	m = floor(x) + 1; n = floor(y); w = 0;
	if(debug) std::cout<<prefix<<"(m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));

		dU[m * W + n] += w * dV;

		if(abs(x - m) < 1) {
			if(m >= x) {
				dpx += max(0, 1 - abs(y - n)) * U[m * W + n] * dV * H / 2;
				if(debug) std::cout<<prefix<<"dpx += "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			} else {
				dpx -= max(0, 1 - abs(y - n)) * U[m * W + n] * dV * H / 2;
				if(debug) std::cout<<prefix<<"dpx -= "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			}
		}

		if(abs(y - n) < 1) {
			if(n >= y) {
				dpy += max(0, 1 - abs(x - m)) * U[m * W + n] * dV * W / 2;
				if(debug) std::cout<<prefix<<"dpy += "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			} else {
				dpy -= max(0, 1 - abs(x - m)) * U[m * W + n] * dV * W / 2;
				if(debug) std::cout<<prefix<<"dpy -= "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			}
		}
	}

	m = floor(x); n = floor(y) + 1; w = 0;
	if(debug) std::cout<<prefix<<"(m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));

		dU[m * W + n] += w * dV;

		if(abs(x - m) < 1) {
			if(m >= x) {
				dpx += max(0, 1 - abs(y - n)) * U[m * W + n] * dV * H / 2;
				if(debug) std::cout<<prefix<<"dpx += "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			} else {
				dpx -= max(0, 1 - abs(y - n)) * U[m * W + n] * dV * H / 2;
				if(debug) std::cout<<prefix<<"dpx -= "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			}
		}

		if(abs(y - n) < 1) {
			if(n >= y) {
				dpy += max(0, 1 - abs(x - m)) * U[m * W + n] * dV * W / 2;
				if(debug) std::cout<<prefix<<"dpy += "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			} else {
				dpy -= max(0, 1 - abs(x - m)) * U[m * W + n] * dV * W / 2;
				if(debug) std::cout<<prefix<<"dpy -= "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			}
		}
	}

	m = floor(x) + 1; n = floor(y) + 1; w = 0;
	if(debug) std::cout<<prefix<<"(m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));

		dU[m * W + n] += w * dV;

		if(abs(x - m) < 1) {
			if(m >= x) {
				dpx += max(0, 1 - abs(y - n)) * U[m * W + n] * dV * H / 2;
				if(debug) std::cout<<prefix<<"dpx += "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			} else {
				dpx -= max(0, 1 - abs(y - n)) * U[m * W + n] * dV * H / 2;
				if(debug) std::cout<<prefix<<"dpx -= "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			}
		}

		if(abs(y - n) < 1) {
			if(n >= y) {
				dpy += max(0, 1 - abs(x - m)) * U[m * W + n] * dV * W / 2;
				if(debug) std::cout<<prefix<<"dpy += "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			} else {
				dpy -= max(0, 1 - abs(x - m)) * U[m * W + n] * dV * W / 2;
				if(debug) std::cout<<prefix<<"dpy -= "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			}
		}
	}

	if(debug) std::cout<<prefix<<"Finished."<<std::endl;
}

template <typename Dtype>
void SpatialTransformerLayer<Dtype>::Backward_cpu(const vector<Blob<Dtype>*>& top,
    const vector<bool>& propagate_down,
    const vector<Blob<Dtype>*>& bottom) {

		string prefix = "\t\tSpatial Transformer Layer:: Backward_cpu: \t";

		// CHECK(false) << "Don't use the CPU implementation! If you really want to, delete the" <<
		//		" CHECK in st_layer.cpp file. Line number: 420-421." << std::endl;

		if(global_debug) std::cout<<prefix<<"Starting!"<<std::endl;

		const Dtype* dV = top[0]->cpu_diff();
		const Dtype* input_grid_data = input_grid.cpu_data();
		const Dtype* U = bottom[0]->cpu_data();

		Dtype* dU = bottom[0]->mutable_cpu_diff();
		Dtype* dTheta = bottom[1]->mutable_cpu_diff();
		Dtype* input_grid_diff = input_grid.mutable_cpu_diff();

		caffe_set(bottom[0]->count(), (Dtype)0, dU);
		caffe_set(bottom[1]->count(), (Dtype)0, dTheta);
		caffe_set(input_grid.count(), (Dtype)0, input_grid_diff);

		for(int i = 0; i < N; ++i) {

			const Dtype* coordinates = input_grid_data + (output_H_ * output_W_ * 2) * i;
			Dtype* coordinates_diff = input_grid_diff + (output_H_ * output_W_ * 2) * i;

			int row_idx; Dtype px, py, dpx, dpy, delta_dpx, delta_dpy;

			for(int s = 0; s < output_H_; ++s)
				for(int t = 0; t < output_W_; ++t) {

					row_idx = output_W_ * s + t;

					px = coordinates[row_idx * 2];
					py = coordinates[row_idx * 2 + 1];

					for(int j = 0; j < C; ++j) {

						delta_dpx = delta_dpy = (Dtype)0.;

						transform_backward_cpu(dV[top[0]->offset(i, j, s, t)], U + bottom[0]->offset(i, j, 0, 0),
								px, py, dU + bottom[0]->offset(i, j, 0, 0), delta_dpx, delta_dpy);

						coordinates_diff[row_idx * 2] += delta_dpx;
						coordinates_diff[row_idx * 2 + 1] += delta_dpy;
					}

					dpx = coordinates_diff[row_idx * 2];
					dpy = coordinates_diff[row_idx * 2 + 1];

					dTheta[6 * i] += dpx * (s * 1.0 / output_H_ * 2 - 1);
					dTheta[6 * i + 1] += dpx * (t * 1.0 / output_W_ * 2 - 1);
					dTheta[6 * i + 2] += dpx;
					dTheta[6 * i + 3] += dpy * (s * 1.0 / output_H_ * 2 - 1);
					dTheta[6 * i + 4] += dpy * (t * 1.0 / output_W_ * 2 - 1);
					dTheta[6 * i + 5] += dpy;
				}
		}

		if(global_debug) std::cout<<prefix<<"Finished."<<std::endl;
}

#ifdef CPU_ONLY
STUB_GPU(SpatialTransformerLayer);
#endif

INSTANTIATE_CLASS(SpatialTransformerLayer);
REGISTER_LAYER_CLASS(SpatialTransformer);

}  // namespace caffe


#include "cuNDFFT.h"
#include "cuNDArray_math.h"
#include "complext.h"
#include <gtest/gtest.h>
#include <boost/random.hpp>

using namespace Gadgetron;
using testing::Types;

template<typename REAL> class cuNDFFT_test : public ::testing::Test {
protected:
	virtual void SetUp(){
		boost::random::mt19937 rng;
		boost::random::uniform_real_distribution<REAL> uni(0,1);
		std::vector<size_t > dimensions(3,128);

		hoNDArray<complext<REAL> > tmp(dimensions);
		complext<REAL>* data = tmp.get_data_ptr();

		for (size_t i = 0; i < tmp.get_number_of_elements(); i++)
			data[i] = complext<REAL>(uni(rng),uni(rng));

		Array = cuNDArray<complext<REAL> >(tmp);
		Array2 = Array;
	}

	cuNDArray<complext<REAL> > Array;

	cuNDArray<complext<REAL> > Array2;

};
typedef Types<float, double> realImplementations;
TYPED_TEST_SUITE(cuNDFFT_test, realImplementations);

TYPED_TEST(cuNDFFT_test,fftNrm2Test){
	cuNDFFT<TypeParam>::instance()->fft(&this->Array);

	EXPECT_NEAR(nrm2(&this->Array2),nrm2(&this->Array),nrm2(&this->Array)*1e-3);

}

TYPED_TEST(cuNDFFT_test,ifftNrm2Test){
	cuNDFFT<TypeParam>::instance()->ifft(&this->Array);

	EXPECT_NEAR(nrm2(&this->Array2),nrm2(&this->Array),nrm2(&this->Array)*1e-3);

}