#include <algorithm>
using namespace std;

#include "regression_tree.h"
#include <boost/bind.hpp>
#include <boost/thread/thread.hpp>
using namespace boost;

extern vector<vector<int> > fInds;

int feature_to_split_on;

/*bool mysortpred(const tuple* d1, const tuple* d2) {
  return d1->features[feature_to_split_on] < d2->features[feature_to_split_on];
}
*/
/*void sort_data_by_feature(vector<tuple*>& data, int f) {
  feature_to_split_on = f;
  sort(data.begin(), data.end(), mysortpred);
}
*/

void sort_data_by_feature(vector<int>& location,  vector<int> dataCount, vector<int> invertIdx, int f){
	int cur=0;
        for (int i = 0; i < fInds[f].size(); i ++){
                int z = fInds[f][i];
		int loc=invertIdx[z];
                for (int j = 0; j < dataCount[z]; j ++)
			location[cur++]=loc;
        }
}

/*bool mysortpred2(const pair<tuple*, int> tk1, const pair<tuple*, int> tk2) {
  return tk1.first->features[tk1.second] < tk2.first->features[tk2.second];
}*/
/////////

//double best_fc_in_feature(vector<tuple*> data, int f, pair<int, double>& fc) {
double best_fc_in_feature(vector<tuple*> data, vector<int> dataCount, vector<int> invertIdx, int f, int& fs, double& vs, const args_t& args) {  //if (!XX)
/*  vector< pair<tuple*, int> > tk;
  int z;

  for (z = 0; z < data.size(); z++)
    tk.push_back( pair<tuple*,int>(data[z], f) );
  sort(tk.begin(), tk.end(), mysortpred2);
  for (z = 0; z < data.size(); z++)
    data[z] = tk[z].first;
*/
  int n = data.size(), i;
  double min = MY_DBL_MAX;
        vector<int> location(n, -1);
        sort_data_by_feature(location, dataCount, invertIdx, f);

  // get impurity (squared loss) for missing data
  double M = 0.0;
  double W = 0.0;
  int missing = 0;
	int loc = location[missing];
  while (data[loc]->features[f] == UNKNOWN && missing < n-1) {
    //M += data[missing]->target * 1.0;
	M += data[loc]->target * data[loc]->weight;
	W += data[loc]->weight;
    	missing++;
	loc = location[missing];
  }
  if (missing == n-1) // all data is missing
    return MY_DBL_MAX;
  if (missing) {
    //double mbar = M * 1.0 / missing;
	double mbar = M/W;
    M = 0.0;
    for (i = 0; i < missing; i++){
	loc = location[i];
      M += data[loc]->weight * (data[loc]->target - mbar) * (data[loc]->target - mbar);
    }
  }
  int nn = n - missing; // number of data points that arent missing

  // we have impurity = E_{i=0..n} (yi - ybar)^2  (E = summation)
  // this factors to impurty = E(yi^2) + (n+1)ybar^2 - 2*(n+1)*ybar^2
  // we want the impurity for left and right splits for all splits k
  // let ybl = ybar left, ybr = ybar right
  //     s = E_{i=0..k} yi^2
  //     r = E_{i=k+1..n} yi^2

  //impurity =\sum_i  wi (yi - ybar)^2 = \sum_i wi*yi^2 - 2*(\sum_i wi*yi)*ybar + (\sum_i wi) ybar^2
  // ybar = (\sum_i wi*yi) / (sum_i wi) = ywr/WR
  //  r = \sum_i wi*yi^2, ywr = \sum_i wi*yi,  WR = \sum_i wi
  // impurity = r - 2*ywr*ywr/WR + ywr*ywr/WR = r - ywr*ywr/WR
  double ybl, ybr, s, r, L, R, I, ywl, ywr, WL, WR;
  ybl = ybr = s = r = ywl = ywr = WL = WR = 0.0;

  // put all data into right side R
  // get ybar right and r
  int start = missing;
  for (i = start; i < n; i++) {
	loc = location[i];
    	r += data[loc]->target * data[loc]->target * data[loc]->weight;
    	//ybr += data[i]->target;
    	ywr += data[loc]->target * data[loc]->weight;
    	WR += data[loc]->weight;
  }
  //ybr /= 1.0 * nn;
  //r += 0.0000000001 for precision errors

  // for every i
  // put yi into left side, remove it from the right side, and calculate squared lost
  // impurity of putting all data points into the right tree equals putting all the data into the left tree (such cases are not considered)
  for (i = start; i < n-1; i++) {
    int j = i - missing;
    double yn = data[location[i]]->target;
    double w = data[location[i]]->weight;

    s += w *  yn * yn;
    r -= w * yn * yn;
    ywr -= w * yn;
    ywl += w * yn;
    WL += w;
    WR -= w;

    if (r < 0 && r > -0.000001) r = 0;
    if (r < 0) r = 0;

    //ybl = (j*ybl + yn) / (j+1.0);
    //ybr = ((nn-j)*ybr - yn) / (nn-j-1.0);

    //L = s + WL*ybl*ybl - 2*ybl*ywl;
    //R = r + WR*ybr*ybr - 2*ybr*ywr;
    L = s - ywl*ywl/WL;
    R = r - ywr*ywr/WR;

    //L = s + (j+1)*ybl*ybl - 2*(j+1)*ybl*ybl;
    //R = r + (nn-j-1.0)*ybr*ybr - 2*(nn-j-1.0)*ybr*ybr;

    // precision errors?
    if (L < 0 && L > -0.0000001) L = 0;
    if (R < 0 && R > -0.0000001) R = 0;
    if (R < 0) R = 0;
    if (L < 0) L = 0;

    if(0)
    if (L < 0 || R < 0 || r < 0)
      printf("Problem %lf %lf %lf\n", L, R, r);

    // do not consider splitting here if data is the same as next
    if (data[location[i]]->features[f] == data[location[i+1]]->features[f])
      continue;

    I = L + R;// + M;
    I = L + R + M;

    if (I < min) {
      min = I;
      fs = f;
      vs = (data[location[i]]->features[f] + data[location[i+1]]->features[f]) / 2;
    }
  }

  return min;
}

// find best feature to split on in range [start, end)
// store results int I, fs, vs : impurity, feature, value
//void find_split_in_range(vector<tuple*> data, vector<int> dataCount, vector<int> invertIdx, int start, int  end, int& fs, double& vs, double& I, vector<bool> skip, const args_t& args) {
void find_split_in_range(vector<tuple*> data, vector<int> dataCount, vector<int> invertIdx, pair<int,int> range, int& fs, double& vs, double& I, vector<bool>skip, const args_t& args) {

  double min = MY_DBL_MAX;
	int start = range.first, end = range.second;

  for (int i = start; i < end; i++) {
    int f = i + 1;
    if (skip[f]) continue;

    int fi;
    double vi, Ii;
    Ii = best_fc_in_feature(data, dataCount, invertIdx, f, fi, vi, args);
    if (Ii < min) {
      min = Ii;
      fs = fi;
      vs = vi;
    }
  }

  I = min;
}

bool find_split_p(vector<tuple*> data, vector<int> dataCount, vector<int> invertIdx, int NF, int& f_split, double& v_split, vector<bool>& skip, const args_t& args) {

  f_split = -1;
  double min = MY_DBL_MAX;
  int n = data.size(), i;

  pair<int, double>* fc = new pair<int,double>[NF];
  double* I = new double[NF];

  int numthreads = args.processors;
  thread** threads = new thread*[numthreads];

  int* F = new int[numthreads];
  double* V = new double[numthreads];
  double* Imp = new double[numthreads];

  for (i = 0; i < numthreads; i++)
//	find_split_in_range(data, dataCount, invertIdx, i*(NF-1)/numthreads, (i+1)*(NF-1)/numthreads, F[i], V[i], Imp[i], skip, args);
//    threads[i] = new thread(find_split_in_range, data, dataCount, invertIdx, i*(NF-1)/numthreads, (i+1)*(NF-1)/numthreads, ref(F[i]), ref(V[i]), ref(Imp[i]), ref(skip), cref(args));
	threads[i] = new thread(bind(find_split_in_range, data, dataCount, invertIdx, make_pair(i*(NF-1)/numthreads, (i+1)*(NF-1)/numthreads), ref(F[i]), ref(V[i]), ref(Imp[i]), ref(skip), cref(args) ));
  for (i = 0; i < numthreads; i++) {
    threads[i]->join();
    delete threads[i];
  }
  delete[] threads;

  for (i = 0; i < numthreads;i++)
    if (Imp[i] < min) {
      min = Imp[i];
      f_split = F[i];
      v_split = V[i];
    }

  delete[] fc;  delete[] I;  delete[] V;  delete[] Imp; delete[] F;
  return min != MY_DBL_MAX;
}

///////////////////////

bool dt_node::entropy_split(data_t data, vector<int> dataCount, vector<int> invertIdx, int NF, int& f_split, double& v_split, int K, bool par) {
  f_split = -1;
  double min = MY_DBL_MAX;
  int n = data.size(), i;

  vector<bool> skip;

  //min E(i=1..k) pi * log(1/pi)

  for (i = 0; i <= NF; i++)
    skip.push_back( (K > 0) ? true : false);

  for (i = 0; i < K; i++) {
    int f;
    do
      f = rand() % (NF-2) + 1;
    while (!skip[f]);
    skip[f] = false;
  }

  //if (K <= 0)
  //return find_split_p(data, NF, f_split, v_split, skip);

  vector<int> location(n, -1);
  for (int f = 1; f < NF; f++) {
	if (skip[f]) continue;
	sort_data_by_feature(location, dataCount, invertIdx, f);
    //sort_data_by_feature(data,f);

    //if (skip[f]) continue;

    /*
 vector< pair<tuple*, int> > tk;
  int z;

  for (z = 0; z < data.size(); z++)
  tk.push_back( pair<tuple*,int>(data[z], f) );
  sort(tk.begin(), tk.end(), mysortpred2);

  for (z = 0; z < data.size(); z++)
  data[z] = tk[z].first;
    */

  int num_c = 7;
  vector<double> c_miss, c_left, c_right;
  //vector<double> p_miss, p_left, p_right;
  //vector<int> freq;
  //int n_left = 0, n_right = 0;
  for (i=0;i<num_c;i++) {
	c_miss.push_back(0.0);
//	p_miss.push_back(0.0);
    c_left.push_back(0.0);
    c_right.push_back(0.0);
//    p_left.push_back(0.0);
//    p_right.push_back(0.0);
//    freq.push_back(0);
  }

    // get impurity (entropy) for missing data
    double M = 0.0, L, R, W_miss = 0.0, W_left = 0.0, W_right = 0.0;
    int missing = 0;
	int loc = location[missing];
    while (data[loc]->features[f] == UNKNOWN && missing < n-1) {
	c_miss[(int)data[loc]->target] += data[loc]->weight;
	W_miss += data[loc]->weight;
        missing++;
	loc = location[missing];
    }

    if (missing == n-1) // all data is missing
      continue;

    int nn = n - missing; // number of data points that arent missing
    // entropy
    // put all data into right side R
    int start = missing;
    for (i = start; i < n; i++) {
	loc = location[i];
      	c_right[(int)data[loc]->target]+=data[loc]->weight;
      	W_right+=data[loc]->weight;
    }
	if (W_right){
    		for (i = 0; i < num_c; i++) {
      			if (c_right[i])
				R += c_right[i]/W_right * log(W_right / c_right[i]);
    		}
	}

    	if (missing && W_miss) {
                for (i = 0; i < num_c; i ++){
                        if (c_miss[i])
                                M += c_miss[i]/W_miss * log(W_miss/c_miss[i]);
                }
    	}
    	L = 0.0;

    // for every i
    // put yi into left side, remove it from the right side, and calculate squared lost
    for (i = start; i < n-1; i++) {
      int j = i - missing;
      int yn = (int)data[loc]->target;

	W_right -= data[loc]->weight;
	W_left += data[loc]->weight;
      //n_right--;
      //n_left++;
	c_left[yn] += data[loc]->weight;
	c_right[yn] -= data[loc]->weight;
      //c_left[yn]++;
      //c_right[yn]--;

      //p_left[yn] += 1.0 / freq[yn];
      //p_right[yn] -= 1.0 / freq[yn];

      // do not consider splitting here if data is the same as next
      if (data[location[i]]->features[f] == data[location[i+1]]->features[f])
	continue;

      L = 0.0;
      int k;
      for (k = 0; k < num_c; k++) {
      	if (c_left[k])
	  L +=  c_left[k]/W_left * log(W_left / c_left[k]);
      }
      R = 0.0;
      for (k = 0; k < num_c; k++) {
	if (c_right[k])
	  R += c_right[k]/W_right * log(W_right / c_right[k]);
      }
      double ssum = W_left+W_right+W_miss;
      double I = W_left/ssum * L +  W_right/ssum * R + W_miss/ssum * M;
      //I = 1.0*n_left/n *

      /*
      L = 0.0, R = 0.0;
      for (i = 0; i < num_c; i++)
	L += p_left;
      */

      if (I < min) {
	min = I;
	f_split = f;
	v_split = (data[location[i]]->features[f] + data[location[i+1]]->features[f])/2;
      }
    }
  }

  return min != MY_DBL_MAX;
}

//////////////////////

bool dt_node::find_split(vector<tuple*> data, vector<int> dataCount, vector<int> invertIdx, int NF, int& f_split, double& v_split, int K, bool par, const args_t& args) {
  if (args.loss == ALG_ENTROPY)
    return entropy_split(data, dataCount, invertIdx, NF, f_split, v_split, K, par);

  f_split = -1;
  double min = MY_DBL_MAX;
  int n = data.size(), i;

  vector<bool> skip;

  //K = NF/2;

  // pick K random features to split on, if specified
  for (i = 0; i <= NF; i++)
    skip.push_back( (K > 0) ? true : false);
  for (i = 0; i < K; i++) {
    int f;
    do
      f = rand() % (NF-2) + 1;
    while (!skip[f]);
    skip[f] = false;
  }

  //if (K >= 10)
  if (args.alg != ALG_FOREST && args.processors!=1)
    return find_split_p(data, dataCount, invertIdx, NF, f_split, v_split, skip, args);

	vector<int> location(n, -1);
  for (int f = 1; f < NF; f++) {
        if (skip[f]) continue;
    // sort data
/*    vector< pair<tuple*, int> > tk;
    int z;
    for (z = 0; z < data.size(); z++)
      tk.push_back( pair<tuple*,int>(data[z], f) );
    sort(tk.begin(), tk.end(), mysortpred2);
    for (z = 0; z < data.size(); z++)
      data[z] = tk[z].first;
*/
	sort_data_by_feature(location, dataCount, invertIdx, f);

    // get impurity (squared loss) for missing data
/*    double M = 0.0;
    int missing = 0;
    while (data[missing]->features[f] == UNKNOWN && missing < n-1) {
      M += data[missing]->target * 1.0;
      missing++;
    }
    if (missing == n-1) // all data is missing
      continue;
    if (missing) {
      double mbar = M * 1.0 / missing;
      M = 0.0;
      for (i = 0; i < missing; i++)
	M += (data[i]->target - mbar) * (data[i]->target - mbar);
    }
    int nn = n - missing; // number of data points that arent missing
*/
  double M = 0.0;
  double W = 0.0;
  int missing = 0;
	int loc = location[missing];
  while (data[loc]->features[f] == UNKNOWN && missing < n-1) {
    //M += data[missing]->target * 1.0;
        M += data[loc]->target * data[loc]->weight;
        W += data[loc]->weight;
    	missing++;
	loc = location[missing];
  }
  if (missing == n-1) // all data is missing
    return MY_DBL_MAX;
  if (missing) {
    //double mbar = M * 1.0 / missing;
        double mbar = M/W;
    M = 0.0;
    for (i = 0; i < missing; i++){
	loc = location[i];
      M += data[loc]->weight * (data[loc]->target - mbar) * (data[loc]->target - mbar);
   }
  }

  double ybl, ybr, s, r, L, R, I, ywl, ywr, WL, WR;
  ybl = ybr = s = r = ywl = ywr = WL = WR = 0.0;

  // put all data into right side R
  // get ybar right and r
  int start = missing;
  for (i = start; i < n; i++) {
	loc = location[i];
    	r += data[loc]->target * data[loc]->target * data[loc]->weight;
    	//ybr += data[i]->target;
    	ywr += data[loc]->target * data[loc]->weight;
    	WR += data[loc]->weight;
  }
  //ybr /= 1.0 * nn;
  //r += 0.0000000001 for precision errors

  // for every i
  // put yi into left side, remove it from the right side, and calculate squared lost
  // impurity of putting all data points into the right tree equals putting all the data into the left tree (such cases are not considered)
  for (i = start; i < n-1; i++) {
    int j = i - missing;
    double yn = data[location[i]]->target;
    double w = data[location[i]]->weight;

    s += w *  yn * yn;
    r -= w * yn * yn;
    ywr -= w * yn;
    ywl += w * yn;
    WL += w;
    WR -= w;

    if (r < 0 && r > -0.000001) r = 0;
    if (r < 0) r = 0;
    //ybl = (j*ybl + yn) / (j+1.0);
    //ybr = ((nn-j)*ybr - yn) / (nn-j-1.0);
    //L = s + WL*ybl*ybl - 2*ybl*ywl;
    //R = r + WR*ybr*ybr - 2*ybr*ywr;
    L = s - ywl*ywl/WL;
    R = r - ywr*ywr/WR;
    // precision errors?
    if (L < 0 && L > -0.0000001) L = 0;
    if (R < 0 && R > -0.0000001) R = 0;
    if (R < 0) R = 0;
    if (L < 0) L = 0;

    if(0)
    if (L < 0 || R < 0 || r < 0)
      printf("Problem %lf %lf %lf\n", L, R, r);

    // do not consider splitting here if data is the same as next
    if (data[location[i]]->features[f] == data[location[i+1]]->features[f])
      continue;

    I = L + R + M ;

      if (I < min) {
        min = I;
        f_split = f;
        v_split = (data[location[i]]->features[f] + data[location[i+1]]->features[f])/2;
      }

  }
}
  return min != MY_DBL_MAX;
}


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

#include "../include/istr.hpp"
#include <boost/mpl/int.hpp>
#include <boost/mpl/char.hpp>

BOOST_AUTO_TEST_CASE(integer_to_string)
{
    constexpr auto s0 = istr(boost::mpl::int_<42>());
    BOOST_CHECK_EQUAL(s0.c_str(), "42");

    using c = boost::mpl::char_<s0.c_str()[1]>;
    using s = boost::mpl::int_<s0.size()>;

    BOOST_CHECK_EQUAL(c::value, '2');
    BOOST_CHECK_EQUAL(s::value, 2);

    BOOST_CHECK_EQUAL(istr(boost::mpl::int_<0>()).c_str(), "0");
    BOOST_CHECK_EQUAL(istr(boost::mpl::int_<10>()).c_str(), "10");
    BOOST_CHECK_EQUAL(istr(boost::mpl::int_<100>()).c_str(), "100");
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_NEXTAFTER_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_NEXTAFTER_HPP_INCLUDED

#include <boost/simd/ieee/functions/nextafter.hpp>
#include <boost/simd/include/functions/scalar/sign.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/sdk/math.hpp>
#include <boost/math/special_functions/next.hpp>
#include <boost/simd/include/functions/scalar/oneplus.hpp>
#include <boost/simd/include/functions/scalar/minusone.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/dispatch/attributes.hpp>

// workaround for boost.math bug #5823
namespace boost { namespace simd { namespace details
{
  template<class T>
  BOOST_FORCEINLINE T nextafter(T a0, T a1)
  {
    if(a0 > a1 && a0 == Inf<T>())
      return Valmax<T>();
    if(a0 < a1 && a0 == Minf<T>())
      return Valmin<T>();

    using namespace boost::math::policies;
    typedef policy<
                  domain_error<errno_on_error>,
                  pole_error<errno_on_error>,
                  overflow_error<errno_on_error>,
                  evaluation_error<errno_on_error>,
                  rounding_error<errno_on_error>
                > c_policy;

    return boost::math::nextafter(a0, a1, c_policy());
  }
}

namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( nextafter_, tag::cpu_
                                    , (A0)
                                    , (scalar_< arithmetic_<A0> >)(scalar_< arithmetic_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return a0+sign(a1-a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( nextafter_, tag::cpu_
                                    , (A0)
                                    , (scalar_< double_<A0> >)(scalar_< double_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
    #ifdef BOOST_SIMD_HAS_NEXTAFTER
      return ::nextafter(a0, a1);
    #elif defined(BOOST_SIMD_HAS__NEXTAFTER)
      return ::_nextafter(a0, a1);
    #else
      return details::nextafter(a0, a1);
    #endif
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( nextafter_, tag::cpu_
                                    , (A0)
                                    , (scalar_< single_<A0> >)(scalar_< single_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
    #ifdef BOOST_SIMD_HAS_NEXTAFTERF
      return ::nextafterf(a0, a1);
    #elif defined(BOOST_SIMD_HAS__NEXTAFTERF)
      return ::_nextafterf(a0, a1);
    #else
      return details::nextafter(a0, a1);
    #endif
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( nextafter_, tag::cpu_
                                    , (A0)
                                    , (scalar_< unsigned_<A0> >)(scalar_< unsigned_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return (a1 == a0) ? a0 : (a1 > a0) ? oneplus(a0) : minusone(a0);
    }
  };
} } }

#endif


// for friend classes: their cpp files can use this header to access
// implementation

#ifndef FORMAST_EXPR_IMPL_HPP_INCLUDED
#define FORMAST_EXPR_IMPL_HPP_INCLUDED

#include <boost/shared_ptr.hpp>
#include <boost/variant/recursive_variant.hpp>

#include "formast.hpp"

namespace formast
{
namespace detail
{

struct binary_op;
struct unary_op;

typedef boost::variant<boost::uint64_t, std::string,
        boost::recursive_wrapper<binary_op>,
        boost::recursive_wrapper<unary_op>
        >
        ExprTree;

struct binary_op {

    enum type {
        logical_or,
        logical_and,
        bit_or,
        bit_xor,
        bit_and,
        equal,
        not_equal,
        less,
        less_equal,
        greater,
        greater_equal,
        shift_left,
        shift_right,
        plus,
        minus,
        times,
        divide,
        mod,
        pow,
    };

    binary_op(type op_type, Expr const & left, Expr const & right)
        : op_type(op_type), left(left), right(right) {}

    type op_type;
    Expr left;
    Expr right;
};

struct unary_op {

    enum type {
        neg,
        pos,
        logical_not,
    };

    unary_op(type op_type, Expr const & right)
        : op_type(op_type), right(right) {}

    type op_type;
    Expr right;
};

}
}

class formast::Expr::Impl
{
private:
    // see http://www.boost.org/doc/libs/1_51_0/libs/smart_ptr/sp_techniques.html#pimpl
    Impl(Impl const &);
    Impl & operator=(Impl const &);
public:
    Impl();

    template <typename T>
    static boost::shared_ptr<const formast::detail::ExprTree> create(T const & value) {
        return
            boost::shared_ptr<const formast::detail::ExprTree>(
                new formast::detail::ExprTree(value));
    };

    boost::shared_ptr<const formast::detail::ExprTree> tree;
};

#endif


/*
	This file is part of cpp-ethereum.

	cpp-ethereum is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	cpp-ethereum is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/** @file Dagger.cpp
 * @author Gav Wood <i@gavwood.com>
 * @date 2014
 */

#include <boost/detail/endian.hpp>
#include <chrono>
#include <array>
#if WIN32
#pragma warning(push)
#pragma warning(disable:4244)
#else
#pragma GCC diagnostic ignored "-Wunused-function"
#endif
#include <sha3.h>
#if WIN32
#pragma warning(pop)
#endif
#include <random>
#include "Common.h"
#include "Dagger.h"
using namespace std;
using namespace std::chrono;

namespace eth
{

#if FAKE_DAGGER

MineInfo Dagger::mine(h256& o_solution, h256 const& _root, u256 const& _difficulty, uint _msTimeout, bool const& _continue)
{
	MineInfo ret{0, 0, false};
	static std::mt19937_64 s_eng((time(0)));
	u256 s = std::uniform_int_distribution<uint>(0, ~(uint)0)(s_eng);

	bigint d = (bigint(1) << 256) / _difficulty;
	ret.requirement = toLog2((u256)d);

	// 2^ 0      32      64      128      256
	//   [--------*-------------------------]
	//
	// evaluate until we run out of time
	for (auto startTime = steady_clock::now(); (steady_clock::now() - startTime) < milliseconds(_msTimeout) && _continue; s++)
	{
		o_solution = (h256)s;
		auto e = (bigint)(u256)eval(_root, o_solution);
		ret.best = max(ret.best, toLog2((u256)e));
		if (e <= d)
		{
			ret.completed = true;
			break;
		}
	}

	if (ret.completed)
		assert(verify(_root, o_solution, _difficulty));

	return ret;
}

#else

Dagger::Dagger()
{
}

Dagger::~Dagger()
{
}

u256 Dagger::bound(u256 const& _difficulty)
{
	return (u256)((bigint(1) << 256) / _difficulty);
}

bool Dagger::verify(h256 const& _root, u256 const& _nonce, u256 const& _difficulty)
{
	return eval(_root, _nonce) < bound(_difficulty);
}

bool Dagger::mine(u256& o_solution, h256 const& _root, u256 const& _difficulty, uint _msTimeout, bool const& _continue)
{
	// restart search if root has changed
	if (m_root != _root)
	{
		m_root = _root;
		m_nonce = 0;
	}

	// compute bound
	u256 const b = bound(_difficulty);

	// evaluate until we run out of time
	for (auto startTime = steady_clock::now(); (steady_clock::now() - startTime) < milliseconds(_msTimeout) && _continue; m_nonce += 1)
	{
		if (eval(_root, m_nonce) < b)
		{
			o_solution = m_nonce;
			return true;
		}
	}
	return false;
}

template <class _T>
inline void update(_T& _sha, u256 const& _value)
{
	int i = 0;
	for (u256 v = _value; v; ++i, v >>= 8) {}
	byte buf[32];
	bytesRef bufRef(buf, i);
	toBigEndian(_value, bufRef);
	_sha.Update(buf, i);
}

template <class _T>
inline void update(_T& _sha, h256 const& _value)
{
	int i = 0;
	byte const* data = _value.data();
	for (; i != 32 && data[i] == 0; ++i);
	_sha.Update(data + i, 32 - i);
}

template <class _T>
inline h256 get(_T& _sha)
{
	h256 ret;
	_sha.TruncatedFinal(&ret[0], 32);
	return ret;
}

h256 Dagger::node(h256 const& _root, h256 const& _xn, uint_fast32_t _L, uint_fast32_t _i)
{
	if (_L == _i)
		return _root;
	u256 m = (_L == 9) ? 16 : 3;
	CryptoPP::SHA3_256 bsha;
	for (uint_fast32_t k = 0; k < m; ++k)
	{
		CryptoPP::SHA3_256 sha;
		update(sha, _root);
		update(sha, _xn);
		update(sha, (u256)_L);
		update(sha, (u256)_i);
		update(sha, (u256)k);
		uint_fast32_t pk = (uint_fast32_t)(u256)get(sha) & ((1 << ((_L - 1) * 3)) - 1);
		auto u = node(_root, _xn, _L - 1, pk);
		update(bsha, u);
	}
	return get(bsha);
}

h256 Dagger::eval(h256 const& _root, u256 const& _nonce)
{
	h256 extranonce = _nonce >> 26;				// with xn = floor(n / 2^26) -> assuming this is with xn = floor(N / 2^26)
	CryptoPP::SHA3_256 bsha;
	for (uint_fast32_t k = 0; k < 4; ++k)
	{
		//sha256(D || xn || i || k)		-> sha256(D || xn || k)	- there's no 'i' here!
		CryptoPP::SHA3_256 sha;
		update(sha, _root);
		update(sha, extranonce);
		update(sha, _nonce);
		update(sha, (u256)k);
		uint_fast32_t pk = (uint_fast32_t)(u256)get(sha) & 0x1ffffff;	// mod 8^8 * 2  [ == mod 2^25 ?! ] [ == & ((1 << 25) - 1) ] [ == & 0x1ffffff ]
		auto u = node(_root, extranonce, 9, pk);
		update(bsha, u);
	}
	return get(bsha);
}

#endif
}


#include "Stdafx.h"

#pragma unmanaged
#include <Eigen/Cholesky>
#include <Eigen/LU>
#pragma managed

#include "State.h"

using namespace g2o;
using namespace Fugro::G2O;

Vertex::Vertex(int dimension, BaseState^ state) :
_state(state),
_hessian(nullptr, dimension, dimension),
_b(dimension)
{
  _dimension = dimension;
}

const double& Vertex::hessian(int i, int j) const
{
  assert(i < _dimension && j < _dimension);
  return _hessian(i, j);
}

double& Vertex::hessian(int i, int j)
{
  assert(i < _dimension && j < _dimension);
  return _hessian(i, j);
}

double Vertex::hessianDeterminant() const
{
  return _hessian.determinant();
}

double* Vertex::hessianData()
{
  return const_cast<double*>(_hessian.data());
}

void Vertex::mapHessianMemory(double* d)
{
  new (&_hessian) HessianBlockType(d, _dimension, _dimension);
}

int Vertex::copyB(double* b_) const
{
  memcpy(b_, _b.data(), _dimension * sizeof(double));

  return _dimension;
}

const double& Vertex::b(int i) const
{
  assert(i < _dimension);
  return _b(i);
}

double& Vertex::b(int i)
{
  assert(i < _dimension);
  return _b(i);
}

double* Vertex::bData()
{
  return _b.data();
}

void Vertex::clearQuadraticForm()
{
  _b.setZero();
}

double Vertex::solveDirect(double lambda)
{
  MatrixXd tempA = _hessian + MatrixXd::Identity(_dimension, _dimension) * lambda;
  double det = tempA.determinant();

  if (g2o_isnan(det) || det < std::numeric_limits<double>::epsilon())
  {
    return det;
  }

  VectorXd dx = tempA.llt().solve(_b);
  oplus(&dx[0]);

  return det;
}

VectorXd& Vertex::b()
{
  return _b;
}

const VectorXd& Vertex::b() const
{
  return _b;
}

Vertex::HessianBlockType& Vertex::A()
{
  return _hessian;
}

const Vertex::HessianBlockType& Vertex::A() const
{
  return _hessian;
}

void Vertex::push()
{
  _state->Push();
}

void Vertex::pop()
{
  _state->Pop();
  updateCache();
}

void Vertex::discardTop()
{
  _state->DiscardTop();
}

int Vertex::stackSize() const
{
  return _state->GetStackSize();
}

void Vertex::oplusImpl(const double* update)
{
  _state->Oplus(update);
}

void Vertex::addOplusDelta(int index)
{
  _state->AddOplusDelta(index);
  updateCache();
}

void Vertex::subtractOplusDelta(int index)
{
  _state->SubtractOplusDelta(index);
  updateCache();
}

void Vertex::setToOriginImpl()
{
}

bool Vertex::read(std::istream& is)
{
  return true;
}

bool Vertex::write(std::ostream& os) const
{
  return os.good();
}


#include <cuda_runtime.h>
#include <helper_cuda.h>
#include <helper_functions.h>
#include <mpi.h>
#include <unistd.h>

#include <armadillo>
#include <cassert>
#include <chrono>
#include <iostream>

#include "gpu_func.h"
#include "neural_network.h"
#include "utils/common.h"
#include "utils/mnist.h"
#include "utils/tests.h"
#include "utils/types.h"

#define FILE_TRAIN_IMAGES "/data/train-images-idx3-ubyte"
#define FILE_TRAIN_LABELS "/data/train-labels-idx1-ubyte"
#define FILE_TEST_IMAGES "/data/t10k-images-idx3-ubyte"
#define FILE_TEST_OUTPUT "Outputs/Pred_testset.txt"
#define NUM_TRAIN 60000
#define IMAGE_SIZE 784  // 28 x 28
#define NUM_CLASSES 10
#define NUM_TEST 10000

#define MPI_SAFE_CALL(call)                                                  \
  do {                                                                       \
    int err = call;                                                          \
    if (err != MPI_SUCCESS) {                                                \
      fprintf(stderr, "MPI error %d in file '%s' at line %i", err, __FILE__, \
              __LINE__);                                                     \
      exit(1);                                                               \
    }                                                                        \
  } while (0)

int main(int argc, char* argv[]) {
  // Initialize MPI
  int num_procs = 0, rank = 0;
  MPI_Init(&argc, &argv);
  MPI_SAFE_CALL(MPI_Comm_size(MPI_COMM_WORLD, &num_procs));
  MPI_SAFE_CALL(MPI_Comm_rank(MPI_COMM_WORLD, &rank));

  // Assign a GPU device to each MPI proc
  int nDevices;
  cudaGetDeviceCount(&nDevices);

  if (nDevices < num_procs) {
    std::cerr << "Please allocate at least as many GPUs as\
		 the number of MPI procs."
              << std::endl;
  }

  checkCudaErrors(cudaSetDevice(rank));

  if (rank == 0) {
    std::cout << "Number of MPI processes = " << num_procs << std::endl;
    std::cout << "Number of CUDA devices = " << nDevices << std::endl;
  }

  // Read in command line arguments
  std::vector<int> H(3);
  real reg = 1e-4;
  real learning_rate = 0.001;
  int num_epochs = 20;
  int batch_size = 800;
  int num_neuron = 1000;
  int run_seq = 0;
  int debug = 0;
  int grade = 0;
  int print_every = 0;

  int option = 0;

  while ((option = getopt(argc, argv, "n:r:l:e:b:g:p:sd")) != -1) {
    switch (option) {
      case 'n':
        num_neuron = atoi(optarg);
        break;

      case 'r':
        reg = atof(optarg);
        break;

      case 'l':
        learning_rate = atof(optarg);
        break;

      case 'e':
        num_epochs = atoi(optarg);
        break;

      case 'b':
        batch_size = atoi(optarg);
        break;

      case 'g':
        grade = atoi(optarg);
        break;

      case 'p':
        print_every = atoi(optarg);
        break;

      case 's':
        run_seq = 1;
        break;

      case 'd':
        debug = 1;
        break;
    }
  }

  /* This option is going to be used to test correctness.
     DO NOT change the following parameters */
  switch (grade) {
    case 0:  // No grading
      break;

    case 1:  // Low lr, high iters
      reg = 1e-4;
      learning_rate = 0.0005;
      num_epochs = 40;
      batch_size = 800;
      num_neuron = 100;
      run_seq = 1;
      debug = 1;
      print_every = 0;
      break;

    case 2:  // Medium lr, medium iters
      reg = 1e-4;
      learning_rate = 0.001;
      num_epochs = 10;
      batch_size = 800;
      num_neuron = 100;
      run_seq = 1;
      debug = 1;
      print_every = 0;
      break;

    case 3:  // High lr, very few iters
      reg = 1e-4;
      learning_rate = 0.002;
      num_epochs = 1;
      batch_size = 800;
      num_neuron = 100;
      run_seq = 1;
      debug = 1;
      print_every = 1;
      break;

    case 4:
      break;
  }

  if (grade == 4) {
    if (rank == 0) {
      BenchmarkGEMM();
    }

    MPI_Finalize();
    return 0;
  }

  H[0] = IMAGE_SIZE;
  H[1] = num_neuron;
  H[2] = NUM_CLASSES;

  arma::Mat<real> x_train, y_train, label_train, x_dev, y_dev, label_dev,
      x_test;
  NeuralNetwork nn(H);

  if (rank == 0) {
    std::cout << "num_neuron=" << num_neuron << ", reg=" << reg
              << ", learning_rate=" << learning_rate
              << ", num_epochs=" << num_epochs << ", batch_size=" << batch_size
              << std::endl;
    // Read MNIST images into Armadillo mat vector
    arma::Mat<real> x(IMAGE_SIZE, NUM_TRAIN);
    // label contains the prediction for each
    arma::Row<real> label = arma::zeros<arma::Row<real>>(NUM_TRAIN);
    // y is the matrix of one-hot label vectors where only y[c] = 1,
    // where c is the right class.
    arma::Mat<real> y = arma::zeros<arma::Mat<real>>(NUM_CLASSES, NUM_TRAIN);

    std::cout << "Loading training data..." << std::endl;
    read_mnist(FILE_TRAIN_IMAGES, x);
    read_mnist_label(FILE_TRAIN_LABELS, label);
    label_to_y(label, NUM_CLASSES, y);

    /* Print stats of training data */
    std::cout << "Training data stats..." << std::endl;
    std::cout << "Size of x_train, N =  " << x.n_cols << std::endl;
    std::cout << "Size of label_train = " << label.size() << std::endl;

    assert(x.n_cols == NUM_TRAIN && x.n_rows == IMAGE_SIZE);
    assert(label.size() == NUM_TRAIN);

    /* Split into train set and dev set, you should use train set to train your
       neural network and dev set to evaluate its precision */
    int dev_size = (int)(0.1 * NUM_TRAIN);
    x_train = x.cols(0, NUM_TRAIN - dev_size - 1);
    y_train = y.cols(0, NUM_TRAIN - dev_size - 1);
    label_train = label.cols(0, NUM_TRAIN - dev_size - 1);

    x_dev = x.cols(NUM_TRAIN - dev_size, NUM_TRAIN - 1);
    y_dev = y.cols(NUM_TRAIN - dev_size, NUM_TRAIN - 1);
    label_dev = label.cols(NUM_TRAIN - dev_size, NUM_TRAIN - 1);

    /* Load the test data, we will compare the prediction of your trained neural
       network with test data label to evaluate its precision */
    x_test = arma::zeros<arma::Mat<real>>(IMAGE_SIZE, NUM_TEST);
    read_mnist(FILE_TEST_IMAGES, x_test);
  }

  /* Run the sequential code if the serial flag is set */
  NeuralNetwork seq_nn(H);
  using namespace std::chrono;
  if ((rank == 0) && (run_seq)) {
    std::cout << "Start Sequential Training" << std::endl;

    high_resolution_clock::time_point t1 = high_resolution_clock::now();
    train(seq_nn, x_train, y_train, learning_rate, reg, num_epochs, batch_size,
          false, print_every, debug);
    high_resolution_clock::time_point t2 = high_resolution_clock::now();
    duration<double> time_span = duration_cast<duration<double>>(t2 - t1);

    std::cout << "Time for Sequential Training: " << time_span.count()
              << " seconds" << std::endl;

    arma::Row<real> label_dev_pred;
    predict(seq_nn, x_dev, label_dev_pred);
    real prec = precision(label_dev_pred, label_dev);
    std::cout << "Precision on validation set for sequential training = "
              << prec << std::endl;
  }

  /* Train the Neural Network in Parallel*/
  if (rank == 0) {
    std::cout << std::endl << "Start Parallel Training" << std::endl;
  }

  high_resolution_clock::time_point t1 = high_resolution_clock::now();

  /* ---- Parallel Training ---- */
  parallel_train(nn, x_train, y_train, learning_rate, reg, num_epochs,
                 batch_size, false, print_every, debug);

  high_resolution_clock::time_point t2 = high_resolution_clock::now();
  duration<double> time_span = duration_cast<duration<double>>(t2 - t1);

  if (rank == 0) {
    std::cout << "Time for Parallel Training: " << time_span.count()
              << " seconds" << std::endl;
  }

  /* Note: Make sure after parallel training, rank 0's neural network is up to
   * date */

  /* Do predictions for the parallel NN */
  if (rank == 0) {
    arma::Row<real> label_dev_pred;
    predict(nn, x_dev, label_dev_pred);
    real prec = precision(label_dev_pred, label_dev);
    std::cout << "Precision on validation set for parallel training = " << prec
              << std::endl;
    arma::Row<real> label_test_pred;
    predict(nn, x_test, label_test_pred);
    save_label(FILE_TEST_OUTPUT, label_test_pred);
  }

  /* If grading mode is on, compare CPU and GPU results and check for
   * correctness */
  if (grade && rank == 0) {
    std::cout << std::endl
              << "Grading mode on. Now checking for correctness..."
              << std::endl;
    checkNNErrors(seq_nn, nn, "Outputs/NNErrors.txt");
  }

  MPI_Finalize();
  return 0;
}


#include <tagloc/tracking.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <Eigen/Dense>
#include <math.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#define _USE_MATH_DEFINES
using std::cout;
using std::endl;
using Eigen::MatrixXd;

#define USE_MATH_DEFINES

int main(int argc, char** argv){
	if (argc==1){
		printf(" <prior: x y> <prior cov: (2x2)> [measurement x y z r] \n");
		printf(" need 2 or more measurements! \n");
	}
	FILE* ofile = fopen("tag_loc","w");
	if (ofile==NULL)
		perror("File Open");

	std::vector<double> px,py,z,R;
	double t[2],c[4];
	double tp[2],cp[4];

	int i=1;
	{
		char* cptr = argv[i];
		double fval = strtod(argv[i],&cptr);
		while (fval == 0.0 && cptr==argv[i]){
			printf("Ignoring: %s\n",argv[i]);
			i++;
			fval = strtod(argv[i],&cptr);
		}
	}
	tp[0] = atof(argv[i++]);
	tp[1] = atof(argv[i++]);
	cp[0] = atof(argv[i++]);
	cp[1] = atof(argv[i++]);
	cp[2] = atof(argv[i++]);
	cp[3] = atof(argv[i++]);

	for(;i<argc-1;){
		char* cptr = argv[i];
		double fval = strtod(argv[i],&cptr);
		while (fval == 0.0 && cptr==argv[i]){
			printf("Ignoring: %s\n",argv[i]);
			i++;
			fval = strtod(argv[i],&cptr);
		}
		px.push_back(fval);
		py.push_back(atof(argv[i+1]));
		z.push_back(atof(argv[i+2]));
		R.push_back(atof(argv[i+3]));
		i = i+4;
		//R.push_back(pow(M_PI/6,2));
	}

	int N = (int) R.size();

	printf("prior: %0.3f, %0.3f [%0.3f, %0.3f, %0.3f, %0.3f]\n",tp[0],tp[1],cp[0],cp[1],cp[2],cp[3]);

	//fprintf(ofile,"%d %0.3f %0.3f %0.3f ",0,0,0,0); //first measurement loc
	//fprintf(ofile,"%0.3f %0.3f ", tp[0], tp[1]);
	//fprintf(ofile,"%0.3f %0.3f %0.3f %0.3f %0.3f\n",
			//cp[0],cp[1],cp[2],cp[3]);

	for (i=0;i<N;i++){
		printf(" Z %d: (%f,%f) = %f +/- %f \n",i,px[i],py[i],z[i],sqrt(R[i]));
	}

	RSN::line_intersection(N, px, py, z, t[0], t[1]);
	printf("LI: %0.3f, %0.3f\n",t[0],t[1]);

	printf("==========\n");
	for (i=1;i<N;i++){
		printf(" Up to %d",i);
		RSN::line_intersection(i+1, px, py, z, t[0], t[1]);
		RSN::BOT::IWLS_2D(i+1, px, py, z, R, t, c, true);
		printf(" gives: %0.3f, %0.3f [%0.3f, %0.3f, %0.3f, %0.3f]\n",t[0],t[1],c[0],c[1],c[2],c[3]);

		//fprintf(ofile,"%d %0.3f %0.3f %0.3f ",i+1,0,0,0); //first measurement loc
		//fprintf(ofile,"%0.3f %0.3f ", tp[0], tp[1]);
		//fprintf(ofile,"%0.3f %0.3f %0.3f %0.3f %0.3f\n",
				//cp[0],cp[1],cp[2],cp[3]);
	}

	RSN::line_intersection(N, px, py, z, t[0], t[1]);
	RSN::BOT::IWLS_2D(N, px, py, z, R, t, c, true);

	MatrixXd x0(2,1);
	MatrixXd x1(2,1);
	MatrixXd p0(2,2);
	MatrixXd p1(2,2);
	for (int i=0;i<2;i++){
		x0(i) = tp[i];
		x1(i) = t[i];
		for (int j=0;j<2;j++){
			p0(i,j) = cp[i*2+j];
			p1(i,j) = c[i*2+j];
		}
	}

	MatrixXd S = (p0+p1);
	MatrixXd y = x1 - x0;
	MatrixXd tf = x0 + p0*S.inverse()*y;
	MatrixXd Pf = (MatrixXd::Identity(2,2) - p0*S.inverse())*p0;
	printf("final+prior: %0.3f, %0.3f [%0.3f, %0.3f, %0.3f, %0.3f]\n",tf(0),tf(1),Pf(0,0),Pf(0,1),Pf(1,0),Pf(1,1));

	fclose(ofile);
	return 0;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file   testSubgraphSolver.cpp
 *  @brief  Unit tests for SubgraphSolver
 *  @author Yong-Dian Jian
 **/

#include <CppUnitLite/TestHarness.h>

#if 0

#include <tests/smallExample.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/iterative.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/numericalDerivative.h>

#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp>
using namespace boost::assign;

using namespace std;
using namespace gtsam;
using namespace example;

/* ************************************************************************* */
/** unnormalized error */
static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
  double total_error = 0.;
  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg)
    total_error += factor->error(x);
  return total_error;
}

/* ************************************************************************* */
TEST( SubgraphSolver, constructor1 )
{
  // Build a planar graph
  GaussianFactorGraph Ab;
  VectorValues xtrue;
  size_t N = 3;
  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b

  // The first constructor just takes a factor graph (and parameters)
  // and it will split the graph into A1 and A2, where A1 is a spanning tree
  SubgraphSolverParameters parameters;
  SubgraphSolver solver(Ab, parameters);
  VectorValues optimized = solver.optimize(); // does PCG optimization
  DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}

/* ************************************************************************* */
TEST( SubgraphSolver, constructor2 )
{
  // Build a planar graph
  GaussianFactorGraph Ab;
  VectorValues xtrue;
  size_t N = 3;
  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b

  // Get the spanning tree and corresponding ordering
  GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
  boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);

  // The second constructor takes two factor graphs,
  // so the caller can specify the preconditioner (Ab1) and the constraints that are left out (Ab2)
  SubgraphSolverParameters parameters;
  SubgraphSolver solver(Ab1_, Ab2_, parameters);
  VectorValues optimized = solver.optimize();
  DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}

/* ************************************************************************* */
TEST( SubgraphSolver, constructor3 )
{
  // Build a planar graph
  GaussianFactorGraph Ab;
  VectorValues xtrue;
  size_t N = 3;
  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b

  // Get the spanning tree and corresponding ordering
  GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
  boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);

  // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT
  GaussianBayesNet::shared_ptr Rc1 = //
      EliminationTree<GaussianFactor>::Create(Ab1_)->eliminate(&EliminateQR);

  // The third constructor allows the caller to pass an already solved preconditioner Rc1_
  // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before
  SubgraphSolverParameters parameters;
  SubgraphSolver solver(Rc1, Ab2_, parameters);
  VectorValues optimized = solver.optimize();
  DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}

#endif

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


#include <engine/Neighbours.hpp>
#include <engine/Vectormath.hpp>
#include <engine/Manifoldmath.hpp>
#include <utility/Logging.hpp>
#include <utility/Exception.hpp>

#include <Eigen/Dense>

#include <numeric>
#include <iostream>
#include <cstdio>
#include <cmath>
#include <algorithm>

using namespace Utility;

namespace Engine
{
	namespace Neighbours
	{
		std::vector<scalar> Get_Shell_Radius(const Data::Geometry & geometry, const int n_shells)
		{
			auto shell_radius = std::vector<scalar>(n_shells);

			Vector3 a = geometry.bravais_vectors[0];
			Vector3 b = geometry.bravais_vectors[1];
			Vector3 c = geometry.bravais_vectors[2];

			scalar current_radius=0, dx, min_distance=0;
			int i=0, j=0, k=0;
			int ii, jj, kk;

			// The 15 is a value that is big enough by experience to
			// produce enough needed shells, but is small enough to run sufficiently fast
			int imax = 15, jmax = 15, kmax = 15;
			Vector3 x0={0,0,0}, x1={0,0,0};

			// Abort condidions for all 3 vectors
			if (a.norm() == 0.0) imax = 0;
			if (b.norm() == 0.0) jmax = 0;
			if (c.norm() == 0.0) kmax = 0;

			for (int n = 0; n < n_shells; ++n)
			{
				current_radius = min_distance;
				min_distance = 1e10;
				for (int iatom = 0; iatom < geometry.n_cell_atoms; ++iatom)
				{
				    x0 = geometry.cell_atoms[iatom][0] * a + geometry.cell_atoms[iatom][1] * b + geometry.cell_atoms[iatom][2] * c;
					for (ii = imax; ii >= -imax; --ii)
					{
						for (jj = jmax; jj >= -jmax; --jj)
						{
							for (kk = kmax; kk >= -kmax; --kk)
							{
								for (int jatom = 0; jatom < geometry.n_cell_atoms; ++jatom)
								{
									if ( !( iatom==jatom && ii==0 && jj==0 && kk==0 ) )
									{
									    x1 = geometry.cell_atoms[jatom][0] * a + geometry.cell_atoms[jatom][1] * b + geometry.cell_atoms[jatom][2] * c + ii*a + jj*b + kk*c;
										dx = (x0-x1).norm();

										if (dx - current_radius > 1e-6 && dx < min_distance)
										{
											min_distance = dx;
											shell_radius[n] = dx;
										}
									}
								}//endfor jatom
							}//endfor kk
						}//endfor jj
					}//endfor ii
				}//endfor iatom
			}

			return shell_radius;
		}

		pairfield Get_Pairs_in_Shells(const Data::Geometry & geometry, int nShells)
		{
			auto pairs = pairfield(0);

			auto shell_radius = Get_Shell_Radius(geometry, nShells);

			Vector3 a = geometry.bravais_vectors[0];
			Vector3 b = geometry.bravais_vectors[1];
			Vector3 c = geometry.bravais_vectors[2];

			// The nShells + 10 is a value that is big enough by experience to
			// produce enough needed shells, but is small enough to run sufficiently fast
			int tMax = nShells + 10;
			int imax = tMax, jmax = tMax, kmax = tMax;
			int i,j,k;
			scalar dx, delta, radius;
			Vector3 x0={0,0,0}, x1={0,0,0};

			// Abort condidions for all 3 vectors
			if (a.norm() == 0.0) imax = 0;
			if (b.norm() == 0.0) jmax = 0;
			if (c.norm() == 0.0) kmax = 0;

			for (int iatom = 0; iatom < geometry.n_cell_atoms; ++iatom)
			{
				x0 = geometry.cell_atoms[iatom];
				for (int ishell = 0; ishell < nShells; ++ishell)
				{
					radius = shell_radius[ishell];
					for (i = imax; i >= -imax; --i)
					{
						for (j = jmax; j >= -jmax; --j)
						{
							for (k = kmax; k >= -kmax; --k)
							{
								for (int jatom = 0; jatom < geometry.n_cell_atoms; ++jatom)
								{
									x1 = geometry.cell_atoms[jatom] + i*a + j*b + k*c;
									dx = (x0-x1).norm();
									delta = std::abs(dx - radius);
									if (delta < 1e-6)
									{
										pairs.push_back( {iatom, jatom, {i, j, k} } );
									}
								}//endfor jatom
							}//endfor k
						}//endfor j
					}//endfor i
				}//endfor ishell
			}//endfor iatom

			return pairs;
		}

		neighbourfield Get_Neighbours_in_Shells(const Data::Geometry & geometry, int nShells)
		{
			auto neighbours = neighbourfield(0);

			auto shell_radius = Get_Shell_Radius(geometry, nShells);

			Vector3 a = geometry.bravais_vectors[0];
			Vector3 b = geometry.bravais_vectors[1];
			Vector3 c = geometry.bravais_vectors[2];

			// The nShells + 10 is a value that is big enough by experience to
			// produce enough needed shells, but is small enough to run sufficiently fast
			int tMax = nShells + 10;
			int imax = std::min(tMax, geometry.n_cells[0]-1), jmax = std::min(tMax, geometry.n_cells[1]-1), kmax = std::min(tMax, geometry.n_cells[2]-1);
			int i,j,k;
			scalar dx, delta, radius;
			Vector3 x0={0,0,0}, x1={0,0,0};

			// Abort condidions for all 3 vectors
			if (a.norm() == 0.0) imax = 0;
			if (b.norm() == 0.0) jmax = 0;
			if (c.norm() == 0.0) kmax = 0;

			for (int iatom = 0; iatom < geometry.n_cell_atoms; ++iatom)
			{
				x0 = geometry.cell_atoms[iatom][0] * a + geometry.cell_atoms[iatom][1] * b + geometry.cell_atoms[iatom][2] * c;
				for (int ishell = 0; ishell < nShells; ++ishell)
				{
					radius = shell_radius[ishell];
					for (i = imax; i >= -imax; --i)
					{
						for (j = jmax; j >= -jmax; --j)
						{
							for (k = kmax; k >= -kmax; --k)
							{
								for (int jatom = 0; jatom < geometry.n_cell_atoms; ++jatom)
								{
									x1 = geometry.cell_atoms[jatom][0] * a + geometry.cell_atoms[jatom][1] * b + geometry.cell_atoms[jatom][2] * c + i*a + j*b + k*c;
									dx = (x0-x1).norm();
									delta = std::abs(dx - radius);
									if (delta < 1e-6)
									{
										Neighbour neigh;
										neigh.i = iatom;
										neigh.j = jatom;
										neigh.translations[0] = i;
										neigh.translations[1] = j;
										neigh.translations[2] = k;
										neigh.idx_shell = ishell;
										neighbours.push_back( neigh );
									}
								}//endfor jatom
							}//endfor k
						}//endfor j
					}//endfor i
				}//endfor ishell
			}//endfor iatom

			return neighbours;
		}

		pairfield Get_Pairs_in_Radius(const Data::Geometry & geometry, scalar radius)
		{
			auto pairs = pairfield(0);

			if (radius > 1e-6)
			{
				Vector3 a = geometry.bravais_vectors[0];
				Vector3 b = geometry.bravais_vectors[1];
				Vector3 c = geometry.bravais_vectors[2];

				Vector3 bounds_diff = geometry.bounds_max - geometry.bounds_min;
				Vector3 ratio = {
					bounds_diff[0]/std::max(1, geometry.n_cells[0]),
					bounds_diff[1]/std::max(1, geometry.n_cells[1]),
					bounds_diff[2]/std::max(1, geometry.n_cells[2]) };

				// This should give enough translations to contain all DDI pairs
				int imax = 0, jmax = 0, kmax = 0;
				if ( bounds_diff[0] > 0 )
					imax = std::min(geometry.n_cells[0], (int)(1.1 * radius * geometry.n_cells[0] / bounds_diff[0]));
				if ( bounds_diff[1] > 0 )
					jmax = std::min(geometry.n_cells[1], (int)(1.1 * radius * geometry.n_cells[1] / bounds_diff[1]));
				if ( bounds_diff[2] > 0 )
					kmax = std::min(geometry.n_cells[2], (int)(1.1 * radius * geometry.n_cells[2] / bounds_diff[2]));

				int i,j,k;
				scalar dx;
				Vector3 x0={0,0,0}, x1={0,0,0};

				// Abort condidions for all 3 vectors
				if (a.norm() == 0.0) imax = 0;
				if (b.norm() == 0.0) jmax = 0;
				if (c.norm() == 0.0) kmax = 0;

				for (int iatom = 0; iatom < geometry.n_cell_atoms; ++iatom)
				{
					x0 = geometry.cell_atoms[iatom];
					for (i = imax; i >= -imax; --i)
					{
						for (j = jmax; j >= -jmax; --j)
						{
							for (k = kmax; k >= -kmax; --k)
							{
								for (int jatom = 0; jatom < geometry.n_cell_atoms; ++jatom)
								{
									x1 = geometry.cell_atoms[jatom] + i*a + j*b + k*c;
									dx = (x0-x1).norm();
									if (dx < radius)
									{
										pairs.push_back( {iatom, jatom, {i, j, k} } );
									}
								}//endfor jatom
							}//endfor k
						}//endfor j
					}//endfor i
				}//endfor iatom
			}

			return pairs;
		}

		neighbourfield Get_Neighbours_in_Radius(const Data::Geometry & geometry, scalar radius)
		{
			auto neighbours = neighbourfield(0);

			if (radius > 1e-6)
			{
				Vector3 a = geometry.bravais_vectors[0];
				Vector3 b = geometry.bravais_vectors[1];
				Vector3 c = geometry.bravais_vectors[2];

				Vector3 bounds_diff = geometry.bounds_max - geometry.bounds_min;
				Vector3 ratio = {
					bounds_diff[0]/std::max(1, geometry.n_cells[0]),
					bounds_diff[1]/std::max(1, geometry.n_cells[1]),
					bounds_diff[2]/std::max(1, geometry.n_cells[2]) };

				// This should give enough translations to contain all DDI pairs
				int imax = 0, jmax = 0, kmax = 0;
				if ( bounds_diff[0] > 0 )
					imax = std::min(geometry.n_cells[0], (int)(1.1 * radius * geometry.n_cells[0] / bounds_diff[0]));
				if ( bounds_diff[1] > 0 )
					jmax = std::min(geometry.n_cells[1], (int)(1.1 * radius * geometry.n_cells[1] / bounds_diff[1]));
				if ( bounds_diff[2] > 0 )
					kmax = std::min(geometry.n_cells[2], (int)(1.1 * radius * geometry.n_cells[2] / bounds_diff[2]));

				int i, j, k;
				scalar dx;
				Vector3 x0 = { 0,0,0 }, x1 = { 0,0,0 };

				// Abort condidions for all 3 vectors
				if (a.norm() == 0.0) imax = 0;
				if (b.norm() == 0.0) jmax = 0;
				if (c.norm() == 0.0) kmax = 0;

				for (int iatom = 0; iatom < geometry.n_cell_atoms; ++iatom)
				{
					x0 = geometry.cell_atoms[iatom];
					for (i = imax; i >= -imax; --i)
					{
						for (j = jmax; j >= -jmax; --j)
						{
							for (k = kmax; k >= -kmax; --k)
							{
								for (int jatom = 0; jatom < geometry.n_cell_atoms; ++jatom)
								{
									x1 = geometry.cell_atoms[jatom] + i*a + j*b + k*c;
									dx = (x0 - x1).norm();
									if (dx < radius)
									{
										Neighbour neigh;
										neigh.i = iatom;
										neigh.j = jatom;
										neigh.translations[0] = i;
										neigh.translations[1] = j;
										neigh.translations[2] = k;
										neigh.idx_shell = 0;
										neighbours.push_back( neigh );
									}
								}//endfor jatom
							}//endfor k
						}//endfor j
					}//endfor i
				}//endfor iatom
			}

			return neighbours;
		}

		Vector3 DMI_Normal_from_Pair(const Data::Geometry & geometry, const Pair & pair, int chirality)
		{
			Vector3 ta = geometry.bravais_vectors[0];
			Vector3 tb = geometry.bravais_vectors[1];
			Vector3 tc = geometry.bravais_vectors[2];

			int da = pair.translations[0];
			int db = pair.translations[1];
			int dc = pair.translations[2];

			Vector3 ipos = geometry.cell_atoms[pair.i];
			Vector3 jpos = geometry.cell_atoms[pair.j] + da*ta + db*tb + dc*tc;

			if (chirality == 1)
			{
				// Bloch chirality
				return (jpos - ipos).normalized();
			}
			else if (chirality == -1)
			{
				// Inverse Bloch chirality
				return (ipos - jpos).normalized();
			}
			else if (chirality == 2)
			{
				// Neel chirality (surface)
				return (jpos - ipos).normalized().cross(Vector3{0,0,1});
			}
			else if (chirality == -2)
			{
				// Inverse Neel chirality (surface)
				return Vector3{0,0,1}.cross((jpos - ipos).normalized());
			}
			else
			{
				return Vector3{ 0,0,0 };
			}
		}

		void DDI_from_Pair(const Data::Geometry & geometry, const Pair & pair, scalar & magnitude, Vector3 & normal)
		{
			Vector3 ta = geometry.bravais_vectors[0];
			Vector3 tb = geometry.bravais_vectors[1];
			Vector3 tc = geometry.bravais_vectors[2];

			int da = pair.translations[0];
			int db = pair.translations[1];
			int dc = pair.translations[2];

			Vector3 ipos = geometry.cell_atoms[pair.i];
			Vector3 jpos = geometry.cell_atoms[pair.j] + da*ta + db*tb + dc*tc;

			// Calculate positions and difference vector
			Vector3 vector_ij = jpos - ipos;

			// Length of difference vector
			magnitude = vector_ij.norm();
			normal = vector_ij.normalized();
		}

	}// end Namespace Neighbours
}// end Namespace Engine


////////////////////////////////////////////////////////////////////////////////
// IncompressibleBalloonEnergy.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Implementation of the incompressible neo-Hookean-based strain energy
//  density used in Skouras 2014: Designing Inflatable Structures (before
//  homogenizing away the wrinkles using a relaxed energy density).
//
//  This energy is implemented as a function of the right Green-Green
//  deformation tensor.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  04/04/2019 18:19:10
////////////////////////////////////////////////////////////////////////////////
#ifndef INCOMPRESSIBLEBALLOONENERGY_HH
#define INCOMPRESSIBLEBALLOONENERGY_HH

#include <Eigen/Dense>
#include <array>
#include <MeshFEM/EnergyDensities/Tensor.hh>

template<typename Real>
struct IncompressibleBalloonEnergy {
    using V2d  = Eigen::Matrix<Real, 2, 1>;
    using M2d  = Eigen::Matrix<Real, 2, 2>;

    IncompressibleBalloonEnergy() { }

    template<typename Derived>
    IncompressibleBalloonEnergy(const Eigen::MatrixBase<Derived> &C) { setMatrix(C); }

    template<typename Derived>
    void setMatrix(const Eigen::MatrixBase<Derived> &C) {
        static_assert((Derived::RowsAtCompileTime == 2) && (Derived::ColsAtCompileTime == 2), "Only 2x2 supported for now");

        Real a = C(0, 0),
             b = C(0, 1),
             c = C(1, 1);
        if (std::abs(b - C(1, 0)) > 1e-15) throw std::runtime_error("Asymmetric matrix");

        m_C = C;
        m_trace_C = C.trace();
        m_det_C = a * c - b * b;
        m_grad_det_C <<  c, -b,
                        -b,  a;
    }

    Real energy() const {
        return stiffness * (m_trace_C + 1.0 / m_det_C - 3.0);
    }

    Real denergy(const M2d &dC) const {
        return stiffness * (dC.trace() - (1.0 / (m_det_C * m_det_C)) * doubleContract(m_grad_det_C, dC));
    }

    M2d denergy() const {
        return stiffness * (M2d::Identity() - (1.0 / (m_det_C * m_det_C)) * m_grad_det_C);
    }

    Real d2energy(const M2d &dC_a, const M2d &dC_b) const {
        return stiffness * ((2.0 / (m_det_C * m_det_C * m_det_C)) * doubleContract(m_grad_det_C, dC_a) * doubleContract(m_grad_det_C, dC_b)
                          - (1.0 / (m_det_C * m_det_C)) * (dC_a(1, 1) * dC_b(0, 0) + dC_a(0, 0) * dC_b(1, 1) - 2 * dC_a(0, 1) * dC_b(0, 1)));
    }

    M2d delta_denergy(const M2d &dC) const {
        M2d adj_dC;
        adj_dC << dC(1, 1), -dC(0, 1),
                 -dC(1, 0),  dC(0, 0);
        return (((2.0 * stiffness / (m_det_C * m_det_C * m_det_C)) * doubleContract(m_grad_det_C, dC)) * m_grad_det_C
                     - (stiffness / (m_det_C * m_det_C))                                               * adj_dC);
    }

    // Second derivatives evaluated at the reference configuration
    M2d delta_denergy_undeformed(const M2d &dC) const {
        return stiffness * (dC.trace() * M2d::Identity() + dC);
    }

    Real stiffness = 1.0;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    Real m_trace_C, m_det_C;
    M2d m_grad_det_C;
    M2d m_C;
};

#endif /* end of include guard: INCOMPRESSIBLEBALLOONENERGY_HH */


/**
 * \file libs/numeric/ublasx/min.cpp
 *
 * \brief Test suite for the \c min operation.
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#include <boost/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublasx/operation/min.hpp>
#include <boost/numeric/ublasx/tags.hpp>
#include <cstddef>
#include <functional>
#include <iostream>
#include "libs/numeric/ublasx/test/utils.hpp"

static const double tol = 1.0e-5;

namespace ublas = ::boost::numeric::ublas;
namespace ublasx = ::boost::numeric::ublasx;

BOOST_UBLASX_TEST_DEF( real_vector )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real Vector" );

	typedef double value_type;
	typedef ublas::vector<value_type> vector_type;

	vector_type v(5);

	v(0) = 0.0;
	v(1) = 0.108929;
	v(2) = 0.0;
	v(3) = 0.0;
	v(4) = 1.023787;

	value_type expect(0);
	value_type res(0);
	ublas::vector<value_type> vexpect(0);
	ublas::vector<value_type> vres(0);

	// min(v)
	expect = value_type(0);
	res = ublasx::min(v);
	BOOST_UBLASX_DEBUG_TRACE( "min(" << v << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

	// min<1>(v)
	vexpect = ublas::vector<value_type>(1, 0);
	vres = ublasx::min<1>(v);
	BOOST_UBLASX_DEBUG_TRACE( "min<1>(" << v << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, 1, tol );
}

BOOST_UBLASX_TEST_DEF( complex_vector )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex Vector" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::vector<value_type> vector_type;

	vector_type v(5);

	v(0) = value_type( 0.000000,-0.54000);
	v(1) = value_type(-0.108929, 2.43000);
	v(2) = value_type( 0.000000, 1.00030);
	v(3) = value_type(-0.050000, 1.00030);
	v(4) = value_type( 1.023787,-4.24959);;

	value_type expect;
	value_type res;
	ublas::vector<value_type> vexpect;
	ublas::vector<value_type> vres;

	// min(v)
	expect = value_type(0.00000,-0.54000);
	res = ublasx::min(v);
	BOOST_UBLASX_DEBUG_TRACE( "min(" << v << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

	// min<1>(v)
	vexpect = ublas::vector<value_type>(1, value_type(0.00000,-0.54000));
	vres = ublasx::min<1>(v);
	BOOST_UBLASX_DEBUG_TRACE( "min<1>(" << v << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, 1, tol );
}

BOOST_UBLASX_TEST_DEF( vector_container )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Vector Container" );

	typedef double value_type;
	typedef ublas::vector<value_type> vector_type;
	typedef ublas::zero_vector<value_type> zero_vector_type;

	vector_type v(5);

	v(0) = 0.0;
	v(1) = 0.108929;
	v(2) = 0.0;
	v(3) = 0.0;
	v(4) = 1.023787;

	zero_vector_type z(5);

	value_type expect(0);
	value_type res(0);
	ublas::vector<value_type> vexpect(0);
	ublas::vector<value_type> vres(0);

	// min(z)
	expect = value_type(0);
	res = ublasx::min(z);
	BOOST_UBLASX_DEBUG_TRACE( "min(" << z << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK( res == expect );

	// min(v)
	expect = value_type(0);
	res = ublasx::min(v);
	BOOST_UBLASX_DEBUG_TRACE( "min(" << v << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

	// min<1>(v)
	vexpect = ublas::vector<value_type>(1, 0);
	vres = ublasx::min<1>(v);
	BOOST_UBLASX_DEBUG_TRACE( "min<1>(" << v << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, 1, tol );
}

BOOST_UBLASX_TEST_DEF( vector_expression )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Vector Expression" );

	typedef double value_type;
	typedef ublas::vector<value_type> vector_type;

	vector_type v(5);

	v(0) = 0.0;
	v(1) = 0.108929;
	v(2) = 0.0;
	v(3) = 0.0;
	v(4) = 1.023787;

	value_type expect(0);
	value_type res(0);
	ublas::vector<value_type> vexpect(0);
	ublas::vector<value_type> vres(0);

	// min(-v)
	expect = value_type(-1.023787);
	res = ublasx::min(-v);
	BOOST_UBLASX_DEBUG_TRACE( "min(" << -v << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

	// min<1>(-v)
	vexpect = ublas::vector<value_type>(1, -1.023787);
	vres = ublasx::min<1>(-v);
	BOOST_UBLASX_DEBUG_TRACE( "min<1>(" << -v << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, 1, tol );
}

BOOST_UBLASX_TEST_DEF( vector_reference )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Vector Reference" );

	typedef double value_type;
	typedef ublas::vector<value_type> vector_type;
	typedef ublas::vector_reference<vector_type> vector_reference_type;

	vector_type v(5);

	v(0) = 0.0;
	v(1) = 0.108929;
	v(2) = 0.0;
	v(3) = 0.0;
	v(4) = 1.023787;

	value_type expect(0);
	value_type res(0);
	ublas::vector<value_type> vexpect(0);
	ublas::vector<value_type> vres(0);

	// min(ref(v))
	expect = value_type(0);
	res = ublasx::min(vector_reference_type(v));
	BOOST_UBLASX_DEBUG_TRACE( "min(" << vector_reference_type(v) << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

	// min<1>(ref(v))
	vexpect = ublas::vector<value_type>(1, 0);
	vres = ublasx::min<1>(vector_reference_type(v));
	BOOST_UBLASX_DEBUG_TRACE( "min<1>(" << vector_reference_type(v) << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, 1, tol );
}

BOOST_UBLASX_TEST_DEF( real_matrix )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real Matrix" );

	typedef double value_type;
	typedef ublas::matrix<value_type> matrix_type;
	typedef ublas::vector<value_type> vector_type;

	std::size_t nr = 5;
	std::size_t nc = 4;

	matrix_type A(nr, nc);

	A(0,0) = 0.0;      A(0,1) = 0.274690; A(0,2) = 0.0;      A(0,3) = 0.798938;
	A(1,0) = 0.108929; A(1,1) = 0.0;      A(1,2) = 0.891726; A(1,3) = 0.0;
	A(2,0) = 0.0;      A(2,1) = 0.0;      A(2,2) = 0.0;      A(2,3) = 0.0;
	A(3,0) = 0.0;      A(3,1) = 0.675382; A(3,2) = 0.0;      A(3,3) = 0.450332;
	A(4,0) = 1.023787; A(4,1) = 1.0;      A(4,2) = 1.231751; A(4,3) = 1.0;

	vector_type min_rows(nr);
	min_rows(0) = 0.0;
	min_rows(1) = 0.0;
	min_rows(2) = 0.0;
	min_rows(3) = 0.0;
	min_rows(4) = 1.0;

	vector_type min_cols(nc);
	min_cols(0) = 0.0;
	min_cols(1) = 0.0;
	min_cols(2) = 0.0;
	min_cols(3) = 0.0;

	value_type expect;
	value_type res;
	vector_type vexpect;
	vector_type vres;

	// min(A)
	expect = value_type(0);
	res = ublasx::min(A);
	BOOST_UBLASX_DEBUG_TRACE( "min(" << A << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

	// min_rows(A)
	vexpect = min_rows;
	vres = ublasx::min_rows(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_rows(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min_columns(A)
	vexpect = min_cols;
	vres = ublasx::min_columns(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_columns(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<1>(A)
	vexpect = min_rows;
	vres = ublasx::min<1>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min<1>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<2>(A)
	vexpect = min_cols;
	vres = ublasx::min<2>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min<2>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<tag::major>(A)
	vexpect = min_rows;
	vres = ublasx::min_by_tag<ublasx::tag::major>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::major>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<tag::minor>(A)
	vexpect = min_cols;
	vres = ublasx::min_by_tag<ublasx::tag::minor>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::minor>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<tag::leading>(A)
	vexpect = min_cols;
	vres = ublasx::min_by_tag<ublasx::tag::leading>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::leading>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );
}

BOOST_UBLASX_TEST_DEF( complex_matrix )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex Matrix" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type> matrix_type;
	typedef ublas::vector<value_type> vector_type;

	std::size_t nr = 5;
	std::size_t nc = 4;

	matrix_type A(nr, nc);

	A(0,0) = value_type( 0.000000,-1.000000); A(0,1) = value_type(0.274690, 1.231751); A(0,2) = value_type(0.090000, 0.108929); A(0,3) = value_type(0.798938,1.000000);
	A(1,0) = value_type( 0.108929, 0.450332); A(1,1) = value_type(0.000000, 1.400000); A(1,2) = value_type(0.891726, 1.023787); A(1,3) = value_type(0.000000,1.230000);
	A(2,0) = value_type(-0.500000, 0.500000); A(2,1) = value_type(0.000000, 2.100000); A(2,2) = value_type(0.090000,-1.230000); A(2,3) = value_type(0.000000,0.675382);
	A(3,0) = value_type( 0.000000,-0.500000); A(3,1) = value_type(0.675382,-1.230000); A(3,2) = value_type(0.090000, 1.231751); A(3,3) = value_type(0.450332,0.891726);
	A(4,0) = value_type( 1.023787, 0.798938); A(4,1) = value_type(1.000000, 0.891726); A(4,2) = value_type(1.231751, 0.000000); A(4,3) = value_type(1.000000,0.500000);

	vector_type min_rows(nr);
	min_rows(0) = value_type(0.090000, 0.108929);
	min_rows(1) = value_type(0.108929, 0.450332);
	min_rows(2) = value_type(0.000000, 0.675382);
	min_rows(3) = value_type(0.000000,-0.500000);
	min_rows(4) = value_type(1.000000, 0.500000);

	vector_type min_cols(nc);
	min_cols(0) = value_type(0.108929,0.450332);
	min_cols(1) = value_type(0.274690,1.231751);
	min_cols(2) = value_type(0.090000,0.108929);
	min_cols(3) = value_type(0.000000,0.675382);

	value_type expect;
	value_type res;
	vector_type vexpect;
	vector_type vres;

	// min(A)
	expect = value_type(0.090000,0.108929);
	res = ublasx::min(A);
	BOOST_UBLASX_DEBUG_TRACE( "min(" << A << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

	// min_rows(A)
	vexpect = min_rows;
	vres = ublasx::min_rows(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_rows(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min_columns(A)
	vexpect = min_cols;
	vres = ublasx::min_columns(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_columns(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<1>(A)
	vexpect = min_rows;
	vres = ublasx::min<1>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min<1>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<2>(A)
	vexpect = min_cols;
	vres = ublasx::min<2>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min<2>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<tag::major>(A)
	vexpect = min_rows;
	vres = ublasx::min_by_tag<ublasx::tag::major>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::major>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<tag::minor>(A)
	vexpect = min_cols;
	vres = ublasx::min_by_tag<ublasx::tag::minor>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::minor>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<tag::leading>(A)
	vexpect = min_cols;
	vres = ublasx::min_by_tag<ublasx::tag::leading>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::leading>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );
}

BOOST_UBLASX_TEST_DEF( row_major_matrix_container )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Row-major Matrix Container" );

	typedef double value_type;
	typedef ublas::matrix<value_type, ublas::row_major> matrix_type;
	typedef ublas::zero_matrix<value_type> zero_matrix_type;
	typedef ublas::vector<value_type> vector_type;

	std::size_t nr = 5;
	std::size_t nc = 4;

	matrix_type A(nr, nc);

	A(0,0) = 0.0;      A(0,1) = 0.274690; A(0,2) = 0.0;      A(0,3) = 0.798938;
	A(1,0) = 0.108929; A(1,1) = 0.0;      A(1,2) = 0.891726; A(1,3) = 0.0;
	A(2,0) = 0.0;      A(2,1) = 0.0;      A(2,2) = 0.0;      A(2,3) = 0.0;
	A(3,0) = 0.0;      A(3,1) = 0.675382; A(3,2) = 0.0;      A(3,3) = 0.450332;
	A(4,0) = 1.023787; A(4,1) = 1.0;      A(4,2) = 1.231751; A(4,3) = 1.0;

	zero_matrix_type Z(nr, nc);

	vector_type min_rows(nr);
	min_rows(0) = 0.0;
	min_rows(1) = 0.0;
	min_rows(2) = 0.0;
	min_rows(3) = 0.0;
	min_rows(4) = 1.0;

	vector_type min_cols(nc);
	min_cols(0) = 0.0;
	min_cols(1) = 0.0;
	min_cols(2) = 0.0;
	min_cols(3) = 0.0;

	value_type expect(0);
	value_type res(0);
	vector_type vexpect(0);
	vector_type vres(0);

	// min(Z)
	expect = value_type(0);
	res = ublasx::min(Z);
	BOOST_UBLASX_DEBUG_TRACE( "min(" << Z << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK( res == expect );

	// min_rows(Z)
	vexpect = ublas::vector<value_type>(nr, 0);
	vres = ublasx::min_rows(Z);
	BOOST_UBLASX_DEBUG_TRACE( "min_rows(" << Z << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min_columns(Z)
	vexpect = ublas::vector<value_type>(nc, 0);
	vres = ublasx::min_columns(Z);
	BOOST_UBLASX_DEBUG_TRACE( "min_columns(" << Z << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<1>(Z)
	vexpect = ublas::vector<value_type>(nr, 0);
	vres = ublasx::min<1>(Z);
	BOOST_UBLASX_DEBUG_TRACE( "min<1>(" << Z << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<2>(Z)
	vexpect = ublas::vector<value_type>(nc, 0);
	vres = ublasx::min<2>(Z);
	BOOST_UBLASX_DEBUG_TRACE( "min<1>(" << Z << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<tag::major>(Z)
	vexpect = ublas::vector<value_type>(nr, 0);
	vres = ublasx::min_by_tag<ublasx::tag::major>(Z);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::major>(" << Z << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<tag::minor>(Z)
	vexpect = ublas::vector<value_type>(nc, 0);
	vres = ublasx::min_by_tag<ublasx::tag::minor>(Z);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::minor>(" << Z << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<tag::leading>(Z)
	vexpect = ublas::vector<value_type>(nc, 0);
	vres = ublasx::min_by_tag<ublasx::tag::leading>(Z);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::leading>(" << Z << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min(A)
	expect = value_type(0);
	res = ublasx::min(A);
	BOOST_UBLASX_DEBUG_TRACE( "min(" << A << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

	// min_rows(A)
	vexpect = min_rows;
	vres = ublasx::min_rows(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_rows(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min_columns(A)
	vexpect = min_cols;
	vres = ublasx::min_columns(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_columns(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<1>(A)
	vexpect = min_rows;
	vres = ublasx::min<1>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min<1>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<2>(A)
	vexpect = min_cols;
	vres = ublasx::min<2>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min<2>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<tag::major>(A)
	vexpect = min_rows;
	vres = ublasx::min_by_tag<ublasx::tag::major>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::major>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<tag::minor>(A)
	vexpect = min_cols;
	vres = ublasx::min_by_tag<ublasx::tag::minor>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::minor>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<tag::leading>(A)
	vexpect = min_cols;
	vres = ublasx::min_by_tag<ublasx::tag::leading>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::leading>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );
}

BOOST_UBLASX_TEST_DEF( col_major_matrix_container )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Column-major Matrix Container" );

	typedef double value_type;
	typedef ublas::matrix<value_type, ublas::column_major> matrix_type;
	typedef ublas::vector<value_type> vector_type;

	std::size_t nr = 5;
	std::size_t nc = 4;

	matrix_type A(nr, nc);

	A(0,0) = 0.0;      A(0,1) = 0.274690; A(0,2) = 0.0;      A(0,3) = 0.798938;
	A(1,0) = 0.108929; A(1,1) = 0.0;      A(1,2) = 0.891726; A(1,3) = 0.0;
	A(2,0) = 0.0;      A(2,1) = 0.0;      A(2,2) = 0.0;      A(2,3) = 0.0;
	A(3,0) = 0.0;      A(3,1) = 0.675382; A(3,2) = 0.0;      A(3,3) = 0.450332;
	A(4,0) = 1.023787; A(4,1) = 1.0;      A(4,2) = 1.231751; A(4,3) = 1.0;

	vector_type min_rows(nr);
	min_rows(0) = 0.0;
	min_rows(1) = 0.0;
	min_rows(2) = 0.0;
	min_rows(3) = 0.0;
	min_rows(4) = 1.0;

	vector_type min_cols(nc);
	min_cols(0) = 0.0;
	min_cols(1) = 0.0;
	min_cols(2) = 0.0;
	min_cols(3) = 0.0;

	value_type expect(0);
	value_type res(0);
	vector_type vexpect(0);
	vector_type vres(0);

	// min(A)
	expect = value_type(0);
	res = ublasx::min(A);
	BOOST_UBLASX_DEBUG_TRACE( "min(" << A << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

	// min_rows(A)
	vexpect = min_rows;
	vres = ublasx::min_rows(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_rows(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min_columns(A)
	vexpect = min_cols;
	vres = ublasx::min_columns(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_columns(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<1>(A)
	vexpect = min_rows;
	vres = ublasx::min<1>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min<1>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<2>(A)
	vexpect = min_cols;
	vres = ublasx::min<2>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min<2>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<tag::major>(A)
	vexpect = min_cols;
	vres = ublasx::min_by_tag<ublasx::tag::major>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::major>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<tag::minor>(A)
	vexpect = min_rows;
	vres = ublasx::min_by_tag<ublasx::tag::minor>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::minor>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<tag::leading>(A)
	vexpect = min_rows;
	vres = ublasx::min_by_tag<ublasx::tag::leading>(A);
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::leading>(" << A << ") = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );
}

BOOST_UBLASX_TEST_DEF( matrix_expression )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Matrix Expression" );

	typedef double value_type;
	typedef ublas::matrix<value_type> matrix_type;
	typedef ublas::vector<value_type> vector_type;

	std::size_t nr = 5;
	std::size_t nc = 4;

	matrix_type A(nr, nc);

	A(0,0) = 0.0;      A(0,1) = 0.274690; A(0,2) = 0.0;      A(0,3) = 0.798938;
	A(1,0) = 0.108929; A(1,1) = 0.0;      A(1,2) = 0.891726; A(1,3) = 0.0;
	A(2,0) = 0.0;      A(2,1) = 0.0;      A(2,2) = 0.0;      A(2,3) = 0.0;
	A(3,0) = 0.0;      A(3,1) = 0.675382; A(3,2) = 0.0;      A(3,3) = 0.450332;
	A(4,0) = 1.023787; A(4,1) = 1.0;      A(4,2) = 1.231751; A(4,3) = 1.0;

	vector_type min_rows(nr);
	min_rows(0) = 0.0;
	min_rows(1) = 0.0;
	min_rows(2) = 0.0;
	min_rows(3) = 0.0;
	min_rows(4) = 1.0;

	vector_type min_cols(nc);
	min_cols(0) = 0.0;
	min_cols(1) = 0.0;
	min_cols(2) = 0.0;
	min_cols(3) = 0.0;

	value_type expect(0);
	value_type res(0);
	vector_type vexpect(0);
	vector_type vres(0);

	// min(A')
	expect = value_type(0);
	res = ublasx::min(ublas::trans(A));
	BOOST_UBLASX_DEBUG_TRACE( "min(" << A << "') = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

	// min_rows(A')
	vexpect = min_cols;
	vres = ublasx::min_rows(ublas::trans(A));
	BOOST_UBLASX_DEBUG_TRACE( "min_rows(" << A << "') = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min_columns(A')
	vexpect = min_rows;
	vres = ublasx::min_columns(ublas::trans(A));
	BOOST_UBLASX_DEBUG_TRACE( "min_columns(" << A << "') = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<1>(A')
	vexpect = min_cols;
	vres = ublasx::min<1>(ublas::trans(A));
	BOOST_UBLASX_DEBUG_TRACE( "min<1>(" << A << "') = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<2>(A')
	vexpect = min_rows;
	vres = ublasx::min<2>(ublas::trans(A));
	BOOST_UBLASX_DEBUG_TRACE( "min<2>(" << A << "') = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<tag::major>(A')
	vexpect = min_rows;
	vres = ublasx::min_by_tag<ublasx::tag::major>(ublas::trans(A));
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::major>(" << A << "') = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<tag::minor>(A')
	vexpect = min_cols;
	vres = ublasx::min_by_tag<ublasx::tag::minor>(ublas::trans(A));
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::minor>(" << A << "') = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<tag::leading>(A')
	vexpect = min_cols;
	vres = ublasx::min_by_tag<ublasx::tag::leading>(ublas::trans(A));
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::leading>(" << A << "') = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );
}

BOOST_UBLASX_TEST_DEF( matrix_reference )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Matrix Reference" );

	typedef double value_type;
	typedef ublas::matrix<value_type> matrix_type;
	typedef ublas::matrix_reference<matrix_type> matrix_reference_type;
	typedef ublas::vector<value_type> vector_type;

	std::size_t nr = 5;
	std::size_t nc = 4;

	matrix_type A(nr, nc);

	A(0,0) = 0.0;      A(0,1) = 0.274690; A(0,2) = 0.0;      A(0,3) = 0.798938;
	A(1,0) = 0.108929; A(1,1) = 0.0;      A(1,2) = 0.891726; A(1,3) = 0.0;
	A(2,0) = 0.0;      A(2,1) = 0.0;      A(2,2) = 0.0;      A(2,3) = 0.0;
	A(3,0) = 0.0;      A(3,1) = 0.675382; A(3,2) = 0.0;      A(3,3) = 0.450332;
	A(4,0) = 1.023787; A(4,1) = 1.0;      A(4,2) = 1.231751; A(4,3) = 1.0;

	vector_type min_rows(nr);
	min_rows(0) = 0.0;
	min_rows(1) = 0.0;
	min_rows(2) = 0.0;
	min_rows(3) = 0.0;
	min_rows(4) = 1.0;

	vector_type min_cols(nc);
	min_cols(0) = 0.0;
	min_cols(1) = 0.0;
	min_cols(2) = 0.0;
	min_cols(3) = 0.0;

	value_type expect(0);
	value_type res(0);
	vector_type vexpect(0);
	vector_type vres(0);

	// min(ref(A))
	expect = value_type(0);
	res = ublasx::min(matrix_reference_type(A));
	BOOST_UBLASX_DEBUG_TRACE( "min(reference(" << A << ")) = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK( res == expect );

	// min_rows(ref(A))
	vexpect = min_rows;
	vres = ublasx::min_rows(matrix_reference_type(A));
	BOOST_UBLASX_DEBUG_TRACE( "min_rows(reference(" << A << ")) = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min_columns(ref(A))
	vexpect = min_cols;
	vres = ublasx::min_columns(matrix_reference_type(A));
	BOOST_UBLASX_DEBUG_TRACE( "min_columns(reference(" << A << ")) = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<1>(ref(A))
	vexpect = min_rows;
	vres = ublasx::min<1>(matrix_reference_type(A));
	BOOST_UBLASX_DEBUG_TRACE( "min<1>(reference(" << A << ")) = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<2>(ref(A))
	vexpect = min_cols;
	vres = ublasx::min<2>(matrix_reference_type(A));
	BOOST_UBLASX_DEBUG_TRACE( "min<2>(reference(" << A << ")) = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<tag::major>(ref(A))
	vexpect = min_rows;
	vres = ublasx::min_by_tag<ublasx::tag::major>(matrix_reference_type(A));
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::major>(reference(" << A << ")) = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

	// min<tag::minor>(ref(A))
	vexpect = min_cols;
	vres = ublasx::min_by_tag<ublasx::tag::minor>(matrix_reference_type(A));
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::minor>(reference(" << A << ")) = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

	// min<tag::leading>(ref(A))
	vexpect = min_cols;
	vres = ublasx::min_by_tag<ublasx::tag::leading>(matrix_reference_type(A));
	BOOST_UBLASX_DEBUG_TRACE( "min_by_tag<tag::leading>(reference(" << A << ")) = " << vres << " ==> " << vexpect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );
}

int main()
{
	BOOST_UBLASX_DEBUG_TRACE("Test Suite: 'min' operation");

	BOOST_UBLASX_TEST_BEGIN();

	BOOST_UBLASX_TEST_DO( real_vector );
	BOOST_UBLASX_TEST_DO( complex_vector );
	BOOST_UBLASX_TEST_DO( vector_container );
	BOOST_UBLASX_TEST_DO( vector_expression );
	BOOST_UBLASX_TEST_DO( vector_reference );
	BOOST_UBLASX_TEST_DO( real_matrix );
	BOOST_UBLASX_TEST_DO( complex_matrix );
	BOOST_UBLASX_TEST_DO( row_major_matrix_container );
	BOOST_UBLASX_TEST_DO( col_major_matrix_container );
	BOOST_UBLASX_TEST_DO( matrix_expression );
	BOOST_UBLASX_TEST_DO( matrix_reference );

	BOOST_UBLASX_TEST_END();
}


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

#include "optimo/core/objects.h"
#include "optimo/solvers/newton.h"
#include "optimo/core/numerical_gradient.h"
#include "optimo/core/numerical_hessian.h"
#include <glog/logging.h>
#include "gtest/gtest.h"
#include <Eigen/LU>
#include <random>
#include <cmath>

namespace optimo {
namespace solvers {
namespace {
using Eigen::Matrix;
// Quadratic Objective
template <typename Scalar, uint n>
class QuadraticObjective : public ObjectiveFunctor<Scalar, n> {
 public:
  QuadraticObjective(const Matrix<Scalar, n, n>& _Q,
                     const Matrix<Scalar, n, 1>& _P,
                     const Scalar& _c) {
    Q = _Q;
    P = _P;
    c = _c;
  }

  ~QuadraticObjective(void) { }

  virtual Scalar operator()(const Matrix<Scalar, n, 1>& x) const {
    Matrix<Scalar, 1, 1> sum = x.transpose()*Q*x + P.transpose()*x;
    return sum(0,0) + c;
  }

 protected:
  Matrix<Scalar, n, n> Q;
  Matrix<Scalar, n, 1> P;
  Scalar c;
};

// Quadratic Program
template <typename Scalar, uint m, uint n>
class QuadraticProgram : public Problem<Scalar, m, n> {
 public:
  QuadraticProgram(const QuadraticObjective<Scalar, n>& qobj,
                   const NoConstraints<Scalar, m, n>& nc,
                   const Matrix<Scalar, m, n>& Aeq,
                   const Matrix<Scalar, m, 1>& beq,
                   const GradientFunctor<Scalar, n>& g,
                   const HessianFunctor<Scalar, n>& h) :
      Problem<Scalar, m, n>(qobj, nc, Aeq, beq, g, h) {
  }

  virtual ~QuadraticProgram(void) { }
};

// Gradient Functor
template <typename Scalar, uint n>
class QGradient : public GradientFunctor<Scalar, n> {
 public:
  QGradient(const Matrix<Scalar, n, n>& _Q,
            const Matrix<Scalar, n, 1>& _P) {
    Q = _Q;
    P = _P;
  }

  virtual void operator()(const Matrix<Scalar, n, 1>& x,
                          Matrix<Scalar, n, 1>* g) const {
    *g = 2*Q*x + P;
  }

 protected:
  Matrix<Scalar, n, n> Q;
  Matrix<Scalar, n, 1> P;
};

// Hessian Functor
template <typename Scalar, uint n>
class QHessian : public HessianFunctor<Scalar, n> {
 public:
  QHessian(const Matrix<Scalar, n, n>& _Q) {
    Q = _Q;
  }

  virtual void operator()(const Matrix<Scalar, n, 1>& x,
                          Matrix<Scalar, n, n>* h) const {
    *h = 2*Q;
  }

 protected:
  Matrix<Scalar, n, n> Q;
};

// Entropy Maximization Problem
template <typename Scalar, uint m, uint n>
struct EntropyObjective : public ObjectiveFunctor<Scalar, n> {
  virtual Scalar operator()(const Matrix<Scalar, n, 1>& x) const {
    Scalar f = static_cast<Scalar>(0.0);
    const Scalar* xdata = x.data();
    for (unsigned int i = 0; i < n; i++) {
      // TODO(vfragoso): Check if log returns NaN when 0 or x  < 0
      Scalar logx = static_cast<Scalar>(log(xdata[i]));
      if (std::isnan(logx)) return logx;
      f += xdata[i]*logx;
    }
    return f;
  }
};

// Gradient Functor
template <typename Scalar, uint n>
class EntropyGradient : public GradientFunctor<Scalar, n> {
  virtual void operator()(const Matrix<Scalar, n, 1>& x,
                          Matrix<Scalar, n, 1>* g) const {
    Scalar* gdata = g->data();
    const Scalar* xdata = x.data();
    for (unsigned int i = 0; i < n; i++) {
      gdata[i] = log(xdata[i]) + 1;
    }
  }
};

// Hessian Functor
template <typename Scalar, uint n>
class EntropyHessian : public HessianFunctor<Scalar, n> {
 public:
  virtual void operator()(const Matrix<Scalar, n, 1>& x,
                          Matrix<Scalar, n, n>* h) const {
    const Scalar* xdata = x.data();
    for (unsigned int i = 0; i < n; i++) {
      (*h)(i, i) = static_cast<Scalar>(1.0/xdata[i]);
    }
  }
};

// Entropy Problem
template <typename Scalar, uint m, uint n>
struct EntropyProblem : public Problem<Scalar, m, n> {
  EntropyProblem(const EntropyObjective<Scalar, m, n>& obj,
                 const NoConstraints<Scalar, m, n>& nc,
                 const Matrix<Scalar, m, n>& Aeq,
                 const Matrix<Scalar, m, 1>& beq,
                 const GradientFunctor<Scalar, n>& g,
                 const HessianFunctor<Scalar, n>& h) :
      Problem<Scalar, m, n>(obj, nc, Aeq, beq, g, h) { }
};
}  // namespace

TEST(Newton, Quadratic_Programs) {
  const uint n = 3;  // Number of unknowns
  const uint m = 1;  // Number of constraints
  Matrix<double, n, n> Q;
  Matrix<double, n, 1> P;
  double c = 15.0;
  Matrix<double, n, 1> x;

  Q << 5, 0, 0,
       0, 4, 0,
       0, 0, 1.5;
  P << 0.01, 0.5, -0.1;
  x << 100, 100, 100;

  QuadraticObjective<double, n> qobj(Q, P, c);
  const NoConstraints<double, m, n>& nc =
      NoConstraints<double, m, n>::getInstance();
  Matrix<double, m, n> Aeq;
  Matrix<double, m, 1> beq;
  QGradient<double, n> qgradient(Q, P);
  QHessian<double, n> qhessian(Q);

  QuadraticProgram<double, m, n>
      qprogram(qobj, nc, Aeq, beq, qgradient, qhessian);

  Newton<double, m, n> newton(Newton<double, m, n>::UNCONSTRAINED);
  double min_val;
  auto res = newton(qprogram, &x, &min_val);
  LOG(INFO) << "Min_val: " << min_val
            << " Minimizer: [" << x.transpose() << "]"
            << " Result: " << res
            << std::endl;
  ASSERT_LT(min_val - c, 0.1);
}

TEST(Newton, EqConstrained_Quadratic_Programs) {
  const uint n = 2;  // Number of unknowns
  const uint m = 1;  // Number of constraints
  Matrix<double, n, n> Q;
  Matrix<double, n, 1> P;
  double c = 0.0;
  Matrix<double, n, 1> x;

  Q << 6, 0, 0, 4;
  P << 0.0, 0.0;
  x << 100, 100;

  QuadraticObjective<double, n> qobj(Q, P, c);
  const NoConstraints<double, m, n>& nc =
      NoConstraints<double, m, n>::getInstance();
  Matrix<double, m, n> Aeq;
  Matrix<double, m, 1> beq;
  QGradient<double, n> qgradient(Q, P);
  QHessian<double, n> qhessian(Q);

  Aeq << 1, -1;
  beq << 0;

  QuadraticProgram<double, m, n>
      qprogram(qobj, nc, Aeq, beq, qgradient, qhessian);

  Newton<double, m, n> newton(Newton<double, m, n>::EQUALITY_CONSTRAINED);
  double min_val;
  auto res = newton(qprogram, &x, &min_val);
  LOG(INFO) << "Min_val: " << min_val
            << " Minimizer: [" << x.transpose() << "]"
            << " Result: " << res
            << std::endl;
  ASSERT_LT(min_val - c, 0.1);
}

TEST(Newton, Unconstrained_Random_Quadratic_Program) {
  const uint n = 16;  // Number of unkowns
  const uint m = 1;  // Unconstrained
  Matrix<double, n, n> Q = Matrix<double, n, n>::Random();
  Matrix<double, n, 1> P = Matrix<double, n, 1>::Zero();
  double c = 5.0;
  Matrix<double, n, 1> x = Matrix<double, n, 1>::Random();

  Q = Q.transpose()*Q;

  QuadraticObjective<double, n> qobj(Q, P, c);
  const NoConstraints<double, m, n>& nc =
      NoConstraints<double, m, n>::getInstance();
  Matrix<double, m, n> Aeq;
  Matrix<double, m, 1> beq;
  QGradient<double, n> qgradient(Q, P);
  QHessian<double, n> qhessian(Q);

  QuadraticProgram<double, m, n>
      qprogram(qobj, nc, Aeq, beq, qgradient, qhessian);

  Newton<double, m, n> newton(Newton<double, m, n>::UNCONSTRAINED);
  double min_val;
  auto res = newton(qprogram, &x, &min_val);
  LOG(INFO) << "Min_val: " << min_val
            << " Minimizer: [" << x.transpose() << "]"
            << " Result: " << res
            << std::endl;
  ASSERT_LT(min_val - c, 0.1);
}

TEST(Newton, EqConstrained_Random_Quadratic_Programs) {
  const uint n = 16;
  const uint m = 1;
  Matrix<double, n, n> Q =  Matrix<double, n, n>::Random();
  Matrix<double, n, 1> P = Matrix<double, n, 1>::Zero();
  double c = 5.0;
  Matrix<double, n, 1> x;
  x = Matrix<double, n, 1>::Zero();
  x(0, 0) = 1;
  x(n - 1, 0) = -1;

  Q = Q.transpose()*Q;

  QuadraticObjective<double, n> qobj(Q, P, c);
  const NoConstraints<double, m, n>& nc =
      NoConstraints<double, m, n>::getInstance();
  Matrix<double, m, n> Aeq;
  Matrix<double, m, 1> beq;
  QGradient<double, n> qgradient(Q, P);
  QHessian<double, n> qhessian(Q);

  Aeq.setOnes();
  beq = Matrix<double, m, 1>::Zero();

  QuadraticProgram<double, m, n>
      qprogram(qobj, nc, Aeq, beq, qgradient, qhessian);

  Newton<double, m, n> newton(Newton<double, m, n>::EQUALITY_CONSTRAINED);
  double min_val;
  auto res = newton(qprogram, &x, &min_val);
  double rprimal = (Aeq*x-beq).norm();
  LOG(INFO) << "Min_val: " << min_val
            << " Minimizer: [" << x.transpose() << "]"
            << " Result: " << res
            << " Primal Residual: " << rprimal
            << std::endl;
  ASSERT_LT(min_val - c, 0.1);
  ASSERT_LT(x.sum(), 0.1);
  ASSERT_LT(rprimal, 0.1);
}

TEST(Newton, EqualityConstrained_Entropy) {
  const uint n = 4;
  const uint m = 2;
  double h = 1e-6;
  EntropyObjective<double, m, n> objective;
  const NoConstraints<double, m, n>& nc =
      NoConstraints<double, m, n>::getInstance();
  EntropyHessian<double, n> hessian;
  EntropyGradient<double, n> gradient;
  // NumericalHessian<double, n> hessian(objective, h);
  // SecantGradientFunctor<double, n> gradient(objective, h);
  Matrix<double, m, n> Aeq;
  std::random_device rng;
  std::uniform_int_distribution<> randi(1, n);
  Matrix<double, n, 1> x;

  double* Adata = Aeq.data();
  double* xdata = x.data();
  for (unsigned int i = 0; i < x.size(); i++) xdata[i] = randi(rng);
  uint rank;
  do {
    for (unsigned int i = 0; i < Aeq.size(); i++) Adata[i] = randi(rng);
    Eigen::FullPivLU<Matrix<double, m, n> > lu(Aeq);
    rank = lu.rank();
  } while (rank != m);

  Matrix<double, m, 1> beq = Aeq*x;
  EntropyProblem<double, m, n> problem(objective, nc, Aeq, beq,
                                       gradient, hessian);
  Newton<double, m, n>
      newton(Newton<double, m, n>::INFEASIBLE_EQUALITY_CONSTRAINED);
  newton.options.epsilon_ = 1e-9;
  double min_val;
  auto res = newton(problem, &x, &min_val);
  double rprimal = (Aeq*x-beq).norm();
  LOG(INFO) << "Min_val: " << min_val
            << " Minimizer: [" << x.transpose() << "]"
            << " Result: " << res
            << " Primal Residual: " << rprimal
            << std::endl;
}
}  // solvers
}  // optimo


/*
 * Chrono: measure time duration between two points with boost.datetime
 *
 * Copyright (c) 2012 Jérémie Decock
 *
 * Required: boost.chrono and boost.system library
 * Usage: g++ chrono.cc -lboost_chrono -lboost_system -o chrono
 *
 */

#include <boost/chrono.hpp>
#include <cmath>
#include <iostream>

int main()
{
    boost::chrono::system_clock::time_point start = boost::chrono::system_clock::now();

    // Do something...
    for(double x=0.0 ; x<10.0 ; x+=0.0000001) {
        double y = std::sqrt(x);
    }

    boost::chrono::duration<double> sec = boost::chrono::system_clock::now() - start;

    std::cout << "Took " << sec.count() << " seconds" << std::endl;
}