//  Copyright (c) 2018-2019 Cem Bassoy
//
//  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
//  Fraunhofer and Google in producing this work
//  which started as a Google Summer of Code project.
//

#include <boost/numeric/ublas/tensor/expression_evaluation.hpp>
#include <boost/numeric/ublas/tensor/expression.hpp>
#include <boost/numeric/ublas/tensor/tensor.hpp>
#include <boost/test/unit_test.hpp>
#include "utility.hpp"

#include <functional>

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

struct fixture
{
	using extents_type = boost::numeric::ublas::shape;
	fixture()
	  : extents{
	      extents_type{},            // 0

	      extents_type{1,1},         // 1
	      extents_type{1,2},         // 2
	      extents_type{2,1},         // 3

	      extents_type{2,3},         // 4
	      extents_type{2,3,1},       // 5
	      extents_type{1,2,3},       // 6
	      extents_type{1,1,2,3},     // 7
	      extents_type{1,2,3,1,1},   // 8

	      extents_type{4,2,3},       // 9
	      extents_type{4,2,1,3},     // 10
	      extents_type{4,2,1,3,1},   // 11
	      extents_type{1,4,2,1,3,1}}  // 12
	{
	}
	std::vector<extents_type> extents;
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_expression_retrieve_extents, value,  test_types, fixture)
{
	using namespace boost::numeric;
	using value_type  = typename value::first_type;
	using layout_type = typename value::second_type;
	using tensor_type = ublas::tensor<value_type, layout_type>;

	auto uplus1 = std::bind(  std::plus<value_type>{}, std::placeholders::_1, value_type(1) );
	auto uplus2 = std::bind(  std::plus<value_type>{}, value_type(2), std::placeholders::_2 );
	auto bplus  = std::plus <value_type>{};
	auto bminus = std::minus<value_type>{};

	for(auto const& e : extents) {

		auto t = tensor_type(e);
		auto v = value_type{};
		for(auto& tt: t){ tt = v; v+=value_type{1}; }

		BOOST_CHECK( ublas::detail::retrieve_extents( t ) == e );

		// uexpr1 = t+1
		// uexpr2 = 2+t
		auto uexpr1 = ublas::detail::make_unary_tensor_expression<tensor_type>( t, uplus1 );
		auto uexpr2 = ublas::detail::make_unary_tensor_expression<tensor_type>( t, uplus2 );

		BOOST_CHECK( ublas::detail::retrieve_extents( uexpr1 ) == e );
		BOOST_CHECK( ublas::detail::retrieve_extents( uexpr2 ) == e );

		// bexpr_uexpr = (t+1) + (2+t)
		auto bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_type>( uexpr1, uexpr2, bplus );

		BOOST_CHECK( ublas::detail::retrieve_extents( bexpr_uexpr ) == e );

		// bexpr_bexpr_uexpr = ((t+1) + (2+t)) - t
		auto bexpr_bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_type>( bexpr_uexpr, t, bminus );

		BOOST_CHECK( ublas::detail::retrieve_extents( bexpr_bexpr_uexpr ) == e );

	}

	for(auto i = 0u; i < extents.size()-1; ++i)
	{

		auto v = value_type{};

		auto t1 = tensor_type(extents[i]);
		for(auto& tt: t1){ tt = v; v+=value_type{1}; }

		auto t2 = tensor_type(extents[i+1]);
		for(auto& tt: t2){ tt = v; v+=value_type{2}; }

		BOOST_CHECK( ublas::detail::retrieve_extents( t1 ) != ublas::detail::retrieve_extents( t2 ) );

		// uexpr1 = t1+1
		// uexpr2 = 2+t2
		auto uexpr1 = ublas::detail::make_unary_tensor_expression<tensor_type>( t1, uplus1 );
		auto uexpr2 = ublas::detail::make_unary_tensor_expression<tensor_type>( t2, uplus2 );

		BOOST_CHECK( ublas::detail::retrieve_extents( t1 )     == ublas::detail::retrieve_extents( uexpr1 ) );
		BOOST_CHECK( ublas::detail::retrieve_extents( t2 )     == ublas::detail::retrieve_extents( uexpr2 ) );
		BOOST_CHECK( ublas::detail::retrieve_extents( uexpr1 ) != ublas::detail::retrieve_extents( uexpr2 ) );

		// bexpr_uexpr = (t1+1) + (2+t2)
		auto bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_type>( uexpr1, uexpr2, bplus );

		BOOST_CHECK( ublas::detail::retrieve_extents( bexpr_uexpr ) == ublas::detail::retrieve_extents(t1) );

		// bexpr_bexpr_uexpr = ((t1+1) + (2+t2)) - t2
		auto bexpr_bexpr_uexpr1 = ublas::detail::make_binary_tensor_expression<tensor_type>( bexpr_uexpr, t2, bminus );

		BOOST_CHECK( ublas::detail::retrieve_extents( bexpr_bexpr_uexpr1 ) == ublas::detail::retrieve_extents(t2) );

		// bexpr_bexpr_uexpr = t2 - ((t1+1) + (2+t2))
		auto bexpr_bexpr_uexpr2 = ublas::detail::make_binary_tensor_expression<tensor_type>( t2, bexpr_uexpr, bminus );

		BOOST_CHECK( ublas::detail::retrieve_extents( bexpr_bexpr_uexpr2 ) == ublas::detail::retrieve_extents(t2) );
	}
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_expression_all_extents_equal, value,  test_types, fixture)
{
	using namespace boost::numeric;
	using value_type  = typename value::first_type;
	using layout_type = typename value::second_type;
	using tensor_type = ublas::tensor<value_type, layout_type>;

	auto uplus1 = std::bind(  std::plus<value_type>{}, std::placeholders::_1, value_type(1) );
	auto uplus2 = std::bind(  std::plus<value_type>{}, value_type(2), std::placeholders::_2 );
	auto bplus  = std::plus <value_type>{};
	auto bminus = std::minus<value_type>{};

	for(auto const& e : extents) {

		auto t = tensor_type(e);
		auto v = value_type{};
		for(auto& tt: t){ tt = v; v+=value_type{1}; }

		BOOST_CHECK( ublas::detail::all_extents_equal( t , e ) );

		// uexpr1 = t+1
		// uexpr2 = 2+t
		auto uexpr1 = ublas::detail::make_unary_tensor_expression<tensor_type>( t, uplus1 );
		auto uexpr2 = ublas::detail::make_unary_tensor_expression<tensor_type>( t, uplus2 );

		BOOST_CHECK( ublas::detail::all_extents_equal( uexpr1, e ) );
		BOOST_CHECK( ublas::detail::all_extents_equal( uexpr2, e ) );

		// bexpr_uexpr = (t+1) + (2+t)
		auto bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_type>( uexpr1, uexpr2, bplus );

		BOOST_CHECK( ublas::detail::all_extents_equal( bexpr_uexpr, e ) );

		// bexpr_bexpr_uexpr = ((t+1) + (2+t)) - t
		auto bexpr_bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_type>( bexpr_uexpr, t, bminus );

		BOOST_CHECK( ublas::detail::all_extents_equal( bexpr_bexpr_uexpr , e ) );

	}

	for(auto i = 0u; i < extents.size()-1; ++i)
	{

		auto v = value_type{};

		auto t1 = tensor_type(extents[i]);
		for(auto& tt: t1){ tt = v; v+=value_type{1}; }

		auto t2 = tensor_type(extents[i+1]);
		for(auto& tt: t2){ tt = v; v+=value_type{2}; }

		BOOST_CHECK( ublas::detail::all_extents_equal( t1, ublas::detail::retrieve_extents(t1) ) );
		BOOST_CHECK( ublas::detail::all_extents_equal( t2, ublas::detail::retrieve_extents(t2) ) );

		// uexpr1 = t1+1
		// uexpr2 = 2+t2
		auto uexpr1 = ublas::detail::make_unary_tensor_expression<tensor_type>( t1, uplus1 );
		auto uexpr2 = ublas::detail::make_unary_tensor_expression<tensor_type>( t2, uplus2 );

		BOOST_CHECK( ublas::detail::all_extents_equal( uexpr1, ublas::detail::retrieve_extents(uexpr1) ) );
		BOOST_CHECK( ublas::detail::all_extents_equal( uexpr2, ublas::detail::retrieve_extents(uexpr2) ) );

		// bexpr_uexpr = (t1+1) + (2+t2)
		auto bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_type>( uexpr1, uexpr2, bplus );

		BOOST_CHECK( ! ublas::detail::all_extents_equal( bexpr_uexpr, ublas::detail::retrieve_extents( bexpr_uexpr  ) ) );

		// bexpr_bexpr_uexpr = ((t1+1) + (2+t2)) - t2
		auto bexpr_bexpr_uexpr1 = ublas::detail::make_binary_tensor_expression<tensor_type>( bexpr_uexpr, t2, bminus );

		BOOST_CHECK( ! ublas::detail::all_extents_equal( bexpr_bexpr_uexpr1, ublas::detail::retrieve_extents( bexpr_bexpr_uexpr1  ) ) );

		// bexpr_bexpr_uexpr = t2 - ((t1+1) + (2+t2))
		auto bexpr_bexpr_uexpr2 = ublas::detail::make_binary_tensor_expression<tensor_type>( t2, bexpr_uexpr, bminus );

		BOOST_CHECK( ! ublas::detail::all_extents_equal( bexpr_bexpr_uexpr2, ublas::detail::retrieve_extents( bexpr_bexpr_uexpr2  ) ) );

		// bexpr_uexpr2 = (t1+1) + t2
		auto bexpr_uexpr2 = ublas::detail::make_binary_tensor_expression<tensor_type>( uexpr1, t2, bplus );
		BOOST_CHECK( ! ublas::detail::all_extents_equal( bexpr_uexpr2, ublas::detail::retrieve_extents( bexpr_uexpr2  ) ) );

		// bexpr_uexpr2 = ((t1+1) + t2) + t1
		auto bexpr_bexpr_uexpr3 = ublas::detail::make_binary_tensor_expression<tensor_type>( bexpr_uexpr2, t1, bplus );
		BOOST_CHECK( ! ublas::detail::all_extents_equal( bexpr_bexpr_uexpr3, ublas::detail::retrieve_extents( bexpr_bexpr_uexpr3  ) ) );

		// bexpr_uexpr2 = t1 + (((t1+1) + t2) + t1)
		auto bexpr_bexpr_uexpr4 = ublas::detail::make_binary_tensor_expression<tensor_type>( t1, bexpr_bexpr_uexpr3, bplus );
		BOOST_CHECK( ! ublas::detail::all_extents_equal( bexpr_bexpr_uexpr4, ublas::detail::retrieve_extents( bexpr_bexpr_uexpr4  ) ) );

	}
}


// Copyright 2014 Jonathan Graehl-http://graehl.org/
//
// 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

    (with options for add-k smoothing) normalize counts into probabilities
*/

#ifndef GRAEHL_SHARED__NORMALIZE_RANGE_HPP
#define GRAEHL_SHARED__NORMALIZE_RANGE_HPP
#pragma once

#include <boost/range.hpp>
#include <boost/range/algorithm/transform.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/value_type.hpp>

namespace graehl {

template <class R>
typename boost::range_value<R>::type sum(R const& r, typename boost::range_value<R>::type z
                                                     = typename boost::range_value<R>::type()) {
  return boost::accumulate(r, z);
}

template <class W>
struct normalize_options {
  W addk_num;
  W addk_denom;
  explicit normalize_options(W addk_num = W(), W addk_denom = W())
      : addk_num(addk_num), addk_denom(addk_denom) {}
  void addUnseen(W n) {
    addk_denom += n * addk_num;  // there are n tokens with 0 count; intend addk_num to apply to them also
  }
  static char const* caption() { return "Probability Normalization"; }
  template <class Config>
  void configure(Config& c) {
    c.is("add-k normalization");
    c('k')("addk,k", &addk_num)("add counts of [addk] to every observed event before normalizing");
    c('K')("denom-addk,K", &addk_denom)("add [denom-addk] to the denominator only when normalizing");
  }
};

template <class W>
struct normalize_addk {
  W knum;
  W denom;
  normalize_addk(std::size_t N, W sum_unsmoothed, W addk_num = 0, W addk_denom = 0) : knum(addk_num) {
    denom = sum_unsmoothed + knum * N + addk_denom;
  }
  normalize_addk(std::size_t N, W sum_unsmoothed, normalize_options<W> const& n) : knum(n.addk_num) {
    denom = sum_unsmoothed + knum * N + n.addk_denom;
  }
  W operator()(W c) const {
    return (c + knum) / denom;
  }  // maybe some semirings don't have * (1/denom) but have / denom ?
};

template <class R, class O>
O normalize_copy(typename boost::range_value<R>::type sum, R const& r, O o,
                 normalize_options<typename boost::range_value<R>::type> n
                 = normalize_options<typename boost::range_value<R>::type>()) {
  typedef typename boost::range_value<R>::type V;
  return boost::transform(r, o, normalize_addk<V>(boost::size(r), sum, n));
}

template <class R, class O>
O normalize_copy(R const& r, O o, normalize_options<typename boost::range_value<R>::type> n
                                  = normalize_options<typename boost::range_value<R>::type>()) {
  return normalize_copy(sum(r), r, boost::begin(r), n);
}

template <class R>
void normalize_sum(typename boost::range_value<R>::type sum, R& r,
                   normalize_options<typename boost::range_value<R>::type> n
                   = normalize_options<typename boost::range_value<R>::type>()) {
  normalize_copy(sum, r, boost::begin(r), n);
}

template <class R>
void normalize(R& r, normalize_options<typename boost::range_value<R>::type> n
                     = normalize_options<typename boost::range_value<R>::type>()) {
  normalize_sum(sum(r), r, n);
}

}

#endif


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

#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>

using namespace popart;

BOOST_AUTO_TEST_CASE(LogicalIf_builder0) {

  // A ---|
  //      |---- (Add) ----------|
  //      |                     |
  //      |                     |
  // B ===|---- (Relu) -----|   |
  //                        |   |
  //                    ------- If ----- output
  //                    |
  //                    |
  // C (bool) ----------|

  TensorInfo infoData{"FLOAT", std::vector<int64_t>{4, 4}};
  TensorInfo infoBool{"BOOL", std::vector<int64_t>{}};

  // Graph, level 0, (top level)
  auto builder0 = Builder::create();
  auto aiOnnx0  = builder0->aiOnnxOpset9();
  auto A        = builder0->addInputTensor(infoData);
  auto B        = builder0->addInputTensor(infoData);
  auto C        = builder0->addInputTensor(infoBool);

  // Graph, level 1, false branch : A + B
  Builder &builder10 = builder0->createSubgraphBuilder();
  auto aiOnnx10      = builder10.aiOnnxOpset9();
  // the name comes from the parent graph
  builder10.addInputTensorFromParentGraph(A);
  builder10.addInputTensorFromParentGraph(B);
  auto out10 = aiOnnx10.add({A, B});
  builder10.addOutputTensor(out10);

  // Graph, level 1, true branch : relu(B)
  Builder &builder11 = builder0->createSubgraphBuilder();
  auto aiOnnx11      = builder11.aiOnnxOpset9();
  builder11.addInputTensorFromParentGraph(B);
  auto out11 = aiOnnx11.relu({B});
  builder11.addOutputTensor(out11);

  auto out_if = aiOnnx0.logical_if(
      {C},
      // number of outputs (must be same along true and false branches)
      1,
      // Builder for false branch
      builder10,
      // Builder for true branch
      builder11);

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

  auto finalGraph = modelProto.graph();

  // A single "If" NodeProto:
  BOOST_CHECK(modelProto.graph().node_size() == 1);
  BOOST_CHECK(modelProto.graph().node(0).op_type() == "If");
  // The "If" NodeProto has 2 (sub-graph) attributes + 1 for debugIf:
  BOOST_CHECK(modelProto.graph().node(0).attribute_size() == 2 + 1);
}


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/concept/struct.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/unpack.hpp>

#include "minimal_struct.hpp"
#include <laws/base.hpp>
#include <support/minimal_product.hpp>
namespace hana = boost::hana;
using hana::test::ct_eq;

int main() {
    constexpr auto pair = ::minimal_product;
    hana::test::_injection<0> f{};

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::unpack(obj(), f),
        f()
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::unpack(obj(ct_eq<0>{}), f),
        f(pair(hana::int_c<0>, ct_eq<0>{}))
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::unpack(obj(ct_eq<0>{}, ct_eq<1>{}), f),
        f(pair(hana::int_c<0>, ct_eq<0>{}),
          pair(hana::int_c<1>, ct_eq<1>{}))
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::unpack(obj(ct_eq<0>{}, ct_eq<1>{}, ct_eq<2>{}), f),
        f(pair(hana::int_c<0>, ct_eq<0>{}),
          pair(hana::int_c<1>, ct_eq<1>{}),
          pair(hana::int_c<2>, ct_eq<2>{}))
    ));
}


/*
 * Copyright (c) 2013-2018 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ODE_AFFINE_HPP
#define ODE_AFFINE_HPP

// ODE using Affine

#include <iostream>
#include <cmath>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <kv/make-candidate.hpp>
#include <kv/psa.hpp>
#include <kv/affine.hpp>
#include <kv/ode-param.hpp>
#include <kv/ode-callback.hpp>

#ifndef ODE_FAST
#define ODE_FAST 1
#endif

#ifndef ODE_RESTART_RATIO
#define ODE_RESTART_RATIO 1
#endif

#ifndef ODE_COEF_MID
#define ODE_CORF_MID 0
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
int
ode_affine(F f, ub::vector< affine<T> >& init, const interval<T>& start, interval<T>& end, ode_param<T> p = ode_param<T>(), ub::vector< psa< interval<T> > >* result_psa = NULL)
{
	int n = init.size();
	int i, j;

	ub::vector< psa< affine<T> > > x, y;
	psa< affine<T> > torg;
	psa< affine<T> > t;

	ub::vector< psa< affine<T> > > z, w;

	psa< affine<T> > temp;
	T m;
	ub::vector<T> newton_step;

	bool flag, resized;

	affine<T> s1, s2;
	interval<T> s2i;

	ub::vector< affine<T> > s1_save;
	ub::vector< interval<T> > s2i_save;

	interval<T> deltat;
	ub::vector< affine<T> > result;

	T radius, radius_tmp;
	T tolerance;
	int n_rad;
	#if ODE_RESTART_RATIO == 1
	T max_ratio;
	#endif

	int ret_val;
	interval<T> end2;
	int restart;

	int maxnum_save = affine<T>::maxnum();

	bool save_mode, save_uh, save_rh;

	m = 1.;
	for (i=0; i<n; i++) {
		m = std::max(m, norm(to_interval(init(i))));
	}
	tolerance = m * p.epsilon;

	x = init;
	torg.v.resize(2);
	torg.v(0) = start; torg.v(1) = 1.;

	save_mode = psa< affine<T> >::mode();
	save_uh = psa< affine<T> >::use_history();
	save_rh = psa< affine<T> >::record_history();
	psa< affine<T> >::mode() = 1;
	psa< affine<T> >::use_history() = false;
	psa< affine<T> >::record_history() = false;
	#if ODE_FAST == 1
	psa< affine<T> >::record_history() = true;
	psa< affine<T> >::history().clear();
	#endif
	for (j=0; j<p.order; j++) {
		#if ODE_FAST == 1
		if (j == 1) psa< affine<T> >::use_history() = true;
		#endif
		t = setorder(torg, j);
		y = f(x, t);
		for (i=0; i<n; i++) {
			y(i) = integrate(y(i));
			y(i) = setorder(y(i), j+1);
		}
		x = init + y;
	}

	if (p.autostep) {
		radius = 0.;
		n_rad = 0;
		for (j = p.order; j>=1; j--) {
			m = 0.;
			for (i=0; i<n; i++) {
				#if ODE_COEF_MID == 1
				using std::abs;
				m = std::max(m, abs(x(i).v(j).get_mid()));
				#else
				m = std::max(m, norm(to_interval(x(i).v(j))));
				#endif
			}
			if (m == 0.) continue;
			radius = std::max(radius, (T)std::pow((double)m, 1./j));
			n_rad++;
			if (n_rad == 2) break;
		}
		radius = std::pow((double)tolerance, 1./p.order) / radius;
	}

	psa< affine<T> >::mode() = 2;

	restart = 0;
	// disable resize because resize algorithm is not suitable
	//  for ode-affine
	// resized = false;
	resized = true;

	while (true) {
		if (p.autostep) {
			end2 = mid(start + radius);
			if (end2 >= end.lower()) {
				end2 = end;
				radius = mid(end2 - start);
				ret_val = 2;
			} else {
				ret_val = 1;
			}
		} else {
			end2 = end;
			ret_val = 2;
		}
		deltat = end2 - start;

		psa< affine<T> >::domain() = interval<T>(0., deltat.upper());

		z = x;
		t = setorder(torg, p.order);

		try {
			w = f(z, t);
		}
		catch (std::domain_error& e) {
			 if (p.autostep && restart < p.restart_max) {
				psa< affine<T> >::use_history() = false;
				if (p.verbose == 1) {
					std::cout << "ode: radius changed: " << radius;
				}
				radius *= 0.5;
				if (p.verbose == 1) {
					std::cout << " -> " << radius << "\n";
				}
				restart++;
				continue;
			} else {
				throw std::domain_error("ode_affine: evaluation error");
			}
		}

		for (i=0; i<n; i++) {
			temp = integrate(w(i));
			w(i) = setorder(temp, p.order);
		}
		w = init + w;

		newton_step.resize(n);
		for (i=0; i<n; i++) {
			newton_step(i) = norm(to_interval(w(i).v(p.order) - z(i).v(p.order)));
		}
		make_candidate(newton_step);

		s1_save.resize(n);
		s2i_save.resize(n);

		for (i=0; i<n; i++) {
			z(i).v(p.order) += (affine<T>)(newton_step(i) * interval<T>(-1., 1.));
			#ifdef ODE_AFFINE_SIMPLE
			s2i = to_interval(z(i).v(p.order));
			s2i_save(i) = s2i;
			z(i).v(p.order) = (affine<T>)s2i;
			#else
			split(z(i).v(p.order), maxnum_save, s1, s2);
			s2i = to_interval(s2);
			s1_save(i) = s1;
			s2i_save(i) = s2i;
			z(i).v(p.order) = append(s1, (affine<T>)s2i);
			#endif
		}

		if (p.autostep && ret_val != 2 && resized == false) {
			resized = true;
			m = (std::numeric_limits<T>::min)();
			for (i=0; i<n; i++) {
				m = std::max(m, rad(eval(z(i), (affine<T>)deltat)) - rad(init(i)));
			}
			m = m / tolerance;
			radius_tmp = radius / std::pow((double)m, 1. / p.order);
			if (radius_tmp >= radius && restart > 0) {
				// do nothing, not continue
			} else {
				radius = radius_tmp;
				continue;
			}
		}

		w = f(z, t);
		for (i=0; i<n; i++) {
			temp = integrate(w(i));
			w(i) = setorder(temp, p.order);
		}
		w = init + w;

		flag = true;
		#if ODE_RESTART_RATIO == 1
		max_ratio = 0.;
		#endif
		for (i=0; i<n; i++) {
			#ifdef ODE_AFFINE_SIMPLE
			s2i = to_interval(w(i).v(p.order));
			#if ODE_RESTART_RATIO == 1
			max_ratio = std::max(max_ratio, width(s2i) / width(s2i_save(i)));
			#endif
			flag = flag && subset(s2i, s2i_save(i));
			s2i_save(i) = s2i;
			w(i).v(p.order) = (affine<T>)s2i;
			#else
			split(w(i).v(p.order), maxnum_save, s1, s2);
			s2i = to_interval(s2);
			#if ODE_RESTART_RATIO == 1
			max_ratio = std::max(max_ratio, width(to_interval(s1 - s1_save(i)) + s2i) / width(s2i_save(i)));
			#endif
			flag = flag && subset(to_interval(s1 - s1_save(i)) + s2i, s2i_save(i));
			s1_save(i) = s1;
			s2i_save(i) = s2i;
			w(i).v(p.order) = append(s1, (affine<T>)s2i);
			#endif
		}
		if (flag) break;

		if (!p.autostep || restart >= p.restart_max) {
			ret_val = 0;
			break;
		}
		if (p.verbose == 1) {
			std::cout << "ode: radius changed: " << radius;
		}
		#if ODE_RESTART_RATIO == 1
		radius *= std::max(std::min((T)0.5, (T)0.5 / max_ratio), (T)0.125);
		#else
		radius *= 0.5;
		#endif
		if (p.verbose == 1) {
			std::cout << " -> " << radius << "\n";
		}
		restart++;
	}

	if (ret_val != 0) {
		for (j=0; j<p.iteration; j++) {
			w = f(w, t);
			for (i=0; i<n; i++) {
				temp = integrate(w(i));
				w(i) = setorder(temp, p.order);
			}
			w = init + w;

			for (i=0; i<n; i++) {
				#ifdef ODE_AFFINE_SIMPLE
				s2i = to_interval(w(i).v(p.order));
				s2i = intersect(s2i, s2i_save(i));
				s2i_save(i) = s2i;
				w(i).v(p.order) = (affine<T>)s2i;
				#else
				split(w(i).v(p.order), maxnum_save, s1, s2);
				s2i = to_interval(s2);
				// s2i = intersect(to_interval(s1 - s1_save(i)) + s2i, s2i_save(i));
				s2i = intersect(to_interval(s1 - s1_save(i)) + s2i_save(i), s2i);
				s1_save(i) = s1;
				s2i_save(i) = s2i;
				w(i).v(p.order) = append(s1, (affine<T>)s2i);
				#endif
			}
		}

		if (result_psa != NULL) {
			// convert w to interval and store it to *result_psa
			(*result_psa).resize(n);
			for (i=0; i<n; i++) {
				(*result_psa)(i).v.resize(w(i).v.size());
				for (j=0; j<w(i).v.size(); j++) {
					(*result_psa)(i).v(j) = to_interval(w(i).v(j));
				}
			}
		}

		result.resize(n);
		for (i=0; i<n; i++) {
			result(i) = eval(w(i), (affine<T>)deltat);
			if (p.ep_reduce == 0) {
				split(result(i), maxnum_save, s1, s2);
				s2i = to_interval(s2);
				s1_save(i) = s1;
				s2i_save(i) = s2i;
			}
		}

		if (p.ep_reduce == 0) {
			affine<T>::maxnum() = maxnum_save;
			for (i=0; i<n; i++) {
				s1_save(i).resize();
				result(i) = append(s1_save(i), (affine<T>)s2i_save(i));
			}
		} else {
			epsilon_reduce(result, p.ep_reduce, p.ep_reduce_limit);
		}

		init = result;
		if (ret_val == 1) end = end2;
	}

	psa< affine<T> >::mode() = save_mode;
	psa< affine<T> >::use_history() = save_uh;
	psa< affine<T> >::record_history() = save_rh;

	return ret_val;
}

template <class T, class F>
int
odelong_affine(
	F f,
	ub::vector< affine<T> >& init,
	const interval<T>& start,
	interval<T>& end,
	ode_param<T> p = ode_param<T>(),
	const ode_callback<T>& callback = ode_callback<T>()
) {
	int s = init.size();
	ub::vector< affine<T> > x, x1;
	interval<T> t, t1;
	int ret_ode;
	int ret_val = 0;
	bool ret_callback;

	ub::vector< psa< interval<T> > > result_tmp;

	x = init;
	t = start;
	p.set_autostep(true);

	while (1) {
		x1 = x;
		t1 = end;

		ret_ode = ode_affine(f, x1, t, t1, p, &result_tmp);
		if (ret_ode == 0) {
			if (ret_val == 1) {
				init = x1;
				end = t;
			}
			return ret_val;
		}
		ret_val = 1;
		if (p.verbose == 1) {
			std::cout << "t: " << t1 << "\n";
			std::cout << to_interval(x1) << "\n";
		}

		callback(t, t1, to_interval(x), to_interval(x1), result_tmp);

		if (ret_callback == false) {
			init = x1;
			end = t1;
			return 3;
		}

		if (ret_ode == 2) {
			init = x1;
			return 2;
		}

		t = t1;
		x = x1;
	}
}

template <class T, class F>
int
odelong_affine(
	F f,
	ub::vector< interval<T> >& init,
	const interval<T>& start,
	interval<T>& end,
	ode_param<T> p = ode_param<T>(),
	const ode_callback<T>& callback = ode_callback<T>()
) {
	int s = init.size();
	int i;
	ub::vector< affine<T> > x;
	int maxnum_save;
	int r;

	maxnum_save = affine<T>::maxnum();
	affine<T>::maxnum() = 0;

	x = init;

	r = odelong_affine(f, x, start, end, p, callback);

	affine<T>::maxnum() = maxnum_save;

	if (r == 0) return 0;

	for (i=0; i<s; i++) init(i) = to_interval(x(i));

	return r;
}

} // namespace kv

#endif // ODE_AFFINE_HPP


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

#include <nt2/table.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/include/functions/colvect.hpp>
#include <nt2/include/functions/cons.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>
#include <boost/dispatch/meta/primitive_of.hpp>
#include <boost/fusion/include/vector.hpp>

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

NT2_TEST_CASE( extent )
{
  using nt2::table;
  using nt2::of_size_;
  using boost::mpl::_;

  table<double> d;
  table<float>  f;
  table<short>  s;
  table<char>   c;

  NT2_TEST_EQUAL( nt2::tie(d).extent()       , d.extent() );
  NT2_TEST_EQUAL( nt2::tie(f,d).extent()     , f.extent() );
  NT2_TEST_EQUAL( nt2::tie(f,d,s).extent()   , f.extent() );
  NT2_TEST_EQUAL( nt2::tie(c,f,d,s).extent() , c.extent() );
}

template<class Expr>
void meta_impl(Expr const&)
{
  typedef std::complex<float> cT;
  typedef nt2::container::composite_reference<cT> cTref;

  NT2_TEST_TYPE_IS(typename boost::dispatch::meta::scalar_of<Expr>::type, (boost::fusion::vector2< cTref, cT >));
  NT2_TEST_TYPE_IS(typename boost::dispatch::meta::primitive_of<Expr>::type, (boost::fusion::vector2<float&, float>));
  NT2_TEST_TYPE_IS(typename Expr::value_type, (boost::fusion::vector2<cT, cT>));
}

NT2_TEST_CASE( meta )
{
  using nt2::table;
  table< std::complex<float>, nt2::deinterleaved_ > a, b, c, d;

  meta_impl(nt2::tie(a, b + c));
}

NT2_TEST_CASE( loop_fusion )
{
  using nt2::table;
  using nt2::cons;

  table<float> a, b, c, d;
  a = nt2::colvect(nt2::_(1.f, 10));
  b = nt2::colvect(nt2::_(11.f, 20));

  nt2::tie(c, d) = nt2::tie(a + b, a - b);
  NT2_TEST_EQUAL(c, cons<float>(12, 14, 16, 18, 20, 22, 24, 26, 28, 30));
  NT2_TEST_EQUAL(d, cons<float>(-10, -10, -10, -10, -10, -10, -10, -10, -10, -10));
}


/*
Copyright 2011, 2012 Rogier van Dalen.

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.
*/

#define BOOST_TEST_MODULE test_rime_variant
#include "utility/test/boost_unit_test.hpp"

#include "rime/core.hpp"

#include <type_traits>

#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/assert.hpp>
#include "rime/check/check_equal.hpp"

BOOST_AUTO_TEST_SUITE(test_rime_operators)

// \TODO make sure this works correctly without Koenig lookup!

template <class Type, Type Content, class ReferenceType>
    void check_constant_equals (rime::constant <Type, Content> const &,
        ReferenceType const & reference)
{
    BOOST_MPL_ASSERT ((std::is_same <Type, ReferenceType>));
    BOOST_CHECK_EQUAL (Content, reference);
}

#define TEST_BINARY_OPERATOR_ON_TYPE(TYPE, NAME, OPERATOR, LEFT, RIGHT) { \
    rime::constant <TYPE, LEFT> left; \
    rime::constant <TYPE, RIGHT> right; \
    boost::mpl::integral_c <TYPE, LEFT> mpl_left; \
    boost::mpl::integral_c <TYPE, RIGHT> mpl_right; \
\
    /* Explicit function */ \
    check_constant_equals (rime::NAME (left, right), (LEFT OPERATOR RIGHT)); \
    /* Infect with MPL constant: return compile-time constant */ \
    check_constant_equals (rime::NAME (mpl_left, right), \
        (LEFT OPERATOR RIGHT)); \
    check_constant_equals (rime::NAME (left, mpl_right), \
        (LEFT OPERATOR RIGHT)); \
    check_constant_equals (rime::NAME (mpl_left, mpl_right), \
        (LEFT OPERATOR RIGHT)); \
    /* Infect with runtime value: return runtime value */ \
    RIME_CHECK_EQUAL (rime::NAME (left, RIGHT), (LEFT OPERATOR RIGHT)); \
    RIME_CHECK_EQUAL (rime::NAME (LEFT, right), (LEFT OPERATOR RIGHT)); \
    RIME_CHECK_EQUAL (rime::NAME (LEFT, RIGHT), (LEFT OPERATOR RIGHT)); \
\
    /* Operator with constant */ \
    check_constant_equals ((left OPERATOR right), (LEFT OPERATOR RIGHT)); \
    /* Infect with MPL constant: return compile-time constant */ \
    check_constant_equals ((mpl_left OPERATOR right), (LEFT OPERATOR RIGHT)); \
    check_constant_equals ((left OPERATOR mpl_right), (LEFT OPERATOR RIGHT)); \
    /* Two MPL constants: no change to behaviour, return runtime value */ \
    RIME_CHECK_EQUAL ((mpl_left OPERATOR mpl_right), (LEFT OPERATOR RIGHT)); \
    /* Infect with runtime value: return runtime value */ \
    RIME_CHECK_EQUAL ((left OPERATOR RIGHT), (LEFT OPERATOR RIGHT)); \
    RIME_CHECK_EQUAL ((LEFT OPERATOR right), (LEFT OPERATOR RIGHT)); \
}

#define TEST_UNARY_OPERATOR_ON_TYPE(TYPE, NAME, OPERATOR, VALUE) { \
    rime::constant <TYPE, VALUE> value; \
    boost::mpl::integral_c <TYPE, VALUE> mpl_value; \
\
    /* Explicit function */ \
    check_constant_equals (rime::NAME (value), (OPERATOR VALUE)); \
    check_constant_equals (rime::NAME (mpl_value), (OPERATOR VALUE)); \
    /* Operator */\
    check_constant_equals (OPERATOR value, (OPERATOR VALUE)); \
    RIME_CHECK_EQUAL (OPERATOR mpl_value, (OPERATOR VALUE)); \
    RIME_CHECK_EQUAL (rime::NAME (VALUE), (OPERATOR VALUE)); \
}

#define TEST_BINARY_OPERATOR_ON(NAME, OPERATOR, LEFT, RIGHT) \
    TEST_BINARY_OPERATOR_ON_TYPE(int, NAME, OPERATOR, LEFT, RIGHT)
#define TEST_UNARY_OPERATOR_ON(NAME, OPERATOR, VALUE) \
    TEST_UNARY_OPERATOR_ON_TYPE(int, NAME, OPERATOR, VALUE)

#define TEST_BINARY_OPERATOR_ON_FLOAT(NAME, OPERATOR, LEFT, RIGHT) { \
    rime::int_ <LEFT> left; \
    rime::int_ <RIGHT> right; \
\
    /* Explicit function */ \
    RIME_CHECK_EQUAL (rime::NAME (left, double (RIGHT)), \
        (LEFT OPERATOR double (RIGHT))); \
    RIME_CHECK_EQUAL (rime::NAME (double (LEFT), right), \
        (double (LEFT) OPERATOR RIGHT)); \
    RIME_CHECK_EQUAL (rime::NAME (double (LEFT), double (RIGHT)), \
        (double (LEFT) OPERATOR double (RIGHT))); \
\
    /* Operator with constant */ \
    RIME_CHECK_EQUAL ((left OPERATOR double (RIGHT)), \
        (LEFT OPERATOR double (RIGHT))); \
    RIME_CHECK_EQUAL ((double (LEFT) OPERATOR right), \
        (double (LEFT) OPERATOR RIGHT)); \
}

#define DEFINE_TEST_BINARY_OPERATOR_IMPLEMENTATION(APPLY, NAME, OPERATOR) \
    APPLY (NAME, OPERATOR, 4, 5) \
    APPLY (NAME, OPERATOR, 7, 5) \
    APPLY (NAME, OPERATOR, 3, 3)

#define DEFINE_TEST_UNARY_OPERATOR_IMPLEMENTATION(APPLY, NAME, OPERATOR) \
    APPLY (NAME, OPERATOR, 5) \
    APPLY (NAME, OPERATOR, -3) \
    APPLY (NAME, OPERATOR, 1) \
    APPLY (NAME, OPERATOR, 0)

// Test all
#define DEFINE_TEST_BINARY_OPERATOR(NAME, OPERATOR) \
BOOST_AUTO_TEST_CASE (test_operator_##NAME) { \
    DEFINE_TEST_BINARY_OPERATOR_IMPLEMENTATION ( \
        TEST_BINARY_OPERATOR_ON, NAME, OPERATOR) \
    DEFINE_TEST_BINARY_OPERATOR_IMPLEMENTATION ( \
        TEST_BINARY_OPERATOR_ON_FLOAT, NAME, OPERATOR) \
    TEST_BINARY_OPERATOR_ON (NAME, OPERATOR, 0, -1) \
    TEST_BINARY_OPERATOR_ON (NAME, OPERATOR, 0, 0) \
}

#define DEFINE_TEST_UNARY_OPERATOR(NAME, OPERATOR) \
BOOST_AUTO_TEST_CASE (test_operator_##NAME) { \
    DEFINE_TEST_UNARY_OPERATOR_IMPLEMENTATION ( \
        TEST_UNARY_OPERATOR_ON, NAME, OPERATOR) \
}

// Test all except 0/0
#define DEFINE_TEST_DIVIDE_OPERATOR(NAME, OPERATOR) \
BOOST_AUTO_TEST_CASE (test_operator_##NAME) { \
    DEFINE_TEST_BINARY_OPERATOR_IMPLEMENTATION ( \
        TEST_BINARY_OPERATOR_ON, NAME, OPERATOR) \
    TEST_BINARY_OPERATOR_ON (NAME, OPERATOR, 0, -1) \
}

// Test all except shift by negative amount
#define DEFINE_TEST_SHIFT_OPERATOR(NAME, OPERATOR) \
BOOST_AUTO_TEST_CASE (test_operator_##NAME) { \
    DEFINE_TEST_BINARY_OPERATOR_IMPLEMENTATION ( \
        TEST_BINARY_OPERATOR_ON, NAME, OPERATOR) \
    TEST_BINARY_OPERATOR_ON (NAME, OPERATOR, 0, 0) \
}

DEFINE_TEST_UNARY_OPERATOR(negate, -)
DEFINE_TEST_UNARY_OPERATOR(not_, !)
DEFINE_TEST_UNARY_OPERATOR(bitwise_not, ~)

DEFINE_TEST_BINARY_OPERATOR(equal_to, ==)
DEFINE_TEST_BINARY_OPERATOR(not_equal_to, !=)
DEFINE_TEST_BINARY_OPERATOR(less, <)
DEFINE_TEST_BINARY_OPERATOR(less_equal, <=)
DEFINE_TEST_BINARY_OPERATOR(greater, >)
DEFINE_TEST_BINARY_OPERATOR(greater_equal, >=)

DEFINE_TEST_BINARY_OPERATOR(plus, +)
DEFINE_TEST_BINARY_OPERATOR(minus, -)
DEFINE_TEST_BINARY_OPERATOR(times, *)
DEFINE_TEST_DIVIDE_OPERATOR(divides, /)
DEFINE_TEST_DIVIDE_OPERATOR(modulo, %)

DEFINE_TEST_SHIFT_OPERATOR(bitwise_and, &)
DEFINE_TEST_SHIFT_OPERATOR(bitwise_or, |)
DEFINE_TEST_SHIFT_OPERATOR(bitwise_xor, ^)
DEFINE_TEST_SHIFT_OPERATOR(shift_right, >>)
DEFINE_TEST_SHIFT_OPERATOR(shift_left, <<)

/*
and_ and or_ are written out, because
    or_ (rime::true_(), false)
is rime::true_() et cetera.
*/
BOOST_AUTO_TEST_CASE (test_operator_and) {
    rime::constant <bool, false> false_;
    rime::constant <bool, true> true_;

    /* Explicit function only */
    // 0 arguments
    RIME_CHECK_EQUAL (rime::and_ (), true_);

    // 1 argument
    RIME_CHECK_EQUAL (rime::and_ (false_), false_);
    RIME_CHECK_EQUAL (rime::and_ (true_), true_);
    RIME_CHECK_EQUAL (rime::and_ (false), false);
    RIME_CHECK_EQUAL (rime::and_ (true), true);

    // 2 arguments
    RIME_CHECK_EQUAL (rime::and_ (false_, false_), false_);
    RIME_CHECK_EQUAL (rime::and_ (false_, false), false_);
    RIME_CHECK_EQUAL (rime::and_ (false, false_), false_);
    RIME_CHECK_EQUAL (rime::and_ (false, false), false);

    RIME_CHECK_EQUAL (rime::and_ (false_, true_), false_);
    RIME_CHECK_EQUAL (rime::and_ (false_, true), false_);
    RIME_CHECK_EQUAL (rime::and_ (false, true_), false);
    RIME_CHECK_EQUAL (rime::and_ (false, true), false);

    RIME_CHECK_EQUAL (rime::and_ (true_, false_), false_);
    RIME_CHECK_EQUAL (rime::and_ (true_, false), false);
    RIME_CHECK_EQUAL (rime::and_ (true, false_), false_);
    RIME_CHECK_EQUAL (rime::and_ (true, false), false);

    RIME_CHECK_EQUAL (rime::and_ (true_, true_), true_);
    RIME_CHECK_EQUAL (rime::and_ (true_, true), true);
    RIME_CHECK_EQUAL (rime::and_ (true, true_), true);
    RIME_CHECK_EQUAL (rime::and_ (true, true), true);

    // 3 arguments: spot check.
    RIME_CHECK_EQUAL (rime::and_ (false_, false_, false_), false_);
    RIME_CHECK_EQUAL (rime::and_ (false_, true_, true_), false_);
    RIME_CHECK_EQUAL (rime::and_ (false_, true_, true), false_);
    RIME_CHECK_EQUAL (rime::and_ (false_, true, true_), false_);
    RIME_CHECK_EQUAL (rime::and_ (true, false_, true_), false_);
    RIME_CHECK_EQUAL (rime::and_ (true, true_, false_), false_);

    RIME_CHECK_EQUAL (rime::and_ (true_, true_, true_), true_);
    RIME_CHECK_EQUAL (rime::and_ (true_, true, true_), true);
    RIME_CHECK_EQUAL (rime::and_ (true_, true, false), false);
    RIME_CHECK_EQUAL (rime::and_ (false, true, true_), false);
    RIME_CHECK_EQUAL (rime::and_ (false, true_, true), false);

    /** operator && **/
    // Rime constants
    RIME_CHECK_EQUAL ((false_ && false_), false_);
    RIME_CHECK_EQUAL ((true_ && false_), false_);
    RIME_CHECK_EQUAL ((false_ && true_), false_);
    RIME_CHECK_EQUAL ((true_ && true_), true_);

    // Run-time and Rime constants: always run-time.
    // (The standard behaviour is not overridden so short-circuiting remains
    // possible.)
    RIME_CHECK_EQUAL ((false && false_), false);
    RIME_CHECK_EQUAL ((true && false_), false);
    RIME_CHECK_EQUAL ((false && true_), false);
    RIME_CHECK_EQUAL ((true && true_), true);
    RIME_CHECK_EQUAL ((false_ && false), false);
    RIME_CHECK_EQUAL ((true_ && false), false);
    RIME_CHECK_EQUAL ((false_ && true), false);
    RIME_CHECK_EQUAL ((true_ && true), true);

    // With MPL constants: known at compile time.
    boost::mpl::bool_ <false> mpl_false;
    boost::mpl::bool_ <true> mpl_true;
    RIME_CHECK_EQUAL ((mpl_false && false_), false_);
    RIME_CHECK_EQUAL ((mpl_true && false_), false_);
    RIME_CHECK_EQUAL ((mpl_false && true_), false_);
    RIME_CHECK_EQUAL ((mpl_true && true_), true_);
    RIME_CHECK_EQUAL ((false_ && mpl_false), false_);
    RIME_CHECK_EQUAL ((true_ && mpl_false), false_);
    RIME_CHECK_EQUAL ((false_ && mpl_true), false_);
    RIME_CHECK_EQUAL ((true_ && mpl_true), true_);

    // MPL constants and run-time values: no change to default behaviour.
    RIME_CHECK_EQUAL ((mpl_false && mpl_false), false);
    RIME_CHECK_EQUAL ((mpl_true && mpl_false), false);
    RIME_CHECK_EQUAL ((mpl_false && mpl_true), false);
    RIME_CHECK_EQUAL ((mpl_true && mpl_true), true);

    RIME_CHECK_EQUAL ((mpl_false && false), false);
    RIME_CHECK_EQUAL ((mpl_true && false), false);
    RIME_CHECK_EQUAL ((mpl_false && true), false);
    RIME_CHECK_EQUAL ((mpl_true && true), true);
    RIME_CHECK_EQUAL ((false && mpl_false), false);
    RIME_CHECK_EQUAL ((true && mpl_false), false);
    RIME_CHECK_EQUAL ((false && mpl_true), false);
    RIME_CHECK_EQUAL ((true && mpl_true), true);
}

BOOST_AUTO_TEST_CASE (test_operator_or) {
    rime::constant <bool, false> false_;
    rime::constant <bool, true> true_;

    /* Explicit function only */
    // 0 arguments
    RIME_CHECK_EQUAL (rime::or_ (), false_);

    // 1 argument
    RIME_CHECK_EQUAL (rime::or_ (false_), false_);
    RIME_CHECK_EQUAL (rime::or_ (true_), true_);
    RIME_CHECK_EQUAL (rime::or_ (false), false);
    RIME_CHECK_EQUAL (rime::or_ (true), true);

    // 2 arguments
    RIME_CHECK_EQUAL (rime::or_ (false_, false_), false_);
    RIME_CHECK_EQUAL (rime::or_ (false_, false), false);
    RIME_CHECK_EQUAL (rime::or_ (false, false_), false);
    RIME_CHECK_EQUAL (rime::or_ (false, false), false);

    RIME_CHECK_EQUAL (rime::or_ (false_, true_), true_);
    RIME_CHECK_EQUAL (rime::or_ (false_, true), true);
    RIME_CHECK_EQUAL (rime::or_ (false, true_), true_);
    RIME_CHECK_EQUAL (rime::or_ (false, true), true);

    RIME_CHECK_EQUAL (rime::or_ (true_, false_), true_);
    RIME_CHECK_EQUAL (rime::or_ (true_, false), true_);
    RIME_CHECK_EQUAL (rime::or_ (true, false_), true);
    RIME_CHECK_EQUAL (rime::or_ (true, false), true);

    RIME_CHECK_EQUAL (rime::or_ (true_, true_), true_);
    RIME_CHECK_EQUAL (rime::or_ (true_, true), true_);
    RIME_CHECK_EQUAL (rime::or_ (true, true_), true_);
    RIME_CHECK_EQUAL (rime::or_ (true, true), true);

    // 3 arguments: spot check.
    RIME_CHECK_EQUAL (rime::or_ (true_, true_, true_), true_);
    RIME_CHECK_EQUAL (rime::or_ (true_, false_, false_), true_);
    RIME_CHECK_EQUAL (rime::or_ (true_, false_, false), true_);
    RIME_CHECK_EQUAL (rime::or_ (true_, false, false_), true_);
    RIME_CHECK_EQUAL (rime::or_ (false, true_, false_), true_);
    RIME_CHECK_EQUAL (rime::or_ (false, false_, true_), true_);

    RIME_CHECK_EQUAL (rime::or_ (false_, false_, false_), false_);
    RIME_CHECK_EQUAL (rime::or_ (false_, false, false_), false);
    RIME_CHECK_EQUAL (rime::or_ (false_, false, true), true);
    RIME_CHECK_EQUAL (rime::or_ (true, false, false_), true);
    RIME_CHECK_EQUAL (rime::or_ (true, false_, false), true);

    /** operator || **/
    // Rime constants
    RIME_CHECK_EQUAL ((false_ || false_), false_);
    RIME_CHECK_EQUAL ((true_ || false_), true_);
    RIME_CHECK_EQUAL ((false_ || true_), true_);
    RIME_CHECK_EQUAL ((true_ || true_), true_);

    // Run-time and Rime constants: always run-time.
    // (The standard behaviour is not overridden so short-circuiting remains
    // possible.)
    RIME_CHECK_EQUAL ((false || false_), false);
    RIME_CHECK_EQUAL ((true || false_), true);
    RIME_CHECK_EQUAL ((false || true_), true);
    RIME_CHECK_EQUAL ((true || true_), true);
    RIME_CHECK_EQUAL ((false_ || false), false);
    RIME_CHECK_EQUAL ((true_ || false), true);
    RIME_CHECK_EQUAL ((false_ || true), true);
    RIME_CHECK_EQUAL ((true_ || true), true);

    // With MPL constants: known at compile time.
    boost::mpl::bool_ <false> mpl_false;
    boost::mpl::bool_ <true> mpl_true;
    RIME_CHECK_EQUAL ((mpl_false || false_), false_);
    RIME_CHECK_EQUAL ((mpl_true || false_), true_);
    RIME_CHECK_EQUAL ((mpl_false || true_), true_);
    RIME_CHECK_EQUAL ((mpl_true || true_), true_);
    RIME_CHECK_EQUAL ((false_ || mpl_false), false_);
    RIME_CHECK_EQUAL ((true_ || mpl_false), true_);
    RIME_CHECK_EQUAL ((false_ || mpl_true), true_);
    RIME_CHECK_EQUAL ((true_ || mpl_true), true_);

    // MPL constants and run-time values: no change to default behaviour.
    RIME_CHECK_EQUAL ((mpl_false || mpl_false), false);
    RIME_CHECK_EQUAL ((mpl_true || mpl_false), true);
    RIME_CHECK_EQUAL ((mpl_false || mpl_true), true);
    RIME_CHECK_EQUAL ((mpl_true || mpl_true), true);

    RIME_CHECK_EQUAL ((mpl_false || false), false);
    RIME_CHECK_EQUAL ((mpl_true || false), true);
    RIME_CHECK_EQUAL ((mpl_false || true), true);
    RIME_CHECK_EQUAL ((mpl_true || true), true);
    RIME_CHECK_EQUAL ((false || mpl_false), false);
    RIME_CHECK_EQUAL ((true || mpl_false), true);
    RIME_CHECK_EQUAL ((false || mpl_true), true);
    RIME_CHECK_EQUAL ((true || mpl_true), true);
}

// Test cases where operators should not do anything.
BOOST_AUTO_TEST_CASE (test_operator_interaction) {
    {
        char c[2] = "g";
        char * p = &c[0];
        char * q = p + rime::int_ <1>();
        BOOST_CHECK_EQUAL (p + 1, q);
    }
}

BOOST_AUTO_TEST_SUITE_END()


/*ckwg +29
 * Copyright 2013-2015 by Kitware, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither name of Kitware, Inc. nor the names of any contributors may be used
 *    to endorse or promote products derived from this software without specific
 *    prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * \file
 * \brief Implementation of \link kwiver::vital::camera_intrinsics_
 *        camera_intrinsics_<T> \endlink class
 *        for \c T = { \c float, \c double }
 */

#include <vital/types/camera_intrinsics.h>
#include <vital/io/eigen_io.h>
#include <Eigen/Dense>

#include <iomanip>

namespace kwiver {
namespace vital {

/// Convert to a 3x3 calibration matrix
matrix_3x3d
camera_intrinsics
::as_matrix() const
{
  matrix_3x3d K;
  const double f = this->focal_length();
  const vector_2d pp = this->principal_point();
  K << f, this->skew(), pp.x(),
    0, f / this->aspect_ratio(), pp.y(),
    0, 0, 1;
  return K;
}

/// Map normalized image coordinates into actual image coordinates
vector_2d
camera_intrinsics
::map( const vector_2d& point ) const
{
  // apply radial and tangential distortion if coefficients are provided
  const vector_2d pt = this->distort( point );
  const vector_2d pp = this->principal_point();
  const double f = this->focal_length();

  return vector_2d( pt.x() * f + pt.y() * this->skew() + pp.x(),
                    pt.y() * f / this->aspect_ratio() + pp.y() );
}

/// Map a 3D point in camera coordinates into actual image coordinates
vector_2d
camera_intrinsics
::map( const vector_3d& norm_hpt ) const
{
  return this->map( vector_2d( norm_hpt[0] / norm_hpt[2],
                               norm_hpt[1] / norm_hpt[2] ) );
}

/// Unmap actual image coordinates back into normalized image coordinates
vector_2d
camera_intrinsics
::unmap( const vector_2d& pt ) const
{
  const double f = this->focal_length();
  const vector_2d p0 = pt - this->principal_point();
  const double y = p0.y() * this->aspect_ratio() / f;
  const double x = ( p0.x() - y * this->skew() ) / f;

  return this->undistort( vector_2d( x, y ) );
}

namespace // anonymous namespace
{

/// Compute the radial distortion scaling
/** Distortion scaling is a function of the squared radius \p r2
 *  and the distortion parameters \p d
 */
template < typename T >
T
radial_distortion_scale( const T                r2,
                         const Eigen::VectorXd&  d )
{
  T scale = T( 1 );

  if ( d.rows() > 0 )
  {
    scale += r2 * d[0];
    if ( d.rows() > 1 )
    {
      const T r4 = r2 * r2;
      scale += r4 * d[1];
      if ( d.rows() > 4 )
      {
        const T r6 = r2 * r4;
        scale += r6 * d[4];
        if ( d.rows() > 7 )
        {
          scale /= T( 1 ) + r2 * d[5] + r4 * d[6] + r6 * d[7];
        }
      }
    }
  }
  return scale;
}

/// Compute radial distortion as a scaling and offset
/** For a point \p pt and distortion coefficients \p d compute
 *  a scale and offset such that distortion can be applied as
 *  \code
 *    distorted_pt = pt * scale + offset;
 *  \endcode
 */
template < typename T >
void
distortion_scale_offset( const Eigen::Matrix< T, 2, 1 >& pt,
                         const Eigen::VectorXd& d,
                         T& scale, Eigen::Matrix< T, 2, 1 >& offset )
{
  const T x2 = pt.x() * pt.x();
  const T y2 = pt.y() * pt.y();
  const T r2 = x2 + y2;

  scale = radial_distortion_scale( r2, d );
  offset = Eigen::Matrix< T, 2, 1 > ( T( 0 ), T( 0 ) );
  if ( d.rows() > 3 )
  {
    const T two_xy = 2 * pt.x() * pt.y();
    offset = Eigen::Matrix< T, 2, 1 > ( d[2] * two_xy + d[3] * ( r2 + 2 * x2 ),
                                        d[3] * two_xy + d[2] * ( r2 + 2 * y2 ) );
  }
}

/// Compute the derivative of the radial distortion as a function of \p r2
template < typename T >
T
radial_distortion_deriv( const T                r2,
                         const Eigen::VectorXd&  d )
{
  T deriv = T( 0 );

  if ( d.rows() > 0 )
  {
    deriv += d[0];
    if ( d.rows() > 1 )
    {
      deriv += 2 * d[1] * r2;
      if ( d.rows() > 4 )
      {
        const T r4 = r2 * r2;
        deriv += 3 * d[4] * r4;
        if ( d.rows() > 7 )
        {
          const T r6 = r4 * r2;
          const T a1 = T( 1 ) / ( d[5] * r2 + d[6] * r4 + d[7] * r6 + T( 1 ) );
          const T a2 = d[5] + 2 * d[6] * r2 + 3 * d[7] * r4;
          deriv -= a2 * a1 * ( d[0] * r2 + d[1] * r4 + d[4] * r6 + T( 1 ) );
          deriv *= a1;
        }
      }
    }
  }
  return deriv;
}

/// Compute the Jacobian of the distortion at a point
template < typename T >
Eigen::Matrix< T, 2, 2 >
distortion_jacobian( const Eigen::Matrix< T, 2, 1 >& pt,
                     const Eigen::VectorXd& d )
{
  const T x2 = pt.x() * pt.x();
  const T y2 = pt.y() * pt.y();
  const T xy = pt.x() * pt.y();
  const T r2 = x2 + y2;
  const T d_scale = 2 * radial_distortion_deriv( r2, d );
  const T scale = radial_distortion_scale( r2, d );
  Eigen::Matrix< T, 2, 2 > J;

  J << d_scale * x2 + scale, d_scale * xy,
    d_scale * xy, d_scale * y2 + scale;
  // add tangential distortion jacobian
  if ( d.rows() > 3 )
  {
    const T axy = 2 * ( d[2] * pt.x() + d[3] * pt.y() );
    const T ay = 2 * d[2] * pt.y();
    const T ax = 2 * d[3] * pt.x();
    J( 0, 0 ) += ay + 3 * ax;
    J( 0, 1 ) += axy;
    J( 1, 0 ) += axy;
    J( 0, 0 ) += 3 * ay + ax;
  }
  return J;
}

} // end anonymous namespace

/// Constructor - from a calibration matrix
simple_camera_intrinsics
::simple_camera_intrinsics( const matrix_3x3d& K,
                            const vector_t& d )
  : focal_length_( K( 0, 0 ) ),
  principal_point_( K( 0, 2 ), K( 1, 2 ) ),
  aspect_ratio_( K( 0, 0 ) / K( 1, 1 ) ),
  skew_( K( 0, 1 ) ),
  dist_coeffs_( d )
{
}

/// Map normalized image coordinates into distorted coordinates
vector_2d
simple_camera_intrinsics
::distort( const vector_2d& norm_pt ) const
{
  double scale;
  vector_2d offset;

  distortion_scale_offset( norm_pt, dist_coeffs_, scale, offset );
  return scale * norm_pt + offset;
}

/// Unnap distorted normalized coordinates into normalized coordinates
vector_2d
simple_camera_intrinsics
::undistort( const vector_2d& dist_pt ) const
{
  double scale;
  vector_2d offset, residual;
  vector_2d norm_pt = dist_pt;

  // iteratively solve for the undistorted point
  for ( unsigned int i = 0; i < 5; ++i )
  {
    distortion_scale_offset( norm_pt, dist_coeffs_, scale, offset );
    // This is a Gauss-Newton update
    // an alternative is a fixed point iteration as used by OpenCV:
    //   norm_pt = (dist_pt - offset) / scale;
    // Gauss-Newton seems to have faster convergence
    matrix_2x2d J = distortion_jacobian( norm_pt, dist_coeffs_ );
    residual = norm_pt * scale + offset - dist_pt;
    // check the maximum absolution residual to test convergence
    if ( residual.cwiseAbs().maxCoeff() < 1e-12 )
    {
      break;
    }
    norm_pt -= J.ldlt().solve( residual );
  }
  return norm_pt;
}

/// output stream operator for a base class camera_intrinsics
std::ostream&
operator<<( std::ostream& s, const camera_intrinsics& k )
{
  using std::setprecision;
  std::vector<double> d = k.dist_coeffs();
  // if no distortion coefficients, create a zero entry as a place holder
  if ( d.empty() )
  {
    d.push_back(0.0);
  }
  s << setprecision( 12 ) << k.as_matrix() << "\n\n";
  for(unsigned i=0; i<d.size(); ++i)
  {
    s << setprecision( 12 ) << d[i] << " ";
  }
  s << "\n";

  return s;
}

/// input stream operator for a camera intrinsics
std::istream&
operator>>( std::istream& s, simple_camera_intrinsics& k )
{
  matrix_3x3d K;
  Eigen::VectorXd d;

  s >> K >> d;
  // a single 0 in d is used as a place holder,
  // if a single 0 was loaded then clear d
  if ( ( d.rows() == 1 ) && ( d[0] == 0.0 ) )
  {
    d.resize( 0 );
  }
  k = simple_camera_intrinsics( K, d );
  return s;
}

} } // end namespace


#include <boost/mpl/assert.hpp>
#include <boost/dispatch/meta/fusion.hpp>
#include <boost/dispatch/meta/is_signed.hpp>

using boost::array;
using boost::dispatch::meta::is_signed;

int main()
{
  BOOST_MPL_ASSERT_NOT(( is_signed<unsigned int>    ));
  BOOST_MPL_ASSERT    (( is_signed<int>             ));
  BOOST_MPL_ASSERT_NOT(( is_signed<bool>            ));
  BOOST_MPL_ASSERT    (( is_signed<float>           ));
  BOOST_MPL_ASSERT    (( is_signed< array<int,9> >  ));
}


#include "Proof.hpp"

#include "crypto/common.h"

#include <boost/static_assert.hpp>
#include <libsnark/common/default_types/r1cs_ppzksnark_pp.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp>
#include <mutex>

using namespace libsnark;

typedef alt_bn128_pp curve_pp;
typedef alt_bn128_pp::G1_type curve_G1;
typedef alt_bn128_pp::G2_type curve_G2;
typedef alt_bn128_pp::GT_type curve_GT;
typedef alt_bn128_pp::Fp_type curve_Fr;
typedef alt_bn128_pp::Fq_type curve_Fq;
typedef alt_bn128_pp::Fqe_type curve_Fq2;

BOOST_STATIC_ASSERT(sizeof(mp_limb_t) == 8);

namespace libvcoin {

// FE2IP as defined in the protocol spec and IEEE Std 1363a-2004.
bigint<8> fq2_to_bigint(const curve_Fq2 &e)
{
    auto modq = curve_Fq::field_char();
    auto c0 = e.c0.as_bigint();
    auto c1 = e.c1.as_bigint();

    bigint<8> temp = c1 * modq;
    temp += c0;
    return temp;
}

// Writes a bigint in big endian
template<mp_size_t LIMBS>
void write_bigint(base_blob<8 * LIMBS * sizeof(mp_limb_t)> &blob, const bigint<LIMBS> &val)
{
    auto ptr = blob.begin();
    for (ssize_t i = LIMBS-1; i >= 0; i--, ptr += 8) {
        WriteBE64(ptr, val.data[i]);
    }
}

// Reads a bigint from big endian
template<mp_size_t LIMBS>
bigint<LIMBS> read_bigint(const base_blob<8 * LIMBS * sizeof(mp_limb_t)> &blob)
{
    bigint<LIMBS> ret;

    auto ptr = blob.begin();

    for (ssize_t i = LIMBS-1; i >= 0; i--, ptr += 8) {
        ret.data[i] = ReadBE64(ptr);
    }

    return ret;
}

template<>
Fq::Fq(curve_Fq element) : data()
{
    write_bigint<4>(data, element.as_bigint());
}

template<>
curve_Fq Fq::to_libsnark_fq() const
{
    auto element_bigint = read_bigint<4>(data);

    // Check that the integer is smaller than the modulus
    auto modq = curve_Fq::field_char();
    element_bigint.limit(modq, "element is not in Fq");

    return curve_Fq(element_bigint);
}

template<>
Fq2::Fq2(curve_Fq2 element) : data()
{
    write_bigint<8>(data, fq2_to_bigint(element));
}

template<>
curve_Fq2 Fq2::to_libsnark_fq2() const
{
    bigint<4> modq = curve_Fq::field_char();
    bigint<8> combined = read_bigint<8>(data);
    bigint<5> res;
    bigint<4> c0;
    bigint<8>::div_qr(res, c0, combined, modq);
    bigint<4> c1 = res.shorten(modq, "element is not in Fq2");

    return curve_Fq2(curve_Fq(c0), curve_Fq(c1));
}

template<>
CompressedG1::CompressedG1(curve_G1 point)
{
    if (point.is_zero()) {
        throw std::domain_error("curve point is zero");
    }

    point.to_affine_coordinates();

    x = Fq(point.X);
    y_lsb = point.Y.as_bigint().data[0] & 1;
}

template<>
curve_G1 CompressedG1::to_libsnark_g1() const
{
    curve_Fq x_coordinate = x.to_libsnark_fq<curve_Fq>();

    // y = +/- sqrt(x^3 + b)
    auto y_coordinate = ((x_coordinate.squared() * x_coordinate) + alt_bn128_coeff_b).sqrt();

    if ((y_coordinate.as_bigint().data[0] & 1) != y_lsb) {
        y_coordinate = -y_coordinate;
    }

    curve_G1 r = curve_G1::one();
    r.X = x_coordinate;
    r.Y = y_coordinate;
    r.Z = curve_Fq::one();

    assert(r.is_well_formed());

    return r;
}

template<>
CompressedG2::CompressedG2(curve_G2 point)
{
    if (point.is_zero()) {
        throw std::domain_error("curve point is zero");
    }

    point.to_affine_coordinates();

    x = Fq2(point.X);
    y_gt = fq2_to_bigint(point.Y) > fq2_to_bigint(-(point.Y));
}

template<>
curve_G2 CompressedG2::to_libsnark_g2() const
{
    auto x_coordinate = x.to_libsnark_fq2<curve_Fq2>();

    // y = +/- sqrt(x^3 + b)
    auto y_coordinate = ((x_coordinate.squared() * x_coordinate) + alt_bn128_twist_coeff_b).sqrt();
    auto y_coordinate_neg = -y_coordinate;

    if ((fq2_to_bigint(y_coordinate) > fq2_to_bigint(y_coordinate_neg)) != y_gt) {
        y_coordinate = y_coordinate_neg;
    }

    curve_G2 r = curve_G2::one();
    r.X = x_coordinate;
    r.Y = y_coordinate;
    r.Z = curve_Fq2::one();

    assert(r.is_well_formed());

    if (alt_bn128_modulus_r * r != curve_G2::zero()) {
        throw std::runtime_error("point is not in G2");
    }

    return r;
}

template<>
PHGRProof::PHGRProof(const r1cs_ppzksnark_proof<curve_pp> &proof)
{
    g_A = CompressedG1(proof.g_A.g);
    g_A_prime = CompressedG1(proof.g_A.h);
    g_B = CompressedG2(proof.g_B.g);
    g_B_prime = CompressedG1(proof.g_B.h);
    g_C = CompressedG1(proof.g_C.g);
    g_C_prime = CompressedG1(proof.g_C.h);
    g_K = CompressedG1(proof.g_K);
    g_H = CompressedG1(proof.g_H);
}

template<>
r1cs_ppzksnark_proof<curve_pp> PHGRProof::to_libsnark_proof() const
{
    r1cs_ppzksnark_proof<curve_pp> proof;

    proof.g_A.g = g_A.to_libsnark_g1<curve_G1>();
    proof.g_A.h = g_A_prime.to_libsnark_g1<curve_G1>();
    proof.g_B.g = g_B.to_libsnark_g2<curve_G2>();
    proof.g_B.h = g_B_prime.to_libsnark_g1<curve_G1>();
    proof.g_C.g = g_C.to_libsnark_g1<curve_G1>();
    proof.g_C.h = g_C_prime.to_libsnark_g1<curve_G1>();
    proof.g_K = g_K.to_libsnark_g1<curve_G1>();
    proof.g_H = g_H.to_libsnark_g1<curve_G1>();

    return proof;
}

PHGRProof PHGRProof::random_invalid()
{
    PHGRProof p;
    p.g_A = curve_G1::random_element();
    p.g_A_prime = curve_G1::random_element();
    p.g_B = curve_G2::random_element();
    p.g_B_prime = curve_G1::random_element();
    p.g_C = curve_G1::random_element();
    p.g_C_prime = curve_G1::random_element();

    p.g_K = curve_G1::random_element();
    p.g_H = curve_G1::random_element();

    return p;
}

static std::once_flag init_public_params_once_flag;

void initialize_curve_params()
{
    std::call_once (init_public_params_once_flag, curve_pp::init_public_params);
}

ProofVerifier ProofVerifier::Strict() {
    initialize_curve_params();
    return ProofVerifier(true);
}

ProofVerifier ProofVerifier::Disabled() {
    initialize_curve_params();
    return ProofVerifier(false);
}

template<>
bool ProofVerifier::check(
    const r1cs_ppzksnark_verification_key<curve_pp>& vk,
    const r1cs_ppzksnark_processed_verification_key<curve_pp>& pvk,
    const r1cs_primary_input<curve_Fr>& primary_input,
    const r1cs_ppzksnark_proof<curve_pp>& proof
)
{
    if (perform_verification) {
        return r1cs_ppzksnark_online_verifier_strong_IC<curve_pp>(pvk, primary_input, proof);
    } else {
        return true;
    }
}

}


/* Author: Mincheul Kang */

#include <ompl/base/spaces/RealVectorStateSpace.h>

#include <fstream>
#include <queue>
#include <stack>
#include <utility>
#include <numeric>
#include <stdio.h>
#include <iostream>
#include <string>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/lookup_edge.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/minmax_element.hpp>
#include <boost/math/constants/constants.hpp>

#include <harmonious_sampling/HarmoniousSampler.h>

ompl::base::HarmoniousSampler::HarmoniousSampler(std::vector<geometry_msgs::Pose> &grasp_poses,
                                                 std::vector<harmonious_msgs::BaseConf> &base_confs,
                                                 std::vector<harmonious_msgs::JointConf> &joint_confs,
                                                 HarmoniousParameters &params,
                                                 MainpulationRegion &mr,
                                                 std::vector<double> &defined_joint_conf):
        grasp_poses_(grasp_poses),
        base_confs_(base_confs),
        joint_confs_(joint_confs),
        params_(params),
        defined_joint_conf_(defined_joint_conf),
        mr_(mr)
{
    size_of_gp = (uint)grasp_poses_.size();
    num_possible_confs_ = 0;
    num_dof_ = defined_joint_conf.size() + NUM_BASE_DOF;

    prob_dist_ = new double**[params_.reg_index_[0]];
    cumul_dist_ = new double**[params_.reg_index_[0]];
    for (uint i = 0; i < params_.reg_index_[0]; i++){
        prob_dist_[i] = new double*[params_.reg_index_[1]];
        cumul_dist_[i] = new double*[params_.reg_index_[1]];
        for (uint j = 0; j < params_.reg_index_[1]; j++){
            prob_dist_[i][j] = new double[params_.reg_index_[2]];
            cumul_dist_[i][j] = new double[params_.reg_index_[2]];
        }
    }

    double pre = 0.0;
    for(uint i = 0; i < params_.reg_index_[0]; i++){
        ddd.push_back(0);
        for(uint j = 0; j < params_.reg_index_[1]; j++) {
            for (uint k = 0; k < params_.reg_index_[2]; k++) {
                if(mr.regions_[i][j][k]){
                    prob_dist_[i][j][k] = params_.v_manip_;
                }
                else{
                    prob_dist_[i][j][k] = params_.v_base_;
                }
                pre += prob_dist_[i][j][k];
                cumul_dist_[i][j][k] = pre;
            }
        }
    }
}

ompl::base::HarmoniousSampler::~HarmoniousSampler(){
    for(uint i=0; i<params_.reg_index_[0]; i++) {
        for(uint j=0; j<params_.reg_index_[1]; j++){
            delete [] prob_dist_[i][j];
            delete [] cumul_dist_[i][j];
        }
        delete [] prob_dist_[i];
        delete [] cumul_dist_[i];
    }
    delete [] prob_dist_;
    delete [] cumul_dist_;
}

bool ompl::base::HarmoniousSampler::makeGoal(const SpaceInformationPtr si_, ompl::base::State *workState){
    base::RealVectorStateSpace::StateType *rstate = static_cast<base::RealVectorStateSpace::StateType*>(workState);

    for(; num_possible_confs_ < base_confs_.size(); num_possible_confs_++){
        if(joint_confs_[num_possible_confs_].j_c.size() != 0){
            for (int i = 0; i < num_dof_; i++){
                if(i < NUM_BASE_DOF){
                    rstate->values[i] = base_confs_[num_possible_confs_].b_c[i];
                }
                else{
                    rstate->values[i] = joint_confs_[num_possible_confs_].j_c[i-NUM_BASE_DOF];
                }
            }
            num_possible_confs_++;
            if (!si_->isValid(workState)) {
                return false;
            }
            else{
                return true;
            }
        }
    }

    return false;
}

bool ompl::base::HarmoniousSampler::getReachable(ompl::base::State *workState){
    base::RealVectorStateSpace::StateType *rstate = static_cast<base::RealVectorStateSpace::StateType*>(workState);

    return mr_.regions_[int((rstate->values[0]-params_.bounds_.low[0]) * params_.resolution_)]
                       [int((rstate->values[1]-params_.bounds_.low[1]) * params_.resolution_)]
                       [int((rstate->values[2]-params_.bounds_.low[2]) * params_.res_yaw_)];
}

void ompl::base::HarmoniousSampler::jointSampling(ompl::base::State *workState) {
    RealVectorStateSpace::StateType *rstate = static_cast<RealVectorStateSpace::StateType*>(workState);

    for(int i = NUM_BASE_DOF; i < num_dof_; i++){
        rstate->values[i] = rng_.uniformReal(params_.bounds_.low[i], params_.bounds_.high[i]);
    }
}

void ompl::base::HarmoniousSampler::uniformSampling(ompl::base::State *workState) {
    RealVectorStateSpace::StateType *rstate = static_cast<RealVectorStateSpace::StateType*>(workState);

    for(int i = 0; i < num_dof_; i++){
        rstate->values[i] = rng_.uniformReal(params_.bounds_.low[i], params_.bounds_.high[i]);
    }
}

void ompl::base::HarmoniousSampler::biasedSampling(ompl::base::State *workState) {
    base::RealVectorStateSpace::StateType *rstate = static_cast<base::RealVectorStateSpace::StateType*>(workState);

    double f = (double)rand() / RAND_MAX;
    double cdf_rnd = f * (cumul_dist_[params_.reg_index_[0]-1][params_.reg_index_[1]-1][params_.reg_index_[2]-1]);

    std::vector<int> index = findIndex(cdf_rnd);

    ddd[index[0]]++;

    double x = (index[0] + 0.5) / params_.resolution_ + params_.bounds_.low[0];
    rstate->values[0] = rng_.uniformReal(x - params_.interval_[0], x + params_.interval_[0]);
    double y = (index[1] + 0.5) / params_.resolution_ + params_.bounds_.low[1];
    rstate->values[1] = rng_.uniformReal(y - params_.interval_[1], y + params_.interval_[1]);
    double theta = (index[2] + 0.5) / params_.res_yaw_ + params_.bounds_.low[2];
    rstate->values[2] = rng_.uniformReal(theta - params_.interval_[2], theta + params_.interval_[2]);
}

void ompl::base::HarmoniousSampler::setBasicPose(ompl::base::State *workState){
    base::RealVectorStateSpace::StateType *rstate = static_cast<base::RealVectorStateSpace::StateType*>(workState);

    for (int i = NUM_BASE_DOF; i < num_dof_; i++)
        rstate->values[i] = defined_joint_conf_[i-NUM_BASE_DOF];
}

std::vector<int> ompl::base::HarmoniousSampler::findIndex(const double value) {
    std::vector<int> index(NUM_BASE_DOF, 0);

    if(value < cumul_dist_[0][0][0]){
        return index;
    }

    index[0] = bSearch(value, 0, params_.reg_index_[0]-1);
    index[1] = bSearch(value, 0, params_.reg_index_[1]-1, index[0]);
    index[2] = bSearch(value, 0, params_.reg_index_[2]-1, index[0], index[1]);

    return index;
}

int ompl::base::HarmoniousSampler::bSearch(double val, int first, int last) {
    int mid;

    while(first <= last) {
        mid = (first + last) / 2;
        if (cumul_dist_[mid][0][0] < val && cumul_dist_[mid][params_.reg_index_[1]-1][params_.reg_index_[2]-1] >= val){
            return mid;
        }

        if (cumul_dist_[mid][0][0] < val) {
            first = mid + 1;
        }
        else if (cumul_dist_[mid][0][0] > val) {
            last = mid - 1;
        }
    }

    return mid;
}

int ompl::base::HarmoniousSampler::bSearch(double val, int first, int last, int x) {
    int mid;

    while(first <= last) {
        mid = (first + last) / 2;
        if (cumul_dist_[x][mid][0] < val && cumul_dist_[x][mid][params_.reg_index_[2]-1] >= val){
            return mid;
        }

        if (cumul_dist_[x][mid][0] < val) {
            first = mid + 1;
        }
        else if (cumul_dist_[x][mid][0] > val) {
            last = mid - 1;
        }
    }

    return mid;
}

int ompl::base::HarmoniousSampler::bSearch(double val, int first, int last, int x, int y) {
    int mid = 0;
    while(first <= last)
    {
        mid = (first + last) / 2;
        if (mid == 0) {
            return mid;
        }
        if (cumul_dist_[x][y][mid] < val && cumul_dist_[x][y][mid-1] >= val){
            return mid;
        }
        if (cumul_dist_[x][y][mid] < val) {
            first = mid + 1;
        }
        else if (cumul_dist_[x][y][mid] > val) {
            last = mid - 1;
        }
    }

    return mid;
}


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

#include <iostream>

#include <boost/safe_numerics/safe_integer.hpp>
#include <boost/safe_numerics/automatic.hpp>
#include "test_multiply_automatic_results.hpp"

template <class T>
using safe_t = boost::safe_numerics::safe<
    T,
    boost::safe_numerics::automatic
>;
#include "test_multiply.hpp"

#include <boost/mp11/list.hpp>
#include <boost/mp11/algorithm.hpp>
#include <boost/core/demangle.hpp>

using namespace boost::mp11;

template<typename L>
struct test {
    static_assert(mp_is_list<L>(), "must be a list of integral constants");
    bool m_error;
    test(bool b = true) : m_error(b) {}
    operator bool(){
        return m_error;
    }
    template<typename T>
    void operator()(const T &){
        static_assert(mp_is_list<T>(), "must be a list of two integral constants");
        constexpr size_t i1 = mp_first<T>(); // index of first argument
        constexpr size_t i2 = mp_second<T>();// index of second argument
        std::cout << i1 << ',' << i2 << ',';
        using T1 = typename mp_at_c<L, i1>::value_type;
        using T2 = typename mp_at_c<L, i2>::value_type;
        m_error &= test_multiply(
            mp_at_c<L, i1>()(), // value of first argument
            mp_at_c<L, i2>()(), // value of second argument
            boost::core::demangle(typeid(T1).name()).c_str(),
            boost::core::demangle(typeid(T2).name()).c_str(),
            test_multiplication_automatic_result[i1][i2]
        );
    }
};

#include "check_symmetry.hpp"

int main(){
    static_assert(
        check_symmetry(test_multiplication_automatic_result),
        "sanity check on test matrix - should be symmetrical"
    );

    test<test_values> rval(true);

    using value_indices = mp_iota_c<mp_size<test_values>::value>;
    mp_for_each<
        mp_product<mp_list, value_indices, value_indices>
    >(rval);

    std::cout << (rval ? "success!" : "failure") << std::endl;
    return ! rval ;
}


/**
 * \file dcs/testbed/padala2009_autocontrol_application_manager.hpp
 *
 * \brief Class modeling the Application Manager component based on the work
 *  of (Padala et al, 2009)
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2012 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * 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.
 */

#ifndef DCS_TESTBED_PADALA2009_AUTOCONTROL_APPLICATION_MANAGER_HPP
#define DCS_TESTBED_PADALA2009_AUTOCONTROL_APPLICATION_MANAGER_HPP

//@{ Consistency checks for macros

#ifndef DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_NEGATIVE_SHARE_ACTION
# define DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_NEGATIVE_SHARE_ACTION 'C'
#endif // DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_NEGATIVE_SHARE_ACTION
#if    DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_NEGATIVE_SHARE_ACTION != 'C' \
    && DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_NEGATIVE_SHARE_ACTION != 'M' \
    && DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_NEGATIVE_SHARE_ACTION != 'R' \
    && DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_NEGATIVE_SHARE_ACTION != 'W'
# error Unknwon value for DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_NEGATIVE_SHARE_ACTION.
#endif // DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_NEGATIVE_SHARE_ACTION

//@} Consistency checks for macros

#include <algorithm>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/p_square_quantile.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublasx/operation/all.hpp>
#ifdef DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_USE_ARX_B0_SIGN_HEURISTIC
# include <boost/numeric/ublasx/operation/any.hpp>
#endif // DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_USE_ARX_B0_SIGN_HEURISTIC
#include <boost/numeric/ublasx/operation/isfinite.hpp>
#include <boost/smart_ptr.hpp>
#include <cmath>
#include <cstddef>
#include <ctime>
#include <dcs/assert.hpp>
//#include <dcs/control/analysis/controllability.hpp>
//#include <dcs/control/analysis/detectability.hpp>
//#include <dcs/control/analysis/observability.hpp>
//#include <dcs/control/analysis/stabilizability.hpp>
#include <dcs/debug.hpp>
#include <dcs/exception.hpp>
#include <dcs/logging.hpp>
#include <dcs/macro.hpp>
#include <dcs/math/traits/float.hpp>
#include <dcs/testbed/application_performance_category.hpp>
#include <dcs/testbed/base_application_manager.hpp>
#include <dcs/testbed/data_smoothers.hpp>
#include <dcs/testbed/system_identification_strategies.hpp>
#include <dcs/testbed/virtual_machine_performance_category.hpp>
#include <fstream>
#ifdef DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_USE_ARX_B0_SIGN_HEURISTIC
# include <functional>
#endif // DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_USE_ARX_B0_SIGN_HEURISTIC
#include <limits>
#include <map>
#include <sstream>
#include <stdexcept>
#include <vector>

//TODO:
// - Currently the code in this class assumes the single resource (CPU) case.
//

namespace dcs { namespace testbed {

template <typename TraitsT>
class padala2009_autocontrol_application_manager: public base_application_manager<TraitsT>
{
	private: typedef base_application_manager<TraitsT> base_type;
	public: typedef typename base_type::traits_type traits_type;
	public: typedef typename traits_type::real_type real_type;
	private: typedef typename base_type::app_type app_type;
	private: typedef typename base_type::app_pointer app_pointer;
	private: typedef typename base_type::vm_identifier_type vm_identifier_type;
	private: typedef typename app_type::sensor_type sensor_type;
	private: typedef typename app_type::sensor_pointer sensor_pointer;
	protected: typedef ::boost::numeric::ublas::vector<real_type> numeric_vector_type;
	protected: typedef ::boost::numeric::ublas::matrix<real_type> numeric_matrix_type;
	private: typedef base_arx_system_identification_strategy<traits_type> sysid_strategy_type;
	private: typedef ::boost::shared_ptr<sysid_strategy_type> sysid_strategy_pointer;
	private: typedef ::std::vector<real_type> observation_container;
	private: typedef ::std::map<application_performance_category,observation_container> observation_map;
	//private: typedef ::std::map<application_performance_category,numeric_vector_type> observation_map;
	private: typedef ::std::map<application_performance_category,sensor_pointer> app_sensor_map;
	private: typedef ::std::map<virtual_machine_performance_category,::std::map<vm_identifier_type,sensor_pointer> > vm_sensor_map;

	private: static const real_type default_sampling_time;
	private: static const real_type default_control_time;
	private: static const real_type default_min_share;
	private: static const real_type default_max_share;
	private: static const real_type default_stability_factor;

	public: padala2009_autocontrol_application_manager()
	: q_(default_stability_factor),
	  ctl_count_(0),
      ctl_skip_count_(0),
      ctl_fail_count_(0),
      sysid_fail_count_(0)
	{
		this->sampling_time(default_sampling_time);
		this->control_time(default_control_time);
	}

	public: void sysid_strategy(sysid_strategy_pointer const& p_strategy)
	{
		p_sysid_alg_ = p_strategy;
	}

	public: sysid_strategy_pointer sysid_strategy()
	{
		return p_sysid_alg_;
	}

	public: sysid_strategy_pointer sysid_strategy() const
	{
		return p_sysid_alg_;
	}

	public: void stability_factor(real_type value)
	{
		q_ = value;
	}

	public: real_type stability_factor() const
	{
		return q_;
	}

	public: void export_data_to(::std::string const& fname)
	{
		dat_fname_ = fname;
	}

	private: void do_reset()
	{
		typedef typename base_type::target_value_map::const_iterator target_iterator;
		typedef typename app_type::vm_pointer vm_pointer;

		// pre: p_sysid_alg_ != null
		DCS_ASSERT(p_sysid_alg_,
				   DCS_EXCEPTION_THROW(::std::runtime_error,
									   "System identification strategy is not set"));
		//[FIXME]
		// Currently, we only handle a single performance metric
		// To handle more than one performance metrics we can take the approach used for instance in:
		// - X. Liu, X. Zhu, P. Padala, Z. Wang, S. Singhal,
		//   "Optimal Multivariate Control for Differentiated Services on a Shared Hosting Platform",
		//   HP Labs Technical Report
		DCS_ASSERT(this->target_values().size() == 1,
				   DCS_EXCEPTION_THROW(::std::logic_error,
									   "Currently, only one application performace category is handled"));
		//[/FIXME]

		p_sysid_alg_->init();

		//const ::std::size_t nu = p_sysid_alg_->num_inputs();
		//const ::std::size_t ny = p_sysid_alg_->num_outputs();

		const ::std::vector<vm_pointer> vms = this->app().vms();
		const ::std::size_t nvms = this->app().num_vms();

		// Reset App sensors
		app_sensors_.clear();
		//ybar_.clear();
		const target_iterator tgt_end_it = this->target_values().end();
		for (target_iterator tgt_it = this->target_values().begin();
			 tgt_it != tgt_end_it;
			 ++tgt_it)
		{
			const application_performance_category cat = tgt_it->first;

			//ybar_[cat] = numeric_vector_type(ny, tgt_it->second);
			app_sensors_[cat] = this->app().sensor(cat);
		}

		// Reset VM sensors
		vm_sensors_.clear();
		for (::std::size_t i = 0; i < nvms; ++i)
		{
			const virtual_machine_performance_category cat = cpu_util_virtual_machine_performance;
			vm_pointer p_vm = vms[i];

			vm_sensors_[cat][p_vm->id()] = p_vm->sensor(cat);
		}

		// Reset counters
		ctl_count_ = ctl_skip_count_
				   = ctl_fail_count_
				   = sysid_fail_count_
				   = 0;

		// Reset VCPU util estimator and smoother
		for (::std::size_t i = 0; i < nvms; ++i)
		{
			//this->data_estimator(cpu_util_virtual_machine_performance, vms[i]->id(), ::boost::make_shared< testbed::mean_estimator<real_type> >());
			this->data_smoother(cpu_util_virtual_machine_performance, vms[i]->id(), ::boost::make_shared< testbed::brown_single_exponential_smoother<real_type> >(0.9));
			//this->data_smoother(cpu_util_virtual_machine_performance, vms[i]->id(), ::boost::make_shared< testbed::holt_winters_double_exponential_smoother<real_type> >(beta_));
		}

		// Reset output data file
		if (p_dat_ofs_ && p_dat_ofs_->is_open())
		{
			p_dat_ofs_->close();
		}
		p_dat_ofs_.reset();
		if (!dat_fname_.empty())
		{
			p_dat_ofs_ = ::boost::make_shared< ::std::ofstream >(dat_fname_.c_str());
			if (!p_dat_ofs_->good())
			{
				::std::ostringstream oss;
				oss << "Cannot open output data file '" << dat_fname_ << "'";

				DCS_EXCEPTION_THROW(::std::runtime_error, oss.str());
			}

			*p_dat_ofs_ << "\"ts\"";

			const ::std::size_t nvms = this->app().num_vms();
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"Cap_{" << vms[i]->id() << "}\",\"Share_{" << vms[i]->id() << "}\",\"Util_{" << vms[i]->id() << "}\"";
			}
			for (target_iterator tgt_it = this->target_values().begin();
				 tgt_it != tgt_end_it;
				 ++tgt_it)
			{
				const application_performance_category cat = tgt_it->first;

				*p_dat_ofs_ << ",\"y_{" << cat << "}\",\"yn_{" << cat << "}\",\"r_{" << cat << "}\"";
			}
			*p_dat_ofs_ << ",\"# Controls\",\"# Skip Controls\",\"# Fail SysIds\",\"# Fail Controls\"";
			*p_dat_ofs_ << ::std::endl;
		}
	}

	private: void do_sample()
	{
		typedef typename vm_sensor_map::const_iterator vm_sensor_iterator;
		typedef typename app_sensor_map::const_iterator app_sensor_iterator;
		typedef ::std::vector<typename sensor_type::observation_type> obs_container;
		typedef typename obs_container::const_iterator obs_iterator;

		DCS_DEBUG_TRACE("(" << this << ") BEGIN Do SAMPLE - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << sysid_fail_count_ << "/" << ctl_fail_count_);

		// Collect VM values
		const vm_sensor_iterator vm_sens_end_it = vm_sensors_.end();
		for (vm_sensor_iterator vm_sens_it = vm_sensors_.begin();
			 vm_sens_it != vm_sens_end_it;
			 ++vm_sens_it)
		{
			const virtual_machine_performance_category cat = vm_sens_it->first;

			const typename vm_sensor_map::mapped_type::const_iterator vm_end_it = vm_sens_it->second.end();
			for (typename vm_sensor_map::mapped_type::const_iterator vm_it = vm_sens_it->second.begin();
				 vm_it != vm_end_it;
				 ++vm_it)
			{
				const vm_identifier_type vm_id = vm_it->first;
				sensor_pointer p_sens = vm_it->second;

				// check: p_sens != null
				DCS_DEBUG_ASSERT( p_sens );

				p_sens->sense();
				if (p_sens->has_observations())
				{
					const obs_container obs = p_sens->observations();
					const obs_iterator end_it = obs.end();
					for (obs_iterator it = obs.begin();
						 it != end_it;
						 ++it)
					{
						//this->data_estimator(cat, vm_id).collect(it->value());
						this->data_smoother(cat, vm_id).smooth(it->value());
					}
				}
			}
		}

		// Collect App values
		const app_sensor_iterator app_sens_end_it = app_sensors_.end();
		for (app_sensor_iterator app_sens_it = app_sensors_.begin();
			 app_sens_it != app_sens_end_it;
			 ++app_sens_it)
		{
			const application_performance_category cat = app_sens_it->first;

			sensor_pointer p_sens = app_sens_it->second;

			// check: p_sens != null
			DCS_DEBUG_ASSERT( p_sens );

			p_sens->sense();
			if (p_sens->has_observations())
			{
				typedef ::std::vector<typename sensor_type::observation_type> obs_container;
				typedef typename obs_container::const_iterator obs_iterator;

				obs_container obs = p_sens->observations();
				obs_iterator end_it = obs.end();
				for (obs_iterator it = obs.begin();
					 it != end_it;
					 ++it)
				{
					this->data_estimator(cat).collect(it->value());
				}
			}
		}

		DCS_DEBUG_TRACE("(" << this << ") END Do SAMPLE - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << sysid_fail_count_ << "/" << ctl_fail_count_);
	}

    private: void do_control()
    {
		namespace ublas = ::boost::numeric::ublas;
		namespace ublasx = ::boost::numeric::ublasx;

		//typedef ::std::map<application_performance_category,numeric_vector_type> performance_container;
		typedef typename base_type::target_value_map::const_iterator target_iterator;
		typedef typename app_type::vm_pointer vm_pointer;
		typedef ::std::vector<vm_pointer> vm_container;
		typedef typename vm_container::iterator vm_iterator;
		typedef typename vm_container::const_iterator vm_citerator;

		DCS_DEBUG_TRACE("(" << this << ") BEGIN Do CONTROL - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << sysid_fail_count_ << "/" << ctl_fail_count_);

		const ::std::size_t ny = p_sysid_alg_->num_outputs();
		const ::std::size_t nu = p_sysid_alg_->num_inputs();
		const ::std::size_t na = p_sysid_alg_->output_order();
		const ::std::size_t nb = p_sysid_alg_->input_order();
		const ::std::size_t nk = p_sysid_alg_->input_delay();

		bool skip_ctl = false;
		::std::map<application_performance_category,numeric_vector_type> yhs; // actual performance measure (this control output)
		numeric_vector_type u(nu, 0); // actual resource share (the control input)

		++ctl_count_;

		vm_container vms = this->app().vms();

		// Update control output measures
		if (ny > 0)
		{
			target_iterator tgt_end_it = this->target_values().end();
			for (target_iterator tgt_it = this->target_values().begin();
				 tgt_it != tgt_end_it;
				 ++tgt_it)
			{
				const application_performance_category cat(tgt_it->first);

				// Compute a summary statistics of collected observation
				if (this->data_estimator(cat).count() > 0)
				{
					const real_type y = this->data_estimator(cat).estimate();
					const real_type yb = this->target_value(cat);

					yhs[cat] = ublas::element_div(ublas::scalar_vector<real_type>(ny, y), ublas::scalar_vector<real_type>(ny, yb));
DCS_DEBUG_TRACE("Observed Normalized Output for Category " << cat << ": " << yhs.at(cat));//XXX
				}
				else
				{
					// No observation collected during the last control interval
					//TODO: what can we do?
					// 1. Skip control?
					// 2. Use a data smoother (e.g., EWMA)?
					// The original paper does not consider this case. So we choose option #1
					DCS_DEBUG_TRACE("No observation collected during the last control interval -> Skip control");
					skip_ctl = true;
					break;
				}

#ifdef DCSXX_TESTBED_EXP_APP_MGR_RESET_ESTIMATION_EVERY_INTERVAL
				this->data_estimator(cat).reset();
#endif // DCSXX_TESTBED_EXP_APP_MGR_RESET_ESTIMATION_EVERY_INTERVAL
			}
		}
		if (!skip_ctl)
		{
			if (nu > 0)
			{
				//FIXME: actual share should be scaled according to the capacity of the "reference" machine

				::std::size_t v(0);

				vm_citerator vm_end_it = vms.end();
				for (vm_citerator vm_it = vms.begin();
					 vm_it != vm_end_it;
					 ++vm_it)
				{
					const vm_pointer p_vm(*vm_it);

					// check: p_vm != null
					DCS_DEBUG_ASSERT( p_vm );

					u(v) = p_vm->cpu_share()/**p_vm->max_num_vcpus()*/;

					++v;
				}
			}

			// Estimate system params
			bool ok(true);
			try
			{
				//FIXME: currently, we handle only one performance category at time
				numeric_vector_type new_yh;
				target_iterator tgt_end_it = this->target_values().end();
				for (target_iterator tgt_it = this->target_values().begin();
					 tgt_it != tgt_end_it;
					 ++tgt_it)
				{
					const application_performance_category cat(tgt_it->first);

					new_yh = p_sysid_alg_->estimate(yhs.at(cat), u);
DCS_DEBUG_TRACE("RLS estimation:");//XXX
DCS_DEBUG_TRACE("Performance category " << cat);//XXX
DCS_DEBUG_TRACE("yh=" << yhs.at(cat));//XXX
DCS_DEBUG_TRACE("u=" << u);//XXX
DCS_DEBUG_TRACE("new_yh=" << new_yh);//XXX
DCS_DEBUG_TRACE("Theta_hat=" << p_sysid_alg_->Theta_hat());//XXX
DCS_DEBUG_TRACE("P=" << p_sysid_alg_->P());//XXX
DCS_DEBUG_TRACE("phi=" << p_sysid_alg_->phi());//XXX
				}

				if (!ublasx::all(ublasx::isfinite(p_sysid_alg_->Theta_hat())))
				{
					::std::ostringstream oss;
					oss << "Unable to estimate system parameters: infinite values in system parameters";
					::dcs::log_warn(DCS_LOGGING_AT, oss.str());

					ok = false;
				}
			}
			catch (::std::exception const& e)
			{
				DCS_DEBUG_TRACE( "Caught exception: " << e.what() );

				::std::ostringstream oss;
				oss << "Unable to estimate system parameters: " << e.what();
				::dcs::log_warn(DCS_LOGGING_AT, oss.str());

				ok = false;
			}

			if (ok && p_sysid_alg_->count() > (na+nb+nk-1))
			{
				numeric_vector_type opt_u;
				try
				{
					// $\bar{\mathbf{u}}_a^*(k)= (\mathbf{b}_0\mathbf{b}_0^T+q\mathbf{I})^{-1}((1+\sum_{i=1}^{n_a}\hat{y}_a(k-i)-\sum_{i=1}^{n_b}\mathbf{b}_i^T\mathbf{u}_a(k-i))\mathbf{b}_0+q\sum_{i=1}^{n_b}\mathbf{u}_a(k-i))$
					// (b1^2 + q)^{-1}*(q*(u2 + u3) + (r - b2*u2 - b3*u3 + a1*y1 + a2*y2 + a3*y3)*b1)

//#ifdef DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_USE_ARX_B0_SIGN_HEURISTIC
//					// Check on B(1) suggested by Karlsson et al "Dynamic Black-Box Performance Model Estimation for Self-Tuning Regulators", 2005
//					// This essentially consider the model as a linear model where u(k) is the free variabile.
//					// They compute the first partial derivative wrt to u(k) which gives the matrix B(1).
//					// In order to preverse reverse proportionality ==> diag(B(1)) < 0
//					if (ublasx::any(p_sysid_alg_->B(1), ::std::bind2nd(::std::greater_equal<real_type>(), 0)))
//					{
//						DCS_EXCEPTION_THROW(::std::runtime_error,
//											"Cannot compute optimal control input: First partial derivative of input-output model has positive elements on the main diagonal");
//					}
//
////FIXME: to be checked
////					// Try to predict a new y value and see if reverse proportionality hold
////					{
////						vector_type pred_out1(n_s_,0);
////						vector_type pred_out2(n_s_,0);
////
////						for (uint_type k = 1; k <= n_a_; ++k)
////						{
////							pred_out1 = pred_out1 - ublas::prod(ptr_ident_strategy_->A(k), ptr_ident_strategy_->y(k));
////						}
////						pred_out2 = pred_out1;
////						for (uint_type k = 1; k <= n_b_; ++k)
////						{
////							pred_out1 = pred_out1 + ublas::prod(ptr_ident_strategy_->B(k), ptr_ident_strategy_->u(k+d_-1));
////							if (k > 1)
////							{
////								pred_out2 = pred_out2 + ublas::prod(ptr_ident_strategy_->B(k), ptr_ident_strategy_->u(k+d_-1));
////							}
////							else
////							{
////								pred_out2 = pred_out2 + ublas::prod(ptr_ident_strategy_->B(1), ublas::scalar_vector<real_type>(n_s_, 1));
////							}
////						}
////
//////::std::cerr << "PREDICTION -- y1 = " << pred_out1 << " - y2 = " << pred_out2 << ::std::endl;//XXX
////						if (ublasx::any(pred_out2-pred_out1, ::std::bind2nd(::std::greater<real_type>(), 0)))
////						{
////							//DCS_EXCEPTION_THROW( ::std::runtime_error, "Cannot compute optimal control input: Bad prediction from Input-Output model" );
////							::std::ostringstream oss;
////							oss << "APP: " << app.id() << " - Bad prediction from Input-Output model";
////							::dcs::testbed::log_warn(cls_id_, oss.str());
////						}
////					}
//#endif // DCS_TESTBED_EXP_PADALA2009_AUTOCONTROL_APP_MGR_USE_ARX_B0_SIGN_HEURISTIC

					real_type yr(1); //FIXME: only one reference target is handled

					// Compute L = (b_0 b_0^T + qI)^{-1}
					numeric_vector_type b0(ublas::row(p_sysid_alg_->B(1), 0));
					numeric_matrix_type L;
					L = ublas::outer_prod(b0, b0)+q_*ublas::identity_matrix<real_type>(nu, nu);
					bool inv = ublasx::inv_inplace(L);

					DCS_ASSERT(inv,
							   DCS_EXCEPTION_THROW(::std::runtime_error,
												   "Cannot compute optimal control input: Matrix (b_0*b_0^T+q*I_{nu,nu}) is not invertible"));

					// Compute say = (a_1 yh(t-1) + a_2 yh(k-2))
					real_type say(0);
					for (::std::size_t k = 1; k <= na; ++k)
					{
						say += ublas::prod(p_sysid_alg_->A(k), p_sysid_alg_->y(k))(0);
					}
					// Compute:
					//  sbu = (b_1^Tu(k-1) + ... + b_{nu}^Tu(k-nb+1)
					//  su  = u(k-1) + ... + u(k-nb+1)
					real_type sbu(0);
					numeric_vector_type su(nu, 0);
					for (::std::size_t k = 2; k <= nb; ++k)
					{
						sbu += ublas::prod(p_sysid_alg_->B(k), p_sysid_alg_->u(k-2+nk))(0);
						su = su+p_sysid_alg_->u(k-2+nk);
					}
					// Compute R = (yr - a_1 yh(t-1) - a_2 yh(k-2) - b_1^Tu(k-1))b_0 + q u(k-1)
					//           = (yr + say - sbu)b_0 + q su
					numeric_vector_type R;
					R = (yr+say-sbu)*b0+q_*su;
					// Compute opt_u = (b_0 b_0^T + qI)^{-1}((yr - a_1 yh(t-1) - a_2 yh(k-2) - b_1^Tu(k-1))b_0 + q u(k-1))
					//               = L R
					opt_u = ublas::prod(L, R);

					if (ublasx::any(opt_u, ::std::bind2nd(::std::less<real_type>(), 0)))
					{
						++ctl_fail_count_;

						::std::ostringstream oss;
						oss << "Control not applied: computed negative share '" << opt_u << "'";
						::dcs::log_warn(DCS_LOGGING_AT, oss.str());

						ok = false;
					}
				}
				catch (::std::exception const& e)
				{
					DCS_DEBUG_TRACE( "Caught exception: " << e.what() );

					::std::ostringstream oss;
					oss << "Unable to compute optimal control: " << e.what();
					::dcs::log_warn(DCS_LOGGING_AT, oss.str());

					ok = false;
				}
				if (ok)
				{
DCS_DEBUG_TRACE("Applying optimal control");
					//FIXME: new share should be scaled according to the capacity of the "real" machine
					//FIXME: implement the Physical Machine Manager

					::std::size_t v(0);
					vm_iterator vm_end_it = vms.end();
					for (vm_iterator vm_it = vms.begin();
						 vm_it != vm_end_it;
						 ++vm_it)
					{
						vm_pointer p_vm(*vm_it);

						// check: p_vm != null
						DCS_DEBUG_ASSERT( p_vm );

						const real_type old_share = p_vm->cpu_share();

						real_type new_share = opt_u(v);

						if (::dcs::math::float_traits<real_type>::definitely_less(new_share, default_min_share))
						{
							::std::ostringstream oss;
							oss << "Optimal share (" << new_share << ") too small; adjusted to " << default_min_share;
							::dcs::log_warn(DCS_LOGGING_AT, oss.str());
						}
//						else if (::dcs::math::float_traits<real_type>::definitely_greater(new_share, default_max_share))
//						{
//							::std::ostringstream oss;
//							oss << "Optimal share (" << new_share << ") too big; adjusted to " << default_max_share;
//							::dcs::log_warn(DCS_LOGGING_AT, oss.str());
//						}

						new_share = /*::std::min(*/::std::max(new_share, default_min_share)/*, default_max_share)*/;

//						u_.push_back(new_share);

DCS_DEBUG_TRACE("VM '" << p_vm->id() << "' - old-share: " << old_share << " - new-share: " << new_share);
						if (::std::isfinite(new_share) && !::dcs::math::float_traits<real_type>::essentially_equal(old_share, new_share))
						{
							p_vm->cpu_share(new_share);
						}

						++v;
					}
DCS_DEBUG_TRACE("Optimal control applied");//XXX
				}
				else
				{
					++ctl_fail_count_;

					::std::ostringstream oss;
					oss << "Control not applied: failed to solve the control problem";
					::dcs::log_warn(DCS_LOGGING_AT, oss.str());
				}
			}
			else if (!ok)
			{
				p_sysid_alg_->reset();
				++sysid_fail_count_;

				::std::ostringstream oss;
				oss << "Control not applied: failed to solve the identification problem";
				::dcs::log_warn(DCS_LOGGING_AT, oss.str());
			}
		}
		else
		{
			++ctl_skip_count_;
		}

		if (p_dat_ofs_)
		{
			*p_dat_ofs_ << ::std::time(0) << ",";
			const vm_citerator vm_end_it = vms.end();
			for (vm_citerator vm_it = vms.begin();
				 vm_it != vm_end_it;
				 ++vm_it)
			{
				const vm_pointer p_vm(*vm_it);

				// check: p_vm != null
				DCS_DEBUG_ASSERT( p_vm );

				if (vm_it != vms.begin())
				{
					*p_dat_ofs_ << ",";
				}
				*p_dat_ofs_ << p_vm->cpu_cap() << "," << p_vm->cpu_share() << "," << this->data_smoother(cpu_util_virtual_machine_performance, p_vm->id()).forecast(0);
			}
			*p_dat_ofs_ << ",";
			const target_iterator tgt_end_it = this->target_values().end();
			for (target_iterator tgt_it = this->target_values().begin();
			tgt_it != tgt_end_it;
			++tgt_it)
			{
				const application_performance_category cat(tgt_it->first);

				if (tgt_it != this->target_values().begin())
				{
					*p_dat_ofs_ << ",";
				}
				const real_type yh = this->data_estimator(cat).estimate();
				const real_type yr = tgt_it->second;
				const real_type yn = yh/yr;
				*p_dat_ofs_ << yh << "," << yn << "," << yr;
			}
			*p_dat_ofs_ << "," << ctl_count_ << "," << ctl_skip_count_ << "," << sysid_fail_count_ << "," << ctl_fail_count_;
			*p_dat_ofs_ << ::std::endl;
		}

		DCS_DEBUG_TRACE("(" << this << ") END Do CONTROL - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << sysid_fail_count_ << "/" << ctl_fail_count_);
	}

//	private: ::std::vector<real_type> do_control_inputs() const
//	{
//		return u_;
//	}

//	private: ::std::vector<real_type> do_control_outputs() const
//	{
//		return y_;
//	}

	private: real_type q_; ///< The stability factor
	private: vm_sensor_map vm_sensors_;
	private: app_sensor_map app_sensors_; ///< Sensor map for the application outputs
	private: sysid_strategy_pointer p_sysid_alg_;
//	private: performance_measure_map yr_; ///< The output vector to be tracked , mapped by application performance category
	private: ::std::size_t ctl_count_; ///< Number of times control function has been invoked
	private: ::std::size_t ctl_skip_count_; ///< Number of times control has been skipped
	private: ::std::size_t ctl_fail_count_; ///< Number of times control has failed
	private: ::std::size_t sysid_fail_count_; ///< Number of times system identification has failed
	private: ::std::string dat_fname_;
	private: ::boost::shared_ptr< ::std::ofstream > p_dat_ofs_;
//	private: numeric_vector_type u_; ///< Current values for inputs
//	private: numeric_vector_type y_; ///< Current (normalized) values for outputs
}; // padala2009_autocontrol_application_manager

template <typename T>
const typename padala2009_autocontrol_application_manager<T>::real_type padala2009_autocontrol_application_manager<T>::default_sampling_time = 1;

template <typename T>
const typename padala2009_autocontrol_application_manager<T>::real_type padala2009_autocontrol_application_manager<T>::default_control_time = 5;

template <typename T>
const typename padala2009_autocontrol_application_manager<T>::real_type padala2009_autocontrol_application_manager<T>::default_min_share = 0.0;

template <typename T>
const typename padala2009_autocontrol_application_manager<T>::real_type padala2009_autocontrol_application_manager<T>::default_max_share = 1.00;

template <typename T>
const typename padala2009_autocontrol_application_manager<T>::real_type padala2009_autocontrol_application_manager<T>::default_stability_factor = 2.0;

}} // Namespace dcs::testbed

#endif // DCS_TESTBED_PADALA2009_AUTOCONTROL_APPLICATION_MANAGER_HPP