#include <sstream>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

#include "Scalar.h"

template <typename Archive>
void Scalar::serialize(Archive & ar, unsigned int const version) {
  if (version > 0) {
  } else {
  }
  ar & variables;
  ar & coefficient;
}

template void Scalar::serialize<boost::archive::text_oarchive>(boost::archive::text_oarchive&, unsigned int const);
template void Scalar::serialize<boost::archive::text_iarchive>(boost::archive::text_iarchive&, unsigned int const);

Scalar::Scalar() : variables(), coefficient(1,1) { }
Scalar::Scalar(Scalar const & other) : variables(other.variables), coefficient(other.coefficient) { }

Scalar::Scalar(Rational const & coefficient_set) : variables(), coefficient(coefficient_set) { }
Scalar::Scalar(size_t variable_set) : variables(), coefficient(1,1) { variables.insert(variable_set); }
Scalar::Scalar(Rational coefficient_set, size_t variable_set) : variables(), coefficient(coefficient_set) { variables.insert(variable_set); }

Rational Scalar::get_coefficient() const { return coefficient; }
std::multiset<size_t> Scalar::get_variables() const { return variables; }

size_t Scalar::Order() const { return (IsZero() ? 0 : variables.size()); }

size_t Scalar::VariableNumber() const { assert(variables.size() == 1); return *(variables.begin()); }

bool Scalar::ContainsVariable(size_t const variable) const { return (std::find(variables.begin(), variables.end(), variable) != variables.end()); }

size_t Scalar::RemoveVariable(size_t const variable) {
  size_t old_size = variables.size();
  auto it = std::find(variables.begin(), variables.end(), variable);
  while (it != variables.end()) {
    variables.erase(it);
    it = std::find(variables.begin(), variables.end(), variable);
  }

  return old_size - variables.size();
}

void Scalar::AddOther(Scalar const & other) { assert(CompareVariables(other)); coefficient = coefficient.AddOther(other.coefficient); }
bool Scalar::CompareVariables(Scalar const & other) const { return variables == other.variables; }
bool Scalar::IsZero() const { return coefficient.IsZero(); }

void Scalar::MultiplyCoefficient(Rational const & coeff) { coefficient = coefficient.MultiplyOther(coeff); }
void Scalar::DivideCoefficient(Rational const & coeff) { coefficient = coefficient.DivideOther(coeff); }

void Scalar::DivideByTwo() { coefficient.DivideByTwo(); }
void Scalar::Negate() { coefficient.Negate(); }

void Scalar::SubstituteVariables (std::map<size_t, size_t> const & substitution_map) {
  std::multiset<size_t> variables_new;
  std::for_each(variables.begin(), variables.end(),
    [&substitution_map,&variables_new] (auto & a) mutable {
      auto find_it = substitution_map.find(a);
      if (find_it != substitution_map.end()) {
        variables_new.insert(find_it->second);
      } else {
        variables_new.insert(a);
      }
    });

  std::swap(variables_new, variables);
}

Scalar Scalar::MultiplyOther(Scalar const & other) const {
  Scalar ret (*this);
  ret.MultiplyCoefficient(other.coefficient);
  std::for_each(other.variables.begin(), other.variables.end(), [&ret] (auto const & a) { ret.variables.insert(a); });
  return ret;
}

std::string Scalar::ToString(std::string base_name, bool plus_sign) const {
  std::stringstream ss;
  ss << coefficient.ToString(plus_sign);

  if (variables.empty() || IsZero()) {
    return ss.str();
  }

  for (auto variable : variables) {
    ss << "*" << base_name << variable;
  }

  return ss.str();
}

bool Scalar::operator< (Scalar const & other) const {
  if (variables < other.variables) {
    return true;
  } else if (variables > other.variables) {
    return false;
  } else {
    return coefficient < other.coefficient;
  }
}

bool Scalar::operator== (Scalar const & other) const {
  return ((variables == other.variables) && (coefficient == other.coefficient)) || (IsZero() && other.IsZero());
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@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 TestInplaceReduce
#include <boost/test/unit_test.hpp>

#include <iostream>

#include <boost/compute/system.hpp>
#include <boost/compute/functional.hpp>
#include <boost/compute/algorithm/iota.hpp>
#include <boost/compute/algorithm/detail/inplace_reduce.hpp>
#include <boost/compute/container/vector.hpp>

#include "quirks.hpp"
#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(sum_int)
{
    if(is_apple_cpu_device(device)) {
        std::cerr
            << "skipping all inplace_reduce tests due to Apple platform"
            << " behavior when local memory is used on a CPU device"
            << std::endl;
        return;
    }

    int data[] = { 1, 5, 3, 4, 9, 3, 5, 3 };
    boost::compute::vector<int> vector(data, data + 8, queue);

    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(33));

    vector.assign(data, data + 8);
    vector.push_back(3);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(36));
}

BOOST_AUTO_TEST_CASE(multiply_int)
{
    if(is_apple_cpu_device(device)) {
        return;
    }

    int data[] = { 1, 5, 3, 4, 9, 3, 5, 3 };
    boost::compute::vector<int> vector(data, data + 8, queue);

    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::multiplies<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(24300));

    vector.assign(data, data + 8);
    vector.push_back(3);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::multiplies<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(72900));
}

BOOST_AUTO_TEST_CASE(reduce_iota)
{
    if(is_apple_cpu_device(device)) {
        return;
    }

    // 1 value
    boost::compute::vector<int> vector(1, context);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(0));

    // 1000 values
    vector.resize(1000);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(499500));

    // 2499 values
    vector.resize(2499);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(3121251));

    // 4096 values
    vector.resize(4096);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(8386560));

    // 5000 values
    vector.resize(5000);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(12497500));
}

BOOST_AUTO_TEST_SUITE_END()


//
// test-estimators.cc
//
//  Created by Peter Gusev on 19 April 2016.
//  Copyright 2013-2016 Regents of the University of California
//

#include <stdlib.h>

#include <boost/thread.hpp>
#include <boost/chrono.hpp>
#include <boost/make_shared.hpp>
#include <boost/assign.hpp>

#include "gtest/gtest.h"
#include "src/estimators.hpp"
#include "client/src/precise-generator.hpp"

using namespace ndnrtc::estimators;

TEST(TestEstimatorWindow, TestSampleWindow)
{
	SampleWindow w(10);
	unsigned int nReached = 0, nNotReached = 0;

	for (int i = 0; i < 100; ++i)
		if (w.isLimitReached())
			nReached++;
		else
			nNotReached++;
	EXPECT_EQ(10, nReached);
	EXPECT_EQ(90, nNotReached);
}

TEST(TestEstimatorWindow, TestTimeWindow)
{
	unsigned int interval = 10;
	unsigned int nReached = 0, nNotReached = 0;

	boost::chrono::high_resolution_clock::time_point t1 = boost::chrono::high_resolution_clock::now();
	boost::thread t([interval, &nReached, &nNotReached](){
		TimeWindow w(100);
		for (int i = 0; i < 100; ++i)
		{
			if (w.isLimitReached()) nReached++;
			else nNotReached++;

			boost::this_thread::sleep_for(boost::chrono::milliseconds(interval));
		}
	});

	t.join();
	boost::chrono::high_resolution_clock::time_point t2 = boost::chrono::high_resolution_clock::now();
	auto duration = boost::chrono::duration_cast<boost::chrono::milliseconds>( t2 - t1 ).count();

	EXPECT_EQ(duration/100, nReached);
	EXPECT_EQ(100-duration/100, nNotReached);
}

TEST(TestSlidingAverage, TestSamplesWindow)
{
	Average avg(boost::make_shared<SampleWindow>(10));

	double v = 0;
	int d = 1;
	for (int i = 1; i <= 100; ++i)
	{
		avg.newValue(v);
		if (i%10 == 0) d = -d;
		else v+=d;
	}

	EXPECT_EQ(4.5, avg.value());
	EXPECT_EQ(8.25, avg.variance());
	EXPECT_LT(2.87 - avg.deviation(), 0.01);
}

TEST(TestSlidingAverage, TestTimeWindow)
{
	Average avg(boost::make_shared<TimeWindow>(500));
	boost::thread t([&avg](){
		double v = 0;
		int d = 1;
		for (int i = 1; i <= 100; ++i)
		{
			avg.newValue(v);
			if (i%10 == 0) d = -d;
			else v+=d;
			boost::this_thread::sleep_for(boost::chrono::milliseconds(10));
		}
	});
	t.join();

	EXPECT_LT(abs(4.5 - avg.value()), 1) ;
	EXPECT_LT(abs(8.25-avg.variance()), 1);
	EXPECT_LT(2.87 - avg.deviation(), 0.5);
}

TEST(TestSlidingAverage, TestEdgeValues)
{
    Average avg(boost::make_shared<SampleWindow>(10));

    EXPECT_EQ(0, avg.oldestValue());
    EXPECT_EQ(0, avg.latestValue());

    double v = 0;
    int d = 1;
    for (int i = 1; i <= 100; ++i)
    {
        avg.newValue(v);
        if (i%10 == 0) d = -d;
        else v+=d;
    }

    EXPECT_EQ(4.5, avg.value());
    EXPECT_EQ(8.25, avg.variance());
    EXPECT_LT(2.87 - avg.deviation(), 0.01);
}

TEST(TestFrequencyMeter, TestTimeWindow)
{
	boost::asio::io_service io;
	boost::shared_ptr<boost::asio::io_service::work> work(new boost::asio::io_service::work(io));
	boost::thread t([&io](){
		io.run();
	});

    // smaller windows give less precise results
	FreqMeter freqMeter(boost::make_shared<TimeWindow>(500));
    double rate = 30;
	PreciseGenerator p(io, rate, [&freqMeter](){
		freqMeter.newValue(0);
	});

	p.start();
	boost::this_thread::sleep_for(boost::chrono::milliseconds(3000));
	p.stop();
	work.reset();
	t.join();

	EXPECT_LT(abs(rate-freqMeter.value())/rate, 0.15);
}

TEST(TestFrequencyMeter, TestSampleWindow)
{
	boost::asio::io_service io;
	boost::shared_ptr<boost::asio::io_service::work> work(new boost::asio::io_service::work(io));
	boost::thread t([&io](){
		io.run();
	});

    // smaller windows give less precise results
	FreqMeter freqMeter(boost::make_shared<SampleWindow>(10));
    double rate = 30;
	PreciseGenerator p(io, rate, [&freqMeter](){
		freqMeter.newValue(0);
	});

	p.start();
	boost::this_thread::sleep_for(boost::chrono::milliseconds(3000));
	p.stop();
	work.reset();
	t.join();

	EXPECT_LT(abs(rate-freqMeter.value())/rate, 0.15);
}

TEST(TestFilter, TestSmoothing)
{
	Filter f(0.05);
	std::vector<double> values = boost::assign::list_of (5.) (6.) (7.) (5.) (10.) (6.) (5.5) (6.1);
	for (auto v:values) f.newValue(v);
	EXPECT_LT(5.5-f.value(), 0.5);
}

int main(int argc, char **argv) {
	::testing::InitGoogleTest(&argc, argv);
	return RUN_ALL_TESTS();
}


//
//  Copyright (c) 2018-2020, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019-2020, Amit Singh, amitsingh19975@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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <random>
#include <boost/numeric/ublas/tensor.hpp>

#include <boost/test/unit_test.hpp>
#include "utility.hpp"

BOOST_AUTO_TEST_SUITE ( test_tensor_static_rank )

using test_types = zip<int,float,std::complex<float>>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

BOOST_AUTO_TEST_CASE_TEMPLATE( test_tensor_ctor, value,  test_types)
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  auto a2 = ublas::tensor_static_rank<value_t, 2,layout_t>{1,1};
  BOOST_CHECK_EQUAL(  a2.size() , 1 );
  BOOST_CHECK( !a2.empty() );
  BOOST_CHECK_NE(  a2.data() , nullptr);

  auto a3 = ublas::tensor_static_rank<value_t, 2,layout_t>{2,1};
  BOOST_CHECK_EQUAL(  a3.size() , 2 );
  BOOST_CHECK( !a3.empty() );
  BOOST_CHECK_NE(  a3.data() , nullptr);

  auto a4 = ublas::tensor_static_rank<value_t, 2,layout_t>{1,2};
  BOOST_CHECK_EQUAL(  a4.size() , 2 );
  BOOST_CHECK( !a4.empty() );
  BOOST_CHECK_NE(  a4.data() , nullptr);

  auto a5 = ublas::tensor_static_rank<value_t, 2,layout_t>{2,1};
  BOOST_CHECK_EQUAL(  a5.size() , 2 );
  BOOST_CHECK( !a5.empty() );
  BOOST_CHECK_NE(  a5.data() , nullptr);

  auto a6 = ublas::tensor_static_rank<value_t, 3,layout_t>{4,3,2};
  BOOST_CHECK_EQUAL(  a6.size() , 4*3*2 );
  BOOST_CHECK( !a6.empty() );
  BOOST_CHECK_NE(  a6.data() , nullptr);

  auto a7 = ublas::tensor_static_rank<value_t, 3,layout_t>{4,1,2};
  BOOST_CHECK_EQUAL(  a7.size() , 4*1*2 );
  BOOST_CHECK( !a7.empty() );
  BOOST_CHECK_NE(  a7.data() , nullptr);

}

struct fixture
{
  template<size_t N>
  using extents_t = boost::numeric::ublas::extents<N>;

  std::tuple<
    extents_t<2>, // 1
    extents_t<2>, // 2
    extents_t<3>, // 3
    extents_t<3>, // 4
    extents_t<4>  // 5
    > extents = {
        extents_t<2>{1,1},
        extents_t<2>{2,3},
        extents_t<3>{4,1,3},
      extents_t<3>{4,2,3},
      extents_t<4>{4,2,3,5}
  };
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_ctor_extents, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  for_each_in_tuple(extents, [](auto const& /*unused*/, auto& e){
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    auto t = ublas::tensor_static_rank<value_t, size, layout_t>{e};

    BOOST_CHECK_EQUAL (  t.size() , ublas::product(e) );
    BOOST_CHECK_EQUAL (  t.rank() , ublas::size   (e) );
    if(ublas::empty(e)) {
      BOOST_CHECK       ( t.empty()    );
      BOOST_CHECK_EQUAL ( t.data() , nullptr);
    }
    else{
      BOOST_CHECK       ( !t.empty()    );
      BOOST_CHECK_NE    (  t.data() , nullptr);
    }
  });

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_copy_ctor, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  for_each_in_tuple(extents, [](auto const& /*unused*/, auto& e){
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    auto r = ublas::tensor_static_rank<value_t, size, layout_t>{e};

    auto t = r;
    BOOST_CHECK_EQUAL (  t.size() , r.size() );
    BOOST_CHECK_EQUAL (  t.rank() , r.rank() );
    //    BOOST_CHECK ( t.strides() == r.strides() );
    BOOST_CHECK ( t.extents() == r.extents() );

    if(ublas::empty(e)) {
      BOOST_CHECK       ( t.empty()    );
      BOOST_CHECK_EQUAL ( t.data() , nullptr);
    }
    else{
      BOOST_CHECK       ( !t.empty()    );
      BOOST_CHECK_NE    (  t.data() , nullptr);
    }

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL( t[i], r[i]  );

  });
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_copy_ctor_layout, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;
  using other_layout_t = std::conditional_t<std::is_same<ublas::layout::first_order,layout_t>::value, ublas::layout::last_order, ublas::layout::first_order>;

  for_each_in_tuple(extents, [](auto const& /*unused*/, auto& e){
    constexpr auto size     = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_t       = ublas::tensor_static_rank<value_t, size, layout_t>;
    using other_tensor_t = ublas::tensor_static_rank<value_t, size, other_layout_t>;
    auto r = tensor_t(e);
    other_tensor_t t = r;
    tensor_t q = t;

    BOOST_CHECK_EQUAL (  t.size() , r.size() );
    BOOST_CHECK_EQUAL (  t.rank() , r.rank() );
    BOOST_CHECK ( t.extents() == r.extents() );

    BOOST_CHECK_EQUAL (  q.size() , r.size() );
    BOOST_CHECK_EQUAL (  q.rank() , r.rank() );
    //    BOOST_CHECK ( q.strides() == r.strides() );
    BOOST_CHECK ( q.extents() == r.extents() );

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL( q[i], r[i]  );

  });
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_copy_move_ctor, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  auto check = [](auto const& /*unused*/, auto& e)
  {
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;
    auto r = tensor_t{e};
    auto t = std::move(r);
    BOOST_CHECK_EQUAL (  t.size() , ublas::product(e) );
    BOOST_CHECK_EQUAL (  t.rank() , ublas::size   (e) );

    if(ublas::empty(e)) {
      BOOST_CHECK       ( t.empty()    );
      BOOST_CHECK_EQUAL ( t.data() , nullptr);
    }
    else{
      BOOST_CHECK       ( !t.empty()    );
      BOOST_CHECK_NE    (  t.data() , nullptr);
    }

  };

  for_each_in_tuple(extents,check);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_ctor_extents_init, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  std::random_device device{};
  std::minstd_rand0 generator(device());

  using distribution_type = std::conditional_t<std::is_integral_v<value_t>, std::uniform_int_distribution<>, std::uniform_real_distribution<> >;
  auto distribution = distribution_type(1,6);

  for_each_in_tuple(extents, [&](auto const& /*unused*/, auto const& e){
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;

    auto r = value_t( static_cast< inner_type_t<value_t> >(distribution(generator)) );
    auto t = tensor_t(e);
    t = r;
    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL( t[i], r );

  });
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_ctor_extents_array, value,  test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  for_each_in_tuple(extents, [](auto const& /*unused*/, auto& e){
    constexpr auto size  = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_t    = ublas::tensor_static_rank<value_t, size, layout_t>;
    using container_t = typename tensor_t::container_type;

    auto a = container_t(product(e));
    auto v = value_t {};

    for(auto& aa : a){
      aa = v;
      v += value_t{1};
    }
    auto t = tensor_t(e, a);
    v = value_t{};

    for(auto i = 0ul; i < t.size(); ++i, v+=value_t{1})
      BOOST_CHECK_EQUAL( t[i], v);

  });
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_read_write_single_index_access, value,  test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  for_each_in_tuple(extents, [](auto const& /*unused*/, auto& e){
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;

    auto t = tensor_t{e};
    auto v = value_t {};
    for(auto i = 0ul; i < t.size(); ++i, v+=value_t{1}){
      t[i] = v;
      BOOST_CHECK_EQUAL( t[i], v );

      t(i) = v;
      BOOST_CHECK_EQUAL( t(i), v );
    }

  });
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_read_write_multi_index_access_at, value,  test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  auto check1 = [](const auto& t)
  {
    auto v = value_t{};
    for(auto k = 0ul; k < t.size(); ++k){
      BOOST_CHECK_EQUAL(t[k], v);
      v+=value_t{1};
    }
  };

  auto check2 = [](const auto& t)
  {
    std::array<unsigned,2> k = {0,0};
    auto r = std::is_same<layout_t,ublas::layout::first_order>::value ? 1 : 0;
    auto q = std::is_same<layout_t,ublas::layout::last_order >::value ? 1 : 0;
    auto v = value_t{};
    for(k[r] = 0ul; k[r] < t.size(r); ++k[r]){
      for(k[q] = 0ul; k[q] < t.size(q); ++k[q]){
        BOOST_CHECK_EQUAL(t.at(k[0],k[1]), v);
        v+=value_t{1};
      }
    }
  };

  auto check3 = [](const auto& t)
  {
    std::array<unsigned,3> k = {0,0,0};
    using op_type = std::conditional_t<std::is_same_v<layout_t,ublas::layout::first_order>, std::minus<>, std::plus<>>;
    auto r = std::is_same_v<layout_t,ublas::layout::first_order> ? 2 : 0;
    auto o = op_type{};
    auto v = value_t{};
    for(k[r] = 0ul; k[r] < t.size(r); ++k[r]){
      for(k[o(r,1)] = 0ul; k[o(r,1)] < t.size(o(r,1)); ++k[o(r,1)]){
        for(k[o(r,2)] = 0ul; k[o(r,2)] < t.size(o(r,2)); ++k[o(r,2)]){
          BOOST_CHECK_EQUAL(t.at(k[0],k[1],k[2]), v);
          v+=value_t{1};
        }
      }
    }
  };

  auto check4 = [](const auto& t)
  {
    static constexpr auto order = 4;
    std::array<unsigned,order> k = {0,0,0,0};
    using op_type = std::conditional_t<std::is_same_v<layout_t,ublas::layout::first_order>, std::minus<>, std::plus<>>;
    auto r = std::is_same_v<layout_t,ublas::layout::first_order> ? (order-1) : 0;
    auto o = op_type{};
    auto v = value_t{};
    for(k[o(r,0)] = 0ul; k[o(r,0)] < t.size(o(r,0)); ++k[o(r,0)]){
      for(k[o(r,1)] = 0ul; k[o(r,1)] < t.size(o(r,1)); ++k[o(r,1)]){
        for(k[o(r,2)] = 0ul; k[o(r,2)] < t.size(o(r,2)); ++k[o(r,2)]){
          for(k[o(r,3)] = 0ul; k[o(r,3)] < t.size(o(r,3)); ++k[o(r,3)]){
            BOOST_CHECK_EQUAL(t.at(k[0],k[1],k[2],k[3]), v);
            v+=value_t{1};
          }
        }
      }
    }
  };

  auto check = [check1,check2,check3,check4](auto const& /*unused*/, auto const& e) {
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;
    auto t = tensor_t(e);
    auto v = value_t {};
    for(auto i = 0ul; i < t.size(); ++i){
      t[i] = v;
      v+=value_t{1};
    }

    if constexpr(size == 1) check1(t);
    else if constexpr(size == 2) check2(t);
    else if constexpr(size == 3) check3(t);
    else if constexpr(size == 4) check4(t);

  };

  for_each_in_tuple(extents,check);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_reshape, value,  test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  for_each_in_tuple(extents,[&](auto const& /*unused*/, auto const& efrom){

    using efrom_t = std::decay_t<decltype(efrom)>;
    using tensor_t = ublas::tensor_static_rank<value_t, std::tuple_size_v<efrom_t>, layout_t>;

    for_each_in_tuple(extents,[&](auto const& /*unused*/, auto& eto){
      using eto_t = std::decay_t<decltype(eto)>;

      if constexpr( std::tuple_size_v<efrom_t> == std::tuple_size_v<eto_t>  ){

        auto v = value_t {};
        v+=value_t{1};
        auto t = tensor_t(efrom);
        t = v;
        for(auto i = 0ul; i < t.size(); ++i)
          BOOST_CHECK_EQUAL( t[i], v );

        auto t2 = reshape(t,eto);
        for(auto i = 0ul; i < std::min(ublas::product(efrom),ublas::product(eto)); ++i)
          BOOST_CHECK_EQUAL( t2[i], v );

        BOOST_CHECK_EQUAL (  t2.size() , ublas::product(eto) );
        BOOST_CHECK_EQUAL (  t2.rank() , ublas::size   (eto) );
        BOOST_CHECK       ( t2.extents() == eto );

        if(efrom != eto){
          for(auto i = product(efrom); i < t.size(); ++i)
            BOOST_CHECK_EQUAL( t2[i], value_t{} );
        }
      }
    });
  });
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_swap, value,  test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  for_each_in_tuple(extents,[&](auto const& /*unused*/, auto const& e_t){
    using e_tt = std::decay_t<  decltype(e_t)  >;
    using tensor_t = ublas::tensor_static_rank<value_t, std::tuple_size_v<e_tt>, layout_t>;

    for_each_in_tuple(extents,[&](auto const& /*unused*/, auto& e_r){

      using e_rt = std::decay_t<  decltype(e_r)  >;

      if constexpr( std::tuple_size_v<e_tt> == std::tuple_size_v<e_rt> ){

        auto v = value_t {} + value_t{1};
        auto w = value_t {} + value_t{2};
        auto t = tensor_t(e_t);
        auto r = tensor_t(e_r);

        t = v;
        r = w;

        std::swap( r, t );

        BOOST_CHECK ( std::all_of(t.begin(),t.end(),[w](auto tt){return tt == w; } ) ) ;
        BOOST_CHECK_EQUAL (  t.size() , ublas::product(e_r) );
        BOOST_CHECK_EQUAL (  t.rank() , ublas::size   (e_r) );
        BOOST_CHECK ( t.extents() == e_r );
        BOOST_CHECK ( t.strides() == ublas::to_strides(e_r,layout_t{}) );

        BOOST_CHECK ( std::all_of(r.begin(),r.end(),[v](auto tt){return tt == v; } ) ) ;
        BOOST_CHECK_EQUAL (  r.size() , ublas::product(e_t) );
        BOOST_CHECK_EQUAL (  r.rank() , ublas::size   (e_t) );
        BOOST_CHECK ( r.extents() == e_t );
        BOOST_CHECK ( r.strides() == ublas::to_strides(e_t,layout_t{}) );

      }
    });
  });
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_standard_iterator, value,  test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  for_each_in_tuple(extents,[](auto const& /*unused*/, auto const& e){
    using et = std::decay_t<  decltype(e)  >;
    using tensor_t = ublas::tensor_static_rank<value_t, std::tuple_size_v<et>, layout_t>;

    auto v = value_t {} + value_t{1};
    auto t = tensor_t(e);
    t = v;

    BOOST_CHECK_EQUAL( std::distance(t.begin(),  t.end ()), t.size()  );
    BOOST_CHECK_EQUAL( std::distance(t.rbegin(), t.rend()), t.size()  );

    BOOST_CHECK_EQUAL( std::distance(t.cbegin(),  t.cend ()), t.size() );
    BOOST_CHECK_EQUAL( std::distance(t.crbegin(), t.crend()), t.size() );

    if(!t.empty()) {
      BOOST_CHECK(  t.data() ==  std::addressof( *t.begin () )  ) ;
      BOOST_CHECK(  t.data() ==  std::addressof( *t.cbegin() )  ) ;
    }
  });

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_throw, value, test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;
  using tensor_t = ublas::tensor_static_rank<value_t, 2, layout_t>;

  auto vec = std::vector<value_t>(1);
  BOOST_CHECK_THROW(tensor_t({5,5},vec), std::length_error);

  // Does not throw but results in a static assertion
//  auto t = tensor_t{{5,5}};
//  auto i = ublas::index::index_type<4>{};
//  BOOST_CHECK_THROW((void)t.operator()(i,i,i), std::runtime_error);

}

BOOST_AUTO_TEST_SUITE_END()


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// 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/config.hpp>
#include <algorithm>
#include <utility>
#include <boost/graph/edmonds_karp_max_flow.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>

namespace boost
{
  template < typename Graph >
    std::pair < typename graph_traits < Graph >::vertex_descriptor,
    typename graph_traits < Graph >::degree_size_type >
    min_degree_vertex(Graph & g)
  {
    typename graph_traits < Graph >::vertex_descriptor p;
    typedef typename graph_traits < Graph >::degree_size_type size_type;
    size_type delta = (std::numeric_limits < size_type >::max)();
    typename graph_traits < Graph >::vertex_iterator i, iend;
    for (tie(i, iend) = vertices(g); i != iend; ++i)
      if (degree(*i, g) < delta)
      {
        delta = degree(*i, g);
        p = *i;
      }
    return std::make_pair(p, delta);
  }

  template < typename Graph, typename OutputIterator >
    void neighbors(const Graph & g,
                   typename graph_traits < Graph >::vertex_descriptor u,
                   OutputIterator result)
  {
    typename graph_traits < Graph >::adjacency_iterator ai, aend;
    for (tie(ai, aend) = adjacent_vertices(u, g); ai != aend; ++ai)
      *result++ = *ai;
  }
  template < typename Graph, typename VertexIterator,
    typename OutputIterator > void neighbors(const Graph & g,
                                             VertexIterator first,
                                             VertexIterator last,
                                             OutputIterator result)
  {
    for (; first != last; ++first)
      neighbors(g, *first, result);
  }

  template < typename VertexListGraph, typename OutputIterator >
  typename graph_traits < VertexListGraph >::degree_size_type
  edge_connectivity(VertexListGraph & g, OutputIterator disconnecting_set)
  {
    typedef typename graph_traits <
      VertexListGraph >::vertex_descriptor vertex_descriptor;
    typedef typename graph_traits <
      VertexListGraph >::degree_size_type degree_size_type;
    typedef color_traits < default_color_type > Color;
    typedef typename adjacency_list_traits < vecS, vecS,
      directedS >::edge_descriptor edge_descriptor;
    typedef adjacency_list < vecS, vecS, directedS, no_property,
      property < edge_capacity_t, degree_size_type,
      property < edge_residual_capacity_t, degree_size_type,
      property < edge_reverse_t, edge_descriptor > > > > FlowGraph;

    vertex_descriptor u, v, p, k;
    edge_descriptor e1, e2;
    bool inserted;
    typename graph_traits < VertexListGraph >::vertex_iterator vi, vi_end;
    degree_size_type delta, alpha_star, alpha_S_k;
    std::set < vertex_descriptor > S, neighbor_S;
    std::vector < vertex_descriptor > S_star, nonneighbor_S;
    std::vector < default_color_type > color(num_vertices(g));
    std::vector < edge_descriptor > pred(num_vertices(g));

    FlowGraph flow_g(num_vertices(g));
    typename property_map < FlowGraph, edge_capacity_t >::type
      cap = get(edge_capacity, flow_g);
    typename property_map < FlowGraph, edge_residual_capacity_t >::type
      res_cap = get(edge_residual_capacity, flow_g);
    typename property_map < FlowGraph, edge_reverse_t >::type
      rev_edge = get(edge_reverse, flow_g);

    typename graph_traits < VertexListGraph >::edge_iterator ei, ei_end;
    for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
      u = source(*ei, g), v = target(*ei, g);
      tie(e1, inserted) = add_edge(u, v, flow_g);
      cap[e1] = 1;
      tie(e2, inserted) = add_edge(v, u, flow_g);
      cap[e2] = 1;
      rev_edge[e1] = e2;
      rev_edge[e2] = e1;
    }

    tie(p, delta) = min_degree_vertex(g);
    S_star.push_back(p);
    alpha_star = delta;
    S.insert(p);
    neighbor_S.insert(p);
    neighbors(g, S.begin(), S.end(),
              std::inserter(neighbor_S, neighbor_S.begin()));
    std::set_difference(vertices(g).first, vertices(g).second,
                        neighbor_S.begin(), neighbor_S.end(),
                        std::back_inserter(nonneighbor_S));

    while (!nonneighbor_S.empty()) {
      k = nonneighbor_S.front();
      alpha_S_k = edmonds_karp_max_flow
        (flow_g, p, k, cap, res_cap, rev_edge, &color[0], &pred[0]);
      if (alpha_S_k < alpha_star) {
        alpha_star = alpha_S_k;
        S_star.clear();
        for (tie(vi, vi_end) = vertices(flow_g); vi != vi_end; ++vi)
          if (color[*vi] != Color::white())
            S_star.push_back(*vi);
      }
      S.insert(k);
      neighbor_S.insert(k);
      neighbors(g, k, std::inserter(neighbor_S, neighbor_S.begin()));
      nonneighbor_S.clear();
      std::set_difference(vertices(g).first, vertices(g).second,
                          neighbor_S.begin(), neighbor_S.end(),
                          std::back_inserter(nonneighbor_S));
    }

    std::vector < bool > in_S_star(num_vertices(g), false);
    typename std::vector < vertex_descriptor >::iterator si;
    for (si = S_star.begin(); si != S_star.end(); ++si)
      in_S_star[*si] = true;
    degree_size_type c = 0;
    for (si = S_star.begin(); si != S_star.end(); ++si) {
      typename graph_traits < VertexListGraph >::out_edge_iterator ei, ei_end;
      for (tie(ei, ei_end) = out_edges(*si, g); ei != ei_end; ++ei)
        if (!in_S_star[target(*ei, g)]) {
          *disconnecting_set++ = *ei;
          ++c;
        }
    }

    return c;
  }

}

int
main()
{
  using namespace boost;
  GraphvizGraph g;
  read_graphviz("figs/edge-connectivity.dot", g);

  typedef graph_traits < GraphvizGraph >::edge_descriptor edge_descriptor;
  typedef graph_traits < GraphvizGraph >::degree_size_type degree_size_type;
  std::vector < edge_descriptor > disconnecting_set;
  degree_size_type c =
    edge_connectivity(g, std::back_inserter(disconnecting_set));

  std::cout << "The edge connectivity is " << c << "." << std::endl;

  property_map < GraphvizGraph, vertex_attribute_t >::type
    attr_map = get(vertex_attribute, g);

  std::cout << "The disconnecting set is {";
  for (std::vector < edge_descriptor >::iterator i =
       disconnecting_set.begin(); i != disconnecting_set.end(); ++i)
    std::
      cout << "(" << attr_map[source(*i, g)]["label"] << "," <<
      attr_map[target(*i, g)]["label"] << ") ";
  std::cout << "}." << std::endl;
  return EXIT_SUCCESS;
}


/**
 *          Copyright Matthias Walter 2010.
 * 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)
 **/

#ifndef MATRIX_TRANSPOSED_HPP_
#define MATRIX_TRANSPOSED_HPP_

#include <boost/numeric/ublas/matrix_expression.hpp>

namespace unimod
{

  template <typename MatrixType>
  void matrix_binary_pivot(MatrixType& matrix, size_t i, size_t j);

  namespace detail
  {

    /// Helper struct to manage orientation tags

    template <typename OrientationCategory>
    struct transpose_orientation
    {
      typedef boost::numeric::ublas::unknown_orientation_tag orientation_category;
    };

    /// Helper struct to manage orientation tags

    template <>
    struct transpose_orientation <boost::numeric::ublas::row_major_tag>
    {
      typedef boost::numeric::ublas::column_major_tag orientation_category;
    };

    /// Helper struct to manage orientation tags

    template <>
    struct transpose_orientation <boost::numeric::ublas::column_major_tag>
    {
      typedef boost::numeric::ublas::row_major_tag orientation_category;
    };
  }

  /**
   * A matrix proxy which exchanges the meaning of rows and columns.
   */

  template <typename M>
  class matrix_transposed: public boost::numeric::ublas::matrix_expression <matrix_transposed <M> >
  {
  public:
    typedef matrix_transposed <M> self_type;
    typedef M matrix_type;
    typedef typename M::size_type size_type;
    typedef typename M::difference_type difference_type;
    typedef typename M::value_type value_type;
    typedef typename M::const_reference const_reference;
    typedef typename boost::mpl::if_ <boost::is_const <M>, typename M::const_reference, typename M::reference>::type reference;
    typedef typename boost::mpl::if_ <boost::is_const <M>, typename M::const_closure_type, typename M::closure_type>::type matrix_closure_type;
    typedef const self_type const_closure_type;
    typedef self_type closure_type;
    typedef typename detail::transpose_orientation <typename M::orientation_category>::orientation_category orientation_category;
    typedef typename M::storage_category storage_category;

  private:
    matrix_type& _data;

  public:

    /**
     * Constructs the matrix proxy.
     *
     * @param matrix The original matrix
     */

    matrix_transposed(matrix_type& matrix) :
      _data(matrix)
    {

    }

    /**
     * @return Height of the matrix
     */

    inline size_type size1() const
    {
      return _data.size2();
    }

    /**
     * @return Width of the matrix
     */

    inline size_type size2() const
    {
      return _data.size1();
    }

    /**
     * @return Reference to the original matrix
     */

    inline matrix_type& data()
    {
      return _data;
    }

    /**
     * Read-only access operator
     *
     * @param i Row index
     * @param j Column index
     * @return original(column, row)
     */

    inline const_reference operator ()(size_type i, size_type j) const
    {
      return _data(j, i);
    }

    /**
     * Access operator
     *
     * @param i Row index
     * @param j Column index
     * @return original(column, row)
     */

    inline reference operator ()(size_type i, size_type j)
    {
      return _data(j, i);
    }

    typedef boost::numeric::ublas::detail::indexed_iterator1 <self_type, typename matrix_type::iterator2::iterator_category> iterator1;
    typedef boost::numeric::ublas::detail::indexed_iterator2 <self_type, typename matrix_type::iterator1::iterator_category> iterator2;
    typedef boost::numeric::ublas::detail::indexed_const_iterator1 <self_type, typename matrix_type::const_iterator2::iterator_category>
        const_iterator1;
    typedef boost::numeric::ublas::detail::indexed_const_iterator2 <self_type, typename matrix_type::const_iterator1::iterator_category>
        const_iterator2;
  };

  /**
   * Creates a transpose proxy of a given matrix.
   *
   * @param matrix The original matrix
   * @return A matrix_transposed proxy
   */

  template <typename MatrixType>
  inline matrix_transposed <MatrixType> make_transposed_matrix(MatrixType& matrix)
  {
    return matrix_transposed <MatrixType> (matrix);
  }

  /**
   * Free function to set a matrix value of a permuted matrix.
   *
   * @param matrix The transposed matrix
   * @param row Row index
   * @param column Column index
   * @param value New value
   */

  template <typename MatrixType>
  inline void matrix_set_value(matrix_transposed <MatrixType>& matrix, size_t row, size_t column, typename MatrixType::value_type value)
  {
    matrix(row, column) = value;
  }

  /**
   * Dummy function for the version with writable orignal matrix.
   */

  template <typename MatrixType>
  inline void matrix_set_value(matrix_transposed <const MatrixType>& matrix, size_t row, size_t column, typename MatrixType::value_type value)
  {

  }

  /**
   * Free function to perform a pivot on a transposed matrix.
   *
   * @param matrix The transposed matrix
   * @param i Row index
   * @param j Column index
   */

  template <typename MatrixType>
  inline void matrix_binary_pivot(matrix_transposed <MatrixType>& matrix, size_t i, size_t j)
  {
    matrix_binary_pivot(matrix.data(), j, i);
  }

}

#endif /* MATRIX_TRANSPOSED_HPP_ */


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

#include <boost/test/unit_test.hpp>
#include <boost/time_series/numeric/fine_grain.hpp>
#include <boost/foreach.hpp>

using namespace boost;
using namespace unit_test;

void test_fine_grain()
{
    using namespace boost::time_series;

    sparse_series<int> s1(discretization = 4);

    make_ordered_inserter(s1)
        (1, 0)(2)(3)(4)(5)(6)(7)
    .commit();

    piecewise_constant_series<int> r11(discretization = 2);

    make_ordered_inserter(r11)
        (1, 0, 2)
        (2, 2, 4)
        (3, 4, 6)
        (4, 6, 8)
        (5, 8, 10)
        (6, 10, 12)
        (7, 12, 14)
    .commit();

    BOOST_CHECK_EQUAL(r11, fine_grain(s1, 2));

    sparse_series<int> r12(discretization = 2);

    make_ordered_inserter(r12)
        (1, 0)
        (2, 2)
        (3, 4)
        (4, 6)
        (5, 8)
        (6, 10)
        (7, 12)
    .commit();

    BOOST_CHECK_EQUAL(r12, fine_grain(s1, 2, sparse_upsample));

    sparse_series<int> s2(discretization = 2);

    make_ordered_inserter(s2)
        (1, -2)(2)(3)(4)
    .commit();

    sparse_series<int> r2;

    make_ordered_inserter(r2)
        (1, -4)(0)(2)(0)(3)(0)(4)(0)
    .commit();

    BOOST_CHECK_EQUAL(r2, fine_grain(s2, 1, sparse_upsample));

    sparse_series<int, mpl::int_<10> > s3;

    make_ordered_inserter(s3)
        (1, 0)(2)(3)(4)
    .commit();

    piecewise_constant_series<int, mpl::int_<5> > r3;

    make_ordered_inserter(r3)
        (1, 0, 2)
        (2, 2, 4)
        (3, 4, 6)
        (4, 6, 8)
    .commit();

    BOOST_CHECK_EQUAL(r3, fine_grain(s3, mpl::int_<5>()));

    piecewise_constant_series<int> o1(discretization = 2);

    ordered_inserter<piecewise_constant_series<int> > out1(o1);
    fine_grain(s1, 2, out1).commit();

    BOOST_CHECK_EQUAL(r11, o1);

    sparse_series<int> o2(discretization = 2);

    ordered_inserter<sparse_series<int> > out2(o2);
    fine_grain(s1, 2, sparse_upsample, out2).commit();

    BOOST_CHECK_EQUAL(r12, o2);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("fine_grain test");

    test->add(BOOST_TEST_CASE(&test_fine_grain));

    return test;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2013 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef BLOCK_DIAGONAL_SCHUR_PRECONDITIONER_HH
#define BLOCK_DIAGONAL_SCHUR_PRECONDITIONER_HH

#include <cmath>
#include <vector>

#include <boost/fusion/include/at_c.hpp>

#include <dune/istl/solvers.hh>
#include <dune/istl/preconditioners.hh>
#include <dune/istl/solvercategory.hh>

#include "fem/istlinterface.hh"
#include "linalg/direct.hh"
#include "linalg/triplet.hh"
#include "linalg/iccprecond.hh"
#include "linalg/iluprecond.hh"
#include "utilities/linalg/scalarproducts.hh"

namespace Kaskade
{
  namespace SchurPreconditionerDetail
  {
    template <class Operator, class Block>
    struct ExtractDomainAndRange
    {
      typedef typename Operator::Assembler::AnsatzVariableSet::template CoefficientVectorRepresentation<Block::firstCol,Block::lastCol>::type Domain;
      typedef typename Operator::Assembler::TestVariableSet::template   CoefficientVectorRepresentation<Block::firstRow,Block::lastRow>::type Range;
    };

    template <class Scalar_>
    class JacobiIteration
    {
      typedef Scalar_ Scalar;
      typedef MatrixAsTriplet<Scalar> Matrix;

    public:
      JacobiIteration() = default;

      template <class Operator>
      explicit JacobiIteration(Operator const& A_)
       : A(A_.template get<Matrix>(false)), diag(A.nrows(),0)
      {
        for(size_t i=0; i<A.ridx.size(); ++i) if(A.ridx[i] == A.cidx[i]) diag[A.ridx[i]] = A.data[i];
      }

      explicit JacobiIteration(Matrix const& A_)
       : A(A_), diag(A.nrows(),0)
      {
        for(size_t i=0; i<A.ridx.size(); ++i) if(A.ridx[i] == A.cidx[i]) diag[A.ridx[i]] = A.data[i];
      }

      template <class Domain, class Range>
      void apply(Domain& x, Range const& y, size_t nIter = 1) const
      {
        std::vector<Scalar> tmpx(x.dim()), tmpy;
        IstlInterfaceDetail::toVector(y,tmpy);
        for(size_t i=0; i<diag.size(); ++i) tmpx[i] = tmpy[i]/diag[i];

        for(size_t iter=0; iter < nIter; ++iter)
        {
          for(size_t i=0; i<A.ridx.size(); ++i) if(A.ridx[i] != A.cidx[i]) tmpy[A.ridx[i]] -= A.data[i]*tmpx[A.cidx[i]];
          for(size_t i=0; i<diag.size(); ++i) tmpx[i] += tmpy[i]/diag[i];
        }
        IstlInterfaceDetail::fromVector(tmpx,x);
      }

    private:
      Matrix const& A;
      std::vector<Scalar> diag;
    };

    template <class Scalar_>
    class InvertLumpedMatrix
    {
      typedef Scalar_ Scalar;
      typedef MatrixAsTriplet<Scalar> Matrix;

    public:
      InvertLumpedMatrix() = default;

      template <class Operator, typename... Args>
      explicit InvertLumpedMatrix(Operator const& A_, Args...)
      {
        Matrix A = A_.template get<Matrix>();
        diag.resize(A.nrows());
        for(size_t i=0; i<A.ridx.size(); ++i) if(A.ridx[i] == A.cidx[i]) diag[A.ridx[i]] = A.data[i];
      }

      template <typename... Args>
      explicit InvertLumpedMatrix(Matrix const& A, Args...)
       : diag(A.nrows(),0)
      {
        for(size_t i=0; i<A.ridx.size(); ++i) if(A.ridx[i] == A.cidx[i]) diag[A.ridx[i]] = A.data[i];
      }

      template <class Domain, class Range>
      void apply(Domain& x, Range const& y) const
      {
        tmpx.resize(x.dim());
        IstlInterfaceDetail::toVector(y,tmpy);
        for(size_t i=0; i<diag.size(); ++i) tmpx[i] = tmpy[i]/diag[i];
        IstlInterfaceDetail::fromVector(tmpx,x);
      }

    private:
      std::vector<Scalar> diag;
      mutable std::vector<Scalar> tmpx, tmpy;
    };

    template <class Domain, class Range>
    class ApplyDirectSolver
    {

    public:
      ApplyDirectSolver() = default;

      template <class Operator>
      explicit ApplyDirectSolver(Operator const& A, DirectType directType = DirectType::MUMPS, MatrixProperties property = MatrixProperties::GENERAL)
       : solver(DirectSolver<Domain,Range>(A,directType,property))
      {}

      explicit ApplyDirectSolver(MatrixAsTriplet<typename Domain::Scalar> const& A, DirectType directType = DirectType::MUMPS, MatrixProperties property = MatrixProperties::GENERAL)
       : solver(DirectSolver<Domain,Range>(A,directType,property))
      {}

      void apply(Domain& x, Range const& y) const
      {
        solver.apply(y,x);
      }

    private:
      InverseLinearOperator<DirectSolver<Domain,Range> > solver;
    };
  } /* end of namespace PreconditionerDetail */

  /// Approximation of the schur complement according to Pearson/Wathen '10
  /**
   *  \param BlockK type: IstlInterfaceDetail::BlockInfo, information on the block containing the stiffness matrix
   *  \param BlockM type: IstlInterfaceDetail::BlockInfo, information on the block containing the mass matrix
   */
  template <class Scalar_, class Domain_, class Range_,
             class BlockK = IstlInterfaceDetail::BlockInfo<2,3,0,1>,
             class BlockM = IstlInterfaceDetail::BlockInfo<0,1,0,1>,
             class LinearSolver = SchurPreconditionerDetail::ApplyDirectSolver<Domain_,Range_>
             >
  class ApproximateSchurComplement
  {
  public:
    typedef Scalar_ Scalar;
    typedef Domain_ Domain;
    typedef Range_ Range;
    typedef MatrixAsTriplet<Scalar> Matrix;

    /**
     * \param alpha Tikhonov regularization parameter
     * \param symmetricK_ true if stiffness matrix is symmetric, else false
     * \param symmetricM_ true if mass matrix is symmetric, else false
     */
    template <class Assembler>
    ApproximateSchurComplement(Assembler const& assembler, Scalar alpha, bool symmetricK_ = false, bool symmetricM_ = false)
    : symmetricK(symmetricK_ && symmetricM_), symmetricM(symmetricM_), propertyK(symmetricK ? MatrixProperties::SYMMETRIC : MatrixProperties::GENERAL), propertyM(symmetricM ? MatrixProperties::SYMMETRIC : MatrixProperties::GENERAL)
    {
      updateMatrices(assembler,alpha);
    }

    template <class Matrix>
    ApproximateSchurComplement(Matrix const& M_, Matrix const& K_, Scalar alpha, bool symmetricK_ = false, bool symmetricM_ = false)
    : sqrtOfAlpha(sqrt(alpha)), symmetricK(symmetricK_ && symmetricM_), symmetricM(symmetricM_),
      propertyK(symmetricK ? MatrixProperties::SYMMETRIC : MatrixProperties::GENERAL), propertyM(symmetricM ? MatrixProperties::SYMMETRIC : MatrixProperties::GENERAL),
      M(M_), K(K_), N(M_)
    {
      N /= sqrtOfAlpha;
      N += K;
      initSolver();
    }

    /**
     * \param alpha Tikhonov regularization parameter
     */
    template <class Assembler>
    void updateMatrices(Assembler const& assembler, Scalar alpha)
    {
      sqrtOfAlpha = sqrt(alpha);
      M = assembler.template get<Matrix,BlockM::firstRow,BlockM::lastRow,BlockM::firstCol,BlockM::lastCol>(symmetricM);
      N = M.get();
      N /= sqrtOfAlpha;
      K = assembler.template get<Matrix,BlockK::firstRow,BlockK::lastRow,BlockK::firstCol,BlockK::lastCol>(symmetricK);
      N += K;

      initSolver();
    }

    template <class Matrix>
    void updateMatrices(Matrix const& M_, Matrix const& K_, Scalar alpha)
    {
      M = M_;
      K = K_;
      N = M.get();
      N /= sqrtOfAlpha;
      N += K;
    }

    /// compute \f$x=S^{-1}y\f$, overrides y
    void solve(Domain& x, Range& y) const
    {
      solver.apply(x,y);
      M.apply(x,y);
      solver.apply(x,y);
    }

    Matrix const& getMassMatrix() const
    {
      return M.get();
    }

    Matrix const& getStiffnessMatrix() const
    {
      return K;
    }

  private:
    void initSolver()
    {
      DirectType directType = DirectType::MUMPS;
      MatrixProperties property = (symmetricK && symmetricM) ? MatrixProperties::SYMMETRIC : MatrixProperties::GENERAL;
      solver = LinearSolver(N,directType,property);
    }

    /// Tikhonov regularization parameter
    Scalar sqrtOfAlpha;
    bool symmetricK, symmetricM;
    MatrixProperties propertyK, propertyM;
    /// mass matrix
    MatrixRepresentedOperator<Matrix,Domain,Range> M;
    /// stiffness matrix
    MatrixAsTriplet<Scalar> K;
    /// sum of stiffness matrix and scaled mass matrix
    /**
     * Let \f$K\f$ be the stiffness matrix, \f$M\f$ be the mass matrix
     * and let \f$\alpha\f$ be the Tikhonov regularization parameter. Then
     * \f$N=K+\frac{1}{\sqrt{\alpha}}M\f$.
     */
    MatrixRepresentedOperator<Matrix,Domain,Range> N;

    LinearSolver solver;
  };

  /// Approximation of the schur complement
  /**
   *  \param BlockK type: IstlInterfaceDetail::BlockInfo, information on the block containing the stiffness matrix
   *  \param BlockM type: IstlInterfaceDetail::BlockInfo, information on the block containing the mass matrix
   */
  template <class Scalar_,
             class BlockK = IstlInterfaceDetail::BlockInfo<2,3,0,1>,
             class BlockM = IstlInterfaceDetail::BlockInfo<0,1,0,1>
             >
  class ApproximateSchurComplement2
  {
  public:
    typedef Scalar_ Scalar;
    typedef MatrixAsTriplet<Scalar> Matrix;

    /**
     * \param alpha Tikhonov regularization parameter
     * \param symmetricK_ true if stiffness matrix is symmetric, else false
     * \param symmetricM_ true if mass matrix is symmetric, else false
     */
    template <class Assembler>
    ApproximateSchurComplement2(Assembler const& assembler, Scalar /*alpha*/, bool symmetricK_ = false, bool symmetricM_ = false)
    : symmetricK(symmetricK_ && symmetricM_), symmetricM(symmetricM_), propertyK(symmetricK ? MatrixProperties::SYMMETRIC : MatrixProperties::GENERAL), propertyM(symmetricM ? MatrixProperties::SYMMETRIC : MatrixProperties::GENERAL)
    {
      updateMatrices(assembler);
    }

    /**
     * \param alpha Tikhonov regularization parameter
     */
    template <class Assembler>
    void updateMatrices(Assembler const& assembler)
    {
      M = assembler.template get<Matrix,BlockM::firstRow,BlockM::lastRow,BlockM::firstCol,BlockM::lastCol>(symmetricM);
      L = M;
      K = assembler.template get<Matrix,BlockK::firstRow,BlockK::lastRow,BlockK::firstCol,BlockK::lastCol>(symmetricK);
      L += K;
    }

    template <class Domain, class Range>
    void solve(Domain& x, Range const& y, size_t nIter) const
    {
      InverseLinearOperator<DirectSolver<Domain,Range> > Kinv(DirectSolver<Domain,Range>(K,DirectType::MUMPS,propertyK));

      Kinv.apply(y,x);

      std::vector<Scalar> tmpx, tmpy;
      IstlInterfaceDetail::toVector(x,tmpx);
      L.mv(tmpx,tmpy);
      Range tmp(y);
      IstlInterfaceDetail::fromVector(tmpy,tmp);

      Kinv.apply(tmp,x);
    }

    MatrixAsTriplet<Scalar> const& massMatrix() const
    {
      return M;
    }

    MatrixAsTriplet<Scalar> const& stiffnessMatrix() const
    {
      return K;
    }

  private:
    /// sum of stiffness matrix and mass matrix
    MatrixAsTriplet<Scalar> L;
    /// mass matrix
    MatrixAsTriplet<Scalar> M;
    /// stiffness matrix
    MatrixAsTriplet<Scalar> K;
    bool symmetricK, symmetricM;
    MatrixProperties propertyK, propertyM;
  };

  /**
   * Preconditioner à la Pearson/Wathen '10
   *
   * based on an approximation of the schur complement
   * mesh independent and independent of the Tikhonov regularization parameter
   */
  template <class Operator, template <class,class,class,class,class,class> class SchurComplement = ApproximateSchurComplement,
             class BlockK = IstlInterfaceDetail::BlockInfo<2,3,0,1>,
             class BlockM = IstlInterfaceDetail::BlockInfo<0,1,0,1>,
             class LinearSolver = SchurPreconditionerDetail::ApplyDirectSolver<typename SchurPreconditionerDetail::ExtractDomainAndRange<Operator,BlockM>::Domain, typename SchurPreconditionerDetail::ExtractDomainAndRange<Operator,BlockM>::Range>,
             class LinearSolver_SchurComplement = LinearSolver
            >
  class BlockDiagonalSchurPreconditioner: public Dune::Preconditioner<typename Operator::Domain,typename Operator::Range>
  {
    typedef typename SchurPreconditionerDetail::ExtractDomainAndRange<Operator,BlockM>::Domain Domain0;
    typedef typename SchurPreconditionerDetail::ExtractDomainAndRange<Operator,BlockM>::Range Range0;
    typedef typename Operator::Assembler Assembler;
    typedef MatrixAsTriplet<typename Operator::Scalar> Matrix;

  public:
    typedef typename Operator::Scalar Scalar;
    typedef typename Operator::Domain Domain;
    typedef typename Operator::Range Range;

    static int const category = Dune::SolverCategory::sequential;

    explicit BlockDiagonalSchurPreconditioner(Operator const& A_,Scalar alpha_, bool symmetricK = false, bool symmetricM = false)
     : A(A_), alpha(alpha_), schurComplement(A.getAssembler(), alpha, symmetricK, symmetricM),
       propertyM(symmetricM ? MatrixProperties::SYMMETRIC : MatrixProperties::GENERAL), solver(schurComplement.getMassMatrix(),DirectType::MUMPS,propertyM)
    {}

    virtual ~BlockDiagonalSchurPreconditioner(){}

    virtual void apply (Domain& x, Range const& y)
    {
      using namespace boost::fusion;

      Range0 y0(at_c<0>(y.data));
      Domain0 x0(at_c<0>(x.data));

      std::cout << at_c<0>(x.data).dim() << std::endl;
      solver.apply(x0,y0);
      x0 *= 1./alpha;
      at_c<0>(x.data) = at_c<0>(x0.data);

      at_c<0>(y0.data) = at_c<1>(y.data);
      solver.apply(x0,y0);
//      x0 *= 1./alpha;
      at_c<1>(x.data) = at_c<0>(x0.data);

      at_c<0>(y0.data) = at_c<2>(y.data);
      schurComplement.solve(x0,y0);
      at_c<2>(x.data) = at_c<0>(x0.data);
    }

    virtual void pre (Domain&, Range&) {}
    virtual void post (Domain&) {}

  private:
    Operator const& A;
    Scalar alpha;
    SchurComplement<Scalar,Domain0,Range0,BlockK,BlockM,LinearSolver_SchurComplement> schurComplement;
    MatrixProperties propertyM;
    LinearSolver solver;
  };
} // namespace Kaskade
#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <typeinfo>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Matrix>
void add_row(Matrix& A, typename mtl::Collection<Matrix>::size_type n= 1)
{
    Matrix A_tmp(num_rows(A) + n, num_cols(A));
    A_tmp= 0; // to avoid uninitialized values
    sub_matrix(A_tmp, 0, num_rows(A), 0, num_cols(A))= A;
    swap(A_tmp, A);
}

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    typedef typename mtl::Collection<Matrix>::value_type   value_type;
    typedef typename mtl::Collection<Matrix>::size_type    size_type;

    // std::cout << "\n" << name  << ", type of size_type is " << typeid(size_type).name() << '\n';
    // std::cout << "type of size_type is " << typeid(typename Matrix::size_type).name() << '\n';

    A.change_dim(3, 3);
    {
	mtl::mat::inserter<Matrix>   ins(A);
	for (size_type i= 0; i < num_rows(A); i++)
	    for (size_type j= 0; j < num_cols(A); j++)
		ins[i][j]= value_type(j) - value_type(i);
    }
    cout << "\n" << name << "\n" << "A =\n" << A;

    add_row(A);
    cout << "\nA after adding one row A =\n" << A;

    MTL_THROW_IF(num_rows(A) != 4 || num_cols(A) != 3, mtl::runtime_error("Wrong dimension after adding one row"));
    MTL_THROW_IF(A[2][1] != value_type(-1), mtl::runtime_error("Wrong value in A[2][1] after adding one row"));

    add_row(A, 2);
    cout << "\nA after adding two rows A =\n" << A;

    MTL_THROW_IF(num_rows(A) != 6 || num_cols(A) != 3, mtl::runtime_error("Wrong dimension after adding two rows"));
    MTL_THROW_IF(A[2][1] != value_type(-1), mtl::runtime_error("Wrong value in A[2][1] after adding two rows"));
}

int main(int, char**)
{
    using namespace mtl;

    dense2D<double, mat::parameters<> >               dr;
    dense2D<double, mat::parameters<col_major> >      dc;
    morton_dense<double, recursion::morton_z_mask, mat::parameters<> >       mzd;
    morton_dense<double, recursion::doppled_2_row_mask, mat::parameters<> >  d2r;
    compressed2D<double, mat::parameters<> >          cr;
    compressed2D<double, mat::parameters<col_major> > cc;

    dense2D<complex<double>, mat::parameters<> >      drc;
    compressed2D<complex<double>, mat::parameters<> > crc;

    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
    // test(cr, "Compressed row major");
    test(drc, "Dense row major complex");
    // test(crc, "Compressed row major complex");

    // For better readability I don't want finish with a complex
    // test(cc, "Compressed column major");

    return 0;
}


//
// Created by Zhongshi Jiang on 5/7/17.
//

#include <igl/copyleft/tetgen/tetrahedralize.h>
#include <Eigen/Core>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/triangle/triangulate.h>
#include <igl/cat.h>
#include <igl/writeOBJ.h>
#include <igl/readOBJ.h>
#include <igl/writeMESH.h>
#include <igl/readMESH.h>
#include <igl/barycenter.h>
#include <igl/read_triangle_mesh.h>
#include <iostream>
#include <igl/Timer.h>
#include <fstream>
#include "../util/tetgenio_parser.h"

#include "../util/tet_utils.h"
void flow_initializer(std::string filename,
                      std::string target_file,
    Eigen::MatrixXd &mTV,
                          Eigen::MatrixXi &mTT,
                          Eigen::MatrixXd &wTV,
                          Eigen::MatrixXi &sTT,
                          Eigen::VectorXi &frame_id,
                          Eigen::MatrixXi &surface_F,
                          int& inner_scaf_tets) {
  using namespace Eigen;
  using namespace std;
  Eigen::MatrixXd V_init;

  Eigen::MatrixXd cube_V, id_cube_V;
  Eigen::MatrixXi cube_F;
  igl::read_triangle_mesh("../nice_cube.obj", id_cube_V, cube_F);

  MatrixXi mTF;
  igl::read_triangle_mesh(target_file, mTV, mTT);
  igl::read_triangle_mesh(filename, V_init, surface_F);

  cube_V.resizeLike(id_cube_V);
  double max_V0 = V_init.maxCoeff();
  for (int i = 0; i < 3; i++)
    cube_V.col(i) = 3 * max_V0 * id_cube_V.col(i);
  MatrixXd wV;
  MatrixXi wF, TF;

  MatrixXi F2_m = cube_F.array() + V_init.rows();
  igl::cat(1, surface_F, F2_m, wF);
  igl::cat(1, V_init, cube_V, wV);

  MatrixXd TR,R;
  igl::dev::tetgen::tetrahedralize(wV, wF, R, "pqYYA", wTV, sTT, TF,
                                   TR);
  vector<RowVector4i> TT_R1, TT_R2;
  for(int i=0; i<TR.rows(); i++) {
    if(TR(i) == 2)
      TT_R1.push_back(sTT.row(i));
    else
      TT_R2.push_back(sTT.row(i));
  }

  inner_scaf_tets = TT_R1.size();
  for(int i=0; i<inner_scaf_tets; i++) {
    sTT.row(i) = TT_R1[i];
  }
  for(int i=0; i<TT_R2.size(); i++) {
    sTT.row(i + inner_scaf_tets) = TT_R2[i];
  }

  auto tet_quality = [&wTV](int a, int b, int c, int d) -> double {
    return -quality_utils::quality(wTV.row(a), wTV.row(b),
                                   wTV.row(c), wTV.row(d));
  };
  {
    double min_q = INFINITY;
    for (auto i=0; i<sTT.rows(); i++) {
      RowVector4i r = sTT.row(i);
      min_q = (std::min)(min_q, tet_quality(r(0), r(1), r(2), r(3)));
    }
    cout << endl << "q_in" << min_q << '\t';
    assert(min_q > 0);
  }

  int bnd_size = cube_V.rows();
  frame_id = Eigen::VectorXi::LinSpaced(
      bnd_size, V_init.rows(), V_init.rows() + bnd_size - 1);

}


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#include <toolkits/nearest_neighbors/lsh_family.hpp>
#include <core/random/random.hpp>
#include <toolkits/ml_data_2/ml_data_iterators.hpp>

#include <time.h>
#include <boost/functional/hash.hpp>

namespace turi {
namespace nearest_neighbors {

void lsh_family::init_options(const std::map<std::string, flexible_type>& _opts) {

#define __EXTRACT(var) var = variant_get_value<decltype(var)>(_opts.at(#var));
  __EXTRACT(num_tables);
  __EXTRACT(num_projections_per_table);
#undef __EXTRACT
  num_projections = num_tables * num_projections_per_table;
  num_input_dimensions = 0;

  lookup_table.assign(num_tables, hash_map_container<size_t, std::vector<size_t>>());
}

void lsh_family::save(turi::oarchive& oarc) const {
  oarc << num_input_dimensions
      << num_tables
      << num_projections_per_table
      << num_projections
      << lookup_table;
}

void lsh_family::load(turi::iarchive& iarc) {
  iarc >> num_input_dimensions
      >> num_tables
      >> num_projections_per_table
      >> num_projections
      >> lookup_table;
}

std::vector<int> lsh_family::hash_vector_to_codes(const DenseVector& vec,
                                                  bool is_reference_data) const {
  log_and_throw(std::string("DenseVector is not supported for LSH ")
                + distance_type_name());
  return {};
}

std::vector<int> lsh_family::hash_vector_to_codes(const SparseVector& vec,
                                                  bool is_reference_data) const {
  log_and_throw(std::string("SparseVector is not supported for LSH ")
                + distance_type_name());
  return {};
}

// Random sample a subset, and set w using the average distance
void lsh_euclidean::pre_lsh(const v2::ml_data& mld_ref, bool is_sparse) {
  size_t num_samples = std::min(size_t(100), mld_ref.size());
  v2::ml_data sampled_data = mld_ref.create_subsampled_copy(num_samples, time(NULL));
  DenseMatrix distance_matrix(num_samples, num_samples);

  if (!is_sparse) { // dense
    DenseMatrix sub_matrix(num_samples, num_input_dimensions);
    in_parallel([&](size_t thread_idx, size_t num_threads) {
      for (auto it = sampled_data.get_iterator(thread_idx, num_threads); !it.done(); ++it) {
        it.fill_eigen_row(sub_matrix.row(it.row_index()));
      }
    });

    all_pairs_squared_euclidean(sub_matrix, sub_matrix, distance_matrix);
    distance_matrix = distance_matrix.unaryExpr([](double x) { return std::sqrt(x); });
  } else { // sparse
    std::vector<SparseVector> sub_matrix(num_samples, SparseVector(num_input_dimensions));
    in_parallel([&](size_t thread_idx, size_t num_threads) {
      for (auto it = sampled_data.get_iterator(thread_idx, num_threads); !it.done(); ++it) {
        it.fill_observation(sub_matrix[it.row_index()]);
      }
    });

    std::vector<double> vec_norms(num_samples, 0.);
    parallel_for (0, num_samples, [&](size_t idx){
      vec_norms[idx] = sub_matrix[idx].squaredNorm();
    });
    parallel_for (0, num_samples, [&](size_t idx_a){
      for (size_t idx_b = 0; idx_b < num_samples; ++idx_b) {
        distance_matrix(idx_a, idx_b) = std::sqrt(vec_norms[idx_a]
          + vec_norms[idx_b] - 2. * sub_matrix[idx_a].dot(sub_matrix[idx_b]));
      }
    });
  }
  w = std::max(size_t(1), static_cast<size_t>(distance_matrix.mean()));
  rand_vec = rand_vec.unaryExpr([&](double x) { return random::fast_uniform<double>(0., w); });
}

void lsh_euclidean::init_model(size_t num_dimensions) {
  num_input_dimensions = num_dimensions;
  w = 4;
  rand_mat.resize(num_projections, num_input_dimensions);
  rand_vec.resize(num_projections);
  rand_mat = rand_mat.unaryExpr([](double x) { return random::normal(0., 1.); });
  rand_vec = rand_vec.unaryExpr([&](double x) { return random::fast_uniform<double>(0., w); });
}

void lsh_euclidean::save(turi::oarchive& oarc) const {
  lsh_family::save(oarc);
  oarc << w << rand_mat << rand_vec;
}

void lsh_euclidean::load(turi::iarchive& iarc) {
  lsh_family::load(iarc);
  iarc >> w >> rand_mat >> rand_vec;
}

std::vector<int> lsh_euclidean::hash_vector_to_codes(const DenseVector& vec,
                                                     bool is_reference_data) const {
  std::vector<int> ret(num_projections, -1);
  DenseVector hash_vec = rand_mat * vec + rand_vec;
  parallel_for (0, num_projections, [&](size_t hash_idx) {
    ret[hash_idx] = static_cast<int>(std::floor(hash_vec(hash_idx) / w));
  });
  return ret;
}

std::vector<int> lsh_euclidean::hash_vector_to_codes(const SparseVector& vec,
                                                     bool is_reference_data) const {
  std::vector<int> ret(num_projections, -1);
  if (vec.nonZeros() == 0) return ret;

  DenseVector hash_vec = rand_mat * vec + rand_vec;
  parallel_for (0, num_projections, [&](size_t hash_idx) {
    ret[hash_idx] = static_cast<int>(std::floor(hash_vec(hash_idx) / w));
  });
  return ret;
}

// Random sample a subset, and set w using the average distance
void lsh_manhattan::pre_lsh(const v2::ml_data& mld_ref, bool is_sparse) {
  size_t num_samples = std::min(size_t(100), mld_ref.size());
  v2::ml_data sampled_data = mld_ref.create_subsampled_copy(num_samples, time(NULL));
  DenseMatrix distance_matrix(num_samples, num_samples);

  if (!is_sparse) { // dense
    DenseMatrix sub_matrix(num_samples, num_input_dimensions);
    in_parallel([&](size_t thread_idx, size_t num_threads) {
      for (auto it = sampled_data.get_iterator(thread_idx, num_threads); !it.done(); ++it) {
        it.fill_eigen_row(sub_matrix.row(it.row_index()));
      }
    });
    parallel_for (0, num_samples, [&](size_t idx_a){
      for (size_t idx_b = 0; idx_b < num_samples; ++idx_b) {
        distance_matrix(idx_a, idx_b) = (sub_matrix.row(idx_a) - sub_matrix.row(idx_b)).cwiseAbs().sum();
      }
    });
  } else { // sparse
    std::vector<SparseVector> sub_matrix(num_samples, SparseVector(num_input_dimensions));
    in_parallel([&](size_t thread_idx, size_t num_threads) {
      for (auto it = sampled_data.get_iterator(thread_idx, num_threads); !it.done(); ++it) {
        it.fill_observation(sub_matrix[it.row_index()]);
      }
    });

    parallel_for (0, num_samples, [&](size_t idx_a){
      for (size_t idx_b = 0; idx_b < num_samples; ++idx_b) {
        distance_matrix(idx_a, idx_b) = (sub_matrix[idx_a] - sub_matrix[idx_b]).cwiseAbs().sum();
      }
    });
  }

  w = std::max(size_t(1), static_cast<size_t>(distance_matrix.mean()));
  rand_vec = rand_vec.unaryExpr([&](double x) { return random::fast_uniform<double>(0., w); });
}

void lsh_manhattan::init_model(size_t num_dimensions) {
  num_input_dimensions = num_dimensions;
  w = 4;
  rand_mat.resize(num_projections, num_input_dimensions);
  rand_vec.resize(num_projections);
  rand_mat = rand_mat.unaryExpr([](double x) { return random::cauchy(0., 1.); });
  rand_vec = rand_vec.unaryExpr([&](double x) { return random::fast_uniform<double>(0., w); });
}

void lsh_cosine::init_model(size_t num_dimensions) {
  num_input_dimensions = num_dimensions;
  rand_mat.resize(num_projections, num_input_dimensions);
  rand_mat = rand_mat.unaryExpr([](double x) { return random::normal(0., 1.); });
}

void lsh_cosine::save(turi::oarchive& oarc) const {
  lsh_family::save(oarc);
  oarc << rand_mat;
}

void lsh_cosine::load(turi::iarchive& iarc) {
  lsh_family::load(iarc);
  iarc >> rand_mat;
}

std::vector<int> lsh_cosine::hash_vector_to_codes(const DenseVector& vec,
                                                  bool is_reference_data) const {
  std::vector<int> ret(num_projections, -1);
  DenseVector hash_vec = rand_mat * vec;
  parallel_for (0, num_projections, [&](size_t hash_idx) {
    ret[hash_idx] = (hash_vec(hash_idx) > 0.) ? 1 : 0;
  });
  return ret;
}

std::vector<int> lsh_cosine::hash_vector_to_codes(const SparseVector& vec,
                                                  bool is_reference_data) const {
  std::vector<int> ret(num_projections, -1);

  if (vec.nonZeros() == 0) return ret;

  DenseVector hash_vec = rand_mat * vec;
  parallel_for (0, num_projections, [&](size_t hash_idx) {
    ret[hash_idx] = (hash_vec(hash_idx) > 0.) ? 1 : 0;
  });
  return ret;
}

void lsh_jaccard::init_model(size_t num_dimensions) {
  num_input_dimensions = num_dimensions;
  if (num_input_dimensions < num_projections) {
    log_and_throw("When number of input dimensions is smaller than \
                  num_tables * num_projections_per_table, LSH-Jaccard is not recommended.");
  }
  rand_permutation.assign(num_input_dimensions, 0);
  rand_sign.assign(num_input_dimensions, 0);
  parallel_for (0, num_input_dimensions, [&](size_t idx) {
    rand_permutation[idx] = idx;
    if (random::fast_uniform<double>(0., 1.) > 0.5) {
      rand_sign[idx] = 1;
    }
  });
  random::shuffle(rand_permutation);
}

void lsh_jaccard::save(turi::oarchive& oarc) const {
  lsh_family::save(oarc);
  oarc << rand_permutation
      << rand_sign;
}

void lsh_jaccard::load(turi::iarchive& iarc) {
  lsh_family::load(iarc);
  iarc >> rand_permutation
      >> rand_sign;
}

// The procedure is similar with hash_vector_to_codes(SparseVector)
// See details in that function
std::vector<int> lsh_jaccard::hash_vector_to_codes(const DenseVector& vec,
                                                   bool is_reference_data) const {
  std::vector<int> ret(num_projections, num_input_dimensions);

  // Note that the size of the last chunk might be larger than chunk_size
  size_t chunk_size = num_input_dimensions / num_projections;
  size_t chunk_idx, chunk_offset;
  size_t permuted_idx;
  size_t cnt = 0;
  for (size_t idx = 0; idx < size_t(vec.size()); ++idx) {
    if (vec(idx) < 1e-8) {
      // skip
    } else {
      permuted_idx = rand_permutation[idx];
      chunk_idx = std::min(permuted_idx / chunk_size, num_projections - 1);
      chunk_offset = permuted_idx - chunk_idx * chunk_size;
      DASSERT_LT(chunk_idx, num_projections);
      ret[chunk_idx] = std::min(int(chunk_offset), ret[chunk_idx]);
      ++cnt;
    }
  }
  if (cnt > 0) {
    fill_empty_bins(ret);
  }
  return ret;
}

std::vector<int> lsh_jaccard::hash_vector_to_codes(const SparseVector& vec,
                                                   bool is_reference_data) const {
  // Details in
  // http://jmlr.org/proceedings/papers/v32/shrivastava14.pdf
  // Figure 4
  // and
  // http://www.auai.org/uai2014/proceedings/individuals/225.pdf
  // Figure 5

  // Initialize the hash code. All the values are set to D (num_input_dimensions).
  std::vector<int> ret(num_projections, num_input_dimensions);

  if (vec.nonZeros() == 0) return ret;

  // chunk_size = D/K, where K is num_projections
  // Note that the size of the last chunk might be larger than chunk_size,
  // when D % K != 0
  size_t chunk_size = num_input_dimensions / num_projections;
  size_t chunk_idx, chunk_offset;
  size_t permuted_idx;
  SparseVector::InnerIterator it(vec, 0);
  size_t idx;
  while (it) {
    idx = it.index();
    permuted_idx = rand_permutation[idx];
    // which chunk it is in
    chunk_idx = std::min(permuted_idx / chunk_size, num_projections - 1);
    // the offset inside the chunk
    chunk_offset = permuted_idx - chunk_idx * chunk_size;
    // update
    DASSERT_LT(chunk_idx, num_projections);
    ret[chunk_idx] = std::min(int(chunk_offset), ret[chunk_idx]);
    ++it;
  }

  // there are still some empty bins left
  fill_empty_bins(ret);
  return ret;
}

// this function is used to fill the empty bins in the hash codes
void lsh_jaccard::fill_empty_bins(std::vector<int>& vec) const {
  size_t chunk_size = num_input_dimensions / num_projections;

  int64_t num_input_dimensions_i64 = truncate_check<int64_t>(num_input_dimensions);
  int64_t num_projections_i64 = truncate_check<int64_t>(num_projections);
  int64_t chunk_size_i64 = truncate_check<int64_t>(chunk_size);

  // find the first non-empty bin for left rotating
  int start_idx_left = 0;
  // find the first non-empty bin for right rotating
  int start_idx_right = num_projections - 1;

  while (vec[start_idx_left] == num_input_dimensions_i64 // if it is not updated since initialized
         && start_idx_left < num_projections_i64) {
    ++start_idx_left;
  }

  while (vec[start_idx_right] == num_input_dimensions_i64 // if it is not updated since initialized
         && start_idx_right >= 0) {
    --start_idx_right;
  }

  // no empty bins
  if (start_idx_left == num_projections_i64 || start_idx_right == int(-1)) {
    return;
  }

  // OK. We get a non-empty bin, go back to update empty bins
  int current_offset = vec[start_idx_left];
  int num_straight_empty_bins = 0;

  for (int idx = start_idx_left + num_projections; idx != start_idx_left; --idx) {
    if (vec[idx % num_projections] >= 2 * chunk_size_i64) { // empty
      num_straight_empty_bins += 1;
      // h_j = h_{j+t} + t * C, where t is distance to the nearest non-empty bin
      // C is a constant >= chunk_size, here we take C = chunk_size * 2
      if (rand_sign[idx % num_projections] == 1) {
        vec[idx % num_projections] = current_offset + num_straight_empty_bins * chunk_size * 2;
      }
    } else { // else, update the cached offset
      current_offset = vec[idx % num_projections];
      num_straight_empty_bins = 0;
    }
  }

  current_offset = vec[start_idx_right];
  num_straight_empty_bins = 0;

  for (int idx = start_idx_right; idx != start_idx_right + num_projections_i64; ++idx) {
    if (vec[idx % num_projections] >= 2 * chunk_size_i64) {
      num_straight_empty_bins += 1;
      // h_j = h_{j+t} + t * C, where t is distance to the nearest non-empty bin
      // C is a constant >= chunk_size, here we take C = chunk_size * 2
      if (rand_sign[idx % num_projections] == 0) {
        vec[idx % num_projections] = current_offset + num_straight_empty_bins * chunk_size * 2;
      }
    } else {
      current_offset = vec[idx % num_projections];
      num_straight_empty_bins = 0;
    }
  }
}

void lsh_dot_product::init_model(size_t num_dimensions) {
  num_input_dimensions = num_dimensions;
  max_vec_norm = 0.;

  // one extra dimension for the asymmetric vector transformation
  rand_mat.resize(num_projections, num_input_dimensions);
  rand_mat = rand_mat.unaryExpr([](double x) { return random::normal(0., 1.); });
  rand_vec.resize(num_projections);
  rand_vec = rand_vec.unaryExpr([](double x) { return random::normal(0., 1.); });
}

// get the max of the norm of the reference data
void lsh_dot_product::pre_lsh(const v2::ml_data& mld_ref, bool is_sparse) {
  size_t max_num_threads = thread::cpu_count();
  std::vector<double> local_max_vec_norms(max_num_threads, 0.);
  if (!is_sparse) {
    in_parallel([&](size_t thread_idx, size_t num_threads) GL_GCC_ONLY(GL_HOT) {
      DenseVector v(num_input_dimensions);
      for (auto it = mld_ref.get_iterator(thread_idx, num_threads); !it.done(); ++it) {
        it.fill_observation(v);
        local_max_vec_norms[thread_idx] = std::max(local_max_vec_norms[thread_idx], v.norm());
      }
    });
  } else {
    in_parallel([&](size_t thread_idx, size_t num_threads) GL_GCC_ONLY(GL_HOT) {
      SparseVector s(num_input_dimensions);
      for (auto it = mld_ref.get_iterator(thread_idx, num_threads); !it.done(); ++it) {
        it.fill_observation(s);
        if (s.nonZeros() > 0) {
          local_max_vec_norms[thread_idx] = std::max(local_max_vec_norms[thread_idx], s.norm());
        }
      }
    });
  }
  max_vec_norm = * std::max_element(local_max_vec_norms.begin(), local_max_vec_norms.end());
}

void lsh_dot_product::save(turi::oarchive& oarc) const {
  lsh_family::save(oarc);
  oarc << max_vec_norm << rand_mat << rand_vec;
}

void lsh_dot_product::load(turi::iarchive& iarc) {
  lsh_family::load(iarc);
  iarc >> max_vec_norm >> rand_mat >> rand_vec;
}

// Implementation of section 4.2 of http://jmlr.org/proceedings/papers/v37/neyshabur15.pdf
std::vector<int> lsh_dot_product::hash_vector_to_codes(const DenseVector& vec,
                                                       bool is_reference_data) const {
  std::vector<int> ret(num_projections, -1);
  DenseVector hash_vec(num_projections);

  if (is_reference_data) {
    hash_vec = rand_mat * vec / max_vec_norm +
        rand_vec * std::sqrt(1. - vec.squaredNorm() / (max_vec_norm * max_vec_norm));
  } else {
    // query vecs are normalized
    double vec_norm = vec.norm();
    if (vec_norm > 1e-16) {
      hash_vec = rand_mat * vec / vec_norm;
    }
  }

  parallel_for (0, num_projections, [&](size_t hash_idx) {
    ret[hash_idx] = (hash_vec(hash_idx) > 0.) ? 1 : 0;
  });
  return ret;
}

// Implementation of section 4.2 of http://jmlr.org/proceedings/papers/v37/neyshabur15.pdf
std::vector<int> lsh_dot_product::hash_vector_to_codes(const SparseVector& vec,
                                                       bool is_reference_data) const {
  std::vector<int> ret(num_projections, -1);

  if (vec.nonZeros() == 0) return ret;

  DenseVector hash_vec(num_projections);

  if (is_reference_data) {
    hash_vec = rand_mat * vec / max_vec_norm +
        rand_vec * std::sqrt(1. - vec.squaredNorm() / (max_vec_norm * max_vec_norm));
  } else {
    // query vecs are normalized
    double vec_norm = vec.norm();
    if (vec_norm > 1e-16) {
      hash_vec = rand_mat * vec / vec_norm;
    }
  }
  parallel_for (0, num_projections, [&](size_t hash_idx) {
    ret[hash_idx] = (hash_vec(hash_idx) > 0.) ? 1 : 0;
  });
  return ret;
}

} // namespace nearest_neighbors
} // namespace turi


// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/**
 * \file stereo_transform_model.cpp
 * \brief
 * \details This file defines the StereoTransformModel class, which provides a
 * solver for stereo pose estimation problems
 *
 * \author Kirk MacTavish, Autonomous Space Robotics Lab (ASRL)
 */
#include <chrono>

#include <Eigen/Geometry>
#include <Eigen/SVD>

#include <vtr_vision/sensors/register_svd.hpp>
#include <vtr_vision/sensors/stereo_transform_model.hpp>

namespace vtr {
namespace vision {

template class CameraModelInterface<Eigen::Matrix3Xd, Eigen::Matrix3Xd>;

bool StereoTransformModel::solveModel(const SimpleMatches& matches,
                                      SolutionType* p_solution,
                                      double threshold) const {
  // Check the input
  if (matches.size() < N || !p_solution) return false;

  // Copy inlier points into an Eigen matrix
  Eigen::Matrix<double, 3, Eigen::Dynamic> inliers_ref(3, matches.size());
  Eigen::Matrix<double, 3, Eigen::Dynamic> inliers_cam(3, matches.size());
  for (unsigned int i = 0; i < matches.size(); ++i) {
    inliers_ref.col(i) = pts_ref_->col(matches[i].first);
    inliers_cam.col(i) = pts_query_->col(matches[i].second);
  }

  // Compute inverse distance weights for the SVD
  Eigen::Array<double, 1, Eigen::Dynamic> weights =
      1. / inliers_ref.row(2).array();

  // Run SVD and return on failure
  if (!registerSVD<3>(inliers_cam, inliers_ref, p_solution, weights)) {
    return false;
  }

#if 0
   std::cout << "ref:\n" << inliers_ref
   	    << "\ncam:\n" << inliers_cam
   	    << "\nr2c:\n" << (*p_solution)*inliers_ref.colwise().homogeneous()
   	    << std::endl;
#endif

  // Calculate error for the fit, if the points aren't inliers, it's a bad
  // sample
  ErrorList errors;
  computeError(matches, *p_solution, &errors, nullptr);

  if ((errors.colwise().norm().array() >= threshold).count() != 0) {
    return false;
  }

  // Good sample!
  return true;
}

bool StereoTransformModel::verifyMatches(const SimpleMatches& matches) const {
  // Check existence
  if (!pts_ref_ || !pts_query_) return false;

  // Verify indices to be within points size
  for (const auto& matche : matches)
    if (matche.first >= pts_ref_->size() || matche.second >= pts_query_->size())
      return false;

  return true;
}

bool StereoTransformModel::computeError(const SimpleMatches& matches,
                                        const SolutionType& model,
                                        ErrorList* whitened_errors,
                                        double* robust_error, double stop_error,
                                        double sigma2) const {
  // Initialize counters
  if (!whitened_errors && !robust_error) return false;
  if (whitened_errors) {
    whitened_errors->resize(matches.size());
  }
  double cum_error = 0.;

  // Convert the model to a transformation
  Eigen::Affine3d tf(model);

  // Calculate the error for each point
  for (unsigned int i = 0; i < matches.size() && cum_error < stop_error; ++i) {
    // Transform the reference point into the camera frame
    Eigen::Vector4d pt3dh_ref =
        model * pts_ref_->col(matches[i].first).homogeneous();
    Eigen::Vector4d pt3dh_cam =
        pts_query_->col(matches[i].second).homogeneous();

    // Project the reference and camera points into the camera frames
    Eigen::Vector2d pt2d_l_ref = (projection_l_ * pt3dh_ref).hnormalized();
    Eigen::Vector2d pt2d_l_cam = (projection_l_ * pt3dh_cam).hnormalized();

    Eigen::Vector2d pt2d_r_ref = (projection_r_ * pt3dh_ref).hnormalized();
    Eigen::Vector2d pt2d_r_cam = (projection_r_ * pt3dh_cam).hnormalized();

    // calculate the squared error for left and right cameras, taking into
    // account the covariance
    Eigen::Matrix2d S = Eigen::Matrix2d::Identity();
    if (2 * matches[i].first + 1 <= inv_r_list_.cols()) {
      S = inv_r_list_.block(0, 2 * matches[i].first, 2, 2);
    }

    Eigen::Vector2d dl = pt2d_l_cam - pt2d_l_ref;
    Eigen::Vector2d dr = pt2d_r_cam - pt2d_r_ref;
    double i_error_l = dl.transpose() * S * dl;
    double i_error_r = dr.transpose() * S * dr;

    // Calculate the mahalanobis error
    double i_mahalanobis2 = i_error_l + i_error_r;

#if 0
    std::cout << "model: " << model << "\ncam: "
          << camera_matrix_ << "\nref: "
          << pt3d_ref.transpose() << "\nref: "
          << pt2d_ref.transpose() << "\ncam: " << pts_cam_->col(matches[i].second).transpose() << "\ncam: "
          << pt2d_cam.transpose() << "\nerr: "
          << i_error << "\n\n" << std::endl;
#endif

    // Add to the error recorders
    if (whitened_errors) {
      (*whitened_errors)(i) = std::sqrt(i_mahalanobis2);
    }
    // sanity check
    if (i_mahalanobis2 != i_mahalanobis2 || std::isinf(i_mahalanobis2)) {
      cum_error++;
    } else {
      cum_error += i_mahalanobis2 / (i_mahalanobis2 + sigma2);
    }
  }

  // Copy to output
  if (robust_error) *robust_error = cum_error;

  // Not the best so far
  if (cum_error >= stop_error) return false;

  // The best so far!
  return true;
}

}  // namespace vision
}  // namespace vtr


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@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 TestClosure
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/closure.hpp>
#include <boost/compute/function.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/algorithm/transform_reduce.hpp>
#include <boost/compute/container/array.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/counting_iterator.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace compute = boost::compute;

BOOST_AUTO_TEST_CASE(add_two)
{
    int two = 2;
    BOOST_COMPUTE_CLOSURE(int, add_two, (int x), (two),
                          {
                              return x + two;
                          });

    int data[] = {1, 2, 3, 4};
    compute::vector<int> vector(data, data + 4, queue);

    compute::transform(
        vector.begin(), vector.end(), vector.begin(), add_two, queue);
    CHECK_RANGE_EQUAL(int, 4, vector, (3, 4, 5, 6));
}

BOOST_AUTO_TEST_CASE(add_two_and_pi)
{
    int two = 2;
    float pi = 3.14f;
    BOOST_COMPUTE_CLOSURE(float, add_two_and_pi, (float x), (two, pi),
                          {
                              return x + two + pi;
                          });

    float data[] = {1.9f, 2.2f, 3.4f, 4.7f};
    compute::vector<float> vector(data, data + 4, queue);

    compute::transform(
        vector.begin(), vector.end(), vector.begin(), add_two_and_pi, queue);

    std::vector<float, mi_stl_allocator<float>> results(4);
    compute::copy(vector.begin(), vector.end(), results.begin(), queue);
    BOOST_CHECK_CLOSE(results[0], 7.04f, 1e-6);
    BOOST_CHECK_CLOSE(results[1], 7.34f, 1e-6);
    BOOST_CHECK_CLOSE(results[2], 8.54f, 1e-6);
    BOOST_CHECK_CLOSE(results[3], 9.84f, 1e-6);
}

BOOST_AUTO_TEST_CASE(add_y)
{
    // setup input and output vectors
    int data[] = {1, 2, 3, 4};
    compute::vector<int> input(data, data + 4, queue);
    compute::vector<int> output(4, context);

    // make closure which adds 'y' to each value
    int y = 2;
    BOOST_COMPUTE_CLOSURE(int, add_y, (int x), (y),
                          {
                              return x + y;
                          });

    compute::transform(
        input.begin(), input.end(), output.begin(), add_y, queue);
    CHECK_RANGE_EQUAL(int, 4, output, (3, 4, 5, 6));

    // change y and run again
    y = 4;

    compute::transform(
        input.begin(), input.end(), output.begin(), add_y, queue);
    CHECK_RANGE_EQUAL(int, 4, output, (5, 6, 7, 8));
}

BOOST_AUTO_TEST_CASE(scale_add_vec)
{
    const int N = 10;
    float s = 4.5;
    compute::vector<float> a(N, context);
    compute::vector<float> b(N, context);
    a.assign(N, 1.0f, queue);
    b.assign(N, 2.0f, queue);

    BOOST_COMPUTE_CLOSURE(float, scaleAddVec, (float b, float a), (s),
                          {
                              return b * s + a;
                          });
    compute::transform(b.begin(), b.end(), a.begin(), b.begin(), scaleAddVec, queue);
}

BOOST_AUTO_TEST_CASE(capture_vector)
{
    int data[] = {6, 7, 8, 9};
    compute::vector<int> vec(data, data + 4, queue);

    BOOST_COMPUTE_CLOSURE(int, get_vec, (int i), (vec),
                          {
                              return vec[i];
                          });

    // run using a counting iterator to copy from vec to output
    compute::vector<int> output(4, context);
    compute::transform(
        compute::make_counting_iterator(0),
        compute::make_counting_iterator(4),
        output.begin(),
        get_vec,
        queue);
    CHECK_RANGE_EQUAL(int, 4, output, (6, 7, 8, 9));

    // fill vec with 4's and run again
    compute::fill(vec.begin(), vec.end(), 4, queue);
    compute::transform(
        compute::make_counting_iterator(0),
        compute::make_counting_iterator(4),
        output.begin(),
        get_vec,
        queue);
    CHECK_RANGE_EQUAL(int, 4, output, (4, 4, 4, 4));
}

BOOST_AUTO_TEST_CASE(capture_array)
{
    int data[] = {1, 2, 3, 4};
    compute::array<int, 4> array(context);
    compute::copy(data, data + 4, array.begin(), queue);

    BOOST_COMPUTE_CLOSURE(int, negative_array_value, (int i), (array),
                          {
                              return -array[i];
                          });

    compute::vector<int> output(4, context);
    compute::transform(
        compute::make_counting_iterator(0),
        compute::make_counting_iterator(4),
        output.begin(),
        negative_array_value,
        queue);
    CHECK_RANGE_EQUAL(int, 4, output, (-1, -2, -3, -4));
}

BOOST_AUTO_TEST_CASE(triangle_area)
{
    using compute::float4_;
    using compute::uint4_;

    compute::vector<uint4_> triangle_indices(context);
    compute::vector<float4_> triangle_vertices(context);

    triangle_vertices.push_back(float4_(0, 0, 0, 1), queue);
    triangle_vertices.push_back(float4_(1, 1, 0, 1), queue);
    triangle_vertices.push_back(float4_(1, 0, 0, 1), queue);
    triangle_vertices.push_back(float4_(2, 0, 0, 1), queue);

    triangle_indices.push_back(uint4_(0, 1, 2, 0), queue);
    triangle_indices.push_back(uint4_(2, 1, 3, 0), queue);

    queue.finish();

    BOOST_COMPUTE_CLOSURE(float, triangle_area, (const uint4_ i), (triangle_vertices),
                          {
                              // load triangle vertices
                              const float4 a = triangle_vertices[i.x];
                              const float4 b = triangle_vertices[i.y];
                              const float4 c = triangle_vertices[i.z];

                              // return area of triangle
                              return length(cross(b - a, c - a)) / 2;
                          });

    // compute area of each triangle
    compute::vector<float> triangle_areas(triangle_indices.size(), context);

    compute::transform(
        triangle_indices.begin(),
        triangle_indices.end(),
        triangle_areas.begin(),
        triangle_area,
        queue);

    // compute total area of all triangles
    float total_area = 0;

    compute::transform_reduce(
        triangle_indices.begin(),
        triangle_indices.end(),
        &total_area,
        triangle_area,
        compute::plus<float>(),
        queue);
    BOOST_CHECK_CLOSE(total_area, 1.f, 1e-6);
}

BOOST_AUTO_TEST_SUITE_END()