//
// Created by everettjf on 2019-09-29.
//

#include "utils.h"
#include <unistd.h>
#include "networkhandler.h"
#include "dep/logic/tinyformat.h"
#include "dep/logic/anybase.h"
#include <boost/algorithm/string.hpp>
#include <vector>
#include <boost/lexical_cast.hpp>

namespace utils {

std::string ip2code(const std::string & ip) {
    std::vector<std::string> result;
    boost::split(result, ip, boost::is_any_of("."));
    if (result.size() != 4) {
        return "";
    }

    int n0=0,n1=0,n2=0,n3=0;
    try {
        n0 = boost::lexical_cast<int>(result[0]);
        n1 = boost::lexical_cast<int>(result[1]);
        n2 = boost::lexical_cast<int>(result[2]);
        n3 = boost::lexical_cast<int>(result[3]);
    }catch(const boost::bad_lexical_cast &) {
        return "";
    }

    if (n0 == 192 && n1 == 168) {
        long codeBase10 = n2 * 256 + n3;

        std::string codeBase36 = anybase::Decimal2AnyBase(codeBase10, 32);
        return tfm::format("x%s",codeBase36);
    } else {
        long codeBase10 = n0 * 256 * 256 * 256 +
                          n1 * 256 * 256 +
                          n2 * 256 +
                          n3;

        std::string codeBase36 = anybase::Decimal2AnyBase(codeBase10, 32);
        return codeBase36;
    }
}

}


//  (C) Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
//  Constepxr implementation of fabs (see c.math.abs secion 26.8.2 of the ISO standard)

#ifndef BOOST_MATH_CCMATH_FABS
#define BOOST_MATH_CCMATH_FABS

#include <boost/math/ccmath/abs.hpp>

namespace boost::math::ccmath {

template <typename T>
inline constexpr auto fabs(T x) noexcept
{
    return boost::math::ccmath::abs(x);
}

inline constexpr float fabsf(float x) noexcept
{
    return boost::math::ccmath::abs(x);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr long double fabsl(long double x) noexcept
{
    return boost::math::ccmath::abs(x);
}
#endif

}

#endif // BOOST_MATH_CCMATH_FABS


#pragma once

#include "set_stream_precision.hxx"

#include <boost/multiprecision/mpfr.hpp>
#include <sstream>

using Boost_Float = boost::multiprecision::mpfr_float;

inline std::string to_string(const Boost_Float &boost_float)
{
  // Using a stringstream seems to the best way to convert between
  // MPFR and GMP.  It may lose a bit or two since string
  // conversion is not sufficient for round-tripping.
  std::stringstream ss;
  set_stream_precision(ss);
  ss << boost_float;
  return ss.str();
}


//
// Project: panoramachine
// File: Momentum.hpp
//
// Copyright (c) 2022 Miika 'Lehdari' Lehtimäki
// You may use, distribute and modify this code under the terms
// of the licence specified in file LICENSE which is distributed
// with this source code package.
//

#ifndef PANORAMACHINE_MOMENTUM_HPP
#define PANORAMACHINE_MOMENTUM_HPP

#include <Eigen/Dense>

template <typename T>
class Momentum {
public:
    Momentum(double momentum, const T& v = T()) :
        _momentum   (momentum),
        _variance   (0.0*T()),
        _v          (v)
    {}

    const T& operator()(const T& v)
    {
        T variance = std::pow(v-_v, 2.0);
        _variance = _momentum*_variance + (1.0-_momentum)*variance;
        _v = _momentum*_v + (1.0-_momentum)*v;
        return _v;
    }

    operator const T&() const
    {
        return _v;
    }

    const T& variance() const
    {
        return std::sqrt(_variance);
    }

private:
    double      _momentum;
    T           _variance;
    T           _v;
};

template <typename T_Scalar, int Rows, int Cols>
class Momentum<Eigen::Matrix<T_Scalar, Cols, Rows>> {
private:
    using T = Eigen::Matrix<T_Scalar, Cols, Rows>;

public:
    Momentum(T_Scalar momentum, const T& v = T()) :
        _momentum   (momentum),
        _variance   (0.0*T()),
        _v          (v)
    {}

    const T& operator()(const T& v)
    {
        T variance = (v-_v).array().square().matrix();
        _variance = _momentum*_variance + (1.0-_momentum)*variance;
        _v = _momentum*_v + (1.0-_momentum)*v;
        return _v;
    }

    operator const T&() const
    {
        return _v;
    }

    T_Scalar variance() const
    {
        return _variance.cwiseSqrt().norm();
    }

private:
    T_Scalar    _momentum;
    T           _variance;
    T           _v;
};

#endif //PANORAMACHINE_MOMENTUM_HPP


/**
 *  testReachSet.cpp
 *
 *  Test abstraction by using a car kinematics model.
 *
 *  Created by Yinan Li on Nov. 25, 2020.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <algorithm>
#include <cmath>
#include <sys/stat.h>
#include <boost/numeric/odeint.hpp>

#include "src/grid.h"
#include "src/definitions.h"
#include "src/abstraction.hpp"
#include "src/hdf5io.h"

const double h = 0.3;  // sampling time
const double dt = 0.001; //integration step size for odeint

/* For reachable set computation */
struct twoagent {
    static const int n = 3;  // system dimension
    static const int nu = 2;  // control dimension
    rocs::ivec d{rocs::interval(-0.8, 0.8),
		 rocs::interval(-0.8, 0.8)};

    /* template constructor
     * @param[out] dx
     * @param[in] x = [xr, yr, psir]
     * @param u = [v, w]
     * @param d = [v', w']
     */
    template<typename S>
    twoagent(S *dx, const S *x, rocs::Rn u) {
	dx[0] = -u[0] + d[0]*cos(x[2]) + u[1]*x[1];
	dx[1] = d[0]*sin(x[2]) - u[1]*x[0];
	dx[2] = d[1] - u[1];
    }
};

/* For solving ode */
struct car_ode {
    rocs::Rn u;
    rocs::Rn d;
    car_ode (const rocs::Rn p1, const rocs::Rn p2): u(p1), d(p2){}
    /**
     * ODE model
     * @param x system state: [x,y,theta], n=3
     * @param dxdt vector field
     * @param t time
     */
    void operator() (rocs::Rn &x, rocs::Rn &dxdt, double t) const
    {
	dxdt[0] = -u[0] + d[0]*cos(x[2]) + u[1]*x[1];
	dxdt[1] = d[0]*sin(x[2]) - u[1]*x[0];
	dxdt[2] = d[1] - u[1];
    }
};

int main()
{
    /* Config */
    clock_t tb, te;
    boost::numeric::odeint::runge_kutta_cash_karp54<rocs::Rn> rk45;

    /**
     * Case I
     */
    /* Set the state and control space */
    const int xdim = 3;
    const int udim = 2;

    double xlb[] = {-3, -3, -M_PI};
    double xub[] = {3, 3, M_PI};
    double eta[] = {0.2, 0.2, 0.2};

    double ulb[] = {-1.0, -1.0};
    double uub[] = {1.0, 1.0};
    double mu[] = {0.3, 0.3};

    /**
     * Define the two-agent system
     */
    double t = 0.3;
    double delta = 0.01;
    /* parameters for computing the flow */
    int kmax = 5;
    double tol = 0.01;
    double alpha = 0.5;
    double beta = 2;
    rocs::params controlparams(kmax, tol, alpha, beta);
    rocs::CTCntlSys<twoagent> safety("collision-free", t,
    				     twoagent::n, twoagent::nu,
    				     delta, &controlparams);

    safety.init_workspace(xlb, xub);
    safety.init_inputset(mu, ulb, uub);
    safety.allocate_flows();

    rocs::abstraction< rocs::CTCntlSys<twoagent> > abst(&safety);
    abst.init_state(eta, xlb, xub);
    std::cout << "# of in-domain nodes: " << abst._x._nv << '\n';

    /* Test reachable set computation */
    int suc = 1;
    size_t na = safety._ugrid._nv;
    rocs::Rn x_test{-1.4, 0.6, -1.9};
    // rocs::Rn x_test{1.4, 0.6, -1.9};
    rocs::ivec x0 = {rocs::interval(x_test[0]-eta[0]/2., x_test[0]+eta[0]/2.),
		     rocs::interval(x_test[1]-eta[1]/2., x_test[1]+eta[1]/2.),
		     rocs::interval(x_test[2]-eta[2]/2., x_test[2]+eta[2]/2.)};
    std::cout << "Checking rechable set computation for " << x0 << "...\n";
    rocs::Rn u(udim);
    rocs::Rn xpost(xdim);
    rocs::ivec box(xdim);
    std::vector<rocs::ivec> reachset(na, rocs::ivec(xdim));
    rocs::Rn corner(xdim), p0(xdim);
    int quo, rem;
    rocs::ivec margin{rocs::interval(-rocs::EPSIVAL, rocs::EPSIVAL),
    		      rocs::interval(-rocs::EPSIVAL, rocs::EPSIVAL),
    		      rocs::interval(-rocs::EPSIVAL, rocs::EPSIVAL)};
    rocs::ivec yt(xdim);

    safety.get_reach_set(reachset, x0);
    std::vector<rocs::Rn> d(4, rocs::Rn {0.7, 0.7});
    d[1][0] = -0.7;
    d[2][0] = -0.7;
    d[2][1] = -0.7;
    d[3][1] = -0.7;
    /* Test valid control inputs */
    for(size_t j = 0; j < na; ++j) {
	safety._ugrid.id_to_val(u, j); //get control values
	for(auto de:d) {
	    /* Test if the reachable set covers ode solutions of all corners */
	    for(int k = 0; k < std::pow(2, xdim); ++k) {
		quo = k;
		for(int l = 0; l < xdim; ++l) {
		    if(quo % 2) {
			corner[l] = x_test[l]+eta[l]/2.; //upper bound
			p0[l] = corner[l];
		    } else {
			corner[l] = x_test[l]-eta[l]/2.; //lower bound
			p0[l] = corner[l];
		    }
		    quo /= 2;
		}
		// std::cout << "Corner "
		// 	  << '(' << corner[0] << ',' << corner[1] << ',' << corner[2] << ")\n";
		boost::numeric::odeint::integrate_const(rk45, car_ode(u,de), corner, 0.0, h, dt);
		yt = reachset[j] + margin;
		if(!yt.isin(corner)) {
		    std::cout << "Incorrect at u=" << j
			      << '(' << u[0] << ',' << u[1] << ')'
			      << ", d=" << '(' << de[0] << ',' << de[1] << ") for "
			      << '(' << p0[0] << ',' << p0[1] << ',' << p0[2] << "):\n"
			      << '(' << corner[0] << ',' << corner[1] << ',' << corner[2] << ')'
			      << " is not in " << yt << '\n';
		    suc = 0;
		}
	    }
	}
    }//end for control values

    if(suc)
	std::cout << "The test point passes the test.\n";

    return suc;
}


#include <boost/test/unit_test.hpp>

#include "gridworld_route.h"
#include <fstream>
#include "logs.h"
#include "types.h"
#include "route.h"

using namespace GPS;

// This function generates GPXlogfiles with the given positions
std::string GPXlogFiles(std::string RoutePath, GridWorldRoute gridw)
{
    std::string routeName = RoutePath + "_N0731739.gpx";

    std::ofstream finalroute(LogFiles::GPXRoutesDir + routeName);

    finalroute << gridw.toGPX(true, RoutePath);

    finalroute.close();

    return routeName;
}

BOOST_AUTO_TEST_SUITE( Route_netLength_No731739)

const bool isFileName = true;
const metres horizontalGridUnit = 30000;

// Single point on the gdidworld
BOOST_AUTO_TEST_CASE ( SinglePoint )
{
    Route Qroute = Route(LogFiles::GPXRoutesDir + "Q.gpx", isFileName);
    BOOST_CHECK_EQUAL( Qroute.netLength(), 0.0);
}

// First and last location are the same
BOOST_AUTO_TEST_CASE ( SameFirstLastPoint )
{
    GridWorldRoute Logroute = GridWorldRoute("LOL");
    Route LOLroute = Route(LogFiles::GPXRoutesDir + GPXlogFiles("test4",Logroute), isFileName);
    BOOST_CHECK_EQUAL( LOLroute.netLength(), 0 );
}
// Using different points
BOOST_AUTO_TEST_CASE ( differentPoints )
{
    GridWorldRoute Logroute = GridWorldRoute("VITA");
    Route VITAroute = Route(LogFiles::GPXRoutesDir + GPXlogFiles("test",Logroute), isFileName);
    BOOST_CHECK_EQUAL( VITAroute.netLength(), 41254.444525376712  );
}

// Longitude, latitude and elevation are zero
BOOST_AUTO_TEST_CASE ( lon_lat_ele_0 )
{
    GridWorldRoute Logroute = GridWorldRoute("SNHB" ,GridWorld(Earth::EquatorialMeridian,0,0));
    Route SNHBroute = Route(LogFiles::GPXRoutesDir + GPXlogFiles("test1",Logroute), isFileName);
    BOOST_CHECK_EQUAL( SNHBroute.netLength(), 0  );
}
// Only longitude is zero
BOOST_AUTO_TEST_CASE ( longitude_0 )
{
    GridWorldRoute Logroute = GridWorldRoute("QWERTYUIOP" ,GridWorld(Earth::NorthPole,0,0));
    Route QWERTYUIOProute = Route(LogFiles::GPXRoutesDir + GPXlogFiles("test2",Logroute), isFileName);
    BOOST_CHECK_EQUAL( QWERTYUIOProute.netLength(), 0  );
}

// Only latitude is zero
BOOST_AUTO_TEST_CASE ( latitude_0 )
{
    GridWorldRoute Logroute = GridWorldRoute("MNO");
    Route MNOroute = Route(LogFiles::GPXRoutesDir + GPXlogFiles("test3",Logroute), isFileName);
    BOOST_CHECK_EQUAL( MNOroute.netLength(), 20015.114442036094  );
}

// Granularity is smaler than the netLength
BOOST_AUTO_TEST_CASE( GranularityIsSmaler )
{
   const metres granularity = horizontalGridUnit * 0.99;
   Route ABCDroute = Route(LogFiles::GPXRoutesDir + "ABCD.gpx", isFileName, granularity);
   BOOST_CHECK_EQUAL( ABCDroute.netLength(), 30022.523566211392 );
}
// Granularity is bigger than the netLength
BOOST_AUTO_TEST_CASE( GranularityIsBigger )
{
   const metres granularity = horizontalGridUnit * 1.01;
   Route ABCDroute = Route(LogFiles::GPXRoutesDir + "ABCD.gpx", isFileName, granularity);
   BOOST_CHECK_EQUAL( ABCDroute.netLength(), 0 );
}

BOOST_AUTO_TEST_SUITE_END()


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

 * 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    DoglegOptimizer.h
 * @brief   Unit tests for DoglegOptimizer
 * @author  Richard Roberts
 */

#include <CppUnitLite/TestHarness.h>

#include <tests/smallExample.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/base/numericalDerivative.h>

#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/assign/list_of.hpp> // for 'list_of()'
#include <functional>
#include <boost/iterator/counting_iterator.hpp>

using namespace std;
using namespace gtsam;

// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* */
TEST(DoglegOptimizer, ComputeBlend) {
  // Create an arbitrary Bayes Net
  GaussianBayesNet gbn;
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      0, (Vector(2) << 1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0),
      3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0),
      4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      1, (Vector(2) << 15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0),
      2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0),
      4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      2, (Vector(2) << 29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0),
      3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      3, (Vector(2) << 39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0),
      4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0)));

  // Compute steepest descent point
  VectorValues xu = gbn.optimizeGradientSearch();

  // Compute Newton's method point
  VectorValues xn = gbn.optimize();

  // The Newton's method point should be more "adventurous", i.e. larger, than the steepest descent point
  EXPECT(xu.vector().norm() < xn.vector().norm());

  // Compute blend
  double Delta = 1.5;
  VectorValues xb = DoglegOptimizerImpl::ComputeBlend(Delta, xu, xn);
  DOUBLES_EQUAL(Delta, xb.vector().norm(), 1e-10);
}

/* ************************************************************************* */
TEST(DoglegOptimizer, ComputeDoglegPoint) {
  // Create an arbitrary Bayes Net
  GaussianBayesNet gbn;
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      0, (Vector(2) << 1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0),
      3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0),
      4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      1, (Vector(2) << 15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0),
      2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0),
      4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      2, (Vector(2) << 29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0),
      3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      3, (Vector(2) << 39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0),
      4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0)));

  // Compute dogleg point for different deltas

  double Delta1 = 0.5;  // Less than steepest descent
  VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, gbn.optimizeGradientSearch(), gbn.optimize());
  DOUBLES_EQUAL(Delta1, actual1.vector().norm(), 1e-5);

  double Delta2 = 1.5;  // Between steepest descent and Newton's method
  VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, gbn.optimizeGradientSearch(), gbn.optimize());
  VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, gbn.optimizeGradientSearch(), gbn.optimize());
  DOUBLES_EQUAL(Delta2, actual2.vector().norm(), 1e-5);
  EXPECT(assert_equal(expected2, actual2));

  double Delta3 = 5.0;  // Larger than Newton's method point
  VectorValues expected3 = gbn.optimize();
  VectorValues actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, gbn.optimizeGradientSearch(), gbn.optimize());
  EXPECT(assert_equal(expected3, actual3));
}

/* ************************************************************************* */
TEST(DoglegOptimizer, Iterate) {
  // really non-linear factor graph
  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();

  // config far from minimum
  Point2 x0(3,0);
  Values config;
  config.insert(X(1), x0);

  double Delta = 1.0;
  for(size_t it=0; it<10; ++it) {
    GaussianBayesNet gbn = *fg.linearize(config)->eliminateSequential();
    // Iterate assumes that linear error = nonlinear error at the linearization point, and this should be true
    double nonlinearError = fg.error(config);
    double linearError = GaussianFactorGraph(gbn).error(config.zeroVectors());
    DOUBLES_EQUAL(nonlinearError, linearError, 1e-5);
//    cout << "it " << it << ", Delta = " << Delta << ", error = " << fg->error(*config) << endl;
    VectorValues dx_u = gbn.optimizeGradientSearch();
    VectorValues dx_n = gbn.optimize();
    DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, dx_u, dx_n, gbn, fg, config, fg.error(config));
    Delta = result.Delta;
    EXPECT(result.f_error < fg.error(config)); // Check that error decreases
    Values newConfig(config.retract(result.dx_d));
    config = newConfig;
    DOUBLES_EQUAL(fg.error(config), result.f_error, 1e-5); // Check that error is correctly filled in
  }
}

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


//------------------------------------------------------------------------------
// \file NarrowCast_test.cpp
//------------------------------------------------------------------------------

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

#include "Utilities/NarrowCast.h"

using Utilities::narrow_cast;

BOOST_AUTO_TEST_SUITE(Utilities)
BOOST_AUTO_TEST_SUITE(NarrowCast_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
// cf. Stroustrup. C++ Programming Language, 4th. Ed. pp. 299 Sec. 11.5
BOOST_AUTO_TEST_CASE(NarrowCastWorks)
{
  std::cout << "\n NarrowCastIsFun\n";

  const auto c1 = narrow_cast<char>(64);

  // Will throw if chars are unsigned.
  //BOOST_CHECK_THROW(narrow_cast<char>(-64), std::runtime_error);
  const auto c2 = narrow_cast<char>(-64); // Will throw if chars are unsigned

  //const auto c3 = narrow_cast<char>(264); // will throw if chars are 8-bit and signed
  BOOST_CHECK_THROW(narrow_cast<char>(264), std::runtime_error);

  const auto d1 = narrow_cast<double>(1/3.0F); // OK

  //const auto f1 = narrow_cast<float>(1/3.0); // will probably throw
  BOOST_CHECK_THROW(narrow_cast<float>(1/3.0), std::runtime_error);

  const auto c4 = narrow_cast<char>(42); // may throw
  const auto f2 = narrow_cast<float>(42.0); // may throw

  //const auto p1 = narrow_cast<char*>(42); // compile-time error, invalid static_cast
  //const auto i1 = narrow_cast<int>("chararray"); // compile-time error, invalid static_Cast

  const auto d2 = narrow_cast<double>(42); // may throw (but probably will not)
  const auto i2 = narrow_cast<int>(42.0); // may throw

  BOOST_TEST(true);
}

BOOST_AUTO_TEST_SUITE_END() // NarrowCast_tests
BOOST_AUTO_TEST_SUITE_END() // Utilities


//  (C) Copyright Eric Niebler 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

template<typename T>
void assert_is_double(T const &)
{
    BOOST_MPL_ASSERT((is_same<T, double>));
}

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
        accumulator_set<
        int
      , stats<
            tag::weighted_mean
          , tag::mean_of_weights
          , tag::weighted_mean_of_variates<int, tag::covariate1>
        >
      , int
    > acc, test_acc(sample = 0);

    acc(1, weight = 2, covariate1 = 3);
    BOOST_CHECK_CLOSE(1., weighted_mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(1u, count(acc));
    BOOST_CHECK_EQUAL(2, sum(acc));
    BOOST_CHECK_CLOSE(2., mean_of_weights(acc), 1e-5);
    BOOST_CHECK_CLOSE(3., (weighted_mean_of_variates<int, tag::covariate1>(acc)), 1e-5);

    acc(0, weight = 4, covariate1 = 4);
    BOOST_CHECK_CLOSE(1./3., weighted_mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(2u, count(acc));
    BOOST_CHECK_EQUAL(2, sum(acc));
    BOOST_CHECK_CLOSE(3., mean_of_weights(acc), 1e-5);
    BOOST_CHECK_CLOSE(11./3., (weighted_mean_of_variates<int, tag::covariate1>(acc)), 1e-5);

    acc(2, weight = 9, covariate1 = 8);
    BOOST_CHECK_CLOSE(4./3., weighted_mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(3u, count(acc));
    BOOST_CHECK_EQUAL(20, sum(acc));
    BOOST_CHECK_CLOSE(5., mean_of_weights(acc), 1e-5);
    BOOST_CHECK_CLOSE(94./15., (weighted_mean_of_variates<int, tag::covariate1>(acc)), 1e-5);

    assert_is_double(mean(acc));

    accumulator_set<
        int
      , stats<
            tag::weighted_mean(immediate)
          , tag::mean_of_weights(immediate)
          , tag::weighted_mean_of_variates<int, tag::covariate1>(immediate)
        >
      , int
    > acc2, test_acc2(sample = 0);

    acc2(1, weight = 2, covariate1 = 3);
    BOOST_CHECK_CLOSE(1., weighted_mean(acc2), 1e-5);
    BOOST_CHECK_EQUAL(1u, count(acc2));
    BOOST_CHECK_CLOSE(2., mean_of_weights(acc2), 1e-5);
    BOOST_CHECK_CLOSE(3., (weighted_mean_of_variates<int, tag::covariate1>(acc2)), 1e-5);

    acc2(0, weight = 4, covariate1 = 4);
    BOOST_CHECK_CLOSE(1./3., weighted_mean(acc2), 1e-5);
    BOOST_CHECK_EQUAL(2u, count(acc2));
    BOOST_CHECK_CLOSE(3., mean_of_weights(acc2), 1e-5);
    BOOST_CHECK_CLOSE(11./3., (weighted_mean_of_variates<int, tag::covariate1>(acc2)), 1e-5);

    acc2(2, weight = 9, covariate1 = 8);
    BOOST_CHECK_CLOSE(4./3., weighted_mean(acc2), 1e-5);
    BOOST_CHECK_EQUAL(3u, count(acc2));
    BOOST_CHECK_CLOSE(5., mean_of_weights(acc2), 1e-5);
    BOOST_CHECK_CLOSE(94./15., (mean_of_variates<int, tag::covariate1>(acc2)), 1e-5);

    assert_is_double(mean(acc2));
}

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

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

    return test;
}


#pragma once

#include <Eigen/Core>

#include <vector>
#include <array>

#include "Spirit_Defines.h"

// Dynamic Eigen typedefs
typedef Eigen::Matrix<scalar, -1,  1> VectorX;
typedef Eigen::Matrix<scalar,  1, -1> RowVectorX;
typedef Eigen::Matrix<scalar, -1, -1> MatrixX;

// 3D Eigen typedefs
typedef Eigen::Matrix<scalar, 3, 1> Vector3;
typedef Eigen::Matrix<scalar, 1, 3> RowVector3;
typedef Eigen::Matrix<scalar, 3, 3> Matrix3;

// Vectorfield and Scalarfield typedefs
#ifdef USE_CUDA
    #include "Managed_Allocator.hpp"
    typedef std::vector<int,             managed_allocator<int>>             intfield;
    typedef std::vector<scalar,          managed_allocator<scalar>>          scalarfield;
    typedef std::vector<Vector3,         managed_allocator<Vector3>>         vectorfield;
    struct Pair
    {
        // Basis indices of first and second atom of pair
        int i, j;
        // Translations of the basis cell of second atom of pair
        int translations[3];
    };
    struct Triplet
    {
        int i, j, k;
        int d_j[3], d_k[3];
        scalar n[3];
    };
    struct Quadruplet
    {
        int i, j, k, l;
        int d_j[3], d_k[3], d_l[3];
    };
    struct Neighbour : Pair
    {
        // Shell index
        int idx_shell;
    };
    typedef std::vector<Pair,       managed_allocator<Pair>>       pairfield;
    typedef std::vector<Triplet,    managed_allocator<Triplet>>    tripletfield;
    typedef std::vector<Quadruplet, managed_allocator<Quadruplet>> quadrupletfield;
    typedef std::vector<Neighbour,  managed_allocator<Neighbour>>  neighbourfield;
#else
    typedef std::vector<int>     intfield;
    typedef std::vector<scalar>  scalarfield;
    typedef std::vector<Vector3> vectorfield;
    struct Pair
    {
        int i, j;
        std::array<int,3> translations;
    };
    struct Triplet
    {
        int i, j, k;
        std::array<int,3> d_j, d_k;
        std::array<scalar, 3> n;
    };
    struct Quadruplet
    {
        int i, j, k, l;
        std::array<int,3> d_j, d_k, d_l;
    };
    struct Neighbour : Pair
    {
        // Shell index
        int idx_shell;
    };
    typedef std::vector<Pair>       pairfield;
    typedef std::vector<Triplet>    tripletfield;
    typedef std::vector<Quadruplet> quadrupletfield;
    typedef std::vector<Neighbour>  neighbourfield;

    // Definition for OpenMP reduction operation using Vector3's
    #pragma omp declare reduction (+: Vector3: omp_out=omp_out+omp_in)\
        initializer(omp_priv=Vector3::Zero())
#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// This file was modified by Oracle on 2015, 2016, 2017.
// Modifications copyright (c) 2015-2017, Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <boost/geometry.hpp>
#include <geometry_test_common.hpp>

namespace bg = boost::geometry;

//Testing geographic strategies
template <typename CT>
void test_geo_strategies()
{
    std::string poly = "POLYGON((52 0, 41 -74, -23 -43, -26 28, 52 0))";

    typedef bg::model::point<CT, 2, bg::cs::geographic<bg::degree> > pt_geo;

    bg::strategy::area::geographic<pt_geo> geographic_default;

    bg::strategy::area::geographic<pt_geo, bg::strategy::andoyer, 1>
        geographic_andoyer1;
    bg::strategy::area::geographic<pt_geo, bg::strategy::andoyer, 2>
        geographic_andoyer2;
    bg::strategy::area::geographic<pt_geo, bg::strategy::andoyer, 3>
        geographic_andoyer3;
    bg::strategy::area::geographic<pt_geo, bg::strategy::andoyer, 4>
        geographic_andoyer4;
    bg::strategy::area::geographic<pt_geo, bg::strategy::andoyer, 5>
        geographic_andoyer5;

    bg::strategy::area::geographic<pt_geo, bg::strategy::thomas, 1>
        geographic_thomas1;
    bg::strategy::area::geographic<pt_geo, bg::strategy::thomas, 2>
        geographic_thomas2;
    bg::strategy::area::geographic<pt_geo, bg::strategy::thomas, 3>
        geographic_thomas3;
    bg::strategy::area::geographic<pt_geo, bg::strategy::thomas, 4>
        geographic_thomas4;
    bg::strategy::area::geographic<pt_geo, bg::strategy::thomas, 5>
        geographic_thomas5;

    bg::strategy::area::geographic<pt_geo, bg::strategy::vincenty, 1>
        geographic_vincenty1;
    bg::strategy::area::geographic<pt_geo, bg::strategy::vincenty, 2>
        geographic_vincenty2;
    bg::strategy::area::geographic<pt_geo, bg::strategy::vincenty, 3>
        geographic_vincenty3;
    bg::strategy::area::geographic<pt_geo, bg::strategy::vincenty, 4>
        geographic_vincenty4;
    bg::strategy::area::geographic<pt_geo, bg::strategy::vincenty, 5>
        geographic_vincenty5;

    bg::strategy::area::geographic<pt_geo, bg::strategy::andoyer>
        geographic_andoyer_default;
    bg::strategy::area::geographic<pt_geo, bg::strategy::thomas>
        geographic_thomas_default;
    bg::strategy::area::geographic<pt_geo, bg::strategy::vincenty>
        geographic_vincenty_default;

    bg::model::polygon<pt_geo> geometry_geo;

    //GeographicLib         63316536351834.289
    //PostGIS (v2.2.2)      6.33946+13
    //MS SQL SERVER         632930207487035

    bg::read_wkt(poly, geometry_geo);
    CT area;
    CT err = 0.0000001;

    CT area_default = bg::area(geometry_geo);
    BOOST_CHECK_CLOSE(area_default, 63316309346280.18, err);
    area = bg::area(geometry_geo, geographic_default);
    BOOST_CHECK_CLOSE(area, 63316309346280.18, err);

    CT area_less_accurate = bg::area(geometry_geo, geographic_andoyer1);
    BOOST_CHECK_CLOSE(area, 63316309346280.18, err);
    area = bg::area(geometry_geo, geographic_andoyer2);
    BOOST_CHECK_CLOSE(area, 63316309224306.5, err);
    area = bg::area(geometry_geo, geographic_andoyer3);
    BOOST_CHECK_CLOSE(area, 63316309224411.195, err);
    area = bg::area(geometry_geo, geographic_andoyer4);
    BOOST_CHECK_CLOSE(area, 63316309224411.094, err);
    area = bg::area(geometry_geo, geographic_andoyer5);
    BOOST_CHECK_CLOSE(area, 63316309224411.094, err);

    area = bg::area(geometry_geo, geographic_thomas1);
    BOOST_CHECK_CLOSE(area, 63316536214315.32, err);
    area = bg::area(geometry_geo, geographic_thomas2);
    BOOST_CHECK_CLOSE(area, 63316536092341.266, err);
    area = bg::area(geometry_geo, geographic_thomas3);
    BOOST_CHECK_CLOSE(area, 63316536092445.961, err);
    area = bg::area(geometry_geo, geographic_thomas4);
    BOOST_CHECK_CLOSE(area, 63316536092445.859, err);
    area = bg::area(geometry_geo, geographic_thomas5);
    BOOST_CHECK_CLOSE(area, 63316536092445.859, err);

    area = bg::area(geometry_geo, geographic_vincenty1);
    BOOST_CHECK_CLOSE(area, 63316536473798.984, err);
    area = bg::area(geometry_geo, geographic_vincenty2);
    BOOST_CHECK_CLOSE(area, 63316536351824.93, err);
    area = bg::area(geometry_geo, geographic_vincenty3);
    BOOST_CHECK_CLOSE(area, 63316536351929.625, err);
    area = bg::area(geometry_geo, geographic_vincenty4);
    BOOST_CHECK_CLOSE(area, 63316536351929.523, err);
    CT area_most_accurate = bg::area(geometry_geo, geographic_vincenty5);
    BOOST_CHECK_CLOSE(area, 63316536351929.523, err);

    area = bg::area(geometry_geo, geographic_andoyer_default);
    BOOST_CHECK_CLOSE(area, 63316309346280.18, err);
    area = bg::area(geometry_geo, geographic_thomas_default);
    BOOST_CHECK_CLOSE(area, 63316536092341.266, err);
    area = bg::area(geometry_geo, geographic_vincenty_default);
    BOOST_CHECK_CLOSE(area, 63316536351929.523, err);

    BOOST_CHECK_CLOSE(area_most_accurate, area_less_accurate, .001);
    BOOST_CHECK_CLOSE(area_most_accurate, area_default, .001);
/*
    // timings and accuracy
    std::cout.precision(25);
    std::size_t exp_times = 100000;
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_andoyer1);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_andoyer2);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_andoyer3);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_andoyer4);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_andoyer5);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_thomas1);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_thomas2);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_thomas3);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_thomas4);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_thomas5);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_vincenty1);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_vincenty2);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_vincenty3);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_vincenty4);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
    {   clock_t startTime = clock();
        for (int j=0; j < exp_times; j++) area = bg::area(geometry_geo, geographic_vincenty5);
        std::cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " ";
        std::cout  << area << std::endl;}
*/
}

int test_main(int, char* [])
{

    test_geo_strategies<double>();

    return 0;
}


#include "plane_engaging.h"

#include <ctime>    // For time()
#include <cstdlib>  // For srand() and rand()
#include <Eigen/SVD>

#include <solvehfvc.h>
#include <RobotUtilities/utilities.h>
#include <RobotUtilities/TimerLinux.h>

using namespace RUT;

PlaneEngaging::PlaneEngaging(ForceControlHardware *robot,
        ForceControlController *controller) {
  robot_ = robot;
  controller_ = controller;
}

PlaneEngaging::~PlaneEngaging() {
}

bool PlaneEngaging::initialize(const std::string& file_name,
    const int main_loop_rate, ros::NodeHandle& root_nh) {
  main_loop_rate_ = main_loop_rate;
  folder_path_ = file_name;
  // parameters in file
  //  G, b_G
  ifstream fp;
  fp.open(file_name + "plane_engaging/para.txt");
  if (!fp) {
    cerr << "Unable to open parameter file.";
    return false;
  }

  int nRowG, nColG;

  fp >> nRowG >> nColG;
  G_.resize(nRowG, nColG);
  b_G_.resize(nRowG);
  for (int i = 0; i < nRowG; ++i)
    for (int j = 0; j < nColG; ++j)
      fp >> G_(i,j);

  for (int i = 0; i < nRowG; ++i) fp >> b_G_(i);

  fp.close();

  // double check
  cout << "G_: " << G_.format(MatlabFmt) << endl;
  cout << "b_G_: " << b_G_.format(MatlabFmt) << endl;

  // parameters in ROS server
  std::vector<double> scale_force_vector;
  root_nh.getParam("/constraint_estimation/scale_force_vector", scale_force_vector);
  if (!root_nh.hasParam("/constraint_estimation/scale_force_vector"))
    ROS_WARN_STREAM("Parameter [/constraint_estimation/scale_force_vector] not found!");

  force_scale_matrix_inv_ = Matrix6d::Zero();
  for (int i = 0; i < 6; ++i) force_scale_matrix_inv_(i,i) = 1.0/scale_force_vector[i];

  root_nh.param(string("/constraint_estimation/v_singular_value_threshold"),
      v_singular_value_threshold_, 0.1);
  root_nh.param(string("/constraint_estimation/f_singular_value_threshold"),
      f_singular_value_threshold_, 0.1);
  if (!root_nh.hasParam("/constraint_estimation/v_singular_value_threshold"))
      ROS_WARN_STREAM("Parameter "
      "[/constraint_estimation/v_singular_value_threshold] not found, "
      " using default: " << v_singular_value_threshold_);
  if (!root_nh.hasParam("/constraint_estimation/f_singular_value_threshold"))
      ROS_WARN_STREAM("Parameter "
      "[/constraint_estimation/f_singular_value_threshold] not found, "
      "using default: " << f_singular_value_threshold_);

  root_nh.param(string("/plane_engaging/Number_of_frames"),
      N_TRJ_, 1);
  if (!root_nh.hasParam("/plane_engaging/Number_of_frames"))
      ROS_WARN_STREAM("Parameter "
      "[/plane_engaging/Number_of_frames] not found, "
      " using default: " << N_TRJ_);

  return true;
}

bool PlaneEngaging::run() {
  MatrixXd f_data, v_data;
  controller_->reset();

  if (controller_->_f_queue.size() < 50) {
    cout << "Run update() for " << main_loop_rate_ << " frames:" << endl;
    // first, run update for 1s to populate the data deques
    ros::Rate pub_rate(main_loop_rate_);
    ros::Duration period(EGM_PERIOD);
    for (int i = 0; i < main_loop_rate_; ++i) {
      ros::Time time_now = ros::Time::now();
      bool b_is_safe = controller_->update(time_now, period);
      if(!b_is_safe) break;
      pub_rate.sleep();
    }
    cout << "Done." << endl;
  }

  Timer timer;
  std::srand(std::time(0));
  for (int fr = 0; fr < N_TRJ_; ++fr) {
    timer.tic();
    /* Estimate Natural Constraints from pool of data */
    // get the weighted data
    // debug
    f_data = MatrixXd::Zero(6, controller_->_f_queue.size());
    v_data = MatrixXd::Zero(6, controller_->_v_queue.size());
    for (int i = 0; i < controller_->_f_queue.size(); ++i)
      f_data.col(i) = controller_->_f_queue[i] * controller_->_f_weights[i];
    for (int i = 0; i < controller_->_v_queue.size(); ++i)
      v_data.col(i) = controller_->_v_queue[i] * controller_->_v_weights[i];

    // SVD on velocity data
    Eigen::JacobiSVD<MatrixXd> svd_v(v_data.transpose(), Eigen::ComputeThinV);
    VectorXd sigma_v = svd_v.singularValues();
    int DimV = 0;
    for (int i = 0; i < 6; ++i)
      if (sigma_v(i) > v_singular_value_threshold_) DimV ++;

    // get a basis for row space of velocity data
    MatrixXd rowspace_v = svd_v.matrixV().leftCols(DimV);

    // filter out force data that:
    //    1. has a small weight
    std::vector<int> f_id;
    for (int i = 0; i < f_data.cols(); ++i) {
      double length = f_data.col(i).norm();
      if (length > 1.5) { // weighted length in newton
        f_id.push_back(i);
      }
    }

    int f_data_length = f_id.size();
    MatrixXd f_data_filtered = MatrixXd::Zero(6, f_data_length);
    for (int i = 0; i < f_data_length; ++i)
      f_data_filtered.col(i) = f_data.col(f_id[i]);

    int DimF = 0;
    MatrixXd Nf;
    MatrixXd f_data_selected;
    if (f_data_length > 5) {
      // SVD to filtered force data
      Eigen::JacobiSVD<MatrixXd> svd_f(f_data_filtered.transpose(), Eigen::ComputeThinV);
      VectorXd sigma_f = svd_f.singularValues();
      // check dimensions, estimate natural constraints
      double threshold = max(f_singular_value_threshold_, 0.1*sigma_f(0));
      for (int i = 0; i < 6; ++i)
        if (sigma_f(i) > threshold) DimF ++;

      if (DimF > 3) {
        cout << "DimF: " << DimF << endl;
        cout << "Press Enter to continue" << endl;
        getchar();
      }
      // MatrixXd N = SVD_V_f.block<6, DimF>(0, 0).transpose();
      // Sample DimF force directions
      MatrixXd f_data_normalized = f_data_filtered;
      for (int i = 0; i < f_data_length; ++i)
        f_data_normalized.col(i).normalize();

      int kNFSamples = (int)pow(double(f_data_length), 0.7);
      f_data_selected = MatrixXd(6, DimF);
      if (DimF == 1) {
        f_data_selected = f_data_filtered.rowwise().mean();
        f_data_selected.normalize();
      } else {
        MatrixXd f_data_selected_new(6, DimF);
        double f_distance = 0;
        assert(f_data_length < 32767);
        for (int i = 0; i < kNFSamples; ++i) {
          // 1. sample
          for (int s = 0; s < DimF; s++) {
            int r = (rand() % f_data_length);
            f_data_selected_new.col(s) = f_data_normalized.col(r);
          }
          // 2. compute distance
          double f_distance_new = 0;
          for (int ii = 0; ii < DimF-1; ++ii)
            for (int jj = ii+1; jj < DimF; ++jj)
              f_distance_new += (f_data_selected_new.col(ii) -
                  f_data_selected_new.col(jj)).norm();
          // 3. Update data
          if (f_distance_new > f_distance) {
            f_data_selected = f_data_selected_new;
            f_distance = f_distance_new;
          }
        } // end sampling
      }

      // unscale
      Nf = f_data_selected.transpose() * force_scale_matrix_inv_;
    } else {
      DimF = 0;
      Nf = MatrixXd(0, 6);
      f_data_selected = MatrixXd(6, 0);
    }

    /* Do Hybrid Servoing */
    HFVC action;
    int kDimActualized      = 6;
    int kDimUnActualized    = 0;
    int kDimSlidingFriction = 0;
    int kNumSeeds           = 3;
    int kDimLambda          = DimF;
    int kPrintLevel         = 1;

    VectorXd F = VectorXd::Zero(6);
    MatrixXd Aeq(0, DimF+6); // dummy
    VectorXd beq(0); // dummy
    MatrixXd A = MatrixXd::Zero(DimF, DimF + 6);
    VectorXd b_A = VectorXd::Zero(DimF);
    A.leftCols(DimF) = -MatrixXd::Identity(DimF,DimF);
    double kLambdaMin = 10.0;
    for (int i = 0; i < DimF; ++i) b_A(i) = -kLambdaMin;

    solvehfvc(Nf, G_, b_G_, F, Aeq, beq, A, b_A,
      kDimActualized, kDimUnActualized,
      kDimSlidingFriction, kDimLambda,
      kNumSeeds, kPrintLevel,
      &action);

    double computation_time_ms = timer.toc();
    /*  Execute the hybrid action */
    Vector6d v_Tr = Vector6d::Zero();
    for (int i = 0; i < action.n_av; ++i)  v_Tr(i+action.n_af) = action.w_av(i);

    double pose_fb[7];
    robot_->getPose(pose_fb);
    Matrix4d SE3_WT_fb = posemm2SE3(pose_fb);
    Matrix6d Adj_WT = SE32Adj(SE3_WT_fb);
    Matrix6d R_a = action.R_a;
    Matrix6d R_a_inv = R_a.inverse();
    Vector6d v_T = R_a_inv*v_Tr;

    const double dt = 0.1; // s
    const double kVMax = 0.002; // m/s,  maximum speed limit
    const double scale_rot_to_tran = 0.5; // 1m = 2rad
    Vector6d v_T_scaled = v_T;
    v_T_scaled.tail(3) *= scale_rot_to_tran;
    if (v_T_scaled.norm() > kVMax) {
      double scale_safe = kVMax/v_T_scaled.norm();
      v_T *= scale_safe;
    }

    Vector6d v_W = Adj_WT*v_T;
    Matrix4d SE3_WT_command;
    SE3_WT_command = SE3_WT_fb + wedge6(v_W)*SE3_WT_fb*dt;
    double pose_set[7];
    SE32Posemm(SE3_WT_command, pose_set);

    Vector6d force_Tr_set = Vector6d::Zero();
    for (int i = 0; i < action.n_af; ++i)  force_Tr_set[i] = action.eta_af(i);
    Vector6d force_T = R_a_inv*force_Tr_set;
    Matrix6d Adj_TW = SE32Adj(SE3Inv(SE3_WT_fb));
    Vector6d force_W = Adj_TW.transpose() * force_T;

    printf("V in world: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", v_W[0], v_W[1],
        v_W[2],v_W[3],v_W[4],v_W[5]);
    printf("F in world: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", force_W[0],
        force_W[1], force_W[2],force_W[3],force_W[4],force_W[5]);
    cout << "Computation Time: " << computation_time_ms << endl << endl;

    if (fr == N_TRJ_-1) {
      // print to file
      ofstream fp;
      // f_queue
      fp.open(folder_path_ + "plane_engaging/f_queue.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < controller_->_f_queue.size(); ++i) {
        stream_array_in(fp, controller_->_f_queue[i].data(), 6);
        fp << endl;
      }
      fp.close();
      // v_queue
      fp.open(folder_path_ + "plane_engaging/v_queue.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < controller_->_v_queue.size(); ++i) {
        stream_array_in(fp, controller_->_v_queue[i].data(), 6);
        fp << endl;
      }
      fp.close();
      // f_weights
      fp.open(folder_path_ + "plane_engaging/f_weights.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < controller_->_f_weights.size(); ++i)
        fp << controller_->_f_weights[i] << endl;
      fp.close();

      // v_weights
      fp.open(folder_path_ + "plane_engaging/v_weights.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < controller_->_v_weights.size(); ++i)
        fp << controller_->_v_weights[i] << endl;
      fp.close();

      // f_data_filtered
      fp.open(folder_path_ + "plane_engaging/f_data_filtered.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << f_data_filtered.transpose() << endl;
      fp.close();
      // f_data_selected
      fp.open(folder_path_ + "plane_engaging/f_data_selected.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << f_data_selected.transpose() << endl;
      fp.close();
      // others
      fp.open(folder_path_ + "plane_engaging/process.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << DimV << endl << DimF << endl;
      stream_array_in(fp, v_T.data(), 6);
      fp << endl;
      stream_array_in(fp, force_T.data(), 6);
      fp.close();
    }

    if (std::isnan(force_Tr_set[0])) {
      cout << "================== NaN =====================" << endl;
      cout << "Press Enter to continue.." << endl;
      getchar();
    }

    cout << "motion begins:" << endl;
    controller_->ExecuteHFVC(action.n_af, action.n_av,
        action.R_a, pose_set, force_Tr_set.data(),
        HS_CONTINUOUS, main_loop_rate_);

  } // end for
}


//
// Created by fjh on 17-10-31.
//

#include <caffe/caffe.hpp>
#include <net.h>
#include <boost/shared_ptr.hpp>
#include <loader/loader.h>
#include <commons/commons.h>
#include <math/gemm.h>
#include <opencv2/core.hpp>
#include <opencv2/shape.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#define num_channels_ 3
#define input_geometry_ cv::Size(640,360)
mdl::Loader* load_net;
cv::Mat mean_;
void init(){
    mdl::Gemmer::gemmers.push_back(new mdl::Gemmer);
    load_net=mdl::Loader::shared_instance();
    load_net->load("tool/model.min.json","tool/data.min.bin");
}
void WrapInputLayer(std::vector<cv::Mat>* input_channels,Mtype*in_data) {

    int width = 640;
    int height = 360;
    float* input_data = in_data;
    for (int i = 0; i < 3; ++i) {
        cv::Mat channel(height, width, CV_32FC1, input_data);
        input_channels->push_back(channel);
        input_data += width * height;
    }
}
void Preprocess(const cv::Mat& img,
                std::vector<cv::Mat>* input_channels,Mtype*in_data) {
    /* Convert the input image to the input image format of the network. */
    cv::Mat sample;
    if (img.channels() == 3 && num_channels_ == 1)
        cv::cvtColor(img, sample, cv::COLOR_BGR2GRAY);
    else if (img.channels() == 4 && num_channels_ == 1)
        cv::cvtColor(img, sample, cv::COLOR_BGRA2GRAY);
    else if (img.channels() == 4 && num_channels_ == 3)
        cv::cvtColor(img, sample, cv::COLOR_BGRA2BGR);
    else if (img.channels() == 1 && num_channels_ == 3)
        cv::cvtColor(img, sample, cv::COLOR_GRAY2BGR);
    else
        sample = img;

    cv::Mat sample_resized;
    if (sample.size() != input_geometry_)
        cv::resize(sample, sample_resized, input_geometry_);
    else
        sample_resized = sample;

    cv::Mat sample_float;
    if (num_channels_ == 3)
        sample_resized.convertTo(sample_float, CV_32FC3);
    else
        sample_resized.convertTo(sample_float, CV_32FC1);

     cv::Mat sample_normalized;
     cv::subtract(sample_float, mean_, sample_normalized);

    /* This operation will write the separate BGR planes directly to the
     * input layer of the network because it is wrapped by the cv::Mat
     * objects in input_channels. */
    cv::split(sample_normalized, *input_channels);

    CHECK(reinterpret_cast<float*>(input_channels->at(0).data)
          == in_data)
    << "Input channels are not wrapping the input layer of the network.";
}
void SetMean(const string& mean_value) {
    cv::Scalar channel_mean;

    if (!mean_value.empty()) {
        stringstream ss(mean_value);
        vector<float> values;
        string item;
        while (getline(ss, item, ',')) {
            float value = std::atof(item.c_str());
            values.push_back(value);
        }
        CHECK(values.size() == 1 || values.size() == num_channels_) <<
                                                                    "Specify either 1 mean_value or as many as channels: " << num_channels_;

        std::vector<cv::Mat> channels;
        for (int i = 0; i < num_channels_; ++i) {
            /* Extract an individual channel. */
            cv::Mat channel(input_geometry_.height, input_geometry_.width, CV_32FC1,
                            cv::Scalar(values[i]));
            channels.push_back(channel);
        }
        cv::merge(channels, mean_);
    }
    else{
        LOG(INFO)<<"there is no mean_value been set!";

    }
}
void caffe_WrapInputLayer(std::vector<cv::Mat>* input_channels,caffe::Blob<float>*input_layer) {

    int width = input_layer->width();
    int height = input_layer->height();
    float* input_data = input_layer->mutable_cpu_data();
    for (int i = 0; i < input_layer->channels(); ++i) {
        cv::Mat channel(height, width, CV_32FC1, input_data);
        input_channels->push_back(channel);
        input_data += width * height;
    }
}

void caffe_Preprocess(const cv::Mat& img,
                            std::vector<cv::Mat>* input_channels,caffe::Blob<float>*input_layer) {
    /* Convert the input image to the input image format of the network. */
    cv::Mat sample;
    if (img.channels() == 3 && num_channels_ == 1)
        cv::cvtColor(img, sample, cv::COLOR_BGR2GRAY);
    else if (img.channels() == 4 && num_channels_ == 1)
        cv::cvtColor(img, sample, cv::COLOR_BGRA2GRAY);
    else if (img.channels() == 4 && num_channels_ == 3)
        cv::cvtColor(img, sample, cv::COLOR_BGRA2BGR);
    else if (img.channels() == 1 && num_channels_ == 3)
        cv::cvtColor(img, sample, cv::COLOR_GRAY2BGR);
    else
        sample = img;

    cv::Mat sample_resized;
    if (sample.size() != input_geometry_)
        cv::resize(sample, sample_resized, input_geometry_);
    else
        sample_resized = sample;

    cv::Mat sample_float;
    if (num_channels_ == 3)
        sample_resized.convertTo(sample_float, CV_32FC3);
    else
        sample_resized.convertTo(sample_float, CV_32FC1);

    cv::Mat sample_normalized;
    cv::subtract(sample_float, mean_, sample_normalized);

    /* This operation will write the separate BGR planes directly to the
     * input layer of the network because it is wrapped by the cv::Mat
     * objects in input_channels. */
    cv::split(sample_normalized, *input_channels);

    CHECK(reinterpret_cast<float*>(input_channels->at(0).data)
          == input_layer->cpu_data())
    << "Input channels are not wrapping the input layer of the network.";
}
int main(){

    boost::shared_ptr<caffe::Net<float>> net_;
    net_.reset(new caffe::Net<float>("/home/fjh/CLionProjects/mobile-deep-learning/cmake-build-debug/tools/build/E23.prototxt",caffe::TEST));
    net_->CopyTrainedLayersFrom("/home/fjh/CLionProjects/mobile-deep-learning/cmake-build-debug/tools/build/E23.caffemodel");
    cv::Mat img=cv::imread("/home/fjh/123.jpg");
    cv::Mat img2;
    SetMean("104,117,123");
    cv::resize(img,img2,cv::Size(640,360));
    float *img_data=new float[640*360*3];
    std::vector<cv::Mat> input_channels;
    caffe_WrapInputLayer(&input_channels,net_->input_blobs()[0]);
    caffe_Preprocess(img2,&input_channels,net_->input_blobs()[0]);
    net_->Forward();
    Mtype *img_data2=new Mtype[640*360*3];
    std::vector<cv::Mat> input_channels2;
    WrapInputLayer(&input_channels2,img_data2);
    Preprocess(img2,&input_channels2,img_data2);

    int thread_num = 1;
    if (mdl::Gemmer::gemmers.size() == 0) {

        mdl::Gemmer::gemmers.push_back(new mdl::Gemmer());

    }
    mdl::Loader *loader = mdl::Loader::shared_instance();
    std::string prefix("/home/fjh/CLionProjects/mobile-deep-learning/cmake-build-debug/tools/build/");
    auto t1 = mdl::time();
    bool load_success = loader->load(prefix + "model.min.json", prefix + "data.min.bin");
    auto t2 = mdl::time();
    cout << "load time : " << mdl::time_diff(t1, t2) << "ms" << endl;
    if (!load_success) {
        cout << "load failure" << endl;
        loader->clear();
        return -1;
    }
    if (!loader->get_loaded()) {
        LOG(FATAL)<<"loader is not loaded yet";
    }
    mdl::Net *net = new mdl::Net(loader->_model);
    net->set_thread_num(thread_num);
    int count = 1;
    double total = 0;
    vector<Mtype > result;
    for (int i = 0; i < count; i++) {
        Time t1 = mdl::time();
/*
        for(int j=0;j<net->_layers.size()-1;j++) {
            if(net_->has_blob(net->_layers[j]->name())) {
               // std::cout<<net->_layers[j]->name()<<std::endl;
                for (int k = 0; k < net->_layers[j]->input().size(); k++) {
                    Mtype *input_data=new Mtype[net->_layers[j]->input()[k]->count(0)];

                    if(net_->has_blob(net->_layers[j]->input()[k]->get_name())) {
                        std::cout<<net->_layers[j]->input()[k]->get_name()<<"  ";
                        for (int l = 0; l < net->_layers[j]->input()[k]->count(0); l++) {
                            input_data[l] = (Mtype) *(
                                    net_->blob_by_name(net->_layers[j]->input()[k]->get_name())->cpu_data() + l);
                        }
                        net->_layers[j]->input()[k]->set_data(input_data);
                    }
                  //  std::cout<<std::endl;
                }
                net->_layers[j]->forward();
                for (int k = 0; k < net->_layers[j]->output().size(); k++) {
                    int error_perlayer=0;
                    for(int l=0;l<net->_layers[j]->output()[k]->count(0);l++){
                        if(abs(abs(*(net->_layers[j]->output()[k]->get_data()+l)-*(net_->blob_by_name(net->_layers[j]->name())->cpu_data()+l))/(*(net->_layers[j]->output()[k]->get_data()+l)))>0.01){
                            // std::cout<<*(net->_layers[j]->output()[k]->get_data()+l)<<"    "<<*(net_->blob_by_name(net->_layers[j]->name())->cpu_data()+l)<<std::endl;
                            //std::cout<<abs(*(net->_layers[j]->output()[k]->get_data()+l)-*(net_->blob_by_name(net->_layers[j]->name())->cpu_data()+l))/(*(net->_layers[j]->output()[k]->get_data()+l))<<std::endl;
                            error_perlayer++;
                        }
                        //std::cout<<*(net->_layers[j]->output()[k]->get_data()+l)<<"    "<<*(net_->blob_by_name(net->_layers[j]->name())->cpu_data()+l)<<std::endl;
                    }
                   // std::cout<<net->_layers[j]->output()[0]->descript_dimention()<<std::endl;
                    //std::cout<<net_->blob_by_name(net->_layers[j]->name())->shape_string()<<std::endl;
                    std::cout<<net->_layers[j]->name()<<"::error_ratio:    "<<(error_perlayer*1.0)/net->_layers[j]->output()[k]->count(0)<<std::endl;
                    //net->_layers[j]->input()[k]->set_data(input_data);
                }
            }
            for(int k=0;k<net->_layers[j]->input()[0]->count();k++){

            }
        }
        net->_layers[net->_layers.size()-1]->forward();
        */
/*
        Mtype *input_data=new Mtype[loader->_matrices["mbox_loc"]->count(0)];
        std::cout<<net_->has_blob("mbox_loc")<<std::endl;

        for (int k = 0; k < loader->_matrices["mbox_loc"]->count(0); k++) {
            input_data[k] = net_->bottom_vecs()[net_->layers().size()-1][0]->cpu_data()[k];
            //std::cout<<input_data[k]<<std::endl;
        }
        Mtype *input_data2=new Mtype[loader->_matrices["mbox_conf_flatten"]->count(0)];
        for (int k = 0; k < loader->_matrices["mbox_conf_flatten"]->count(0); k++) {
            input_data2[k] = net_->bottom_vecs()[net_->layers().size()-1][1]->cpu_data()[k];
        }
        Mtype *input_data3=new Mtype[loader->_matrices["mbox_priorbox"]->count(0)];
        for (int k = 0; k < loader->_matrices["mbox_priorbox"]->count(0); k++) {
            input_data3[k] = net_->bottom_vecs()[net_->layers().size()-1][2]->cpu_data()[k];
        }
        loader->_matrices["mbox_loc"]->set_data(input_data);
        loader->_matrices["mbox_conf_flatten"]->set_data(input_data2);
        loader->_matrices["mbox_priorbox"]->set_data(input_data3);
        net->_layers[net->_layers.size()-1]->forward();
        */
        net->predict(img_data2);
        Time t2 = mdl::time();
        double diff = mdl::time_diff(t1, t2);
        total += diff;
    }
    cout << "total cost: " << total / count << "ms." << endl;
    for (int num: result) {
        cout << num << " ";
    }
    cout <<endl;
    // uncomment while testing clacissification models
//    cout << "the max prob index = "<<find_max(result)<<endl;
    cout << "Done!" << endl;
//    cout << "it " << (is_correct_result(result) ? "is" : "isn't") << " a correct result." << endl;
    int num_weight=0;

    for(int i=0;i<net_->blob_names().size();i++){
        mdl::Matrix *layer_data=loader->_matrices[net_->blob_names()[i]];
            if (layer_data!= nullptr) {
                int error_perlayer=0;

                for(int j=0;j<layer_data->count(0);j++){
                    if(abs(abs(*(layer_data->get_data()+j)-*(net_->blob_by_name(net_->blob_names()[i])->cpu_data()+j))/(*(layer_data->get_data()+j)))>0.01){
                       // std::cout<<*(layer_data->get_data()+j)<<"    "<<*(net_->blob_by_name(net_->blob_names()[i])->cpu_data()+j)<<std::endl;
                        error_perlayer++;
                    }
                    //std::cout<<*(layer_data->get_data()+j)<<"    "<<*(net_->blob_by_name(net_->blob_names()[i])->cpu_data()+j)<<std::endl;
                }
                std::cout<<layer_data->get_name()<<"::error_ratio:    "<<(error_perlayer*1.0)/layer_data->count(0)<<std::endl;
              //  std::cout<<layer_data->descript_dimention()<<"     "<<net_->blob_by_name(net_->blob_names()[i])->shape_string()<<std::endl;
            }

    }

    Mtype* get_data=net->_layers[net->_layers.size()-1]->output()[0]->get_data();
    for(int i=0;i<net->_layers[net->_layers.size()-1]->output()[0]->dimension(2);i++) {
        float score = get_data[2];
        if (score >= 0.7) {
            int xmin = static_cast<int>(get_data[3] * img.cols);
            int ymin = static_cast<int>(get_data[4] * img.rows);
            int xmax = static_cast<int>(get_data[5] * img.cols);
            int ymax = static_cast<int>(get_data[6] * img.rows);
            cv::rectangle(img, cv::Rect(xmin, ymin, (xmax - xmin), (ymax - ymin)), cv::Scalar(255, 0, 0), 2);
        }
        get_data+=7;
    }
    cv::imshow("a",img);
    cv::waitKey(0);
    /*
    for(int i=0;i<net->_layers.size();i++){
        if(net->_layers[i]->layer_type()==mdl::LayerType::CONVOLUTION){
            int error_perlayer=0;
            for(int j=0;j<net->_layers[i]->_weight[0]->count(0);j++){
                if(abs(abs(*(net_->params()[num_weight].get()->cpu_data()+j)-*(net->_layers[i]->_weight[0]->get_data()+j))/(*(net_->params()[num_weight].get()->cpu_data()+j)))>0.01){
                    error_perlayer++;
                }
              //  std::cout<<*(net_->params()[num_weight].get()->cpu_data()+j)<<"    "<<*(net->_layers[i]->_weight[0]->get_data()+j)<<std::endl;
            }
            std::cout<<net->_layers[i]->name()<<"::error_ratio:    "<<(error_perlayer*1.0)/net->_layers[i]->_weight[0]->count(0)<<std::endl;
            num_weight+=2;
            if(num_weight==20)num_weight+=1;
        }
    }
*/

    loader->clear();
    delete net;

    return 1;
}


/**
 * @file banditpam.cpp
 * @date 2021-07-25
 *
 * This file contains the primary C++ implementation of the BanditPAM code.
 *
 */
#include "banditpam.hpp"

#include <carma>
#include <armadillo>
#include <unordered_map>
#include <regex>

/**
 * \brief Runs BanditPAM algorithm.
 *
 * Run the BanditPAM algorithm to identify a dataset's medoids.
 *
 * @param input_data Input data to find the medoids of
 */
void BanditPAM::fit_bpam(const arma::mat& input_data) {
  data = input_data;
  data = arma::trans(data);
  arma::mat medoids_mat(data.n_rows, n_medoids);
  arma::rowvec medoid_indices(n_medoids);
  // runs build step
  BanditPAM::build(data, medoid_indices, medoids_mat);
  steps = 0;

  medoid_indices_build = medoid_indices;
  arma::rowvec assignments(data.n_cols);
  // runs swap step
  BanditPAM::swap(data, medoid_indices, medoids_mat, assignments);
  medoid_indices_final = medoid_indices;
  labels = assignments;
}

/**
 * \brief Build step for BanditPAM
 *
 * Runs build step for the BanditPAM algorithm. Draws batch sizes with replacement
 * from reference set, and uses the estimated reward of the potential medoid
 * solutions on the reference set to update the reward confidence intervals and
 * accordingly narrow the solution set.
 *
 * @param data Transposed input data to find the medoids of
 * @param medoid_indices Uninitialized array of medoids that is modified in place
 * as medoids are identified
 * @param medoids Matrix of possible medoids that is updated as the bandit
 * learns which datapoints will be unlikely to be good candidates
 */
void BanditPAM::build(
  const arma::mat& data,
  arma::rowvec& medoid_indices,
  arma::mat& medoids) {
    // Parameters
    size_t N = data.n_cols;
    arma::rowvec N_mat(N);
    N_mat.fill(N);
    size_t p = (buildConfidence * N); // reciprocal of
    bool use_absolute = true;
    arma::rowvec estimates(N, arma::fill::zeros);
    arma::rowvec best_distances(N);
    best_distances.fill(std::numeric_limits<double>::infinity());
    arma::rowvec sigma(N); // standard deviation of induced losses on reference points
    arma::urowvec candidates(
      N,
      arma::fill::ones); // one hot encoding of candidates -- points not filtered out yet
    arma::rowvec lcbs(N);
    arma::rowvec ucbs(N);
    arma::rowvec T_samples(N, arma::fill::zeros); // number of times calculating induced loss for reference point
    arma::rowvec exact_mask(N, arma::fill::zeros); // computed the loss exactly for this datapoint

    for (size_t k = 0; k < n_medoids; k++) {
        // instantiate medoids one-by-online
        size_t step_count = 0;
        candidates.fill(1);
        T_samples.fill(0);
        exact_mask.fill(0);
        estimates.fill(0);
        sigma = build_sigma(
                data, best_distances, batchSize, use_absolute); // computes std dev amongst batch of reference points

        while (arma::sum(candidates) > precision) { // while some candidates exist
            arma::umat compute_exactly =
              ((T_samples + batchSize) >= N_mat) != exact_mask;
            if (arma::accu(compute_exactly) > 0) {
                arma::uvec targets = find(compute_exactly);
                logHelper.comp_exact_build.push_back(targets.n_rows);
                arma::rowvec result =
                  build_target(data, targets, N, best_distances, use_absolute); // induced loss for these targets over all reference points
                estimates.cols(targets) = result;
                ucbs.cols(targets) = result;
                lcbs.cols(targets) = result;
                exact_mask.cols(targets).fill(1);
                T_samples.cols(targets) += N;
                candidates.cols(targets).fill(0);
            }
            if (arma::sum(candidates) < precision) {
                break;
            }
            arma::uvec targets = arma::find(candidates);
            arma::rowvec result = build_target(
              data, targets, batchSize, best_distances, use_absolute); // induced loss for the targets (sample)
            estimates.cols(targets) =
              ((T_samples.cols(targets) % estimates.cols(targets)) +
               (result * batchSize)) /
              (batchSize + T_samples.cols(targets)); // update the running average
            T_samples.cols(targets) += batchSize;
            arma::rowvec adjust(targets.n_rows);
            adjust.fill(p);
            adjust = arma::log(adjust);
            arma::rowvec cb_delta =
              sigma.cols(targets) %
              arma::sqrt(adjust / T_samples.cols(targets));
            ucbs.cols(targets) = estimates.cols(targets) + cb_delta;
            lcbs.cols(targets) = estimates.cols(targets) - cb_delta;
            candidates = (lcbs < ucbs.min()) && (exact_mask == 0);
            step_count++;
        }

        medoid_indices.at(k) = lcbs.index_min();
        medoids.unsafe_col(k) = data.unsafe_col(medoid_indices(k));

        // don't need to do this on final iteration
        for (size_t i = 0; i < N; i++) {
            double cost = (this->*lossFn)(data, i, medoid_indices(k));
            if (cost < best_distances(i)) {
                best_distances(i) = cost;
            }
        }
        use_absolute = false; // use difference of loss for sigma and sampling,
                              // not absolute
        logHelper.loss_build.push_back(arma::mean(arma::mean(best_distances)));
        logHelper.p_build.push_back(static_cast<float>(1)/static_cast<float>(p));
    }
}

/**
 * \brief Estimates the mean reward for each arm in build step
 *
 * Estimates the mean reward (or loss) for each arm in the identified targets
 * in the build step and returns a list of the estimated reward.
 *
 * @param data Transposed input data to find the medoids of
 * @param target Set of target datapoints to be estimated
 * @param batch_size Number of datapoints sampled for updating confidence
 * intervals
 * @param best_distances Array of best distances from each point to previous set
 * of medoids
 * @param use_absolute Determines whether the absolute cost is added to the total
 */
arma::rowvec BanditPAM::build_target(
  const arma::mat& data,
  arma::uvec& target,
  size_t batch_size,
  arma::rowvec& best_distances,
  bool use_absolute) {
    size_t N = data.n_cols;
    arma::rowvec estimates(target.n_rows, arma::fill::zeros);
    arma::uvec tmp_refs = arma::randperm(N,
                                   batch_size); // without replacement, requires
                                                // updated version of armadillo
#pragma omp parallel for
    for (size_t i = 0; i < target.n_rows; i++) {
        double total = 0;
        for (size_t j = 0; j < tmp_refs.n_rows; j++) {
            double cost =
              (this->*lossFn)(data, tmp_refs(j), target(i));
            if (use_absolute) {
                total += cost;
            } else {
                total += cost < best_distances(tmp_refs(j))
                           ? cost
                           : best_distances(tmp_refs(j));
                total -= best_distances(tmp_refs(j));
            }
        }
        estimates(i) = total / batch_size;
    }
    return estimates;
}

/**
 * \brief Swap step for BanditPAM
 *
 * Runs Swap step for the BanditPAM algorithm. Draws batch sizes with replacement
 * from reference set, and uses the estimated reward of the potential medoid
 * solutions on the reference set to update the reward confidence intervals and
 * accordingly narrow the solution set.
 *
 * @param data Transposed input data to find the medoids of
 * @param medoid_indices Array of medoid indices created from the build step
 * that is modified in place as better medoids are identified
 * @param medoids Matrix of possible medoids that is updated as the bandit
 * learns which datapoints will be unlikely to be good candidates
 * @param assignments Uninitialized array of indices corresponding to each
 * datapoint assigned the index of the medoid it is closest to
 */
void BanditPAM::swap(
  const arma::mat& data,
  arma::rowvec& medoid_indices,
  arma::mat& medoids,
  arma::rowvec& assignments) {
    size_t N = data.n_cols;
    size_t p = (N * n_medoids * swapConfidence); // reciprocal

    arma::mat sigma(n_medoids, N, arma::fill::zeros);

    arma::rowvec best_distances(N);
    arma::rowvec second_distances(N);
    size_t iter = 0;
    bool swap_performed = true;
    arma::umat candidates(n_medoids, N, arma::fill::ones);
    arma::umat exact_mask(n_medoids, N, arma::fill::zeros);
    arma::mat estimates(n_medoids, N, arma::fill::zeros);
    arma::mat lcbs(n_medoids, N);
    arma::mat ucbs(n_medoids, N);
    arma::umat T_samples(n_medoids, N, arma::fill::zeros);

    // continue making swaps while loss is decreasing
    while (swap_performed && iter < max_iter) {
        iter++;

        // calculate quantities needed for swap, best_distances and sigma
        calc_best_distances_swap(
          data, medoid_indices, best_distances, second_distances, assignments);

        sigma = swap_sigma(data,
                           batchSize,
                           best_distances,
                           second_distances,
                           assignments);

        candidates.fill(1);
        exact_mask.fill(0);
        estimates.fill(0);
        T_samples.fill(0);

        // while there is at least one candidate (double comparison issues)
        while (arma::accu(candidates) > 0.5) {
            calc_best_distances_swap(
              data, medoid_indices, best_distances, second_distances, assignments);

            // compute exactly if it's been samples more than N times and hasn't
            // been computed exactly already
            arma::umat compute_exactly =
              ((T_samples + batchSize) >= N) != (exact_mask);
            arma::uvec targets = arma::find(compute_exactly);

            if (targets.size() > 0) {
                logHelper.comp_exact_swap.push_back(targets.size());
                arma::vec result = swap_target(data,
                                               medoid_indices,
                                               targets,
                                               N,
                                               best_distances,
                                               second_distances,
                                               assignments);
                estimates.elem(targets) = result;
                ucbs.elem(targets) = result;
                lcbs.elem(targets) = result;
                exact_mask.elem(targets).fill(1);
                T_samples.elem(targets) += N;

                candidates = (lcbs < ucbs.min()) && (exact_mask == 0);
            }
            if (arma::accu(candidates) < precision) {
                break;
            }
            targets = arma::find(candidates);
            arma::vec result = swap_target(data,
                                           medoid_indices,
                                           targets,
                                           batchSize,
                                           best_distances,
                                           second_distances,
                                           assignments);
            estimates.elem(targets) =
              ((T_samples.elem(targets) % estimates.elem(targets)) +
               (result * batchSize)) /
              (batchSize + T_samples.elem(targets));
            T_samples.elem(targets) += batchSize;
            arma::vec adjust(targets.n_rows);
            adjust.fill(p);
            adjust = arma::log(adjust);
            arma::vec cb_delta = sigma.elem(targets) %
                                 arma::sqrt(adjust / T_samples.elem(targets));

            ucbs.elem(targets) = estimates.elem(targets) + cb_delta;
            lcbs.elem(targets) = estimates.elem(targets) - cb_delta;
            candidates = (lcbs < ucbs.min()) && (exact_mask == 0);
            targets = arma::find(candidates);
        }
        // now switch medoids
        arma::uword new_medoid = lcbs.index_min();
        // extract medoid of swap
        size_t k = new_medoid % medoids.n_cols;

        // extract data point of swap
        size_t n = new_medoid / medoids.n_cols;
        swap_performed = medoid_indices(k) != n;
        steps++;

        medoid_indices(k) = n;
        medoids.col(k) = data.col(medoid_indices(k));
        calc_best_distances_swap(
          data, medoid_indices, best_distances, second_distances, assignments);
        sigma_log(sigma);
        logHelper.loss_swap.push_back(arma::mean(arma::mean(best_distances)));
        logHelper.p_swap.push_back(static_cast<float>(1)/static_cast<float>(p));
    }
}

/**
 * \brief Estimates the mean reward for each arm in swap step
 *
 * Estimates the mean reward (or loss) for each arm in the identified targets
 * in the swap step and returns a list of the estimated reward.
 *
 * @param data Transposed input data to find the medoids of
 * @param sigma Dispersion paramater for each datapoint
 * @param targets Set of target datapoints to be estimated
 * @param batch_size Number of datapoints sampled for updating confidence
 * intervals
 * @param best_distances Array of best distances from each point to previous set
 * of medoids
 * @param second_best_distances Array of second smallest distances from each
 * point to previous set of medoids
 * @param assignments Assignments of datapoints to their closest medoid
 */
arma::vec BanditPAM::swap_target(
  const arma::mat& data,
  arma::rowvec& medoid_indices,
  arma::uvec& targets,
  size_t batch_size,
  arma::rowvec& best_distances,
  arma::rowvec& second_best_distances,
  arma::rowvec& assignments) {
    size_t N = data.n_cols;
    arma::vec estimates(targets.n_rows, arma::fill::zeros);
    arma::uvec tmp_refs = arma::randperm(N,
                                   batch_size); // without replacement, requires
                                                // updated version of armadillo

// for each considered swap
#pragma omp parallel for
    for (size_t i = 0; i < targets.n_rows; i++) {
        double total = 0;
        // extract data point of swap
        size_t n = targets(i) / medoid_indices.n_cols;
        size_t k = targets(i) % medoid_indices.n_cols;
        // calculate total loss for some subset of the data
        for (size_t j = 0; j < batch_size; j++) {
            double cost = (this->*lossFn)(data, n, tmp_refs(j));
            if (k == assignments(tmp_refs(j))) {
                if (cost < second_best_distances(tmp_refs(j))) {
                    total += cost;
                } else {
                    total += second_best_distances(tmp_refs(j));
                }
            } else {
                if (cost < best_distances(tmp_refs(j))) {
                    total += cost;
                } else {
                    total += best_distances(tmp_refs(j));
                }
            }
            total -= best_distances(tmp_refs(j));
        }
        estimates(i) = total / tmp_refs.n_rows;
    }
    return estimates;
}


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

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>

namespace nt2
{
  namespace tag
  {
    /*!
      @brief Log_2 generic tag

      Represents the Log_2 constant in generic contexts.

      @par Models:
      Hierarchy
    **/
    BOOST_SIMD_CONSTANT_REGISTER( Log_2, double
                                , 0, 0x3f317218UL
                                , 0x3fe62e42fefa39efULL
                                )
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::Log_2, Site> dispatching_Log_2(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::Log_2, Site>();
    }
    template<class... Args>
    struct impl_Log_2;
  }
 /*!
    Generates constant Log_2. (\f$\log(2)\f$)

    @par Semantic:

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

    is similar to:

    @code
      r =  T(0.6931471805599453094172321214581765680755001343602553);
    @endcode

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Log_2, Log_2);
}

#endif


#include <iostream>
#include <sferes/phen/parameters.hpp>
#include <sferes/gen/evo_float.hpp>
#include <sferes/ea/nsga2.hpp>
#include <sferes/eval/eval.hpp>
#include <sferes/stat/pareto_front.hpp>
#include <sferes/modif/dummy.hpp>
#include <sferes/run.hpp>
#include <boost/program_options.hpp>

#include <cuda_runtime.h>
using namespace sferes;
using namespace sferes::gen::evo_float;

struct Params
{
  struct evo_float
  {

	SFERES_CONST float cross_rate = 0.1f;
	SFERES_CONST float mutation_rate = 0.1f;
    SFERES_CONST float eta_m = 15.0f;
    SFERES_CONST float eta_c = 10.0f;
    SFERES_CONST mutation_t mutation_type = polynomial;
    SFERES_CONST cross_over_t cross_over_type = sbx;
  };
  struct pop
  {
    SFERES_CONST unsigned size = 300;
    SFERES_CONST unsigned nb_gen = 500;
    SFERES_CONST int dump_period = 50;
    SFERES_CONST int initial_aleat = 1;
  };
  struct parameters
  {
    SFERES_CONST float min = 0.0f;
    SFERES_CONST float max = 1.0f;
  };
};

template<typename Indiv>
float _g(const Indiv &ind)
{
  float g = 0.0f;
  assert(ind.size() == 30);
  for (size_t i = 1; i < 30; ++i)
    g += ind.data(i);
  g = 9.0f * g / 29.0f;
  g += 1.0f;
  return g;
}

SFERES_FITNESS(FitZDT2, sferes::fit::Fitness)
{
 public:
  FitZDT2()  {}
  template<typename Indiv>
    void eval(Indiv& ind)
  {
    this->_objs.resize(2);
    float f1 = ind.data(0);
    float g = _g(ind);
    float h = 1.0f - pow((f1 / g), 2.0);
    float f2 = g * h;
    this->_objs[0] = -f1;
    this->_objs[1] = -f2;
  }
};

int main(int argc, char **argv)
{
  std::cout<<"running "<<argv[0]<<" ... try --help for options (verbose)"<<std::endl;

  typedef gen::EvoFloat<30, Params> gen_t;
  typedef phen::Parameters<gen_t, FitZDT2<Params>, Params> phen_t;
  typedef eval::Eval<Params> eval_t;
  typedef boost::fusion::vector<stat::ParetoFront<phen_t, Params> >  stat_t;
  typedef modif::Dummy<> modifier_t;
  typedef ea::Nsga2<phen_t, eval_t, stat_t, modifier_t, Params> ea_t;
  ea_t ea;

  run_ea(argc, argv, ea);

  return 0;
}


#pragma once

#include <memory>
#include <Eigen/Dense>
#include <vector>
#include "math/fields/ScalarField.h"

class VoxelGrid2D {
public:
    /**
     * Initialize from edge soup using extremely fast winding number
     * @param edges
     * @param grid_spacing
     * @param max_resolution
     */
    VoxelGrid2D(const std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d>> &edges, double grid_spacing, int max_resolution=std::numeric_limits<int>::max());

    /**
     * Initialize from 2D points
     * @param points
     * @param grid_spacing
     * @param max_resolution
     */
    VoxelGrid2D(const Eigen::Ref<const Eigen::MatrixX2d> &points, double grid_spacing, int max_resolution=std::numeric_limits<int>::max());

    /**
     * Initialize directly from data (x,y) -> data[x + y * x_res]
     * @param data data in row-major order
     * @param width number of columns
     * @param height number of rows (width * height must equal the number of elements in data)
     * @param min_x x coordinate of element 0
     * @param min_y y coordinate of element 0
     * @param spacing spacing between grid elements
     */
    VoxelGrid2D(std::vector<double> data, int width, int height, double min_x, double min_y, double spacing);
    double query(const Eigen::Ref<const Eigen::Vector2d> &pt) const;
    double query(int row, int col) const;
    /**
     * Compute marching squares contours from grid data. Contour is counter-clockwise encircling regions greater than the threshold.
     * @param hierarchy Map from indices of returned contours to lists of interior contours, if any. If there are N contours,
     * the index N gives the list of outer contours not contained by any other contours.
     * @param threshold value at which to extract an isosurface from the voxel grid.
     * @return
     */
    std::vector<std::vector<Eigen::Vector2d>> marching_squares(std::vector<std::vector<int>> &hierarchy, double threshold=NAN, bool bisect=true) const;

    /**
     * Initialize from a scalar field
     * @param field 2D scalar field
     * See data constructor for other argument descriptions
     */
    VoxelGrid2D(ScalarField<2>::Handle field, double min_x, double min_y, double max_x, double max_y, double spacing, int max_resolution=std::numeric_limits<int>::max());

    /**
     * Compute marching squares contours from grid data.
     * @param threshold value at which to extract an isosurface from the voxel grid.
     * @return
     */
    std::vector<std::vector<Eigen::Vector2d>> marching_squares(double threshold=NAN, bool bisect=true) const;
    Eigen::Vector2i resolution() const;
    void dilate(double threshold, bool erode, const Eigen::Ref<const Eigen::MatrixXi> &kernel=Eigen::Matrix3i::Ones(), int centerRow=1, int centerCol=1);
    void discretize(double threshold);
private:
    /**
     * Sets the resolution and grid spacing based on a maximum resolution and the current dimensions
     * @param grid_spacing
     * @param max_resolution
     * @param minPt
     * @param maxPt
     * @param clear
     */
    void set_resolution(double grid_spacing, int max_resolution, const Eigen::Vector2d &minPt, const Eigen::Vector2d &maxPt, bool clear);
    std::vector<std::vector<Eigen::Vector2d>> marching_squares(std::vector<std::vector<int>> &hierarchy, bool compute_hierarchy, double threshold, bool bisect=true) const;
    std::vector<double> data_;
    Eigen::Vector2d minPt_;
    Eigen::Array2i res_; //x, y
    double spacing_;
    double min_value_;
    ScalarField<2>::Handle field_;
};


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_HALFEPS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_HALFEPS_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/constant/register.hpp>
#include <boost/simd/sdk/constant/constant.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_halfeps Halfeps
 *
 * \par Description
 * Constant Halfeps
 * \arg 1 for integer types
 * \arg \f$= \2^{-53}\f$ for double
 * \arg \f$= \2^{-24}\f$ for float
 * \par
 * The value of this constant is type dependant. This means that for different
 * types it does not represent the same mathematical number.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/halfeps.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::halfeps_(A0)>::type
 *     Halfeps();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Halfeps
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag Halfeps of functor Halfeps
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/
    BOOST_SIMD_CONSTANT_REGISTER( Halfeps, double, 1
                                , 0x33800000, 0x3CA0000000000000ULL
                                );
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Halfeps, Halfeps)
} }

#include <boost/simd/sdk/constant/common.hpp>

#endif


/*

Copyright (c) 2005-2021, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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 Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef CHASTEELLIPSOID_HPP_
#define CHASTEELLIPSOID_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include "AbstractChasteRegion.hpp"
#include "ChastePoint.hpp"

/**
 * This class defines a 3D ellipsoid and provides a method to check
 * if a given point is contained in the volume.
 */
template <unsigned SPACE_DIM>
class ChasteEllipsoid : public AbstractChasteRegion<SPACE_DIM>
{
    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the member variables.
     *
     * @param archive
     * @param version
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractChasteRegion<SPACE_DIM> >(*this);
    }

private:
    /** Centre of the ellipsoid. */
    ChastePoint<SPACE_DIM> mCentre;

    /** Radii of the ellipsoid. */
    ChastePoint<SPACE_DIM> mRadii;

public:
    /**
     * The (axis aligned) ellipsoid is defined by its centre and its radii in the x, y and z directions.
     *
     * @param rCentre Centre of the ellipsoid.
     * @param rRadii Radii of the ellipsoid.
     */
    ChasteEllipsoid(ChastePoint<SPACE_DIM>& rCentre, ChastePoint<SPACE_DIM>& rRadii);

    /**
     * @return true if a given point is contained in the ellipsoid.
     *
     * @param rPointToCheck Point to be checked to be contained in the ellipsoid.
     */
    bool DoesContain(const ChastePoint<SPACE_DIM>& rPointToCheck) const;

    /** @return centre of the ellipsoid */
    const ChastePoint<SPACE_DIM>& rGetCentre() const;

    /** @return radii of the ellipsoid */
    const ChastePoint<SPACE_DIM>& rGetRadii() const;
};

// Declare identifier for the serializer
#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(ChasteEllipsoid)

namespace boost
{
namespace serialization
{

template<class Archive, unsigned SPACE_DIM>
inline void save_construct_data(
    Archive & ar, const ChasteEllipsoid<SPACE_DIM> * t, const unsigned int file_version)
{
    const ChastePoint<SPACE_DIM>* p_centre =  &(t->rGetCentre());
    const ChastePoint<SPACE_DIM>* p_radii =  &(t->rGetRadii());
    ar & p_centre;
    ar & p_radii;
}

/**
 * Allow us to not need a default constructor, by specifying how Boost should
 * instantiate an instance (using existing constructor)
 */
template<class Archive, unsigned SPACE_DIM>
inline void load_construct_data(
    Archive & ar, ChasteEllipsoid<SPACE_DIM> * t, const unsigned int file_version)
{
    ChastePoint<SPACE_DIM>* p_centre;
    ChastePoint<SPACE_DIM>* p_radii;

    ar & p_centre;
    ar & p_radii;

    ::new(t)ChasteEllipsoid<SPACE_DIM>((*p_centre), (*p_radii));

    delete p_centre;
    delete p_radii;
}
}
} // namespace ...

#endif /*CHASTEELLIPSOID_HPP_*/


/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <cmath>

#include <string>
#include <sstream>
#include <random>

#include <boost/math/constants/constants.hpp>

// definitions for Tomographer test framework -- this must be included before any
// <Eigen/...> or <tomographer/...> header
#include "test_tomographer.h"

#include <tomographer/mathtools/check_derivatives.h>
#include <tomographer/tools/eigenutil.h>

// -----------------------------------------------------------------------------
// fixture(s)

template<typename EigenPointType_, typename EigenDerivativesType_, int XDims, int ValDims>
struct check_derivatives_fixture
{
  static constexpr int xdims = XDims;
  static constexpr int valdims = ValDims;

  typedef EigenPointType_ EigenPointType;
  typedef EigenDerivativesType_ EigenDerivativesType;

  typedef typename EigenPointType::Scalar XScalar;
  typedef typename EigenDerivativesType::Scalar ValScalar;

  check_derivatives_fixture() { }
  ~check_derivatives_fixture() { }

  inline EigenPointType random_point(int seed) const
  {
    std::mt19937 rng((std::mt19937::result_type)seed);
    std::uniform_real_distribution<XScalar> dist(0.1, 2.0);
    return Tomographer::Tools::denseRandom<EigenPointType>(rng, dist, xdims);
  }

  //
  // Function: f_j(\vec x) =  \sum (1+i+2*j) * x_i^(1+(i%2))
  //
  static void fn(Eigen::Ref<Eigen::Matrix<ValScalar, Eigen::Dynamic, 1> > vals,
                 const Eigen::Ref<const EigenPointType> & x)
  {
    vals = Eigen::Matrix<ValScalar, Eigen::Dynamic, 1>::Zero(valdims);
    for (int j = 0; j < valdims; ++j) {
      for (int i = 0; i < xdims; ++i) {
        vals(j) += (1+i+2*j) * std::pow(x(i), 1+(i%2));
      }
    }
  }

  void derivative_at(Eigen::Ref<EigenDerivativesType> derivatives,
                     const Eigen::Ref<const EigenPointType> & x)
  {
    for (int j = 0; j < valdims; ++j) {
      for (int i = 0; i < xdims; ++i) {
        derivatives(j,i) = (float)( (1+i+2*j) * (1+(i%2)) * std::pow(x(i),i%2) );
      }
    }
  }

};

template<typename EigenPointType_, typename EigenDerivativesType_, int XDims, int ValDims>
constexpr int check_derivatives_fixture<EigenPointType_,EigenDerivativesType_, XDims, ValDims>::xdims;
template<typename EigenPointType_, typename EigenDerivativesType_, int XDims, int ValDims>
constexpr int check_derivatives_fixture<EigenPointType_,EigenDerivativesType_, XDims, ValDims>::valdims;

// -----------------------------------------------------------------------------
// test suites

typedef check_derivatives_fixture<Eigen::VectorXd, Eigen::ArrayXXd, 4, 1> Fixture1val;
typedef check_derivatives_fixture<Eigen::VectorXd, Eigen::ArrayXXf, 4, 1> Fixture1val_d_f;
typedef check_derivatives_fixture<Eigen::VectorXd, Eigen::ArrayXXd, 10, 6> FixtureSeveralVals;

BOOST_AUTO_TEST_SUITE(test_mathtools_check_derivatives)

BOOST_FIXTURE_TEST_CASE(one_val, Fixture1val)
{
  BOOST_MESSAGE("enter test; xdims="<<xdims<<", valdims="<<valdims) ;

  EigenPointType x(random_point(0));
  EigenDerivativesType der(EigenDerivativesType::Zero(valdims,xdims));

  derivative_at(der, x);

  BOOST_MESSAGE("Derivatives = \n" << der) ;

  BOOST_MESSAGE("test correct ...") ;
  {
    std::stringstream stream;
    bool ok = Tomographer::MathTools::check_derivatives(der, x, fn, valdims, 1e-6, 1e-4, stream);
    BOOST_MESSAGE(stream.str()) ;
    BOOST_CHECK(ok) ;
  }

  der(0,2) = der(0,2)*2 + 1.0;

  BOOST_MESSAGE("test wrong ...") ;
  {
    std::stringstream stream;
    bool ok = Tomographer::MathTools::check_derivatives(der, x, fn, valdims, 1e-6, 1e-4, stream);
    BOOST_MESSAGE(stream.str()) ;
    BOOST_CHECK( !ok ) ;
  }
}

BOOST_FIXTURE_TEST_CASE(one_val_d_f, Fixture1val_d_f)
{
  EigenPointType x(random_point(90876));
  EigenDerivativesType der(EigenDerivativesType::Zero(valdims, xdims));

  derivative_at(der, x);

  {
    std::stringstream stream;
    bool ok = Tomographer::MathTools::check_derivatives(der, x, fn, valdims, 1e-4, 1e-2f, stream);
    BOOST_MESSAGE(stream.str()) ;
    BOOST_CHECK(ok) ;
  }

  der(0,2) = der(0,2)*2 + 1.f;

  {
    std::stringstream stream;
    bool ok = Tomographer::MathTools::check_derivatives(der, x, fn, valdims, 1e-4, 1e-2f, stream);
    BOOST_MESSAGE(stream.str()) ;
    BOOST_CHECK( !ok ) ;
  }
}

BOOST_FIXTURE_TEST_CASE(several_vals, FixtureSeveralVals)
{
  EigenPointType x(random_point(151));
  EigenDerivativesType der(EigenDerivativesType::Zero(valdims, xdims));

  derivative_at(der, x);

  {
    std::stringstream stream;
    bool ok = Tomographer::MathTools::check_derivatives(der, x, fn, valdims, 1e-6, 1e-4, stream);
    BOOST_MESSAGE(stream.str()) ;
    BOOST_CHECK(ok) ;
  }

  der(0,2) = der(0,2)*2 + 1.0;

  {
    std::stringstream stream;
    bool ok = Tomographer::MathTools::check_derivatives(der, x, fn, valdims, 1e-6, 1e-4, stream);
    BOOST_MESSAGE(stream.str()) ;
    BOOST_CHECK( !ok ) ;
  }
}

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

BOOST_FIXTURE_TEST_CASE(checks_for_nan, Fixture1val)
{
  BOOST_MESSAGE("enter test; xdims="<<xdims<<", valdims="<<valdims) ;

  EigenPointType x(random_point(3242));
  EigenDerivativesType der(EigenDerivativesType::Zero(valdims,xdims));

  derivative_at(der, x);

  der(0,2) = std::numeric_limits<double>::quiet_NaN();

  BOOST_MESSAGE("Derivatives = \n" << der) ;

  BOOST_MESSAGE("check that check_derivatives() complains for NaN ...") ;
  {
    EigenAssertTest::setting_scope mysettingvar(true);// eigen_assert() should throw an exception.
    std::stringstream stream;
    auto call_test = [&]() { Tomographer::MathTools::check_derivatives(der, x, fn, valdims, 1e-6, 1e-4, stream); };
    BOOST_CHECK_THROW(call_test(), Tomographer::Tools::EigenAssertException);
    BOOST_MESSAGE(stream.str()) ;
  }
}

BOOST_AUTO_TEST_SUITE_END()