// catch
#include "catch.hpp"

// mapnik-vector-tile
#include "vector_tile_strategy.hpp"
#include "vector_tile_processor.hpp"
#include "vector_tile_geometry_decoder.hpp"

// mapnik
#include <mapnik/version.hpp>
#if MAPNIK_VERSION >= 300100
#include <mapnik/geometry/is_simple.hpp>
#include <mapnik/geometry/is_valid.hpp>
#else
#include <mapnik/geometry_is_simple.hpp>
#include <mapnik/geometry_is_valid.hpp>
#endif
#include <mapnik/json/geometry_parser.hpp>
#include <mapnik/save_map.hpp>
#include <mapnik/util/file_io.hpp>
#include <mapnik/util/fs.hpp>
#include <mapnik/util/geometry_to_geojson.hpp>

// boost
#include <boost/filesystem/operations.hpp>

// test utils
#include "test_utils.hpp"
#include "geometry_equal.hpp"

// std
#include <cmath>
#include <fstream>
#include <sstream>

// protozero
#include <protozero/pbf_reader.hpp>

void clip_geometry(mapnik::Map const& map,
                   std::string const& file,
                   mapnik::box2d<double> const& bbox,
                   int simplify_distance,
                   bool strictly_simple,
                   mapnik::vector_tile_impl::polygon_fill_type fill_type,
                   bool mpu,
                   bool process_all)
{
    unsigned tile_size = 4096;
    int buffer_size = 0;
    std::string geojson_string;

    mapnik::vector_tile_impl::processor ren(map);
    // TODO - test these booleans https://github.com/mapbox/mapnik-vector-tile/issues/165
    ren.set_strictly_simple(strictly_simple);
    ren.set_simplify_distance(simplify_distance);
    ren.set_fill_type(fill_type);
    ren.set_process_all_rings(process_all);
    ren.set_multi_polygon_union(mpu);

    mapnik::vector_tile_impl::tile out_tile = ren.create_tile(bbox, tile_size, buffer_size);
    mapnik::geometry::geometry<double> geom4326_pbf;

    std::string buffer;
    out_tile.serialize_to_string(buffer);
    if (!buffer.empty())
    {
        protozero::pbf_reader tile_reader(buffer);
        if(!tile_reader.next(mapnik::vector_tile_impl::Tile_Encoding::LAYERS))
        {
            throw std::runtime_error("tile buffer contains no layer!");
        }
        protozero::pbf_reader layer_reader = tile_reader.get_message();
        if (layer_reader.next(mapnik::vector_tile_impl::Layer_Encoding::FEATURES))
        {
            protozero::pbf_reader feature_reader = layer_reader.get_message();
            int32_t geometry_type = mapnik::vector_tile_impl::Geometry_Type::UNKNOWN;
            mapnik::vector_tile_impl::GeometryPBF::pbf_itr geom_itr;
            while (feature_reader.next())
            {
                if (feature_reader.tag() == mapnik::vector_tile_impl::Feature_Encoding::GEOMETRY)
                {
                    geom_itr = feature_reader.get_packed_uint32();
                }
                else if (feature_reader.tag() == mapnik::vector_tile_impl::Feature_Encoding::TYPE)
                {
                    geometry_type = feature_reader.get_enum();
                }
                else
                {
                    feature_reader.skip();
                }
            }
            double sx = static_cast<double>(tile_size) / bbox.width();
            double sy = static_cast<double>(tile_size) / bbox.height();
            double i_x = bbox.minx();
            double i_y = bbox.maxy();

            mapnik::vector_tile_impl::GeometryPBF geoms2_pbf(geom_itr);
            geom4326_pbf = mapnik::vector_tile_impl::decode_geometry<double>(geoms2_pbf, geometry_type, 2, i_x, i_y, 1.0 * sx, -1.0 * sy);

            std::string reason;
            std::string is_valid = "false";
            std::string is_simple = "false";
            if (mapnik::geometry::is_valid(geom4326_pbf, reason))
            {
                is_valid = "true";
            }
            if (mapnik::geometry::is_simple(geom4326_pbf))
            {
                is_simple = "true";
            }
            unsigned int n_err = 0;
            mapnik::util::to_geojson(geojson_string,geom4326_pbf);

            geojson_string = geojson_string.substr(0, geojson_string.size()-1);
            geojson_string += ",\"properties\":{\"is_valid\":"+is_valid+", \"is_simple\":"+is_simple+", \"message\":\""+reason+"\"}}";
        }
        else
        {
            geojson_string = "{\"type\": \"Point\", \"coordinates\":[], \"properties\":{\"message\":\"Tile layer had no features\"}}";
        }
    }
    else
    {
        geojson_string = "{\"type\": \"Point\", \"coordinates\":[], \"properties\":{\"message\":\"Tile had no layers\"}}";
    }

    std::string fixture_name = mapnik::util::basename(file);
    fixture_name = fixture_name.substr(0, fixture_name.size()-5);
    if (!mapnik::util::exists("./test/geometry-test-data/output"))
    {
        boost::filesystem::create_directory(("./test/geometry-test-data/output"));
    }
    if (!mapnik::util::exists("./test/geometry-test-data/output/"+fixture_name))
    {
        boost::filesystem::create_directory(("./test/geometry-test-data/output/"+fixture_name));
    }

    std::stringstream file_stream;
    file_stream << "./test/geometry-test-data/output/" << fixture_name << "/"
        << bbox.minx() << ","
        << bbox.miny() << ","
        << bbox.maxx()<< ","
        << bbox.maxy() << ","
        << "simplify_distance=" << simplify_distance << ","
        << "strictly_simple=" << strictly_simple << ","
        << "fill_type=" << fill_type << ","
        << "mpu=" << mpu << ","
        << "par=" << process_all << ".geojson";
    std::string file_path = file_stream.str();
    if (!mapnik::util::exists(file_path) || (std::getenv("UPDATE") != nullptr))
    {
        std::ofstream out(file_path);
        out << geojson_string;
    }
    else
    {
        mapnik::util::file input(file_path);
        if (!input.is_open())
        {
            throw std::runtime_error("failed to open test geojson");
        }
        std::string expected_string(input.data().get(), input.size());
        mapnik::geometry::geometry<double> geom_expected;
        if (mapnik::json::from_geojson(expected_string, geom_expected))
        {
            assert_g_equal(geom_expected, geom4326_pbf);
        }
    }
}

mapnik::box2d<double> middle_fifty(mapnik::box2d<double> const& bbox)
{
    double width = std::fabs(bbox.maxx() - bbox.minx());
    double height = std::fabs(bbox.maxy() - bbox.miny());
    mapnik::box2d<double> new_bbox(
        bbox.minx() + width*0.25,
        bbox.miny() + height*0.25,
        bbox.maxx() - width*0.25,
        bbox.maxy() - height*0.25
    );
    return new_bbox;
}

mapnik::box2d<double> top_left(mapnik::box2d<double> const& bbox)
{
    double width = std::fabs(bbox.maxx() - bbox.minx());
    double height = std::fabs(bbox.maxy() - bbox.miny());
    mapnik::box2d<double> new_bbox(
        bbox.minx(),
        bbox.miny(),
        bbox.maxx() - width*0.5,
        bbox.maxy() - height*0.5
    );
    return new_bbox;
}

mapnik::box2d<double> top_right(mapnik::box2d<double> const& bbox)
{
    double width = std::fabs(bbox.maxx() - bbox.minx());
    double height = std::fabs(bbox.maxy() - bbox.miny());
    mapnik::box2d<double> new_bbox(
        bbox.minx() + width*0.5,
        bbox.miny(),
        bbox.maxx(),
        bbox.maxy() - height*0.5
    );
    return new_bbox;
}

mapnik::box2d<double> bottom_left(mapnik::box2d<double> const& bbox)
{
    double width = std::fabs(bbox.maxx() - bbox.minx());
    double height = std::fabs(bbox.maxy() - bbox.miny());
    mapnik::box2d<double> new_bbox(
        bbox.minx(),
        bbox.miny() + height*0.5,
        bbox.maxx() + width*0.5,
        bbox.maxy()
    );
    return new_bbox;
}

mapnik::box2d<double> bottom_right(mapnik::box2d<double> const& bbox)
{
    double width = std::fabs(bbox.maxx() - bbox.minx());
    double height = std::fabs(bbox.maxy() - bbox.miny());
    mapnik::box2d<double> new_bbox(
        bbox.minx() + width*0.5,
        bbox.miny() + height*0.5,
        bbox.maxx(),
        bbox.maxy()
    );
    return new_bbox;
}

mapnik::box2d<double> zoomed_out(mapnik::box2d<double> const& bbox)
{
    double width = std::fabs(bbox.maxx() - bbox.minx());
    double height = std::fabs(bbox.maxy() - bbox.miny());
    mapnik::box2d<double> new_bbox(
        bbox.minx() - width,
        bbox.miny() - height,
        bbox.maxx() + width,
        bbox.maxy() + height
    );
    return new_bbox;
}

TEST_CASE("geometries visual tests")
{
    std::vector<std::string> geometries = mapnik::util::list_directory("./test/geometry-test-data/input");
    std::vector<std::string> benchmarks = mapnik::util::list_directory("./test/geometry-test-data/benchmark");
    if (std::getenv("BENCHMARK") != nullptr)
    {
        geometries.insert(geometries.end(), benchmarks.begin(), benchmarks.end());
    }
    for (std::string const& file: geometries)
    {
        mapnik::datasource_ptr ds = testing::build_geojson_fs_ds(file);
        mapnik::box2d<double> bbox = ds->envelope();
        mapnik::Map map(256, 256, "+init=epsg:4326");
        mapnik::layer lyr("layer","+init=epsg:4326");
        lyr.set_datasource(ds);
        map.add_layer(lyr);
        for (int simplification_distance : std::vector<int>({0, 4, 8}))
        {
            for (bool strictly_simple : std::vector<bool>({false, true}))
            {
                std::vector<mapnik::vector_tile_impl::polygon_fill_type> types;
                types.emplace_back(mapnik::vector_tile_impl::even_odd_fill);
                types.emplace_back(mapnik::vector_tile_impl::non_zero_fill);
                types.emplace_back(mapnik::vector_tile_impl::positive_fill);
                types.emplace_back(mapnik::vector_tile_impl::negative_fill);
                for (auto const& type : types)
                {
                    for (bool mpu : std::vector<bool>({false, true}))
                    {
                        for (bool process_all : std::vector<bool>({false, true}))
                        {
                            clip_geometry(map, file, bbox, simplification_distance, strictly_simple, type, mpu, process_all);
                            clip_geometry(map, file, middle_fifty(bbox), simplification_distance, strictly_simple, type, mpu, process_all);
                            clip_geometry(map, file, top_left(bbox), simplification_distance, strictly_simple, type, mpu, process_all);
                            clip_geometry(map, file, top_right(bbox), simplification_distance, strictly_simple, type, mpu, process_all);
                            clip_geometry(map, file, bottom_left(bbox), simplification_distance, strictly_simple, type, mpu, process_all);
                            clip_geometry(map, file, bottom_right(bbox), simplification_distance, strictly_simple, type, mpu, process_all);
                            clip_geometry(map, file, zoomed_out(bbox), simplification_distance, strictly_simple, type, mpu, process_all);
                        }
                    }
                }
            }
        }
    }
}


/*  _______________________________________________________________________

    DAKOTA: Design Analysis Kit for Optimization and Terascale Applications
    Copyright 2014 Sandia Corporation.
    This software is distributed under the GNU Lesser General Public License.
    For more information, see the README file in the top Dakota directory.
    _______________________________________________________________________ */

//- Class:        SurfpackApproximation
//- Description:  Class implementation of Surfpack response surface
//-
//- Owner:        Brian Adams

#include <stdexcept>
#include <typeinfo>

#include "SurfpackApproximation.hpp"
#include "SharedSurfpackApproxData.hpp"
#include "ProblemDescDB.hpp"
#include "DakotaVariables.hpp"
#include "dakota_data_io.hpp"

// Headers from Surfpack
#include "SurfData.h"
// for Hessian data
#include "SurfpackMatrix.h"
#include "ModelFactory.h"
#include "ModelFitness.h"
#include "surfaces/SurfpackModel.h"
#include "SurfpackInterface.h"

#include <algorithm>
#include <boost/math/special_functions/round.hpp>

namespace Dakota {

using surfpack::toString;
using surfpack::fromVec;

/** Initialize the embedded Surfpack surface object and configure it
    using the specifications from the input file.  Data for the
    surface is created later. */
SurfpackApproximation::
SurfpackApproximation(const ProblemDescDB& problem_db,
		      const SharedApproxData& shared_data,
                      const String& approx_label):
  Approximation(BaseConstructor(), problem_db, shared_data, approx_label), //surface(NULL),
  surfData(NULL), model(NULL), factory(NULL)
  //sharedDataRep((SharedSurfpackApproxData*)shared_data.data_rep())
{
  SharedSurfpackApproxData* shared_surf_data_rep
    = (SharedSurfpackApproxData*)sharedDataRep;

  ParamMap args;

  args["verbosity"] = toString<short>(sharedDataRep->outputLevel);
  args["ndims"] = toString<size_t>(sharedDataRep->numVars);

  // For now, not exposing Surfpack random seed in the DAKOTA UI;
  // instead fixing at an arbitrary value (treated as int in Surfpack)
  args["seed"] = "8147";

  // For Polynomial surface fits
  if (sharedDataRep->approxType == "global_polynomial") {
    args["type"] = "polynomial";
    args["order"] = toString<unsigned short>(shared_surf_data_rep->approxOrder);
    // TO DO: activate derivative-based regression
  }

  // For Kriging surface fits
  else if (sharedDataRep->approxType == "global_kriging") {

    args["type"]  = "kriging";
    args["order"] = toString<unsigned short>(shared_surf_data_rep->approxOrder);
    args["reduced_polynomial"] =
      (problem_db.get_string("model.surrogate.trend_order") == "quadratic") ?
      toString<bool>(false) : toString<bool>(true);

    // activate derivative information if available
    unsigned short surfpack_derivative_order = 0;
    short bdo = sharedDataRep->buildDataOrder;
    if (bdo & 2)
      surfpack_derivative_order = 1;
    if (bdo & 4) {
      if (bdo & 2)
	surfpack_derivative_order = 2;
      else {
	Cerr << "\nError (global_kriging): Hessian information only used "
	     << "if gradients present.\nbuildDataOrder = " << bdo << std::endl;
	abort_handler(-1);
      }
    }
    args["derivative_order"] =
      toString<unsigned short>(surfpack_derivative_order);

    // optimization options are none | sample | local | global (default)
    args["optimization_method"] = "global";
    const String& optimization_method =
      problem_db.get_string("model.surrogate.kriging_opt_method");
    if (!optimization_method.empty()) {
      if (optimization_method == "none" || optimization_method == "sampling"
	  || optimization_method == "local"
	  || optimization_method == "global")
	args["optimization_method"] = optimization_method;
      else {
	Cerr << "Error (global_kriging): invalid optimization method "
	     << optimization_method << "; valid options are "
	     << "none, sampling, local, or global" << std::endl;
	abort_handler(-1);
      }
    }

    short max_trials
      = problem_db.get_short("model.surrogate.kriging_max_trials");
    if (max_trials > 0)
      args["max_trials"] = toString<short>(max_trials);

    // NIDR support for RealArray (aka std::vector) would eliminate xtra copy!
    // old parameters
    Real nugget = problem_db.get_real("model.surrogate.nugget");
    short find_nugget = problem_db.get_short("model.surrogate.find_nugget");
    if (nugget > 0) {
      args["nugget"] = toString<Real>(nugget);
    }
    else {
      if (find_nugget > 0) {
	if (find_nugget == 1)
	  args["find_nugget"] = toString<bool>(false);
	else if (find_nugget == 2)
	  args["find_nugget"] = toString<bool>(true);
	else {
	  Cerr << " find_nugget must be 1 or 2" << '\n';
	  abort_handler(-1);
	}
      }
    }

    const RealVector& correlation_rv
      = problem_db.get_rv("model.surrogate.kriging_correlations");
    if (!correlation_rv.empty()) {
      RealArray correlation_ra; //std::vector<double>
      copy_data(correlation_rv, correlation_ra);
      args["correlation_lengths"] = fromVec<Real>(correlation_ra);
      args["optimization_method"] = "none";
    }

    /*
    const RealVector& max_correlations_rv
      = problem_db.get_rv("model.surrogate.kriging_max_correlations");
    if (!max_correlations_rv.empty()) {
      RealArray max_correlation_ra; //std::vector<double>
      copy_data(max_correlations_rv, max_correlation_ra);
      args["max_correlations"] = fromVec<Real>(max_correlation_ra);
    }

    const RealVector& min_correlations_rv
      = problem_db.get_rv("model.surrogate.kriging_min_correlations");
    if (!min_correlations_rv.empty()) {
      RealArray min_correlation_ra; //std::vector<double>
      copy_data(min_correlations_rv, min_correlation_ra);
      args["min_correlations"] = fromVec<Real>(min_correlation_ra);
    }

    // bounds set at run time within build()
    if (!sharedDataRep->approxCLowerBnds.empty()) {
      RealArray alb_ra;
      copy_data(sharedDataRep->approxCLowerBnds, alb_ra);
      args["lower_bounds"] = fromVec<Real>(alb_ra);
    }
    if (!sharedDataRep->approxCUpperBnds.empty()) {
      RealArray aub_ra;
      copy_data(sharedDataRep->approxCUpperBnds, aub_ra);
      args["upper_bounds"] = fromVec<Real>(aub_ra);
    }
    */

    // unused for now
    IntVector dimension_groups_iv;
    if (!dimension_groups_iv.empty()) {
      IntArray dg_ra;
      copy_data(dimension_groups_iv, dg_ra);
      args["dimension_groups"] = fromVec<int>(dg_ra);
    }

  }

  // For ANN surface fits
  else if (sharedDataRep->approxType == "global_neural_network") {
    args["type"] = "ann";
    short random_weight
      = problem_db.get_short("model.surrogate.neural_network_random_weight");
    if (random_weight > 0) {
      args["random_weight"] = toString<short>(random_weight);
    }
    short nodes
      = problem_db.get_short("model.surrogate.neural_network_nodes");
    if (nodes > 0) {
      args["nodes"] = toString<short>(nodes);
    }
    const Real& range
      = problem_db.get_real("model.surrogate.neural_network_range");
    if (range > 0) {
      args["range"] = toString<Real>(range);
    }
  }

  //// For moving least squares
  else if (sharedDataRep->approxType == "global_moving_least_squares") {
    args["type"] = "mls";
    short weight
      = problem_db.get_short("model.surrogate.mls_weight_function");
    if (weight > 0) {
      args["weight"] = toString<short>(weight);
    }
    short order = problem_db.get_short("model.surrogate.polynomial_order");
    if (order > 0) {
      args["order"] = toString<short>(order);
    }
  }

  //// For radial basis function networks
  else if (sharedDataRep->approxType == "global_radial_basis") {
    args["type"] = "rbf";
    // mapping number bases to number of centers
    short bases = problem_db.get_short("model.surrogate.rbf_bases");
    if (bases > 0) {
      args["centers"] = toString<short>(bases);
    }
    short min_partition
      = problem_db.get_short("model.surrogate.rbf_min_partition");
    if (min_partition > 0) {
      args["min_partition"] = toString<short>(min_partition);
    }
    short max_subsets
      = problem_db.get_short("model.surrogate.rbf_max_subsets");
    if (max_subsets > 0) {
      args["max_iter"] = toString<short>(max_subsets);
    }
    // mapping max_pts to cvt_pts
    short max_pts = problem_db.get_short("model.surrogate.rbf_max_pts");
    if (max_pts > 0) {
      args["cvt_pts"] = toString<short>(max_pts);
    }
  }

  //// For Mars surface fits
  else if (sharedDataRep->approxType == "global_mars") {
    args["type"] = "mars";
    short max_bases = problem_db.get_short("model.surrogate.mars_max_bases");
    if (max_bases > 0) {
      args["max_bases"] = toString<short>(max_bases);
    }
    const String& interpolation
      = problem_db.get_string("model.surrogate.mars_interpolation");
    if (interpolation != "") {
      args["interpolation"] = interpolation;
    }
  }
  //Cout << "PARAMETERS: " << std::endl;
  //for (ParamMap::iterator itr = args.begin(); itr != args.end(); itr++)
  //   Cout << "     " << itr->first << ": " << itr->second << std::endl;

  factory = ModelFactory::createModelFactory(args);

  //catch(...) {
  //  Cout << "Exception caught in attempt to create Surface object"
  //       << std::endl;
  //  abort_handler(-1);
  //}

  // validate diagnostic settings (preliminary); TODO: do more at
  // run time and move to both ctors
  bool err_found = false;
  const StringArray& diag_set = shared_surf_data_rep->diagnosticSet;
  if (!diag_set.empty()) {
    std::set<std::string> valid_metrics;
    valid_metrics.insert("sum_squared");
    valid_metrics.insert("mean_squared");
    valid_metrics.insert("root_mean_squared");
    valid_metrics.insert("sum_abs");
    valid_metrics.insert("mean_abs");
    valid_metrics.insert("max_abs");
    valid_metrics.insert("rsquared");

    int num_diag = diag_set.size();
    for (int j = 0; j < num_diag; ++j)
      if (valid_metrics.find(diag_set[j]) == valid_metrics.end()) {
	Cerr << "Error: surrogate metric '" << diag_set[j]
	     << "' is not available in Dakota.\n";
	err_found = true;
      }
    if (err_found) {
      Cerr << "Valid surrogate metrics include:\n  ";
      std::copy(valid_metrics.begin(), valid_metrics.end(),
		std::ostream_iterator<std::string>(Cerr, " "));
      Cerr << std::endl;
    }
  }
  if (shared_surf_data_rep->crossValidateFlag) {
    if (shared_surf_data_rep->numFolds > 0 &&
	shared_surf_data_rep->numFolds < 2) {
      Cerr << "Error: cross_validation folds must be 2 or greater."
	   << std::endl;
      err_found = true;
    }
    if (shared_surf_data_rep->percentFold < 0.0 ||
	shared_surf_data_rep->percentFold > 0.5) {
      Cerr << "Error: cross_validation percent must be between 0.0 and 0.5"
	   << std::endl;
      err_found = true;
    }
  }
  if (err_found)
    abort_handler(-1);
}

/// On-the-fly constructor which uses mostly Surfpack model defaults
SurfpackApproximation::
SurfpackApproximation(const SharedApproxData& shared_data):
  Approximation(NoDBBaseConstructor(), shared_data),
  surfData(NULL), model(NULL), factory(NULL)
  //sharedDataRep((SharedSurfpackApproxData*)shared_data.data_rep())
{
  SharedSurfpackApproxData* shared_surf_data_rep
    = (SharedSurfpackApproxData*)sharedDataRep;

  ParamMap args;
  args["verbosity"] = toString<short>(sharedDataRep->outputLevel);
  args["ndims"]     = toString<size_t>(sharedDataRep->numVars);
  args["seed"]      = "8147";

  if (sharedDataRep->approxType == "global_polynomial") {
    args["type"] = "polynomial";
    args["order"] = toString<unsigned short>(shared_surf_data_rep->approxOrder);
  }
  else if (sharedDataRep->approxType == "global_kriging") {

    args["type"] = "kriging";
    args["order"] = toString<unsigned short>(shared_surf_data_rep->approxOrder);
    args["reduced_polynomial"] = toString<bool>(true);
    /*
    // bounds set at run time within build():
    if (!sharedDataRep->approxCLowerBnds.empty()) {
      RealArray alb_ra;
      copy_data(sharedDataRep->approxCLowerBnds, alb_ra);
      args["lower_bounds"] = fromVec<Real>(alb_ra);
    }
    if (!sharedDataRep->approxCUpperBnds.empty()) {
      RealArray aub_ra;
      copy_data(sharedDataRep->approxCUpperBnds, aub_ra);
      args["upper_bounds"] = fromVec<Real>(aub_ra);
    }
    */
    //size_t krig_max_trials=(2*num_vars+1)*(num_vars+1)*10;
    //size_t krig_max_trials=20*(2*num_vars+1)*
      //(1+num_vars+((num_vars+1)*num_vars)/2); //#der0 + #der1 + #der2
    //if(krig_max_trials>10000)
      //krig_max_trials=10000;
    //size_t krig_max_trials=1000;
    //args["max_trials"] = toString<size_t>(krig_max_trials);
    args["max_trials"] = toString<size_t>(5000);

    // activate derivative information if available
    unsigned short surfpack_derivative_order = 0;
    short bdo = sharedDataRep->buildDataOrder;
    if (bdo == 1)      surfpack_derivative_order = 0;
    else if (bdo == 3) surfpack_derivative_order = 1;
    else if (bdo == 7) surfpack_derivative_order = 2;
    else {
      Cerr << "\nError (global_kriging): Unsupported buildDataOrder = " << bdo
	   << std::endl;
      abort_handler(-1);
    }
    args["derivative_order"] =
      toString<unsigned short>(surfpack_derivative_order);

  }
  else if (sharedDataRep->approxType == "global_neural_network")
    args["type"] = "ann";
  else if (sharedDataRep->approxType == "global_moving_least_squares") {
    args["type"] = "mls";
    args["order"] = toString<unsigned short>(shared_surf_data_rep->approxOrder);
  }
  else if (sharedDataRep->approxType == "global_radial_basis")
    args["type"] = "rbf";
  else if (sharedDataRep->approxType == "global_mars")
    args["type"] = "mars";

  factory = ModelFactory::createModelFactory(args);
}

// Embedded Surfpack objects will need to be deleted
SurfpackApproximation::~SurfpackApproximation()
{
  delete surfData;
  delete model;
  delete factory;
}

int SurfpackApproximation::min_coefficients() const
{
  assert(factory);
  return factory->minPointsRequired();
}

int SurfpackApproximation::recommended_coefficients() const
{
  assert(factory);
  return factory->recommendedNumPoints();
}

void SurfpackApproximation::build(size_t index)
{
  // base class implementation checks data set against min required
  Approximation::build(index);

  // Surface object should have been created in constructor
  if (!factory) {
    Cerr << "Error: surface is null in SurfpackApproximation::build()."
	 << std::endl;
    abort_handler(-1);
  }

  SharedSurfpackApproxData* shared_surf_data_rep
    = (SharedSurfpackApproxData*)sharedDataRep;

  /// surfData will be deleted in dtor
  /// \todo Right now, we're completely deleting the old data and then
  /// recopying the current data into a SurfData object.  This was just
  /// the easiest way to arrive at a solution that would build and run.
  /// This function is frequently called from addPoint rebuild, however,
  /// and it's not good to go through this whole process every time one
  /// more data point is added.
  try {
    if (surfData) {
      delete surfData;
      surfData = NULL;
    }
    surfData = surrogates_to_surf_data();

    // set bounds at run time since they are updated by some methods (e.g., SBO)
    if (!sharedDataRep->approxCLowerBnds.empty() ||
	!sharedDataRep->approxDILowerBnds.empty() ||
	!sharedDataRep->approxDRLowerBnds.empty()) {
      RealArray lb;
      shared_surf_data_rep->merge_variable_arrays(
	sharedDataRep->approxCLowerBnds, sharedDataRep->approxDILowerBnds,
	sharedDataRep->approxDRLowerBnds, lb);
      factory->add("lower_bounds", fromVec<Real>(lb));
    }
    if (!sharedDataRep->approxCUpperBnds.empty() ||
	!sharedDataRep->approxDIUpperBnds.empty() ||
	!sharedDataRep->approxDRUpperBnds.empty()) {
      RealArray ub;
      shared_surf_data_rep->merge_variable_arrays(
	sharedDataRep->approxCUpperBnds, sharedDataRep->approxDIUpperBnds,
	sharedDataRep->approxDRUpperBnds, ub);
      factory->add("upper_bounds", fromVec<Real>(ub));
    }

    if (model) {
      delete model;
      model = NULL;
    }
    model = factory->Build(*surfData);
    // TO DO: extract coefficients array
  }
  catch (std::runtime_error& e) {
    Cerr << e.what() << std::endl;
    Cerr << typeid(e).name() << std::endl;
    abort_handler(-1);
  }
  catch (std::string& e) {
    Cerr << "Error: exception with no recourse caught trying to build model:\n"
	 << e << std::endl;
    abort_handler(-1);
  }
  catch (...) {
    Cerr << "Error: exception caught trying to build model" << std::endl;
    abort_handler(-1);
  }

/*  if (!shared_surf_data_rep->exportModelName.empty() &&
      SurfpackInterface::HasFeature("model_save")) {
    if (sharedDataRep->outputLevel >= VERBOSE_OUTPUT)
      Cout << "\nSaving surrogate model to file "
	   << shared_surf_data_rep->exportModelName << std::endl;
    SurfpackInterface::Save(model, shared_surf_data_rep->exportModelName);
  }*/
}

void SurfpackApproximation::export_model(const String& fn_label,
 					 const String& export_prefix,
                                         const unsigned short export_format)
{
  String without_extension;
  unsigned short formats;
  if(export_format) {
    without_extension = export_prefix + "." + fn_label;
    formats = export_format;
  } else {
    without_extension = sharedDataRep->modelExportPrefix + "." + approxLabel;
    formats = sharedDataRep->modelExportFormat;
  }
  //unsigned short formats = export_format;
  const bool &can_save = SurfpackInterface::HasFeature("model_save");
  // Saving to text archive
  if(formats & TEXT_ARCHIVE) {
    if(can_save) {
      String filename = without_extension + ".sps";
      SurfpackInterface::Save(model,filename);
    } else
        Cerr << "\nRequested surrogate export to text archive failed: "
		<< "Surfpack lacks support for model saving.\n";
  }
  // Saving to binary archive
  if(formats & BINARY_ARCHIVE) {
    if(can_save) {
      String filename = without_extension + ".bsps";
      SurfpackInterface::Save(model,filename);
    } else
        Cerr << "\nRequested surrogate export to binary archive failed: "
	        << "Surfpack lacks support for model saving.\n";
  }
  // Saving to algebraic file
  if(formats & ALGEBRAIC_FILE) {
    String filename = without_extension + ".alg";
    std::ofstream af(filename.c_str(),std::ofstream::out);
    af << "Model for response " << fn_label << ":\n" << model->asString();
    af.close();
    Cout << "Model saved in algebraic format to file '" << filename << "'.\n";
  }
  // Writing in algebraic format to screen
  if(formats & ALGEBRAIC_CONSOLE) {
    Cout << "\nModel for response " << fn_label << ":\n";
    Cout << model->asString();
  }
}

Real SurfpackApproximation::value(const Variables& vars)
{
  //static int times_called = 0;
  if (!model) {
    Cerr << "Error: surface is null in SurfpackApproximation::value()"
	 << std::endl;
    abort_handler(-1);
  }

  RealArray x_array;
  ((SharedSurfpackApproxData*)sharedDataRep)->vars_to_realarray(vars, x_array);
  return (*model)(x_array);
}

const RealVector& SurfpackApproximation::gradient(const Variables& vars)
{
  approxGradient.sizeUninitialized(vars.cv());
  try {
    RealArray x_array;
    ((SharedSurfpackApproxData*)sharedDataRep)
      ->vars_to_realarray(vars, x_array);
    VecDbl local_grad = model->gradient(x_array);
    for (unsigned i = 0; i < surfData->xSize(); i++)
      approxGradient[i] = local_grad[i];
  }
  catch (...) {
    Cerr << "Error: gradient() not available for this approximation type."
	 << std::endl;
    abort_handler(-1);
  }
  return approxGradient;
}

const RealSymMatrix& SurfpackApproximation::hessian(const Variables& vars)
{
  size_t num_cv = vars.cv();
  approxHessian.reshape(num_cv);
  try {
    if (sharedDataRep->approxType == "global_moving_least_squares") {
      Cerr << "Have not implemented analytical hessians in this surfpack class"
	   << std::endl;
      abort_handler(-1);
    }
    RealArray x_array;
    ((SharedSurfpackApproxData*)sharedDataRep)
      ->vars_to_realarray(vars, x_array);
    MtxDbl sm = model->hessian(x_array);
    ///\todo Make this acceptably efficient
    for (size_t i = 0; i < num_cv; i++)
      for(size_t j = 0; j < num_cv; j++)
        approxHessian(i,j) = sm(i,j);
  }
  catch (...) {
    Cerr << "Error: hessian() not available for this approximation type."
	 << std::endl;
    abort_handler(-1);
  }
  return approxHessian;
}

Real SurfpackApproximation::prediction_variance(const Variables& vars)
{
  try {
    RealArray x_array;
    ((SharedSurfpackApproxData*)sharedDataRep)
      ->vars_to_realarray(vars, x_array);
    return model->variance(x_array);
  }
  catch (...) {
    Cerr << "Error: prediction_variance() not available for this "
	 << "approximation type." << std::endl;
    abort_handler(-1);
  }
}

Real SurfpackApproximation::value(const RealVector& c_vars)
{
    //static int times_called = 0;
    if (!model) {
        Cerr << "Error: surface is null in SurfpackApproximation::value()"
        << std::endl;
        abort_handler(-1);
    }

    RealArray x_array;
    size_t num_vars = c_vars.length();
    for (size_t i = 0; i < num_vars; i++) x_array.push_back(c_vars[i]);
    return (*model)(x_array);
}

const RealVector& SurfpackApproximation::gradient(const RealVector& c_vars)
{
    approxGradient.sizeUninitialized(c_vars.length());
    try {
        RealArray x_array;
        size_t num_vars = c_vars.length();
        for (size_t i = 0; i < num_vars; i++) x_array.push_back(c_vars[i]);

        VecDbl local_grad = model->gradient(x_array);
        for (unsigned i = 0; i < surfData->xSize(); i++)
            approxGradient[i] = local_grad[i];
    }
    catch (...) {
        Cerr << "Error: gradient() not available for this approximation type."
        << std::endl;
        abort_handler(-1);
    }
    return approxGradient;
}

const RealSymMatrix& SurfpackApproximation::hessian(const RealVector& c_vars)
{
    size_t num_cv = c_vars.length();
    approxHessian.reshape(num_cv);
    try {
        if (sharedDataRep->approxType == "global_moving_least_squares") {
            Cerr << "Have not implemented analytical hessians in this surfpack class"
            << std::endl;
            abort_handler(-1);
        }
        RealArray x_array;
        for (size_t i = 0; i < num_cv; i++) x_array.push_back(c_vars[i]);

        MtxDbl sm = model->hessian(x_array);
        ///\todo Make this acceptably efficient
        for (size_t i = 0; i < num_cv; i++)
            for(size_t j = 0; j < num_cv; j++)
                approxHessian(i,j) = sm(i,j);
    }
    catch (...) {
        Cerr << "Error: hessian() not available for this approximation type."
        << std::endl;
        abort_handler(-1);
    }
    return approxHessian;
}

Real SurfpackApproximation::prediction_variance(const RealVector& c_vars)
{
    try {
        RealArray x_array;
        size_t num_vars = c_vars.length();
        for (size_t i = 0; i < num_vars; i++) x_array.push_back(c_vars[i]);
        return model->variance(x_array);
    }
    catch (...) {
        Cerr << "Error: prediction_variance() not available for this "
        << "approximation type." << std::endl;
        abort_handler(-1);
    }
}

Real SurfpackApproximation::diagnostic(const String& metric_type)
{
  if (!model) {
    Cerr << "Error: surface is null in SurfpackApproximation::diagnostic()"
         << std::endl;
    abort_handler(-1);
  }

  return diagnostic(metric_type, *model, *surfData);
}

Real SurfpackApproximation::diagnostic(const String& metric_type,
				       const SurfpackModel& sp_model,
				       const SurfData& s_data)
{
  Real approx_diag;
  try {
    ModelFitness* SS_fitness = ModelFitness::Create(metric_type);
    approx_diag = (*SS_fitness)(sp_model, s_data);
    delete SS_fitness;
  }
  catch (const std::string& msg) {
    Cerr << "Error evaluating surrogate metric: " << msg << std::endl;
    abort_handler(-1);
  }

  Cout << std::setw(20) << metric_type << std::setw(20) << approx_diag << '\n';
  return approx_diag;
}

void SurfpackApproximation::primary_diagnostics(int fn_index)
{
  String func_description = approxLabel.empty() ?
    "function " + boost::lexical_cast<std::string>(fn_index+1) : approxLabel;
  SharedSurfpackApproxData* shared_surf_data_rep
    = (SharedSurfpackApproxData*)sharedDataRep;
  const StringArray& diag_set = shared_surf_data_rep->diagnosticSet;
  if (diag_set.empty()) {
    // conditionally print default diagnostics
    if (sharedDataRep->outputLevel > NORMAL_OUTPUT) {
      Cout << "\nSurrogate quality metrics for " << func_description << ":\n";
      diagnostic("root_mean_squared");
      diagnostic("mean_abs");
      diagnostic("rsquared");
    }
  }
  else {
    Cout << "\nSurrogate quality metrics for " << func_description << ":\n";
    int num_diag = diag_set.size();
    for (int j = 0; j < num_diag; ++j)
      diagnostic(diag_set[j]);

    // BMA TODO: at runtime verify (though Surfpack will too)
    //  * 1/N <= percentFold <= 0.5
    //  * 2 <= numFolds <= N
    if (shared_surf_data_rep->crossValidateFlag) {
      int num_folds = shared_surf_data_rep->numFolds;
      // if no folds, try percent, otherwise set default = 10
      if (num_folds == 0) {
        if (shared_surf_data_rep->percentFold > 0.0) {
          num_folds = boost::math::iround(1./shared_surf_data_rep->percentFold);
          if (sharedDataRep->outputLevel >= DEBUG_OUTPUT)
            Cout << "Info: cross_validate num_folds = " << num_folds
                 << " calculated from specified percent = "
                 << shared_surf_data_rep->percentFold << "." << std::endl;
        }
        else {
          num_folds = 10;
          if (sharedDataRep->outputLevel >= DEBUG_OUTPUT)
            Cout << "Info: default num_folds = " << num_folds << " used."
                 << std::endl;
        }
      }

      Cout << "\nSurrogate quality metrics (" << num_folds << "-fold CV) for "
           << func_description << ":\n";
      RealArray cv_metrics = cv_diagnostic(diag_set, num_folds);
      //CrossValidationFitness CV_fitness(num_folds);
      //VecDbl cv_metrics;
      //CV_fitness.eval_metrics(cv_metrics, *model, *surfData, diag_set);

      for (int j = 0; j < num_diag; ++j) {
        const String& metric_type = diag_set[j];
        if (metric_type == "rsquared")
          Cout << std::setw(20) << metric_type
               << std::setw(20) << std::numeric_limits<Real>::quiet_NaN()
               << "  (n/a for cross-validation)"
               << std::endl;
        else
          Cout << std::setw(20) << metric_type << std::setw(20) << cv_metrics[j]
               << std::endl;
      }

    }
    if (shared_surf_data_rep->pressFlag) {
      Cout << "\nSurrogate quality metrics (PRESS/leave-one-out) for "
           << func_description << ":\n";

      // perform press as CV with N folds
      RealArray cv_metrics = cv_diagnostic(diag_set, surfData->size());
      //CrossValidationFitness CV_fitness(surfData->size());
      //VecDbl cv_metrics;
      //CV_fitness.eval_metrics(cv_metrics, *model, *surfData, diag_set);

      for (int j = 0; j < num_diag; ++j) {
        const String& metric_type = diag_set[j];
        if (metric_type == "rsquared")
          Cout << std::setw(20) << metric_type
               << std::setw(20) << std::numeric_limits<Real>::quiet_NaN()
               << "  (n/a for PRESS)" << std::endl;
        else
          Cout << std::setw(20) << metric_type << std::setw(20) << cv_metrics[j]
               << std::endl;
      }

    }
  }
}

void SurfpackApproximation::
challenge_diagnostics(int fn_index, const RealMatrix& challenge_points,
                      const RealVector& challenge_responses)
{
  if (!model) {
    Cerr << "Error: surface is null in SurfpackApproximation::diagnostic()"
	 << std::endl;
    abort_handler(-1);
  }

  String func_description = approxLabel.empty() ?
    "function " + boost::lexical_cast<std::string>(fn_index+1) : approxLabel;

  // copy
  StringArray diag_set =
    ((SharedSurfpackApproxData*)sharedDataRep)->diagnosticSet;
  if (diag_set.empty()) {
    // conditionally print default diagnostics
    if (sharedDataRep->outputLevel > NORMAL_OUTPUT) {
      Cout << "\nSurrogate quality metrics (challenge data) for "
	   << func_description << ":\n";
      diag_set.push_back("root_mean_squared");
      diag_set.push_back("mean_abs");
      diag_set.push_back("rsquared");
      challenge_diagnostic(diag_set, challenge_points, challenge_responses);
    }
  }
  else {
    Cout << "\nSurrogate quality metrics (challenge data) for "
	 << func_description << ":\n";
    challenge_diagnostic(diag_set, challenge_points, challenge_responses);
  }

}

RealArray SurfpackApproximation::cv_diagnostic(const StringArray& metric_types,
                                               unsigned num_folds) {
  CrossValidationFitness CV_fitness(num_folds);
  VecDbl cv_metrics;
  try {
    CV_fitness.eval_metrics(cv_metrics, *model, *surfData, metric_types);
  } catch(String cv_error) {
    Cerr << "Error: Exception caught while computing CV score:\n" << cv_error << std::endl;
    cv_metrics.resize(metric_types.size());
    std::fill(cv_metrics.begin(), cv_metrics.end(), std::numeric_limits<Real>::quiet_NaN());
  }
  return cv_metrics;
}

RealArray SurfpackApproximation::challenge_diagnostic(const StringArray& metric_types,
			    const RealMatrix& challenge_points,
                            const RealVector& challenge_responses) {
  // JAS: painful but probably unavoidable data copy on every call.
  SurfData chal_data;
  RealArray chal_metrics;
  size_t num_v = sharedDataRep->numVars;
  for (size_t row=0; row<challenge_points.numRows(); ++row) {
    RealArray x(num_v);
    for (size_t col=0; col<num_v; ++col)
      x[col] = challenge_points[col][row];
    Real f = challenge_responses[row];
    chal_data.addPoint(SurfPoint(x, f));
  }
  for (int j = 0; j < metric_types.size(); ++j)
    chal_metrics.push_back(diagnostic(metric_types[j], *model, chal_data));
  return chal_metrics;
}

/** Copy the data stored in Dakota-style SurrogateData into
    Surfpack-style SurfPoint and SurfData objects. */
SurfData* SurfpackApproximation::surrogates_to_surf_data()
{
  SurfData* surf_data = new SurfData();

  // screen approximation data for failures
  approxData.data_checks();

  // some surrogates, e.g., global_polynomials and kriging, treat the anchor
  // point specially as a constraint; other treat as a regular data point
  SharedSurfpackApproxData* shared_surf_data_rep
    = (SharedSurfpackApproxData*)sharedDataRep;
  if (approxData.anchor()) {
    if (factory->supports_constraints())
      add_anchor_to_surfdata(*surf_data);
    else
      shared_surf_data_rep->
	add_sd_to_surfdata(approxData.anchor_variables(),
			   approxData.anchor_response(),
			   approxData.failed_anchor_data(), *surf_data);
  }
  // add the remaining surrogate data points
  if (sharedDataRep->outputLevel >= DEBUG_OUTPUT)
    Cout << "Requested build data order is " << sharedDataRep->buildDataOrder
	 << '\n';
  size_t i, num_data_pts = approxData.points();
  const Pecos::SDVArray& sdv_array = approxData.variables_data();
  const Pecos::SDRArray& sdr_array = approxData.response_data();
  const Pecos::SizetShortMap& failed_resp = approxData.failed_response_data();
  Pecos::SizetShortMap::const_iterator fit = failed_resp.begin();
  for (i=0; i<num_data_pts; ++i) {
    short fail_code = 0;
    if (fit != failed_resp.end() && fit->first == i)
      { fail_code = fit->second; ++fit; }
    shared_surf_data_rep->add_sd_to_surfdata(sdv_array[i], sdr_array[i],
					     fail_code, *surf_data);
  }

  return surf_data;
}

/** If there is an anchor point, add an equality constraint for its response
    value.  Also add constraints for gradient and hessian, if applicable. */
void SurfpackApproximation::add_anchor_to_surfdata(SurfData& surf_data)
{
  // coarse-grained fault tolerance for now: any failure qualifies for omission
  if (approxData.failed_anchor_data())
    return;

  // Surfpack's RealArray is std::vector<double>
  RealArray x;
  Real f;
  RealArray gradient;
  SurfpackMatrix<Real> hessian;

  // Print out the anchor continuous variables
  SharedSurfpackApproxData* shared_surf_data_rep
    = (SharedSurfpackApproxData*)sharedDataRep;
  shared_surf_data_rep->sdv_to_realarray(approxData.anchor_variables(), x);
  if (sharedDataRep->outputLevel > NORMAL_OUTPUT)
    Cout << "Anchor point vars\n" << x;

  // At a minimum, there should be a response value
  unsigned short anchor_data_order = 1;
  f = approxData.anchor_function();
  if (sharedDataRep->outputLevel > NORMAL_OUTPUT)
    Cout << "Anchor response: " << f << '\n';

  // Check for gradient in anchor point
  const RealVector& anchor_grad = approxData.anchor_gradient();
  if (!anchor_grad.empty()) {
    anchor_data_order |= 2;
    copy_data(anchor_grad, gradient);
    if (sharedDataRep->outputLevel > NORMAL_OUTPUT) {
      Cout << "Anchor gradient:\n";
      write_data(Cout, anchor_grad);
    }
  }

  // Check for hessian in anchor point
  const RealSymMatrix& anchor_hess = approxData.anchor_hessian();
  if (!anchor_hess.empty()) {
    anchor_data_order |= 4;
    shared_surf_data_rep->copy_matrix(anchor_hess, hessian);
    if (sharedDataRep->outputLevel > NORMAL_OUTPUT) {
      Cout << "Anchor hessian:\n";
      write_data(Cout, anchor_hess, false, true, true);
    }
  }

  if (sharedDataRep->outputLevel > NORMAL_OUTPUT)
    Cout << "Requested constraint data order is " << anchor_data_order
	 << '\n';

  // for now only allow builds from exactly 1, 3=1+2, or 7=1+2+4; use
  // different set functions so the SurfPoint data remains empty if
  // not present
  switch (anchor_data_order) {

  case 1:
    surf_data.setConstraintPoint(SurfPoint(x, f));
    break;

  case 3:
    surf_data.setConstraintPoint(SurfPoint(x, f, gradient));
    break;

  case 7:
    surf_data.setConstraintPoint(SurfPoint(x, f, gradient, hessian));
    break;

  default:
    Cerr << "\nError (SurfpackApproximation): derivative data may only be used"
	 << "if all\nlower-order information is also present. Specified "
	 << "anchor_data_order is " << anchor_data_order << "."  << std::endl;
    abort_handler(-1);
    break;

  }

}

} // namespace Dakota


#include "DART_Interface.h"
#include "GUI/GL_function.h"
#include <Eigen/Geometry>
#include <vector>
#include "GL/glut.h"
using namespace dart::dynamics;
using namespace dart::simulation;
/*************************************************************
    MakeRootBody function
This function makes root body for skeleton.
globalOffset param is center of mass of body.
*************************************************************/
void
MakeRootBody(const SkeletonPtr& skel, const std::string& name,const Eigen::Vector3d& size,const Eigen::Vector3d& c_to_joint,const double& mass)
{
    ShapePtr shape = std::shared_ptr<BoxShape>(new BoxShape(size));
    dart::dynamics::Inertia inertia;
    inertia.setMass(mass);
    inertia.setMoment(shape->computeInertia(mass));

    RevoluteJoint::Properties prop;
    prop.mName = name + "_joint";
    prop.mAxis = Eigen::Vector3d::UnitZ();
    prop.mT_ChildBodyToJoint.translation() = c_to_joint;

    BodyNodePtr bn = skel->createJointAndBodyNodePair<RevoluteJoint>(
      nullptr,prop,BodyNode::AspectProperties(name)).second;
    auto sn = bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(shape);
    bn->setInertia(inertia);
}
/*************************************************************
    MakeBody function
This function makes articulated body of skeleton.
Input is straightforward, except globaloffset and jointoffset.
globaloffset parameter is center of mass of body expressed in world coordinate.
jointoffset parameter is position of joint expressed in child(this) body coordinate.
*************************************************************/
void
MakeBody(const SkeletonPtr& skel,const BodyNodePtr& parent,const std::string& name,
    const Eigen::Vector3d& size,
    const Eigen::Vector3d& p_to_joint,
    const Eigen::Vector3d& c_to_joint,const double& mass)
{
    ShapePtr shape = std::shared_ptr<BoxShape>(new BoxShape(size));
    dart::dynamics::Inertia inertia;
    inertia.setMass(mass);
    inertia.setMoment(shape->computeInertia(mass));

    RevoluteJoint::Properties prop;
    prop.mName = name + "_joint";
    prop.mAxis = Eigen::Vector3d::UnitZ();
    prop.mT_ParentBodyToJoint.translation() = p_to_joint;
    prop.mT_ChildBodyToJoint.translation() = c_to_joint;

    BodyNodePtr bn = skel->createJointAndBodyNodePair<RevoluteJoint>(
      parent,prop,BodyNode::AspectProperties(name)).second;

    auto sn = bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(shape);

    bn->setInertia(inertia);
}
void MakeWeldBody(const SkeletonPtr& skel,
    const BodyNodePtr& parent,const std::string& name,const double& radius,
    const Eigen::Vector3d& p_to_joint,const Eigen::Vector3d& c_to_joint,const double& mass)
{
    ShapePtr shape = std::shared_ptr<SphereShape>(new SphereShape(radius));
    dart::dynamics::Inertia inertia;
    inertia.setMass(mass);
    inertia.setMoment(shape->computeInertia(mass));

    WeldJoint::Properties prop;
    prop.mName = name + "_joint";
    prop.mT_ParentBodyToJoint.translation() = p_to_joint;
    prop.mT_ChildBodyToJoint.translation() = c_to_joint;

    BodyNodePtr bn = skel->createJointAndBodyNodePair<WeldJoint>(
      parent,prop,BodyNode::AspectProperties(name)).second;

    auto sn = bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(shape);

    bn->setInertia(inertia);
}
void
MakeBall(const SkeletonPtr& skel,const double& radius,const double& mass)
{
    ShapePtr shape = std::shared_ptr<SphereShape>(new SphereShape(radius));
    dart::dynamics::Inertia inertia;
    inertia.setMass(mass);
    inertia.setMoment(shape->computeInertia(mass));

    FreeJoint::Properties prop;
    prop.mT_ParentBodyToJoint.setIdentity();
    prop.mT_ChildBodyToJoint.setIdentity();

    BodyNodePtr bn = skel->createJointAndBodyNodePair<FreeJoint>(
      nullptr,prop,BodyNode::AspectProperties("ball")).second;

    auto sn = bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(shape);
    bn->setCollidable(false);
    bn->setInertia(inertia);
}
void
DrawSkeleton(const dart::dynamics::SkeletonPtr& skel,const Eigen::Vector3d& color)
{
    glDisable(GL_DEPTH_TEST);

    for(int j =0;j<skel->getNumBodyNodes();j++)
    {
        auto bn = skel->getBodyNode(j);
        auto shapeNodes = bn->getShapeNodesWith<VisualAspect>();
        auto T = bn->getTransform();
        for (auto& sn : shapeNodes)
        {
            auto shape = sn->getShape().get();

            if (shape->is<BoxShape>())
            {
                const auto* box = static_cast<const BoxShape*>(shape);
                const auto& size = box->getSize();
                DrawCapsule(T,size[0],size[1],color);
            }
            else if (shape->is<SphereShape>())
            {
                const auto* sphere = static_cast<const SphereShape*>(shape);
                const auto& r = sphere->getRadius();
                DrawSphere(T,r,color);

            }

        }

    }
    for(int j =0;j<skel->getNumBodyNodes();j++)
    {
        auto bn = skel->getBodyNode(j);
        auto T = bn->getTransform();
        Eigen::Vector3d p = T.translation();
        Eigen::Vector3d v = bn->getCOMLinearVelocity();
        v*= 0.05;
        DrawArrow(p.block<2,1>(0,0),v.block<2,1>(0,0),Eigen::Vector3d(0,0,1));
    }
    glEnable(GL_DEPTH_TEST);
}


//一个VO,简单实现基于optflow获取3d点,再用 solvepnp 获取更新位置.没有优化部分.

#include "Frame.h"
#include "FeatureFrontEndCV.h"
#include "FrameWiseGeometry.h"

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/QuaternionStamped.h> //for DJI.

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Quaternion.h> //for apm and pixhawk.

#include <visualization_msgs/Marker.h> //for visualization.

#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <ros/duration.h>
#include <math.h>

#include <geometry_msgs/PoseStamped.h> //for px4's external pose estimate
#include <sensor_msgs/Imu.h>

#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

#include <Eigen/Core>
#include <Eigen/Dense>  // linear algebra
#include <Eigen/StdVector>

#include <deque>
#include <chrono>
using namespace std;
class VO_simple
{
public:
    VO_simple(int argc,char** argv)
    {
        begin_t = std::chrono::high_resolution_clock::now();
    }
    bool needNewKeyFrame()
    {
        std::chrono::high_resolution_clock::time_point t_now = std::chrono::high_resolution_clock::now();
        if (!ever_init || double(std::chrono::duration_cast<std::chrono::nanoseconds>(t_now-last_kf_update_t).count()/1e9) > 0.5)
        {//每0.5s,创建一次关键帧.
            last_kf_update_t = t_now;
            last_frame_update_t = t_now;
            //ever_init = true;
            return true;
        }
        return false;
    }
    shared_ptr<mcs::Frame> getLastKF()
    {
        return this->pLastKF;
    }
    shared_ptr<mcs::Frame> getLastFrame()
    {
        return this->pLastF;
    }
    void iterateWith4Imgs(shared_ptr<cv::Mat> img1,shared_ptr<cv::Mat> img2,shared_ptr<cv::Mat> img3,shared_ptr<cv::Mat> img4)
    {
        shared_ptr<mcs::Frame> pNewF;
        bool needNewKF;
        shared_ptr < vector<std::pair<shared_ptr<mcs::cvMat_T>,shared_ptr<mcs::cvMat_T> > > > pvInputs( new vector<std::pair<shared_ptr<mcs::cvMat_T>,shared_ptr<mcs::cvMat_T> > >());
        pvInputs->push_back(std::make_pair(img1,img2));
        pvInputs->push_back(std::make_pair(img3,img4));
        if(needNewKeyFrame())
        {
            bool needNewKF = true;
            bool create_frame_success;
            pNewF = mcs::createFrameStereos(pvInputs,this->cam_config,create_frame_success,needNewKF);
            if(!ever_init)
            {
                LOG(INFO)<<"init VO!"<<endl;//初始化VO.
                pNewF->rotation = Eigen::Matrix3d::Identity();
                pNewF->position = Eigen::Vector3d(0,0,0);
                ever_init = true;
            }
            else
            {
                LOG(INFO)<<"VO initiated.Will trackLocalFrame()."<<endl;
                bool track_localframe_success;
                //mcs::trackLocalFramePoints(); // TODO:frame wise using p3d tracking;
                if(track_localframe_success)
                {
                    //pNewF->rotation = ...
                    //pNewF->position =
                    if(this->VORunningState != STATE_TRACKING)
                    {
                        LOG(INFO)<<"State transfer from "<<state_id_map[VORunningState]<<" to STATE_TRACKING!"<<endl;
                    }
                    this->VORunningState = this->STATE_TRACKING;
                }
                else
                {
                    LOG(ERROR)<<"Track failure!Using last frame rt."<<endl;
                    pNewF->rotation = getLastFrame()->rotation;
                    pNewF->position = getLastFrame()->position;
                    if(this->VORunningState!= this->STATE_TRACKING_FALIED)
                    {
                        LOG(WARNING)<<"State transfer from "<<state_id_map[VORunningState]<<" to STATE_TRACKING_FAILED!"<<endl;
                    }
                    this->VORunningState = this->STATE_TRACKING_FALIED;
                }
            }
            pLastKF = pNewF;
            pLastF = pNewF;
        }
        else
        {
            bool needNewKF =false;
            bool create_frame_success;
            last_frame_update_t = std::chrono::high_resolution_clock::now();
            pNewF = mcs::createFrameStereos(pvInputs,this->cam_config,create_frame_success,needNewKF);
            //TODO:
            bool track_and_pnp_ransac_success;
            //mcs::trackAndDoSolvePnPRansacMultiCam(pNewF); //frame_wise tracking....
            if(track_and_pnp_ransac_success)
            {
                //createNewKF...
            }
            else
            {
                //pNewF->rotation = ...
                //pNewF->translation = ...
            }
        }
    }
public:
    static const int STATE_TRACKING = 2;
    static const int STATE_UNSTABLE_TRACKING = 1;
    static const int STATE_TRACKING_FALIED = 0;
    map<int,std::string> state_id_map = {
                                            {0,"STATE_TRACKING"},
                                            {1,"STATE_UNSTABLE_TRACKING"},
                                            {2,"STATE_TRACKING_FAILED"}
                                     };
private:
    int VORunningState = STATE_TRACKING_FALIED; // init.
    int unstable_tracking_patience = 0;//暂时不用.用到的时候使用它计量已经有多少个不稳定追踪.
    deque<mcs::Frame> frameQueue;
    vector<StereoCamConfig> cam_config;
    shared_ptr<mcs::Frame> pLastKF=nullptr,pLastF=nullptr;
    std::chrono::high_resolution_clock::time_point begin_t;
    std::chrono::high_resolution_clock::time_point last_kf_update_t;
    std::chrono::high_resolution_clock::time_point last_frame_update_t;
    bool ever_init = false;

};

VO_simple* pVO;
void FetchImageCallback(const sensor_msgs::ImageConstPtr& img1,const sensor_msgs::ImageConstPtr& img2,const sensor_msgs::ImageConstPtr& img3,const sensor_msgs::ImageConstPtr& img4)
{
    cv_bridge::CvImageConstPtr p1,p2,p3,p4;
    shared_ptr<cv::Mat> m1,m2,m3,m4;
    try
    {
        p1 = cv_bridge::toCvShare(img1);
        p2 = cv_bridge::toCvShare(img2);
        p3 = cv_bridge::toCvShare(img3);
        p4 = cv_bridge::toCvShare(img4);
        m1 = shared_ptr<cv::Mat> (new cv::Mat(p1->image));
        m2 = shared_ptr<cv::Mat> (new cv::Mat(p2->image));
        m3 = shared_ptr<cv::Mat> (new cv::Mat(p3->image));
        m4 = shared_ptr<cv::Mat> (new cv::Mat(p4->image));
    }
    catch (cv_bridge::Exception& e)
    {
        LOG(ERROR)<<"cv_bridge exception: %s"<<e.what()<<endl;
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    LOG(INFO)<<"Images caught!"<<endl;
    pVO->iterateWith4Imgs(m1,m2,m3,m4);
}

int main(int argc,char** argv)
{
    ros::init(argc,argv,"test_VO_node");
    ros::NodeHandle nh;
    std::string front_left_topic,front_right_topic,down_left_topic,down_right_topic;

    message_filters::Subscriber<sensor_msgs::Image> front_left_sub(nh, front_left_topic, 10);
    message_filters::Subscriber<sensor_msgs::Image> front_right_sub(nh, front_right_topic, 10);
    message_filters::Subscriber<sensor_msgs::Image> down_left_sub(nh, down_left_topic, 10);
    message_filters::Subscriber<sensor_msgs::Image> down_right_sub(nh, down_right_topic, 10);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image,sensor_msgs::Image,sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), front_left_sub, front_right_sub,down_left_sub,down_right_sub);
    sync.setMaxIntervalDuration(ros::Duration(0.01));
    sync.registerCallback(boost::bind(FetchImageCallback, _1, _2,_3,_4));
    pVO = new VO_simple(argc,argv);
    ros::spin();
}


/******************************************************************************
 * Copyright (C) 2014 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

#include "aslam/calibration/time-delay/algo/Calibrator.h"

#include <vector>
#include <cmath>

#include <boost/make_shared.hpp>

#include <sm/kinematics/quaternion_algebra.hpp>
#include <sm/kinematics/EulerAnglesYawPitchRoll.hpp>

#include <sm/PropertyTree.hpp>

#include <bsplines/BSplineFitter.hpp>
#include <bsplines/NsecTimePolicy.hpp>

#include <aslam/backend/EuclideanPoint.hpp>
#include <aslam/backend/RotationQuaternion.hpp>
#include <aslam/backend/Scalar.hpp>
#include <aslam/backend/GenericScalar.hpp>
#include <aslam/backend/GenericScalarExpression.hpp>
#include <aslam/backend/EuclideanExpression.hpp>
#include <aslam/backend/RotationExpression.hpp>
#include <aslam/backend/ScalarExpression.hpp>
#include <aslam/backend/Vector2RotationQuaternionExpressionAdapter.hpp>

#include <aslam/calibration/core/IncrementalEstimator.h>

#include "aslam/calibration/time-delay/error-terms/ErrorTermPose.h"
#include "aslam/calibration/time-delay/error-terms/ErrorTermWheel.h"
#include "aslam/calibration/time-delay/design-variables/OdometryDesignVariables.h"
#include "aslam/calibration/time-delay/algo/OptimizationProblemSpline.h"
#include "aslam/calibration/time-delay/algo/bestQuat.h"
#include "aslam/calibration/time-delay/data/PoseMeasurement.h"
#include "aslam/calibration/time-delay/data/WheelSpeedMeasurement.h"

using namespace sm;
using namespace sm::timing;
using namespace sm::kinematics;
using namespace bsplines;
using namespace aslam::splines;
using namespace aslam::backend;

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

    Calibrator::Calibrator(const PropertyTree& config) :
        _currentBatchStartTimestamp(-1),
        _lastTimestamp(-1) {
      // create the underlying estimator
      _estimator = boost::make_shared<IncrementalEstimator>(
        sm::PropertyTree(config, "estimator"));

      // sets the options for the calibrator
      _options = Options(config);

      // create the odometry design variables
      _odometryDesignVariables = boost::make_shared<OdometryDesignVariables>(
        sm::PropertyTree(config, "odometry"));

      // save initial guess
      _odometryVariablesHistory.push_back(
        _odometryDesignVariables->getParameters());
    }

    Calibrator::~Calibrator() {
    }

/******************************************************************************/
/* Accessors                                                                  */
/******************************************************************************/

    const Calibrator::Options& Calibrator::getOptions() const {
      return _options;
    }

    Calibrator::Options& Calibrator::getOptions() {
      return _options;
    }

    const Calibrator::OdometryDesignVariablesSP&
        Calibrator::getOdometryDesignVariables() const {
      return _odometryDesignVariables;
    }

    Calibrator::OdometryDesignVariablesSP&
        Calibrator::getOdometryDesignVariables() {
      return _odometryDesignVariables;
    }

    const Calibrator::IncrementalEstimatorSP Calibrator::getEstimator() const {
      return _estimator;
    }

    Calibrator::IncrementalEstimatorSP Calibrator::getEstimator() {
      return _estimator;
    }

    bool Calibrator::unprocessedMeasurements() const {
      return !_poseMeasurements.empty() ||
        !_leftWheelSpeedMeasurements.empty() ||
        !_rightWheelSpeedMeasurements.empty();
    }

    const std::vector<double> Calibrator::getInformationGainHistory() const {
      return _infoGainHistory;
    }

    const std::vector<Eigen::VectorXd>
        Calibrator::getOdometryVariablesHistory() const {
      return _odometryVariablesHistory;
    }

    Eigen::VectorXd Calibrator::getOdometryVariablesVariance() const {
      return _estimator->getSigma2Theta().diagonal();
    }

    const Calibrator::TranslationSplineSP&
        Calibrator::getTranslationSpline() const {
      return _translationSpline;
    }

    const Calibrator::RotationSplineSP&
        Calibrator::getRotationSpline() const {
      return _rotationSpline;
    }

    const Calibrator::PoseMeasurements& Calibrator::getPoseMeasurements()
        const {
      return _poseMeasurements;
    }

    const Calibrator::PoseMeasurements& Calibrator::getPosePredictions() const {
      return _poseMeasurementsPred;
    }

    const std::vector<Eigen::VectorXd>& Calibrator::getPosePredictionErrors()
        const {
      return _poseMeasurementsPredErrors;
    }

    const std::vector<double>& Calibrator::getPosePredictionErrors2() const {
      return _poseMeasurementsPredErrors2;
    }

    const Calibrator::WheelSpeedMeasurements&
        Calibrator::getLeftWheelMeasurements() const {
      return _leftWheelSpeedMeasurements;
    }

    const Calibrator::WheelSpeedMeasurements&
        Calibrator::getLeftWheelPredictions() const {
      return _leftWheelSpeedMeasurementsPred;
    }

    const std::vector<Eigen::VectorXd>&
        Calibrator::getLeftWheelPredictionErrors() const {
      return _leftWheelSpeedMeasurementsPredErrors;
    }

    const std::vector<double>& Calibrator::getLeftWheelPredictionErrors2()
        const {
      return _leftWheelSpeedMeasurementsPredErrors2;
    }

    const Calibrator::WheelSpeedMeasurements&
        Calibrator::getRightWheelMeasurements() const {
      return _rightWheelSpeedMeasurements;
    }

    const Calibrator::WheelSpeedMeasurements&
        Calibrator::getRightWheelPredictions() const {
      return _rightWheelSpeedMeasurementsPred;
    }

    const std::vector<Eigen::VectorXd>&
        Calibrator::getRightWheelPredictionErrors() const {
      return _rightWheelSpeedMeasurementsPredErrors;
    }

    const std::vector<double>& Calibrator::getRightWheelPredictionErrors2()
        const {
      return _rightWheelSpeedMeasurementsPredErrors2;
    }

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

    void Calibrator::clearPredictions() {
      _poseMeasurementsPred.clear();
      _poseMeasurementsPredErrors.clear();
      _poseMeasurementsPredErrors2.clear();
      _leftWheelSpeedMeasurementsPred.clear();
      _leftWheelSpeedMeasurementsPredErrors.clear();
      _leftWheelSpeedMeasurementsPredErrors2.clear();
      _rightWheelSpeedMeasurementsPred.clear();
      _rightWheelSpeedMeasurementsPredErrors.clear();
      _rightWheelSpeedMeasurementsPredErrors2.clear();
    }

    void Calibrator::predict() {
      auto batch = boost::make_shared<OptimizationProblemSpline>();
      _odometryDesignVariables->addToBatch(batch, 1);
      initSplines(_poseMeasurements);
      predictPoses(_poseMeasurements);
      predictLeftWheel(_leftWheelSpeedMeasurements);
      predictRightWheel(_rightWheelSpeedMeasurements);
    }

    void Calibrator::addPoseMeasurement(const PoseMeasurement& pose, NsecTime
        timestamp) {
      addMeasurement(timestamp);
      _poseMeasurements.push_back(std::make_pair(timestamp, pose));
    }

    void Calibrator::addLeftWheelMeasurement(const WheelSpeedMeasurement& data,
        NsecTime timestamp) {
      addMeasurement(timestamp);
      _leftWheelSpeedMeasurements.push_back(std::make_pair(timestamp, data));
    }

    void Calibrator::addRightWheelMeasurement(const WheelSpeedMeasurement& data,
        NsecTime timestamp) {
      addMeasurement(timestamp);
      _rightWheelSpeedMeasurements.push_back(std::make_pair(timestamp, data));
    }

    void Calibrator::addMeasurement(NsecTime timestamp) {
      _lastTimestamp = timestamp;
      if (_currentBatchStartTimestamp == -1)
        _currentBatchStartTimestamp = timestamp;
      if (nsecToSec(timestamp - _currentBatchStartTimestamp) >=
          _options.windowDuration)
        addMeasurements();
    }

    void Calibrator::addMeasurements() {
      if (_poseMeasurements.size() < 2)
        return;

      auto batch = boost::make_shared<OptimizationProblemSpline>();
      _odometryDesignVariables->addToBatch(batch, 1);
      initSplines(_poseMeasurements);
      batch->addSpline(_translationSpline, 0);
      batch->addSpline(_rotationSpline, 0);
      addPoseErrorTerms(_poseMeasurements, batch);
      addLeftWheelErrorTerms(_leftWheelSpeedMeasurements, batch);
      addRightWheelErrorTerms(_rightWheelSpeedMeasurements, batch);
      clearMeasurements();
      _currentBatchStartTimestamp = _lastTimestamp;
      batch->setGroupsOrdering({0, 1});
      if (_options.verbose) {
        std::cout << "calibration before batch: " << std::endl;
        std::cout << *_odometryDesignVariables << std::endl;
      }
      IncrementalEstimator::ReturnValue ret = _estimator->addBatch(batch);
      if (_options.verbose) {
        std::cout << "IG: " << ret.informationGain << std::endl;
        ret.batchAccepted ? std::cout << "ACCEPTED" : std::cout << "REJECTED";
        std::cout << std::endl;
        std::cout << "calibration after batch: " << std::endl;
        std::cout << *_odometryDesignVariables << std::endl;
        std::cout << "singular values: " << std::endl
          << ret.singularValuesScaled << std::endl;
        std::cout << "observability: " << std::endl;
        std::cout << "b: " << ret.obsBasisScaled.row(0).norm() << std::endl;
        std::cout << "k_l: " << ret.obsBasisScaled.row(1).norm() << std::endl;
        std::cout << "t_l: " << ret.obsBasisScaled.row(2).norm() << std::endl;
        std::cout << "k_r: " << ret.obsBasisScaled.row(3).norm() << std::endl;
        std::cout << "t_r: " << ret.obsBasisScaled.row(4).norm() << std::endl;
        std::cout << "v_r_vp_1: " << ret.obsBasisScaled.row(5).norm()
          << std::endl;
        std::cout << "v_r_vp_2: " << ret.obsBasisScaled.row(6).norm()
          << std::endl;
        std::cout << "v_r_vp_3: " << ret.obsBasisScaled.row(7).norm()
          << std::endl;
        std::cout << "v_R_p_1: " << ret.obsBasisScaled.row(8).norm()
          << std::endl;
        std::cout << "v_R_p_2: " << ret.obsBasisScaled.row(9).norm()
          << std::endl;
        std::cout << "v_R_p_3: " << ret.obsBasisScaled.row(10).norm()
          << std::endl;
      }
      _infoGainHistory.push_back(ret.informationGain);
      _odometryVariablesHistory.push_back(
        _odometryDesignVariables->getParameters());
    }

    void Calibrator::initSplines(const PoseMeasurements& measurements) {
      const size_t numMeasurements = measurements.size();
      std::vector<NsecTime> timestamps;
      timestamps.reserve(numMeasurements);
      std::vector<Eigen::Vector3d> transPoses;
      transPoses.reserve(numMeasurements);
      std::vector<Eigen::Vector4d> rotPoses;
      rotPoses.reserve(numMeasurements);
      const EulerAnglesYawPitchRoll ypr;
      for (auto it = measurements.cbegin(); it != measurements.cend(); ++it) {
        auto timestamp = it->first;
        const Eigen::Matrix3d w_R_p =
          ypr.parametersToRotationMatrix(it->second.w_R_p);
        const Eigen::Vector4d& v_q_p =
          _odometryDesignVariables->v_R_p->getQuaternion();
        const Eigen::Matrix3d v_R_p = quat2r(v_q_p);
        const Eigen::Matrix3d w_R_v = w_R_p * v_R_p.transpose();
        Eigen::Vector4d w_q_v = r2quat(w_R_v);
        if (!rotPoses.empty()) {
          const Eigen::Vector4d lastRotPose = rotPoses.back();
          w_q_v = bestQuat(lastRotPose, w_q_v);
        }
        timestamps.push_back(timestamp);
        rotPoses.push_back(w_q_v);
        Eigen::MatrixXd v_r_vp;
        _odometryDesignVariables->v_r_vp->getParameters(v_r_vp);
        transPoses.push_back(it->second.w_r_wp - w_R_v * v_r_vp);
      }
      const double elapsedTime = (timestamps.back() - timestamps.front()) /
        (double)NsecTimePolicy::getOne();
      const int measPerSec = std::round(numMeasurements / elapsedTime);
      int numSegments;
      if (measPerSec > _options.splineKnotsPerSecond)
        numSegments = std::ceil(_options.splineKnotsPerSecond * elapsedTime);
      else
        numSegments = numMeasurements;

      _translationSpline = boost::make_shared<TranslationSpline>(
        EuclideanBSpline<Eigen::Dynamic, 3, NsecTimePolicy>::CONF(
        EuclideanBSpline<Eigen::Dynamic, 3,
        NsecTimePolicy>::CONF::ManifoldConf(3), _options.transSplineOrder));
      BSplineFitter<TranslationSpline>::initUniformSpline(*_translationSpline,
        timestamps, transPoses, numSegments, _options.transSplineLambda);

      _rotationSpline = boost::make_shared<RotationSpline>(
        UnitQuaternionBSpline<Eigen::Dynamic, NsecTimePolicy>::CONF(
        UnitQuaternionBSpline<Eigen::Dynamic,
        NsecTimePolicy>::CONF::ManifoldConf(), _options.rotSplineOrder));
      BSplineFitter<RotationSpline>::initUniformSpline(*_rotationSpline,
        timestamps, rotPoses, numSegments, _options.rotSplineLambda);
    }

    void Calibrator::addPoseErrorTerms(const PoseMeasurements& measurements,
        const OptimizationProblemSplineSP& batch) {
      for (auto it = measurements.cbegin(); it != measurements.cend(); ++it) {
        auto timestamp = it->first;
        ErrorTermPose::Input w_T_p;
        w_T_p.head<3>() = it->second.w_r_wp;
        w_T_p.tail<3>() = it->second.w_R_p;
        ErrorTermPose::Covariance Q = ErrorTermPose::Covariance::Zero();
        Q.topLeftCorner<3, 3>() = it->second.sigma2_w_r_wp;
        Q.bottomRightCorner<3, 3>() = it->second.sigma2_w_R_p;
        auto translationExpressionFactory =
          _translationSpline->getExpressionFactoryAt<0>(timestamp);
        auto rotationExpressionFactory =
          _rotationSpline->getExpressionFactoryAt<0>(timestamp);

        auto v_r_vp = EuclideanExpression(_odometryDesignVariables->v_r_vp);
        auto w_R_v = Vector2RotationQuaternionExpressionAdapter::adapt(
          rotationExpressionFactory.getValueExpression());
        auto w_r_vp = w_R_v * v_r_vp;
        auto w_r_wv = EuclideanExpression(
          translationExpressionFactory.getValueExpression());
        auto w_r_wp = w_r_wv + w_r_vp;
        auto v_R_p = RotationExpression(_odometryDesignVariables->v_R_p);
        auto w_R_p = w_R_v * v_R_p;
        auto e_pose = boost::make_shared<ErrorTermPose>(
          TransformationExpression(w_R_p, w_r_wp), w_T_p, Q);
        batch->addErrorTerm(e_pose);
      }
    }

    void Calibrator::predictPoses(const PoseMeasurements& measurements) {
      for (auto it = measurements.cbegin(); it != measurements.cend(); ++it) {
        auto timestamp = it->first;
        ErrorTermPose::Input w_T_p;
        w_T_p.head<3>() = it->second.w_r_wp;
        w_T_p.tail<3>() = it->second.w_R_p;
        ErrorTermPose::Covariance Q = ErrorTermPose::Covariance::Zero();
        Q.topLeftCorner<3, 3>() = it->second.sigma2_w_r_wp;
        Q.bottomRightCorner<3, 3>() = it->second.sigma2_w_R_p;
        auto translationExpressionFactory =
          _translationSpline->getExpressionFactoryAt<0>(timestamp);
        auto rotationExpressionFactory =
          _rotationSpline->getExpressionFactoryAt<0>(timestamp);

        auto v_r_vp = EuclideanExpression(_odometryDesignVariables->v_r_vp);
        auto w_R_v = Vector2RotationQuaternionExpressionAdapter::adapt(
          rotationExpressionFactory.getValueExpression());
        auto w_r_vp = w_R_v * v_r_vp;
        auto w_r_wv = EuclideanExpression(
          translationExpressionFactory.getValueExpression());
        auto w_r_wp = w_r_wv + w_r_vp;
        auto v_R_p = RotationExpression(_odometryDesignVariables->v_R_p);
        auto w_R_p = w_R_v * v_R_p;
        auto e_pose = boost::make_shared<ErrorTermPose>(
          TransformationExpression(w_R_p, w_r_wp), w_T_p, Q);
        auto sr = e_pose->evaluateError();
        auto error = e_pose->error();
        PoseMeasurement pose;
        pose.w_r_wp = w_r_wp.toValue();
        const EulerAnglesYawPitchRoll ypr;
        pose.w_R_p = ypr.rotationMatrixToParameters(w_R_p.toRotationMatrix());
        _poseMeasurementsPred.push_back(std::make_pair(timestamp, pose));
        _poseMeasurementsPredErrors.push_back(error);
        _poseMeasurementsPredErrors2.push_back(sr);
      }
    }

    void Calibrator::addLeftWheelErrorTerms(const WheelSpeedMeasurements&
        measurements, const OptimizationProblemSplineSP& batch) {
      for (auto it = measurements.cbegin(); it != measurements.cend(); ++it) {
        auto timestamp = it->first;
        auto timeDelay = _odometryDesignVariables->t_l->toExpression();
        auto timestampDelay = timeDelay +
          GenericScalarExpression<OdometryDesignVariables::Time>(timestamp);
        auto Tmax = _translationSpline->getMaxTime();
        auto Tmin = _translationSpline->getMinTime();
        auto lBound = -_options.delayBound +
          timestampDelay.toScalar().getNumerator();
        auto uBound = _options.delayBound +
          timestampDelay.toScalar().getNumerator();

        if(uBound > Tmax || lBound < Tmin)
          continue;

        auto translationExpressionFactory =
          _translationSpline->getExpressionFactoryAt<1>(timestampDelay,
          lBound, uBound);
        auto rotationExpressionFactory =
          _rotationSpline->getExpressionFactoryAt<1>(timestampDelay,
          lBound, uBound);

        auto w_R_v = Vector2RotationQuaternionExpressionAdapter::adapt(
          rotationExpressionFactory.getValueExpression());
        auto w_v_wv = EuclideanExpression(
          translationExpressionFactory.getValueExpression(1));
        auto v_v_wv = w_R_v.inverse() * w_v_wv;
        auto w_om_wv = -EuclideanExpression(
          rotationExpressionFactory.getAngularVelocityExpression());
        auto v_om_wv = w_R_v.inverse() * w_om_wv;
        auto b = ScalarExpression(_odometryDesignVariables->b);
        auto v_r_wl = EuclideanExpression(Eigen::Vector3d(0.0, 1.0, 0.0)) * b;
        auto v_v_wl = v_v_wv + v_om_wv.cross(v_r_wl);

        auto e_lw = boost::make_shared<ErrorTermWheel>(v_v_wl,
          ScalarExpression(_odometryDesignVariables->k_l),
          it->second.value, Eigen::Vector3d(_options.lwVariance,
          _options.vyVariance, _options.vzVariance).asDiagonal(), false);
        batch->addErrorTerm(e_lw);
      }
    }

    void Calibrator::predictLeftWheel(const WheelSpeedMeasurements&
        measurements) {
      for (auto it = measurements.cbegin(); it != measurements.cend(); ++it) {
        auto timestamp = it->first;
        auto timeDelay = _odometryDesignVariables->t_l->toExpression();
        auto timestampDelay = timeDelay +
          GenericScalarExpression<OdometryDesignVariables::Time>(timestamp);
        auto Tmax = _translationSpline->getMaxTime();
        auto Tmin = _translationSpline->getMinTime();
        auto lBound = -_options.delayBound +
          timestampDelay.toScalar().getNumerator();
        auto uBound = _options.delayBound +
          timestampDelay.toScalar().getNumerator();

        if(uBound > Tmax || lBound < Tmin)
          continue;

        auto translationExpressionFactory =
          _translationSpline->getExpressionFactoryAt<1>(timestampDelay,
          lBound, uBound);
        auto rotationExpressionFactory =
          _rotationSpline->getExpressionFactoryAt<1>(timestampDelay,
          lBound, uBound);

        auto w_R_v = Vector2RotationQuaternionExpressionAdapter::adapt(
          rotationExpressionFactory.getValueExpression());
        auto w_v_wv = EuclideanExpression(
          translationExpressionFactory.getValueExpression(1));
        auto v_v_wv = w_R_v.inverse() * w_v_wv;
        auto w_om_wv = -EuclideanExpression(
          rotationExpressionFactory.getAngularVelocityExpression());
        auto v_om_wv = w_R_v.inverse() * w_om_wv;
        auto b = ScalarExpression(_odometryDesignVariables->b);
        auto v_r_wl = EuclideanExpression(Eigen::Vector3d(0.0, 1.0, 0.0)) * b;
        auto v_v_wl = v_v_wv + v_om_wv.cross(v_r_wl);

        auto e_lw = boost::make_shared<ErrorTermWheel>(v_v_wl,
          ScalarExpression(_odometryDesignVariables->k_l),
          it->second.value, Eigen::Vector3d(_options.lwVariance,
          _options.vyVariance, _options.vzVariance).asDiagonal(), false);
        auto sr_l = e_lw->evaluateError();
        auto error_l = e_lw->error();
        double v0 = v_v_wl.toValue()(0);
        double v1 = v_v_wl.toValue()(1);
        double k = _odometryDesignVariables->k_l->toScalar();
        double temp = std::sqrt(v1 * v1 / (v0 * v0) + 1);
        WheelSpeedMeasurement data;
        data.value = k * (v0 / temp + v1 * v1 / (v0 * temp));
        _leftWheelSpeedMeasurementsPred.push_back(std::make_pair(timestamp,
          data));
        _leftWheelSpeedMeasurementsPredErrors.push_back(error_l);
        _leftWheelSpeedMeasurementsPredErrors2.push_back(sr_l);
      }
    }

    void Calibrator::addRightWheelErrorTerms(const WheelSpeedMeasurements&
        measurements, const OptimizationProblemSplineSP& batch) {
      for (auto it = measurements.cbegin(); it != measurements.cend(); ++it) {
        auto timestamp = it->first;
        auto timeDelay = _odometryDesignVariables->t_r->toExpression();
        auto timestampDelay = timeDelay +
          GenericScalarExpression<OdometryDesignVariables::Time>(timestamp);
        auto Tmax = _translationSpline->getMaxTime();
        auto Tmin = _translationSpline->getMinTime();
        auto lBound = -_options.delayBound +
          timestampDelay.toScalar().getNumerator();
        auto uBound = _options.delayBound +
          timestampDelay.toScalar().getNumerator();

        if(uBound > Tmax || lBound < Tmin)
          continue;

        auto translationExpressionFactory =
          _translationSpline->getExpressionFactoryAt<1>(timestampDelay,
          lBound, uBound);
        auto rotationExpressionFactory =
          _rotationSpline->getExpressionFactoryAt<1>(timestampDelay,
          lBound, uBound);

        auto w_R_v = Vector2RotationQuaternionExpressionAdapter::adapt(
          rotationExpressionFactory.getValueExpression());
        auto w_v_wv = EuclideanExpression(
          translationExpressionFactory.getValueExpression(1));
        auto v_v_wv = w_R_v.inverse() * w_v_wv;
        auto w_om_wv = -EuclideanExpression(
          rotationExpressionFactory.getAngularVelocityExpression());
        auto v_om_wv = w_R_v.inverse() * w_om_wv;
        auto b = ScalarExpression(_odometryDesignVariables->b);
        auto v_r_wr =
          -EuclideanExpression(Eigen::Vector3d(0.0, 1.0, 0.0)) * b;
        auto v_v_wr = v_v_wv + v_om_wv.cross(v_r_wr);

        auto e_rw = boost::make_shared<ErrorTermWheel>(v_v_wr,
          ScalarExpression(_odometryDesignVariables->k_r),
          it->second.value, Eigen::Vector3d(_options.rwVariance,
          _options.vyVariance, _options.vzVariance).asDiagonal(), false);
        batch->addErrorTerm(e_rw);
      }
    }

    void Calibrator::predictRightWheel(const WheelSpeedMeasurements&
        measurements) {
      for (auto it = measurements.cbegin(); it != measurements.cend(); ++it) {
        auto timestamp = it->first;
        auto timeDelay = _odometryDesignVariables->t_r->toExpression();
        auto timestampDelay = timeDelay +
          GenericScalarExpression<OdometryDesignVariables::Time>(timestamp);
        auto Tmax = _translationSpline->getMaxTime();
        auto Tmin = _translationSpline->getMinTime();
        auto lBound = -_options.delayBound +
          timestampDelay.toScalar().getNumerator();
        auto uBound = _options.delayBound +
          timestampDelay.toScalar().getNumerator();

        if(uBound > Tmax || lBound < Tmin)
          continue;

        auto translationExpressionFactory =
          _translationSpline->getExpressionFactoryAt<1>(timestampDelay,
          lBound, uBound);
        auto rotationExpressionFactory =
          _rotationSpline->getExpressionFactoryAt<1>(timestampDelay,
          lBound, uBound);

        auto w_R_v = Vector2RotationQuaternionExpressionAdapter::adapt(
          rotationExpressionFactory.getValueExpression());
        auto w_v_wv = EuclideanExpression(
          translationExpressionFactory.getValueExpression(1));
        auto v_v_wv = w_R_v.inverse() * w_v_wv;
        auto w_om_wv = -EuclideanExpression(
          rotationExpressionFactory.getAngularVelocityExpression());
        auto v_om_wv = w_R_v.inverse() * w_om_wv;
        auto b = ScalarExpression(_odometryDesignVariables->b);
        auto v_r_wr = -EuclideanExpression(Eigen::Vector3d(0.0, 1.0, 0.0)) * b;
        auto v_v_wr = v_v_wv + v_om_wv.cross(v_r_wr);

        auto e_rw = boost::make_shared<ErrorTermWheel>(v_v_wr,
          ScalarExpression(_odometryDesignVariables->k_r),
          it->second.value, Eigen::Vector3d(_options.rwVariance,
          _options.vyVariance, _options.vzVariance).asDiagonal(), false);
        auto sr_r = e_rw->evaluateError();
        auto error_r = e_rw->error();
        double v0 = v_v_wr.toValue()(0);
        double v1 = v_v_wr.toValue()(1);
        double k = _odometryDesignVariables->k_r->toScalar();
        double temp = std::sqrt(v1 * v1 / (v0 * v0) + 1);
        WheelSpeedMeasurement data;
        data.value = k * (v0 / temp + v1 * v1 / (v0 * temp));
        _rightWheelSpeedMeasurementsPred.push_back(std::make_pair(timestamp,
          data));
        _rightWheelSpeedMeasurementsPredErrors.push_back(error_r);
        _rightWheelSpeedMeasurementsPredErrors2.push_back(sr_r);
      }
    }

    void Calibrator::clearMeasurements() {
      _poseMeasurements.clear();
      _leftWheelSpeedMeasurements.clear();
      _rightWheelSpeedMeasurements.clear();
    }

  }
}


// File: finite_grad_check.cc
// Author: Karl Moritz Hermann (mail@karlmoritz.com)
// Created: 01-01-2013
// Last Update: Thu 03 Oct 2013 11:44:36 AM BST

// STL
#include <iostream>
#include <cmath>

// Boost
#include <boost/program_options/variables_map.hpp>
#include <boost/program_options/parsers.hpp>

#include "shared_defs.h"

// L-BFGS
#include <lbfgs.h>

// Local
#include "finite_grad_check.h"
#include "train_update.h"
#include "recursive_autoencoder.h"

using namespace std;
namespace bpo = boost::program_options;

int finite_grad_check(Model &model)
{
  Real* vars = nullptr;
  int number_vars = 0;

  modvars<int> counts;
  model.rae.setIncrementalCounts(counts,vars,number_vars);

  WeightVectorType theta(vars,number_vars);

  Real* data1 = new Real[number_vars]();
  Real* data2 = new Real[number_vars]();
  WeightVectorType grad(data1,number_vars);
  WeightVectorType grad2(data2,number_vars);

  Real error1 = computeCostAndGrad(model,nullptr,data1,number_vars);

  modvars<Real> dists;
  Real dist = 0.0;

  Real delta = 1.0e-7;

  for (int i=0;i<number_vars;++i)
  {
    theta[i] += delta;
    Real error2 = computeCostAndGrad(model,nullptr,data2,number_vars);
    Real xdev = (error2 - error1) / delta;
    theta[i] -= delta;
    if (i < counts.D)   { dists.D += abs(grad2[i] - xdev);   cout << "D   "; }
    else if (i < counts.U)  { dists.U += abs(grad2[i] - xdev);  cout << "U  "; }
    else if (i < counts.V)  { dists.V += abs(grad2[i] - xdev);  cout << "V  "; }
    else if (i < counts.W)  { dists.W += abs(grad2[i] - xdev);  cout << "W  "; }
    else if (i < counts.A)  { dists.A += abs(grad2[i] - xdev);  cout << "A  "; }
    else if (i < counts.Wd)  { dists.Wd += abs(grad2[i] - xdev);  cout << "Wd  "; }
    else if (i < counts.Wdr) { dists.Wdr += abs(grad2[i] - xdev); cout << "Wdr "; }
    else if (i < counts.Bd)  { dists.Bd += abs(grad2[i] - xdev);  cout << "Bd  "; }
    else if (i < counts.Bdr) { dists.Bdr += abs(grad2[i] - xdev); cout << "Bdr "; }
    else if (i < counts.Wf)  { dists.Wf += abs(grad2[i] - xdev);  cout << "Wf  "; }
    else if (i < counts.Wl)  { dists.Wl += abs(grad2[i] - xdev);  cout << "Wl  "; }
    else if (i < counts.Bl)  { dists.Bl += abs(grad2[i] - xdev);  cout << "Bl  "; }

    //template<> void modvars<int>::init() { D = 0; U = 0; V = 0; W = 0; A = 0; Wd = 0; Wdr = 0; Bd = 0; Bdr = 0; Wf = 0; Wl = 0; Bl = 0; alpha_rae = 0; alpha_lbl = 0; }
    cout << i << ": " << grad2[i] << " vs " << (xdev) << "   " << error2 << " - " << error1 << "[" << theta[i] << "]" << endl;

    dist += abs(grad2[i] - xdev);
  }

  cout << "total: " << dist << " D/U/V/W/A/Wd/Wdr/Bd/Bdr/Wf/Wl/Bl " << endl;
  cout << dists.D << " ";
  cout << dists.U << " ";
  cout << dists.V << " ";
  cout << dists.W << " ";
  cout << dists.A << " ";
  cout << dists.Wd << " " << dists.Wdr << " " << dists.Bd << " " << dists.Bdr << " ";
  cout << dists.Wf << " ";
  cout << dists.Wl << " " << dists.Bl << endl;
  // Don't care about what's next
  assert(false);

  return 0;
}


#include <boost/test/unit_test.hpp>

#include <math.h>

#include "registration_check_common.hpp"
#include "registration_helpers.hpp"
#include "actor.hpp"

#include <scorum/chain/evaluators/registration_pool_evaluator.hpp>

#include <sstream>

using namespace database_fixture;

class create_by_committee_evaluator_check_fixture : public registration_check_fixture
{
public:
    create_by_committee_evaluator_check_fixture()
    {
        this->set_hardfork(SCORUM_HARDFORK_0_2);

        genesis_state = create_registration_genesis();
        create_registration_objects(genesis_state);
        predictor.initialize(registration_supply(), registration_bonus(), genesis_state.registration_schedule);

        schedule_input = genesis_state.registration_schedule;
    }

    schedule_inputs_type schedule_input;
    registration_helpers::predictor predictor;
    registration_helpers::fuzzer fuzzer;

    const account_object& create_new_by_committee()
    {
        static int next_id = 1;
        std::stringstream store;
        store << "user" << next_id++;
        Actor new_account(store.str());

        account_create_by_committee_operation op;
        op.creator = committee_member().name;
        op.new_account_name = new_account.name;
        op.owner = authority(1, new_account.public_key, 1);
        op.active = authority(1, new_account.public_key, 1);
        op.posting = authority(1, new_account.public_key, 1);
        op.memo_key = new_account.public_key;
        op.json_metadata = "";

        registration_pool_evaluator ev(db);
        ev.do_apply(op);

        return account_service.get_account(new_account.name);
    }
};

BOOST_FIXTURE_TEST_SUITE(create_by_committee_evaluator_check, create_by_committee_evaluator_check_fixture)

SCORUM_TEST_CASE(allocate_through_all_schedule_stages_per_one_block_check)
{
    const registration_pool_object& pool = registration_pool_service.get();

    BOOST_REQUIRE_EQUAL(pool.schedule_items.size(), schedule_input.size());

    // alice registers users per block from stage #1 to #schedule_input.size() (step is named as input_pos)
    auto it = schedule_input.begin();
    for (uint64_t input_pos = 0, stage = 0; it != schedule_input.end(); ++it, ++stage)
    {
        for (uint64_t register_attempts = 0; register_attempts < (uint64_t)(*it).users;
             ++register_attempts, ++input_pos)
        {
            generate_block();

            asset predicted_bonus = predictor.schedule_input_predicted_allocate_by_pos(input_pos, pool.maximum_bonus);

            asset allocated_bonus(0, SCORUM_SYMBOL);
            db_plugin->debug_update(
                [&](database&) {
                    const account_object& account = create_new_by_committee();
                    allocated_bonus = asset(account.scorumpower.amount, SCORUM_SYMBOL);
                },
                get_skip_flags());

            BOOST_REQUIRE_EQUAL(allocated_bonus, predicted_bonus);
        }
    }
}

SCORUM_TEST_CASE(allocate_through_all_schedule_stages_per_wave_block_distribution_check)
{
    const registration_pool_object& pool = registration_pool_service.get();

    BOOST_REQUIRE_EQUAL(pool.schedule_items.size(), schedule_input.size());

    uint64_t input_max_pos = predictor.schedule_input_max_pos();
    asset maximum_bonus = pool.maximum_bonus;

    // alice registers users per block from stage #1 to #schedule_input_size (step is named as input_pos)
    auto it = schedule_input.begin();
    for (uint64_t input_pos = 0, stage = 0; it != schedule_input.end(); ++it, ++stage)
    {
        for (uint64_t register_attempts = 0; register_attempts < (uint64_t)(*it).users;
             ++register_attempts, ++input_pos)
        {
            uint64_t ampl
                = fuzzer.get_sin_f(input_pos, input_max_pos, SCORUM_REGISTRATION_BONUS_LIMIT_PER_MEMBER_N_BLOCK + 1);
            // produce enough block amount to break out of sliding window in center of schedule
            ampl++; // make at least one generate_block
            for (uint64_t cblock = 0; cblock < ampl; ++cblock)
            {
                generate_block();
            }

            asset predicted_bonus = predictor.schedule_input_predicted_allocate_by_pos(input_pos, maximum_bonus);

            asset allocated_bonus(0, SCORUM_SYMBOL);
            db_plugin->debug_update(
                [&](database&) {
                    const account_object& account = create_new_by_committee();
                    allocated_bonus = asset(account.scorumpower.amount, SCORUM_SYMBOL);
                },
                get_skip_flags());

            BOOST_CHECK_EQUAL(allocated_bonus, predicted_bonus);
        }
    }
}

SCORUM_TEST_CASE(allocate_limits_check)
{
    const registration_pool_object& pool = registration_pool_service.get();

    BOOST_REQUIRE_EQUAL(pool.schedule_items.size(), schedule_input.size());

    asset predicted_limit = predictor.schedule_input_predicted_limit(1);

    auto fn = [&](uint64_t) -> asset {
        const account_object& account = create_new_by_committee();
        asset allocated_bonus(account.scorumpower.amount, SCORUM_SYMBOL);
        return allocated_bonus;
    };

    uint64_t input_pos = 0;
    bool found_limit = predictor.schedule_input_pos_reach_limit(input_pos, predictor.schedule_input_max_pos(),
                                                                predicted_limit, pool.maximum_bonus, fn);
    BOOST_REQUIRE(found_limit);

    // limit must be reached
    BOOST_REQUIRE_THROW(fn(0), fc::assert_exception);
}

SCORUM_TEST_CASE(allocate_limits_through_blocks_through_window_check)
{
    const registration_pool_object& pool = registration_pool_service.get();

    BOOST_REQUIRE_EQUAL(pool.schedule_items.size(), schedule_input.size());

    asset predicted_limit = predictor.schedule_input_predicted_limit(1);

    auto fn = [&](uint64_t) -> asset {
        const account_object& account = create_new_by_committee();
        asset allocated_bonus(account.scorumpower.amount, SCORUM_SYMBOL);
        return allocated_bonus;
    };

    asset allocated_bonus(0, SCORUM_SYMBOL);
    auto fn_db = [&](uint64_t) -> asset {
        db_plugin->debug_update(
            [&](database&) {
                // std::cout << allocated_bonus << std::endl;
                allocated_bonus = fn(0);
            },
            get_skip_flags());
        return allocated_bonus;
    };

    uint64_t input_pos = 0;
    uint64_t input_max_pos = predictor.schedule_input_max_pos();

    generate_block();

    bool found_limit = predictor.schedule_input_pos_reach_limit(input_pos, input_max_pos, predicted_limit,
                                                                pool.maximum_bonus, fn_db);
    BOOST_REQUIRE(found_limit);

    BOOST_REQUIRE_THROW(fn(0), fc::assert_exception);

    // move to next sliding window pos (reset limit)
    for (int ci = 0; ci < SCORUM_REGISTRATION_BONUS_LIMIT_PER_MEMBER_N_BLOCK; ++ci)
    {
        generate_block();
    }

    predicted_limit = predictor.schedule_input_predicted_limit(1);

    // find next limit
    found_limit = predictor.schedule_input_pos_reach_limit(input_pos, input_max_pos, predicted_limit,
                                                           pool.maximum_bonus, fn_db);
    BOOST_REQUIRE(found_limit);

    // limit must be reached
    BOOST_REQUIRE_THROW(fn(0), fc::assert_exception);
}

SCORUM_TEST_CASE(allocate_out_of_schedule_remain_check)
{
    const registration_pool_object& pool = registration_pool_service.get();

    generate_block();

    BOOST_REQUIRE_EQUAL(pool.schedule_items.size(), schedule_input.size());

    uint64_t input_max_pos = predictor.schedule_input_max_pos();

    asset total_balance = pool.balance;
    asset total_allocated_bonus(0, SCORUM_SYMBOL);

    // alice registers users per block from stage #1 to #schedule_input_size (step is named as input_pos)
    auto fn = [&]() -> asset {

        // to prevent reaching limit
        generate_blocks(SCORUM_REGISTRATION_BONUS_LIMIT_PER_MEMBER_N_BLOCK);

        asset allocated_bonus(0, SCORUM_SYMBOL);
        db_plugin->debug_update(
            [&](database&) {
                const account_object& account = create_new_by_committee();
                allocated_bonus = asset(account.scorumpower.amount, SCORUM_SYMBOL);
            },
            get_skip_flags());
        return allocated_bonus;
    };
    total_allocated_bonus = predictor.schedule_input_vest_all(fn);

    BOOST_CHECK_EQUAL(total_allocated_bonus, predictor.schedule_input_total_bonus(pool.maximum_bonus));
    BOOST_CHECK_EQUAL(pool.balance, total_balance - total_allocated_bonus);
    BOOST_CHECK_EQUAL(pool.already_allocated_count, input_max_pos);
    BOOST_CHECK_EQUAL(pool.balance, rest_of_supply());
}

SCORUM_TEST_CASE(autoclose_pool_with_valid_vesting_rest_check)
{
    const registration_pool_object& pool = registration_pool_service.get();

    asset total_allocated_bonus(0, SCORUM_SYMBOL);

    // alice registers users per block from stage #1 to #schedule_input_size (step is named as input_pos)
    auto fn = [&]() -> asset {

        // to prevent reaching limit
        generate_blocks(SCORUM_REGISTRATION_BONUS_LIMIT_PER_MEMBER_N_BLOCK);

        asset allocated_bonus(0, SCORUM_SYMBOL);
        db_plugin->debug_update(
            [&](database&) {
                const account_object& account = create_new_by_committee();
                allocated_bonus = asset(account.scorumpower.amount, SCORUM_SYMBOL);
            },
            get_skip_flags());
        return allocated_bonus;
    };
    total_allocated_bonus = predictor.schedule_input_vest_all(fn);

    BOOST_REQUIRE_EQUAL(total_allocated_bonus, predictor.schedule_input_total_bonus(pool.maximum_bonus));

    BOOST_REQUIRE_GT(rest_of_supply(), asset(0, SCORUM_SYMBOL));

    asset last_bonus = predictor.schedule_input_predicted_allocate_by_pos(predictor.schedule_input_max_pos() - 1,
                                                                          pool.maximum_bonus);

    BOOST_REQUIRE_GT(last_bonus, asset(0, SCORUM_SYMBOL));

    int last_regs = (int)rest_of_supply().amount.value / (int)last_bonus.amount.value;

    BOOST_REQUIRE_GT(last_regs, 0);
    for (int ci = 0; ci < last_regs; ++ci)
    {
        // to prevent reaching limit
        generate_blocks(SCORUM_REGISTRATION_BONUS_LIMIT_PER_MEMBER_N_BLOCK);

        db_plugin->debug_update([&](database&) { create_new_by_committee(); }, get_skip_flags());
    }

    BOOST_CHECK_EQUAL(registration_pool_service.get().balance, ASSET_NULL_SCR);
}

BOOST_AUTO_TEST_SUITE_END()


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

#include <boost/simd/sdk/simd/native_fwd.hpp>
#include <boost/simd/sdk/simd/meta/vector_of.hpp>
#include <boost/simd/sdk/simd/meta/as_simd.hpp>
#include <boost/dispatch/meta/model_of.hpp>
#include <boost/dispatch/meta/primitive_of.hpp>
#include <boost/simd/sdk/simd/details/max_value.hpp>

namespace boost { namespace dispatch { namespace meta
{
  template<class T, class Ext>
  struct model_of< simd::native<T, Ext> >
  {
    struct type
    {
      template<class X>
      struct apply
      {
        typedef typename boost::dispatch::meta::primitive_of< typename simd::details::max_value_noseq<T, mpl::sizeof_<mpl::_> >::type>::type TT;
        typedef typename boost::dispatch::meta::primitive_of< typename simd::details::max_value_noseq<X, mpl::sizeof_<mpl::_> >::type>::type XX;

        typedef typename simd::meta::vector_of< X
                                              , sizeof(typename simd::meta::as_simd<TT,Ext>::type) / sizeof(XX)
                                              >::type type;
      };
    };
  };
} } }

#endif