#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <random>
#include <Eigen/Cholesky>
#include <fstream>
#include <iomanip>
#include <srrg_system_utils/parse_command_line.h>
#include <srrg_boss/deserializer.h>
#include <srrg_config/configurable_manager.h>

#include "srrg_solver/solver_core/instances.h"
#include "srrg_solver/solver_core/factor_graph.h"
#include "srrg_solver/variables_and_factors/types_2d/instances.h"
#include "srrg_solver/variables_and_factors/types_2d/variable_se2_ad.h"
#include "srrg_solver/variables_and_factors/types_2d/variable_point2_ad.h"
#include "srrg_solver/variables_and_factors/types_2d/se2_pose_pose_geodesic_error_factor.h"
#include "srrg_solver/variables_and_factors/types_2d/se2_pose_point_error_factor.h"

#include "srrg_solver/variables_and_factors/types_3d/instances.h"
#include "srrg_solver/variables_and_factors/types_3d/variable_se3_ad.h"
#include "srrg_solver/variables_and_factors/types_3d/variable_point3_ad.h"
#include "srrg_solver/variables_and_factors/types_3d/se3_pose_pose_geodesic_error_factor.h"
#include "srrg_solver/variables_and_factors/types_3d/se3_pose_point_offset_error_factor.h"

#include "srrg_solver/variables_and_factors/types_projective/instances.h"
#include "srrg_solver/variables_and_factors/types_projective/se3_pose_point_omni_ba_error_factor.h"
#include "srrg_solver/variables_and_factors/types_projective/sim3_pose_pose_error_factor_ad.h"

#include "srrg_solver/utils/solver_evaluator.h"
#include "srrg_solver/solver_core/solver.h"

using namespace srrg2_core;
using namespace srrg2_solver;
using namespace std;

extern char** environ;
const std::string exe_name = environ[0];
#define LOG std::cerr << exe_name << "|"

static const char* banner[] = {
  "evaluates a  factor graph,  by registering the poses in the input graph with the corresponding poses of the gt",
  0
};

struct AssignerBase{
  virtual bool assign(FactorBase* factor) = 0;
};
using AssignerBasePtr=std::unique_ptr<AssignerBase>;

template <typename FactorType_>
struct Assigner_ : public AssignerBase {
  using FactorType=FactorType_;
  bool assign(FactorBase* factor_) {
    FactorType* factor=dynamic_cast<FactorType*>(factor_);
    if (! factor)
      return false;
    do_assign(*factor);
    return true;
  }
  virtual void do_assign(FactorType& factor) = 0;
};

struct AssignerSE3PosePoseGeodesicErrorFactor: public Assigner_<SE3PosePoseGeodesicErrorFactor> {
  void do_assign(SE3PosePoseGeodesicErrorFactor& factor) override {
    VariableSE3Base* from=factor.variables().at<0>();
    VariableSE3Base* to=factor.variables().at<1>();
    factor.setMeasurement(from->estimate().inverse()*to->estimate());
  }
};

struct AssignerSE2PosePoseGeodesicErrorFactor: public Assigner_<SE2PosePoseGeodesicErrorFactor> {
  void do_assign(SE2PosePoseGeodesicErrorFactor& factor) override {
    VariableSE2Base* from=factor.variables().at<0>();
    VariableSE2Base* to=factor.variables().at<1>();
    factor.setMeasurement(from->estimate().inverse()*to->estimate());
  }
};

struct AssignerSE3PosePointOffsetErrorFactor: public Assigner_<SE3PosePointOffsetErrorFactor> {
  void do_assign(SE3PosePointOffsetErrorFactor& factor) override {
    VariableSE3Base* pose=factor.variables().at<0>();
    VariablePoint3*  point=factor.variables().at<1>();
    VariableSE3Base* offset=factor.variables().at<2>();
    Vector3f p_local=offset->estimate().inverse()*pose->estimate().inverse()*point->estimate();
    factor.setMeasurement(p_local);
  }
};

struct AssignerSE3PosePointOmniBAErrorFactor: public Assigner_<SE3PosePointOmniBAErrorFactor> {
  void do_assign(SE3PosePointOmniBAErrorFactor& factor) override {
    VariableSE3Base* pose=factor.variables().at<0>();
    VariablePoint3*  point=factor.variables().at<1>();
    VariableSE3Base* offset=factor.variables().at<2>();
    Vector3f p_local=offset->estimate().inverse()*pose->estimate().inverse()*point->estimate();
    p_local.normalize();
    factor.setMeasurement(p_local);
  }
};

struct AssignerSim3PosePoseErrorFactorAD: public Assigner_<Sim3PosePoseErrorFactorAD> {
  void do_assign(Sim3PosePoseErrorFactorAD& factor) override {
    VariableSim3Base* from=factor.variables().at<0>();
    VariableSim3Base* to=factor.variables().at<1>();
    factor.setMeasurement(from->estimate().inverse()*to->estimate());
  }
};

std::list<AssignerBasePtr> assigners;

void constructAssigners() {
  assigners.push_back(AssignerBasePtr(new(AssignerSE3PosePoseGeodesicErrorFactor)));
  assigners.push_back(AssignerBasePtr(new(AssignerSE2PosePoseGeodesicErrorFactor)));
  assigners.push_back(AssignerBasePtr(new(AssignerSE3PosePointOffsetErrorFactor)));
  assigners.push_back(AssignerBasePtr(new(AssignerSE3PosePointOmniBAErrorFactor)));
  assigners.push_back(AssignerBasePtr(new(AssignerSim3PosePoseErrorFactorAD)));
}

bool assign(FactorBase* factor) {
  for (auto& a: assigners) {
    if (a->assign(factor))
      return true;
  }
  cerr << "error in setting factor of type [ " << factor->className() << "]" << endl;
  return false;
}

// ia register types
void initTypes() {
  variables_and_factors_2d_registerTypes();
  variables_and_factors_3d_registerTypes();
  variables_and_factors_projective_registerTypes();
  solver_registerTypes();
}

// ia THE PROGRAM
int main(int argc, char** argv) {
  initTypes();
  constructAssigners();
  using namespace std;
  ParseCommandLine cmd_line(argv, banner);
  ArgumentString input_file          (&cmd_line, "i",    "input-file",             "file where to read the input ", "");
  ArgumentString output_file          (&cmd_line, "o",    "output-file",             "file where to write the output ", "");
  ArgumentString config_file          (&cmd_line, "c",    "config-file",           "solver config file", "solver.config");
  ArgumentString solver_name          (&cmd_line, "sn",    "solver-name",           "solver name in the config", "solver");
  cmd_line.parse();

  FactorGraphPtr graph;

  std::cerr << "loading file: [" << input_file.value() << "]... ";
  graph = FactorGraph::read(input_file.value());
  std::cerr << "done, factors:" << graph->factors().size() << " vars: " << graph->variables().size() << std::endl;

  VariableBase* v0=graph->variable(0);
  if (! v0) {
    cerr << "dunno which variable to fix" << endl;
  } else {
    v0->setStatus(VariableBase::Fixed);
  }

  ConfigurableManager manager;
  manager.read(config_file.value());

  // ia check if solver with this specific name exists
  LOG << "loading solver [ " << config_file.value() << " ]\n";
  SolverPtr solver = manager.getByName<Solver>(solver_name.value());
  if (!solver) {
    throw std::runtime_error(exe_name + "|ERROR, cannot find solver with name [ " +
                             solver_name.value() + " ] in configuration file [ " +
                             config_file.value() + " ]");
  }
  solver->param_actions.pushBack(SolverVerboseActionPtr(new SolverVerboseAction));
  solver->setGraph(graph);
  solver->compute();

  cerr << "setting measurements... ";
  for (auto f_it: graph->factors()) {
    if (! assign(f_it.second)) {
      throw std::runtime_error("fatal");
    }
  }
  cerr << "done" << endl;

  if (output_file.isSet()) {
    cerr << "saving optimized graph" << endl;
    graph->write(output_file.value());
  }
  return 0;
}


#pragma once

#include <skynet/config.hpp>

#include <Eigen/Core>

namespace skynet{namespace statistics{
	namespace detail{

		pair<Eigen::MatrixXd, Eigen::MatrixXd>	get_missing_data_covariance_sub_matrixes(
			const Eigen::MatrixXd &mat, const std::vector<size_t> &missing_indexes)
		{
			ASSERT(mat.cols() == mat.rows(), "The rols is not equal to cols!.");

			std::vector<size_t> observed_indexes;
			for (int i = 0, j = 0; (i <mat.rows()) && (j < missing_indexes.size()); ++i){
				if (i != missing_indexes[j]){
					observed_indexes.push_back(i);
				}
				else{
					++j;
				}
			}

			Eigen::MatrixXd Sigma_oo(observed_indexes.size(), observed_indexes.size());
			for (int i = 0; i < observed_indexes.size(); ++i){
				for (int j = 0; j < observed_indexes.size(); ++j){
					Sigma_oo(i, j) = mat(observed_indexes[i], observed_indexes[j]);
				}
			}

			Eigen::MatrixXd Sigma_mo(missing_indexes.size(), observed_indexes.size());
			for (int i = 0; i < missing_indexes.size(); ++i){
				for (int j = 0; j < observed_indexes.size(); ++j){
					Sigma_mo(i,j) = mat(missing_indexes[i], observed_indexes[j]);
				}
			}

			return  make_pair(Sigma_oo, Sigma_mo);
		}

		pair<Eigen::VectorXd, Eigen::VectorXd> get_missing_data_means(
			const Eigen::MatrixXd &mat, const std::vector<size_t> &missing_indexes)
		{
			std::vector<size_t> observed_indexes;
			for (int i = 0, j = 0; (i <mat.rows()) && (j < missing_indexes.size()); ++i){
				if (i != missing_indexes[j]){
					observed_indexes.push_back(i);
				}
				else{
					++j;
				}
			}

			Eigen::VectorXd mean_o(observed_indexes.size());
			for (int i = 0; i < observed_indexes.size(); ++i){
				mean_o[i] = mat.row(observed_indexes[i]).mean();
			}

			Eigen::VectorXd mean_m(missing_indexes.size());
			for (int i = 0; i < missing_indexes.size(); ++i){
				mean_m[i] = mat.row(missing_indexes[i]).mean();
			}

			return make_pair(mean_o, mean_m);
		}

		template <typename V>
		Eigen::VectorXd get_observed_values(const V &vec, const std::vector<size_t> &missing_indexes){
			std::vector<size_t> observed_indexes;
			for (int i = 0, j = 0; (i <vec.size()) && (j < missing_indexes.size()); ++i){
				if (i != missing_indexes[j]){
					observed_indexes.push_back(i);
				}
				else{
					++j;
				}
			}

			Eigen::VectorXd observed_values(observed_indexes.size());
			for (int i = 0; i < observed_indexes.size(); ++i){
				observed_values[i] = vec[observed_indexes[i]];
			}

			return observed_values;
		}

	}

	class missing_data_EM{
	public:
		missing_data_EM(const  Eigen::MatrixXd &mat_data, const Eigen::Matrix<byte, -1, -1, 0,-1,-1> &mat_observed)
			: _mat_data(mat_data), _mat_observed(mat_observed)
		{
			ASSERT(mat_data.cols() == mat_observed.cols() && mat_data.rows() == mat_observed.rows(),
				"The size not match!");
			_missing_indexes_vec.resize(mat_data.cols());

			for (int col = 0; col < mat_observed.cols(); ++col){
				for (int row = 0; row < mat_observed.rows(); ++row){
					if (!mat_observed(row, col)){
						_missing_indexes_vec[col].push_back(row);
					}
				}
			}
		}

		Eigen::MatrixXd em(size_t  iter_num){
			for (int iter = 0; iter < iter_num; ++iter){
				refresh_covariance_matrix();
				for (size_t  sample_i = 0; sample_i < _mat_data.cols(); ++sample_i){
					auto missing_indexes = _missing_indexes_vec[sample_i];

					if (missing_indexes.empty()) continue;

					auto sub_matrixes =
						detail::get_missing_data_covariance_sub_matrixes(_mat_covariance, missing_indexes);
					auto means = detail::get_missing_data_means(_mat_data, missing_indexes);

					auto y_o = detail::get_observed_values(_mat_data.col(sample_i), missing_indexes);
					auto y_m = means.second +
						sub_matrixes.second * sub_matrixes.first.inverse() * (y_o - means.first);

					for (int i = 0; i < missing_indexes.size(); ++i){
						_mat_data.col(sample_i)[missing_indexes[i]] = y_m[i];
					}
				}
			}

			return _mat_data;
		}

	protected:

		void refresh_covariance_matrix(){
			_mat_covariance = _mat_data * _mat_data.transpose();
			_mat_covariance /= _mat_covariance.cols();
		}

	private:
		Eigen::MatrixXd								_mat_data;
		Eigen::Matrix<byte, -1, -1, 0, -1, -1>		_mat_observed;
		Eigen::MatrixXd								_mat_covariance;

		std::vector<std::vector<size_t>>	_missing_indexes_vec;
	};

}}


/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#ifndef SOLVER_NLOPT_HPP
#define SOLVER_NLOPT_HPP
#include <Eigen/Dense>

#include <iomanip>  //set precision
#include <nlopt.hpp>
#include "mader_types.hpp"
#include "utils.hpp"
#include "timer.hpp"
#include <decomp_geometry/polyhedron.h>  //For Polyhedron  and Hyperplane definition
#include "separator.hpp"
#include "octopus_search.hpp"
#include "solver_params.hpp"

typedef MADER_timers::Timer MyTimer;

class SolverNlopt
{
public:
  SolverNlopt(ms::par_solver &par);

  ~SolverNlopt();

  bool optimize();

  // setters
  void setMaxRuntimeKappaAndMu(double runtime, double kappa, double mu);
  bool setInitStateFinalStateInitTFinalT(mt::state initial_state, mt::state final_state, double t_init,
                                         double &t_final);
  void setHulls(mt::ConvexHullsOfCurves_Std &hulls);

  mt::trajectory traj_solution_;

  // getters
  void getPlanes(std::vector<Hyperplane3D> &planes);
  int getNumOfLPsRun();
  int getNumOfQCQPsRun();
  void getSolution(mt::PieceWisePol &solution);
  double getTimeNeeded();

  int B_SPLINE = 1;  // B-Spline Basis
  int MINVO = 2;     // Minimum volume basis
  int BEZIER = 3;    // Bezier basis

  bool checkGradientsUsingFiniteDiff();

  double improvement_ = 0.0;

protected:
private:
  void saturateQ(std::vector<Eigen::Vector3d> &q);

  bool isDegenerate(const std::vector<double> &x);

  void transformPosBSpline2otherBasis(const Eigen::Matrix<double, 3, 4> &Qbs, Eigen::Matrix<double, 3, 4> &Qmv,
                                      int interval);
  void transformVelBSpline2otherBasis(const Eigen::Matrix<double, 3, 3> &Qbs, Eigen::Matrix<double, 3, 3> &Qmv,
                                      int interval);

  void generateRandomGuess();
  void generateAStarGuess();
  void generateStraightLineGuess();

  void sampleFeasible(Eigen::Vector3d &qiP1, std::vector<Eigen::Vector3d> &q);

  void printStd(const std::vector<Eigen::Vector3d> &v);
  void printStd(const std::vector<double> &v);
  void generateGuessNDFromQ(const std::vector<Eigen::Vector3d> &q, std::vector<Eigen::Vector3d> &n,
                            std::vector<double> &d);

  void fillPlanesFromNDQ(std::vector<Hyperplane3D> &planes_, const std::vector<Eigen::Vector3d> &n,
                         const std::vector<double> &d, const std::vector<Eigen::Vector3d> &q);

  void generateRandomD(std::vector<double> &d);
  void generateRandomN(std::vector<Eigen::Vector3d> &n);
  void generateRandomQ(std::vector<Eigen::Vector3d> &q);

  nlopt::algorithm getSolver(std::string &solver);

  bool isADecisionCP(int i);

  template <class T>
  bool isFeasible(const T x);

  bool isFeasible(const std::vector<Eigen::Vector3d> &q, const std::vector<Eigen::Vector3d> &n,
                  const std::vector<double> &d);

  void printQVA(const std::vector<Eigen::Vector3d> &q);

  void assignEigenToVector(double *grad, int index, const Eigen::Vector3d &tmp);

  template <class T>
  void x2qnd(T &x, std::vector<Eigen::Vector3d> &q, std::vector<Eigen::Vector3d> &n, std::vector<double> &d);

  void qnd2x(const std::vector<Eigen::Vector3d> &q, const std::vector<Eigen::Vector3d> &n, const std::vector<double> &d,
             std::vector<double> &x);

  int gIndexQ(int i);  // Element jth of control point ith
  int gIndexN(int i);  // Element jth of normal ith
  int gIndexD(int i);

  void printQND(std::vector<Eigen::Vector3d> &q, std::vector<Eigen::Vector3d> &n, std::vector<double> &d);

  // r is the constraint index
  // nn is the number of variables
  // var_gindex is the index of the variable of the first element of the vector
  void toGradDiffConstraintsDiffVariables(int var_gindex, const Eigen::Vector3d &tmp, double *grad, int r, int nn);

  void toGradSameConstraintDiffVariables(int var_gindex, const Eigen::Vector3d &tmp, double *grad, int r, int nn);

  void assignValueToGradConstraints(int var_gindex, const double &tmp, double *grad, int r, int nn);

  // This function has to be static, see example
  // https://github.com/HKUST-Aerial-Robotics/Fast-Planner/blob/master/fast_planner/bspline_opt/src/bspline_optimizer.cpp
  // and here: https://github.com/stevengj/nlopt/issues/246
  static double myObjFunc(unsigned nn, const double *x, double *grad, void *my_func_data);

  // See example https://github.com/stevengj/nlopt/issues/168
  static void myIneqConstraints(unsigned m, double *result, unsigned nn, const double *x, double *grad, void *f_data);

  // double computeObjFunction(unsigned nn, double *grad, std::vector<Eigen::Vector3d> &q, std::vector<Eigen::Vector3d>
  // &n,
  //                           std::vector<double> &d);

  double computeObjFunctionJerk(unsigned nn, double *grad, std::vector<Eigen::Vector3d> &q,
                                std::vector<Eigen::Vector3d> &n, std::vector<double> &d);

  void computeConstraints(unsigned m, double *constraints, unsigned nn, double *grad,
                          const std::vector<Eigen::Vector3d> &q, const std::vector<Eigen::Vector3d> &n,
                          const std::vector<double> &d);

  void initializeNumOfConstraints();

  void printInfeasibleConstraints(std::vector<Eigen::Vector3d> &q, std::vector<Eigen::Vector3d> &n,
                                  std::vector<double> &d);

  // template <class T>
  // void printInfeasibleConstraints(const T x);

  template <class T>
  int getNumberOfInfeasibleConstraints(const T &constraints);

  template <class T>
  bool areTheseConstraintsFeasible(const T &constraints);

  template <class T>
  void printInfeasibleConstraints(const T &constraints);

  std::string getResultCode(int &result);

  void findCentroidHull(const mt::Polyhedron_Std &hull, Eigen::Vector3d &centroid);

  int lastDecCP();

  // bool intersects();

  // void computeVeli(Eigen::Vector3d &vel, std::vector<Eigen::Vector3d> &q);

  // void computeAcceli(Eigen::Vector3d &accel, std::vector<Eigen::Vector3d> &q);

  // bool satisfiesVmaxAmax(std::vector<Eigen::Vector3d> &q);

  void printIndexesConstraints();
  void printIndexesVariables();

  mt::PieceWisePol solution_;

  int basis_ = B_SPLINE;

  int deg_pol_ = 3;
  int num_pol_ = 5;
  int p_ = 5;
  int i_min_;
  int i_max_;
  int j_min_;
  int j_max_;
  int k_min_;
  int k_max_;
  int M_;
  int N_;

  int num_of_variables_;
  int num_of_normals_;
  int num_of_constraints_;
  int num_of_obst_;
  int num_of_segments_;

  nlopt::algorithm solver_;

  bool got_a_feasible_solution_ = false;
  double time_first_feasible_solution_ = 0.0;

  double best_cost_so_far_ = std::numeric_limits<double>::max();

  std::vector<double> best_feasible_sol_so_far_;

  std::vector<Hyperplane3D> planes_;

  double epsilon_tol_constraints_;
  double xtol_rel_;
  double ftol_rel_;

  std::vector<double> x_;  // Here the initial guess, and the solution, are stored

  double lowest_cost_so_far_;
  double dc_;
  Eigen::RowVectorXd knots_;
  double t_init_;
  double t_final_;
  double deltaT_;
  Eigen::Vector3d v_max_;
  Eigen::Vector3d a_max_;

  double weight_ = 10000;
  double weight_modified_ = 10000;

  // bool force_final_state_ = true;

  mt::state initial_state_;
  mt::state final_state_;

  // double constraints_[10000];  // this number should be very big!! (hack, TODO)

  Eigen::Vector3d q0_, q1_, q2_, qNm2_, qNm1_, qN_;

  mt::ConvexHullsOfCurves_Std hulls_;

  MyTimer opt_timer_;

  double max_runtime_ = 2;  //[seconds]

  // Eigen::Vector3d initial_point_;
  // Eigen::Vector3d final_point_;
  // nlopt::opt *opt_ = nullptr;
  // nlopt::opt *local_opt_ = nullptr;

  // nlopt::opt opt_;
  // nlopt::opt local_opt_;

  // Guesses
  std::vector<Eigen::Vector3d> n_guess_;  // Guesses for the normals
  std::vector<Eigen::Vector3d> q_guess_;  // Guesses for the normals
  std::vector<double> d_guess_;           // Guesses for the normals

  Eigen::MatrixXd R_;  // This matrix is [r0, r1, r2, r3, r0, r1, r2, r3] (for two segments)

  // separator::Separator *separator_solver;

  int index_const_obs_ = 0;
  int index_const_vel_ = 0;
  int index_const_accel_ = 0;
  int index_const_normals_ = 0;

  double kappa_ = 0.2;  // kappa_*max_runtime_ is spent on the initial guess
  double mu_ = 0.5;     // mu_*max_runtime_ is spent on the optimization

  double x_min_ = -std::numeric_limits<double>::max();
  double x_max_ = std::numeric_limits<double>::max();

  double y_min_ = -std::numeric_limits<double>::max();
  double y_max_ = std::numeric_limits<double>::max();

  double z_min_ = -std::numeric_limits<double>::max();
  double z_max_ = std::numeric_limits<double>::max();

  int num_of_LPs_run_ = 0;
  int num_of_QCQPs_run_ = 0;

  int a_star_samp_x_ = 7;
  int a_star_samp_y_ = 7;
  int a_star_samp_z_ = 7;

  double time_needed_;
  double dist_to_use_straight_guess_ = std::numeric_limits<double>::max();

  // transformation between the B-spline control points and other basis
  std::vector<Eigen::Matrix<double, 4, 4>> M_pos_bs2basis_;
  std::vector<Eigen::Matrix<double, 3, 3>> M_vel_bs2basis_;
  std::vector<Eigen::Matrix<double, 4, 4>> A_pos_bs_;

  double a_star_bias_ = 1.0;
  double a_star_fraction_voxel_size_ = 0.5;
  bool allow_infeasible_guess_ = false;

  separator::Separator *separator_solver_;
  OctopusSearch *octopusSolver_;

  double Ra_ = 1e10;
};
#endif


/* ScaFES
 * Copyright (c) 2011-2015, ZIH, TU Dresden, Federal Republic of Germany.
 * For details, see the files COPYING and LICENSE in the base directory
 * of the package.
 */

/**
 *  @file ScaFES_Ntuple.hpp
 *  @brief Contains the class template Ntuple.
 */
#ifndef SCAFES_NTUPLE_HPP_
#define SCAFES_NTUPLE_HPP_

#include "ScaFES_Config.hpp"

//#include <cstdint>
#include <iomanip>
#include <iostream>
#include <ios>
#include <vector>
#include <cmath>
#include <type_traits>
#include <stdexcept>

#ifdef SCAFES_HAVE_BOOST
#include <boost/version.hpp>
#endif

#ifdef SCAFES_HAVE_BOOST_SERIALIZATION
namespace boost
{
namespace serialization
{
    class access;
}
}
#include <boost/serialization/version.hpp>
#if BOOST_VERSION < 105900
   #include <boost/serialization/pfto.hpp>
#endif
#endif

namespace ScaFES
{

/*******************************************************************************
 ******************************************************************************/
/** \class Ntuple
 * @brief The class template \c Ntuple represents a
 * \c NN dimensional vector of a given type \c TT.
 *
 * All important unary and binary operators are implemented resp. overloaded
 * and behave as expected. Especially, many operators work componentwise.
 *
 * Usually, the memory for the vector elements must be allocated dynamically,
 * because the number of elements is known at run time. This is related
 * with additional effort for creating and deleting the elements on the
 * heap.
 * Due to the nontype template parameter \c NN the number of vector elements
 * is known at compile time. Thus, it is possible to allocate the memory
 * for the vector elements statically.
 *
 * Advantages:
 * - Automatic compiler SIMD vectorizations can be better applied.
 * - Using the class template \c Ntuple with three elements
 * instead of a class containing explicitly three elements does not affect
 * the performance, but will give more flexibility in solving other than
 * three-dimensional problems.
 *
 * \code
 * // Constructors.
 * ScaFES::Ntuple<double,3> a(1.1, 2.2, 3.3);  // a = (1.1, 2.2, 3.3)
 * ScaFES::Ntuple<double,3> b(6.0, 6.0, 4.0);  // b = (3.0, 3.0, 2.0)
 * ScaFES::Ntuple<double,3> c = a + 2 * b;     // c = (7.1, 8.2, 7.3)
 *
 * // Arithmetic operators.
 * a += 2.0;                                   // a = (3.1, 4.2, 5.3)
 * c = b * b;                                  // c = (9.0, 9.0, 4.0)
 *
 * // Relational operators
 * a < b;                                      // false: 3.3 > 2.0
 * b *= 2.0;                                   // b = (6.0, 6.0, 4.0)
 * b > a;                                      // true
 * \endcode
 */
template <typename TT, std::size_t NN = 3> class Ntuple
{

public:
    /*----------------------------------------------------------------------
    | TYPE DEFINITIONS.
    ----------------------------------------------------------------------*/
    /** Re-export typename TT STL-like. */
    typedef TT value_type;

    /*----------------------------------------------------------------------
    | FRIEND CLASSES.
    ----------------------------------------------------------------------*/
#ifdef SCAFES_HAVE_BOOST_SERIALIZATION
    friend class boost::serialization::access;
#endif

    /*----------------------------------------------------------------------
    | LIFE CYCLE METHODS.
    ----------------------------------------------------------------------*/
    /** Creates the default constructor.
     *  All elements are initialized by zero.*/
    Ntuple<TT, NN>();

    /** Creates a special constructor for twodimensional vectors:
     *  The elements are initialized by given values. */
    Ntuple<TT, NN>(const TT& a, const TT& b);

    /** Creates a special constructor for threedimensional vectors:
     *  The elements are initialized by given values. */
    Ntuple<TT, NN>(const TT& a, const TT& b, const TT& c);

    /** Creates own constructor:
     *  All elements are initalized by the same value. */
    Ntuple<TT, NN>(const TT& scal);

    /** Creates own constructor: Conversion method.
     *  All elements are set by a given STL vector. */
    Ntuple<TT, NN>(const std::vector<TT>& vec);

    /** Creates copy constructor.
     * \remarks Use copy-and-swap idiom.
     */
    Ntuple<TT, NN>(const Ntuple<TT, NN>& rhs);

    /** Creates copy assignment operator.
     * All elements of the rhs. vector will be assigned. */
    Ntuple<TT, NN>& operator=(Ntuple<TT, NN> rhs);

    /** Creates the destructor. */
    ~Ntuple() {};

    /*----------------------------------------------------------------------
    | GETTER METHODS.
    ----------------------------------------------------------------------*/
    /** Returns the dimension (= number of elements) of the n-tuple. */
    size_t dim() const;

    /** Returns a reference to the element at idx \c ii in the n-tuple.
     * \remarks With range check!
     */
    const TT& elem(const std::size_t& ii) const;

    /** Returns the element at idx \c ii in the n-tuple.
     * \remarks Without range check! */
    const TT& operator[](const std::size_t& ii) const;

    /*----------------------------------------------------------------------
    | SETTER METHODS.
    ----------------------------------------------------------------------*/
    /** Returns a reference to the element at idx ii in the Ntuple.
     * \remarks With range check!
     */
    TT& elem(const std::size_t& ii);

    /** Returns a reference to the element at idx \c ii in the n-tuple.
     * \remarks Without range check! */
    TT& operator[](const std::size_t& ii);

    /*----------------------------------------------------------------------
    | ARITHMETIC METHODS.
    ----------------------------------------------------------------------*/
    /** Creates an assignment operator.
     * All elements will be assigned to a given scalar.*/
    ScaFES::Ntuple<TT, NN>& operator=(const TT& sca);

    /** Multiplies this n-tuple by a given scalar (elementwise). */
    template <typename CT> ScaFES::Ntuple<TT, NN>& operator*=(const CT& sca);

    /** Divides this n-tuple by a given scalar (elementwise). */
    template <typename CT> ScaFES::Ntuple<TT, NN>& operator/=(const CT& sca);

    /** Adds a given rhs n-tuple to this n-tuple. */
    ScaFES::Ntuple<TT, NN>& operator+=(const ScaFES::Ntuple<TT, NN>& rhs);

    /** Subtracts a given rhs n-tuple from this n-tuple. */
    ScaFES::Ntuple<TT, NN>& operator-=(const ScaFES::Ntuple<TT, NN>& rhs);

    /** Multiplies this n-tuple by a given rhs n-tuple. */
    ScaFES::Ntuple<TT, NN>& operator*=(const ScaFES::Ntuple<TT, NN>& rhs);

    /** Divides this n-tuple by a given rhs n-tuple. */
    ScaFES::Ntuple<TT, NN>& operator/=(const ScaFES::Ntuple<TT, NN>& rhs);

    /*----------------------------------------------------------------------
    | COMPARISON METHODS.
    ----------------------------------------------------------------------*/
    /** Compares elementwise if this n-tuple is smaller than a a given rhs
     *  n-tuple. */
    bool operator<(const ScaFES::Ntuple<TT, NN>& rhs) const;

    /** Compares elementwise if this n-tuple is greater or equal
     *  than a a given rhs n-tuple. */
    bool operator>=(const ScaFES::Ntuple<TT, NN>& rhs) const;

    /** Compares elementwise if this n-tuple is smaller or equal
     *  than a a given rhs n-tuple. */
    bool operator<=(const ScaFES::Ntuple<TT, NN>& rhs) const;

    /** Compares elementwise if this n-tuple is greater than a a given rhs
     *  n-tuple. */
    bool operator>(const ScaFES::Ntuple<TT, NN>& rhs) const;

    /** Compares elementwise if this n-tuple is equal to a given rhs
     *  n-tuple. */
    bool operator==(const ScaFES::Ntuple<TT, NN>& rhs) const;

    /** Compares elementwise if this n-tuple is unequal to a given rhs
     *  n-tuple. */
    bool operator!=(const ScaFES::Ntuple<TT, NN>& rhs) const;

    /*----------------------------------------------------------------------
    | WORK METHODS.
    ----------------------------------------------------------------------*/
    /** Returns the index of the element with maximal value. */
    std::size_t idxMaxElem() const;

    /** Computes the dimension of the n-tuple,
     * i.e., all elements will be multiplied. */
    TT size() const;

    /** Computes the 1-norm. */
    double norm1() const;

#ifdef SCAFES_HAVE_BOOST_SERIALIZATION
    /** Serializes this class. */
    template <class Archive>
    void serialize(Archive& ar, unsigned int const version);
#endif

    /*----------------------------------------------------------------------
    | FREE METHODS WHICH ARE FRIENDS OF THIS CLASS.
    ----------------------------------------------------------------------*/
    /** Overloads the output operator \c operator<<.
     * An n-tuple is printed this way: '(a_0, a_1,..., a_{n-1})' */
    template <typename ST, std::size_t MM>
    friend std::ostream& operator<<(std::ostream& output,
                                    const ScaFES::Ntuple<ST, MM>& t);

    /** Method to swap members of two ScaFES::Ntuples. */
    template <typename ST, std::size_t MM>
    friend void swap(ScaFES::Ntuple<ST, MM>& first,
                     ScaFES::Ntuple<ST, MM>& second);

    /*----------------------------------------------------------------------
    | CONSTANTS.
    ----------------------------------------------------------------------*/
    /** Dimension of n-tuple. */
    static const std::size_t DIM = NN;

private:
    /*----------------------------------------------------------------------
    | MEMBER VARIABLES.
    ----------------------------------------------------------------------*/
    /** Static array for storing the elements of an n-tuple. */
    TT mElem[NN];

}; // End of class //

/*******************************************************************************
 * FREE METHODS.
 ******************************************************************************/
/** Swaps two objects of the class \c ScaFES::Ntuple. */
template <typename TT, std::size_t NN>
void swap(ScaFES::Ntuple<TT, NN>& first, ScaFES::Ntuple<TT, NN>& second);
/*----------------------------------------------------------------------------*/
/** Preprares writing an object of the class \c ScaFES::Ntuple to output. */
template <typename TT, std::size_t NN>
std::ostream& operator<<(std::ostream& output, const ScaFES::Ntuple<TT, NN>& t);

/*******************************************************************************
 * LIFE CYCLE METHODS.
 ******************************************************************************/
template <typename TT, std::size_t NN> inline Ntuple<TT, NN>::Ntuple()
{
    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        this->mElem[iii] = static_cast<TT>(0);
    }
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline Ntuple<TT, NN>::Ntuple(const TT& scal)
{
    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        this->mElem[iii] = scal;
    }
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline Ntuple<TT, NN>::Ntuple(const TT& a, const TT& b)
{
    static_assert((NN == 2), "Constructor is valid in case NN=2, only.");
    this->mElem[0] = a;
    this->mElem[1] = b;
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline Ntuple<TT, NN>::Ntuple(const TT& a, const TT& b, const TT& c)
{
    static_assert((NN == 3), "Constructor is valid in case NN=3, only.");
    this->mElem[0] = a;
    this->mElem[1] = b;
    this->mElem[2] = c;
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline Ntuple<TT, NN>::Ntuple(const std::vector<TT>& vec)
{
    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        this->mElem[iii] = vec.at(iii);
    }
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline Ntuple<TT, NN>::Ntuple(const ScaFES::Ntuple<TT, NN>& rhs)
{
    for (std::size_t iii = 0; iii < rhs.dim(); ++iii)
    {
        this->mElem[iii] = rhs.elem(iii);
    }
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline ScaFES::Ntuple<TT, NN>& Ntuple<TT, NN>::
operator=(ScaFES::Ntuple<TT, NN> rhs)
{
    swap(*this, rhs);
    return *this;
}

/*******************************************************************************
 * GETTER METHODS.
 ******************************************************************************/
template <typename TT, std::size_t NN>
inline std::size_t Ntuple<TT, NN>::dim() const
{
    return NN;
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline const TT& ScaFES::Ntuple<TT, NN>::elem(const std::size_t& idx) const
{
    return this->mElem[idx];
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline const TT& ScaFES::Ntuple<TT, NN>::
operator[](const std::size_t& idx) const
{
    return this->mElem[idx];
}

/*******************************************************************************
 * SETTER METHODS.
 ******************************************************************************/
template <typename TT, std::size_t NN>
inline TT& Ntuple<TT, NN>::elem(const std::size_t& idx)
{
    return this->mElem[idx];
    // const_cast<TT&>(static_cast<const
    // ScaFES::Ntuple<TT,NN>&>(*this).elem(idx));
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline TT& Ntuple<TT, NN>::operator[](const std::size_t& idx)
{
    return this->mElem[idx];
    // const_cast<TT&>(static_cast<const ScaFES::Ntuple<TT,NN>&>(*this)[idx]);
}

/*******************************************************************************
 * COMPARISON METHODS.
 ******************************************************************************/
template <typename TT, std::size_t NN>
inline bool Ntuple<TT, NN>::operator<(const ScaFES::Ntuple<TT, NN>& rhs) const
{
    bool res = true;

    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        if (!(this->mElem[iii] < rhs.elem(iii)))
        {
            res = false;
            break;
        }
    }

    return res;
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline bool Ntuple<TT, NN>::operator<=(const ScaFES::Ntuple<TT, NN>& rhs) const
{
    bool res = true;

    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        if (this->mElem[iii] > rhs.elem(iii))
        {
            res = false;
            break;
        }
    }

    return res;
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline bool Ntuple<TT, NN>::operator>=(const ScaFES::Ntuple<TT, NN>& rhs) const
{
    bool res = true;

    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        if ((this->mElem[iii] < rhs.elem(iii)))
        {
            res = false;
            break;
        }
    }

    return res;
    // return !(*this < rhs); Does not work because we are working elementwise!
}

/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline bool Ntuple<TT, NN>::operator>(const ScaFES::Ntuple<TT, NN>& rhs) const
{
    bool res = true;

    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        if (!(this->mElem[iii] > rhs.elem(iii)))
        {
            res = false;
            break;
        }
    }

    return res;
    // return !(*this <= rhs); Does not work because we are working elementwise!
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline bool Ntuple<TT, NN>::operator==(const ScaFES::Ntuple<TT, NN>& rhs) const
{
    bool res = true;

    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        if (!(::fabs(this->mElem[iii] - rhs.elem(iii)) < 2.2e-15))
        {
            res = false;
            break;
        }
    }

    return res;
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline bool Ntuple<TT, NN>::operator!=(const ScaFES::Ntuple<TT, NN>& rhs) const
{
    return !(*this == rhs);
}

/*******************************************************************************
 * WORK METHODS.
 ******************************************************************************/
template <typename TT, std::size_t NN>
inline std::size_t Ntuple<TT, NN>::idxMaxElem() const
{
    std::size_t idx = 0;
    TT maxElem = this->mElem[0];

    for (std::size_t iii = 1; iii < NN; ++iii)
    {
        if (maxElem < this->mElem[iii])
        {
            maxElem = this->mElem[iii];
            idx = iii;
        }
    }

    return idx;
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN> inline TT Ntuple<TT, NN>::size() const
{
    TT prod = this->mElem[0];

    for (std::size_t iii = 1; iii < NN; ++iii)
    {
        prod *= this->mElem[iii];
    }

    return prod;
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline double Ntuple<TT, NN>::norm1() const
{
    double res = ::fabs(this->mElem[0]);

    for (std::size_t iii = 1; iii < NN; ++iii)
    {
        res += ::fabs(this->mElem[iii]);
    }

    return res;
}
/*----------------------------------------------------------------------------*/
#ifdef SCAFES_HAVE_BOOST_SERIALIZATION
template <typename TT, std::size_t NN>
template <class Archive>
inline void Ntuple<TT, NN>::serialize(Archive& ar, unsigned int const version)
{
    if (1 <= version)
    {
        ar&(this->mElem);
    }
}
#endif

/*******************************************************************************
 * ARITHMETIC METHODS.
 ******************************************************************************/
template <typename TT, std::size_t NN>
inline ScaFES::Ntuple<TT, NN>& Ntuple<TT, NN>::operator=(const TT& sca)
{
    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        this->mElem[iii] = static_cast<TT>(sca);
    }

    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
template <typename CT>
inline ScaFES::Ntuple<TT, NN>& Ntuple<TT, NN>::operator*=(const CT& sca)
{
    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        this->mElem[iii] *= static_cast<TT>(sca);
    }

    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
template <typename CT>
inline ScaFES::Ntuple<TT, NN>& Ntuple<TT, NN>::operator/=(const CT& sca)
{
    if (::fabs(sca) < 2.2e-12)
    {
        throw std::runtime_error("Given scalar is very near to zero.");
    }
    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        this->mElem[iii] /= static_cast<TT>(sca);
    }

    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline ScaFES::Ntuple<TT, NN>& Ntuple<TT, NN>::
operator+=(const ScaFES::Ntuple<TT, NN>& rhs)
{
    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        this->mElem[iii] += rhs.elem(iii);
    }

    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline ScaFES::Ntuple<TT, NN>& Ntuple<TT, NN>::
operator-=(const ScaFES::Ntuple<TT, NN>& rhs)
{
    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        this->mElem[iii] -= rhs.elem(iii);
    }

    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline ScaFES::Ntuple<TT, NN>& Ntuple<TT, NN>::
operator*=(const ScaFES::Ntuple<TT, NN>& rhs)
{
    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        this->mElem[iii] *= rhs.elem(iii);
    }

    return *this;
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline ScaFES::Ntuple<TT, NN>& Ntuple<TT, NN>::
operator/=(const ScaFES::Ntuple<TT, NN>& rhs)
{
    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        if (::fabs(rhs.elem(iii)) < 2.2e-12)
        {
            throw std::runtime_error("Element of rhs is very near to zero.");
        }
    }
    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        this->mElem[iii] /= rhs.elem(iii);
    }

    return *this;
}

/*******************************************************************************
* FREE METHODS WHICH ARE FRIENDS OF THIS CLASS.
 ******************************************************************************/
template <typename TT, std::size_t NN>
inline void swap(ScaFES::Ntuple<TT, NN>& first, ScaFES::Ntuple<TT, NN>& second)
{
    TT tmp;

    for (std::size_t iii = 0; iii < second.dim(); ++iii)
    {
        tmp = second.mElem[iii];
        second.mElem[iii] = first.mElem[iii];
        first.mElem[iii] = tmp;
    }
}
/*----------------------------------------------------------------------------*/
template <typename TT, std::size_t NN>
inline std::ostream& operator<<(std::ostream& output,
                                const ScaFES::Ntuple<TT, NN>& t)
{
    output << "[";
    for (std::size_t iii = 0; iii < NN; ++iii)
    {
        output << ::std::setw(4) << ::std::right << t.elem(iii);

        if (NN - 1 > iii)
        {
            output << "; ";
        }
    }

    output << "]";
    return output;
}

} // End of namespace. //

/*******************************************************************************
 ******************************************************************************/
#ifdef SCAFES_HAVE_BOOST_SERIALIZATION
namespace boost
{
namespace serialization
{
    /** Designed to set the boost serialization version of a class template. */
    template <typename TT, std::size_t NN>
    struct version<ScaFES::Ntuple<TT, NN>>
    {
        /** Sets the version number for serialization. */
        BOOST_STATIC_CONSTANT(unsigned long int, value = 2);
    };
}
}
#endif

#endif


/////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::poisson::random.hpp               	   //
//                                                                             //
//  (C) Copyright 2010 Erwann Rogard                                           //
//  Use, modification and distribution are subject to the                      //
//  Boost Software License, Version 1.0. (See accompanying file                //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)           //
/////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_POISSON_RANDOM_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_POISSON_RANDOM_HPP_ER_2010
#include <boost/math/distributions/poisson.hpp>
#include <boost/random/poisson_distribution.hpp>
#include <boost/statistics/detail/distribution_common/meta/random/distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace meta{

    template<typename T,typename P>
    struct random_distribution<
        boost::math::poisson_distribution<T,P>
    >{
        typedef boost::math::poisson_distribution<T,P> dist_;
        typedef boost::poisson_distribution<int,T> type;

        static type call(const dist_& d){
            return type(d.mean());
        }
    };

}// meta
}// distribution
}// detail
}// statistics
}// boost

#endif


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Craidon, C.B. A Desription of the Langley Wireframe Geometry Standard (LaWGS) format, NASA
 *          TECHNICAL MEMORANDUM 85767.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/make_shared.hpp>
#include <memory>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "tudat/math/basic/mathematicalConstants.h"

#include "tudat/math/geometric/lawgsPartGeometry.h"
#include "tudat/math/geometric/sphereSegment.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_Lawgs_Surface_Geometry )

//! Test implementation of Lawgs surface geometry.
BOOST_AUTO_TEST_CASE( testLawgsSurfaceGeometry )
{
    using namespace tudat;
    using namespace geometric_shapes;

    // Create a full sphere as test geometry, with a radius of 2.0.
    const double sphereRadius = 2.0;
    std::shared_ptr< SphereSegment > sphere = std::make_shared< SphereSegment >(
                sphereRadius );

    // Create a Lawgs mesh of the sphere.
    LawgsPartGeometry lawgsSurface;
    const int numberOfLines = 21;
    const int numberOfPoints = 21;
    lawgsSurface.setMesh( sphere, numberOfLines, numberOfPoints );

    // Retrieve the total surface area and check if it is sufficiently close
    // to the expected value.
    using mathematical_constants::PI;
    const double totalArea = lawgsSurface.getTotalArea( );
    BOOST_CHECK_SMALL( std::fabs( totalArea - 4.0 * PI
                                  * ( std::pow( sphereRadius, 2.0 ) ) ), 0.6 );

    // Test if number of lines on mesh is correct.
    BOOST_CHECK_EQUAL( lawgsSurface.getNumberOfLines( ), numberOfLines );

    // Test if number of points per line on mesh is correct.
    BOOST_CHECK_EQUAL( lawgsSurface.getNumberOfPoints( ), numberOfPoints );

    // Set part name.
    std::string partName = "sphere";
    lawgsSurface.setName( partName );

    // Test if part name is properly retrieved.
    BOOST_CHECK_EQUAL( lawgsSurface.getName( ), partName );

    // Retrieve normal and centroid for panel 0, 0.
    Eigen::Vector3d testNormal = lawgsSurface.getPanelSurfaceNormal( 0, 0 );
    Eigen::Vector3d testCentroid = lawgsSurface.getPanelCentroid( 0, 0 );

    // Test whether centroid and normal are collinear for panel 0, 0.
    BOOST_CHECK_SMALL( std::fabs( testCentroid.normalized( ).dot(
                                      testNormal.normalized( ) ) ) - 1.0, 1.0e-5 );

    // Test if the position of the x- and y-coordinate of panel 0, 0 is correct.
    BOOST_CHECK_SMALL( std::fabs( std::atan( testCentroid.y( ) / testCentroid.x( ) ) - PI / 20.0 ),
                       std::numeric_limits< double >::epsilon( ) );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <NTL/LLL.h>

NTL_CLIENT

long SubsetSumSolution(const vec_ZZ& z)
{
   long n = z.length()-3;
   long j;

   if (z(n+1) != 0) return 0;
   if (z(n+2) != -1 && z(n+2) != 1) return 0;
   for (j = 1; j <= n; j++)
      if (z(j) != -1 && z(j) != 1) return 0;

   return 1;
}

int main()
{
   RR::SetPrecision(150);
   long n, b, size;

   cerr << "n: ";
   cin >> n;

   cerr << "b: ";
   cin >> b;

   cerr << "size: ";
   cin >> size;

   cerr << "prune: ";
   long prune;
   cin >> prune;

   ZZ seed;
   cerr << "seed: ";
   cin >> seed;

   if (seed != 0)
      SetSeed(seed);

   char alg;
   cerr << "alg [fqQxr]: ";
   cin >> alg;

   double TotalTime = 0;
   long TotalSucc = 0;

   long iter;

   for (iter = 1; iter <= 20; iter++) {
      vec_ZZ a;
      a.SetLength(n);

      ZZ bound;

      LeftShift(bound, to_ZZ(1), b);

      long i;
      for (i = 1; i <= n; i++) {
         RandomBnd(a(i), bound);
         a(i) += 1;
      }

      ZZ S;

      do {
         RandomLen(S, n+1);
      } while (weight(S) != n/2+1);

      ZZ s;
      clear(s);
      for (i = 1; i <= n; i++)
         if (bit(S, i-1))
            s += a(i);

      mat_ZZ B(INIT_SIZE, n+1, n+3);

      for (i = 1; i <= n; i++) {
         B(i, i) = 2;
         B(i, n+1) = a(i) * n;
         B(i, n+3) = n;
      }

      for (i = 1; i <= n; i++)
         B(n+1, i) = 1;

      B(n+1, n+1) = s * n;
      B(n+1, n+2) = 1;
      B(n+1, n+3) = n;
      B(n+1, n+3) *= n/2;

      swap(B(1), B(n+1));

      for (i = 2; i <= n; i++) {
         long j = RandomBnd(n-i+2) + i;
         swap(B(i), B(j));
      }

      double t;

      LLLStatusInterval = 10;

      t = GetTime();
      switch (alg) {
      case 'f':
         BKZ_FP(B, 0.99, size, prune, SubsetSumSolution);
         break;
      case 'q':
         BKZ_QP(B, 0.99, size, prune, SubsetSumSolution);
         break;
      case 'Q':
         BKZ_QP1(B, 0.99, size, prune, SubsetSumSolution);
         break;
      case 'x':
         BKZ_XD(B, 0.99, size, prune, SubsetSumSolution);
         break;
      case 'r':
         BKZ_RR(B, 0.99, size, prune, SubsetSumSolution);
         break;
      default:
         Error("invalid algorithm");
      }

      t = GetTime()-t;

      long succ = 0;
      for (i = 1; i <= n+1; i++)
         if (SubsetSumSolution(B(i)))
            succ = 1;

      TotalTime += t;
      TotalSucc += succ;

      if (succ)
         cerr << "+";
      else
         cerr << "-";
   }

   cerr << "\n";

   cerr << "number of success: " << TotalSucc << "\n";
   cerr << "average time: " << TotalTime/20 << "\n";

   return 0;
}


#ifndef STAN_MATH_PRIM_MAT_FUN_LOG_SUM_EXP_HPP
#define STAN_MATH_PRIM_MAT_FUN_LOG_SUM_EXP_HPP

#include <stan/math/prim/scal/fun/log1p.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <boost/math/tools/promotion.hpp>
#include <limits>
#include <vector>

namespace stan {
namespace math {

/**
 * Return the log of the sum of the exponentiated values of the specified
 * matrix of values.  The matrix may be a full matrix, a vector,
 * or a row vector.
 *
 * The function is defined as follows to prevent overflow in exponential
 * calculations.
 *
 * \f$\log \sum_{n=1}^N \exp(x_n) = \max(x) + \log \sum_{n=1}^N \exp(x_n -
 * \max(x))\f$.
 *
 * @param[in] x Matrix of specified values
 * @return The log of the sum of the exponentiated vector values.
 */
template <int R, int C>
double log_sum_exp(const Eigen::Matrix<double, R, C>& x) {
  using std::exp;
  using std::log;
  using std::numeric_limits;
  double max = -numeric_limits<double>::infinity();
  for (int i = 0; i < x.size(); i++)
    if (x(i) > max)
      max = x(i);

  double sum = 0.0;
  for (int i = 0; i < x.size(); i++)
    if (x(i) != -numeric_limits<double>::infinity())
      sum += exp(x(i) - max);

  return max + log(sum);
}

}  // namespace math
}  // namespace stan

#endif


#ifndef qlex_sabr_interpolation_hpp
#define qlex_sabr_interpolation_hpp

#include <math/interpolations/xabrinterpolation.hpp>
#include <ql/termstructures/volatility/sabr.hpp>

#include <boost/make_shared.hpp>
#include <boost/assign/list_of.hpp>

using namespace QuantLib;

namespace QLExtension {

namespace detail {

    class SABRWrapper {
      public:
        SABRWrapper(const Time t, const Real &forward,
                    const std::vector<Real> &params)
        : t_(t), forward_(forward), params_(params) {
            validateSabrParameters(params[0], params[1], params[2], params[3]);
        }
        Real volatility(const Real x) {
            return sabrVolatility(x, forward_, t_, params_[0], params_[1],
                                  params_[2], params_[3]);
        }

      private:
        const Real t_, &forward_;
        const std::vector<Real> &params_;
    };

    struct SABRSpecs {
        Size dimension() { return 4; }
        void defaultValues(std::vector<Real> &params, std::vector<bool> &,
                           const Real &forward, const Real expiryTIme) {
            if (params[1] == Null<Real>())
                params[1] = 0.5;
            if (params[0] == Null<Real>())
                // adapt alpha to beta level
                params[0] =
                    0.2 *
                    (params[1] < 0.9999 ? std::pow(forward, 1.0 - params[1]) : 1.0);
            if (params[2] == Null<Real>())
                params[2] = std::sqrt(0.4);
            if (params[3] == Null<Real>())
                params[3] = 0.0;
        }
        void guess(Array &values, const std::vector<bool> &paramIsFixed,
                   const Real &forward, const Real expiryTime,
                   const std::vector<Real> &r) {
            Size j = 0;
            if (!paramIsFixed[1])
                values[1] = (1.0 - 2E-6) * r[j++] + 1E-6;
            if (!paramIsFixed[0]) {
                values[0] =
                    (1.0 - 2E-6) * r[j++] + 1E-6; // lognormal vol guess
                // adapt this to beta level
                if(values[1] < 0.999)
                    values[0] *=
                        std::pow(forward,
                                 1.0 - values[1]);
            }
            if (!paramIsFixed[2])
                values[2] = 1.5 * r[j++] + 1E-6;
            if (!paramIsFixed[3])
                values[3] = (2.0 * r[j++] - 1.0) * (1.0 - 1E-6);
        }
        Real eps1() { return .0000001; }
        Real eps2() { return .9999; }
        Real dilationFactor() { return 0.001; }
        Array inverse(const Array &y, const std::vector<bool>&,
                      const std::vector<Real>&, const Real) {
            Array x(4);
            x[0] = y[0] < 25.0 + eps1() ? std::sqrt(y[0] - eps1())
                                        : (y[0] - eps1() + 25.0) / 10.0;
            // y_[1] = std::tan(M_PI*(x[1] - 0.5))/dilationFactor();
            x[1] = std::sqrt(-std::log(y[1]));
            x[2] = y[2] < 25.0 + eps1() ? std::sqrt(y[2] - eps1())
                                        : (y[2] - eps1() + 25.0) / 10.0;
            x[3] = std::asin(y[3] / eps2());
            return x;
        }
        Array direct(const Array &x, const std::vector<bool>&,
                     const std::vector<Real>&, const Real) {
            Array y(4);
            y[0] = std::fabs(x[0]) < 5.0
                       ? x[0] * x[0] + eps1()
                       : (10.0 * std::fabs(x[0]) - 25.0) + eps1();
            // y_[1] = std::atan(dilationFactor_*x[1])/M_PI + 0.5;
            y[1] = std::fabs(x[1]) < std::sqrt(-std::log(eps1()))
                       ? std::exp(-(x[1] * x[1]))
                       : eps1();
            y[2] = std::fabs(x[2]) < 5.0
                       ? x[2] * x[2] + eps1()
                       : (10.0 * std::fabs(x[2]) - 25.0) + eps1();
            y[3] = std::fabs(x[3]) < 2.5 * M_PI
                       ? eps2() * std::sin(x[3])
                       : eps2() * (x[3] > 0.0 ? 1.0 : (-1.0));
            return y;
        }
        typedef SABRWrapper type;
        boost::shared_ptr<type> instance(const Time t, const Real &forward,
                                         const std::vector<Real> &params) {
            return boost::make_shared<type>(t, forward, params);
        }
    };

}

    //! %SABR smile interpolation between discrete volatility points.
    class SABRInterpolation : public Interpolation {
      public:
        template <class I1, class I2>
        SABRInterpolation(const I1 &xBegin,  // x = strikes
                          const I1 &xEnd,
                          const I2 &yBegin,  // y = volatilities
                          Time t,            // option expiry
                          const Real& forward,
                          Real alpha,
                          Real beta,
                          Real nu,
                          Real rho,
                          bool alphaIsFixed,
                          bool betaIsFixed,
                          bool nuIsFixed,
                          bool rhoIsFixed,
                          bool vegaWeighted = true,
                          const boost::shared_ptr<EndCriteria>& endCriteria
                                  = boost::shared_ptr<EndCriteria>(),
                          const boost::shared_ptr<OptimizationMethod>& optMethod
                                  = boost::shared_ptr<OptimizationMethod>(),
                          const Real errorAccept = 0.0020,
                          const bool useMaxError = false,
                          const Size maxGuesses = 50) {

            impl_ = boost::shared_ptr<Interpolation::Impl>(
                new detail::XABRInterpolationImpl<I1, I2, detail::SABRSpecs>(
                    xBegin, xEnd, yBegin, t, forward,
                    boost::assign::list_of(alpha)(beta)(nu)(rho),
                    boost::assign::list_of(alphaIsFixed)(betaIsFixed)(nuIsFixed)(rhoIsFixed),
                    vegaWeighted, endCriteria, optMethod, errorAccept, useMaxError,
                    maxGuesses));
            coeffs_ = boost::dynamic_pointer_cast<
                detail::XABRCoeffHolder<detail::SABRSpecs> >(impl_);
        }
        Real expiry()  const { return coeffs_->t_; }
        Real forward() const { return coeffs_->forward_; }
        Real alpha()   const { return coeffs_->params_[0]; }
        Real beta()    const { return coeffs_->params_[1]; }
        Real nu()      const { return coeffs_->params_[2]; }
        Real rho()     const { return coeffs_->params_[3]; }
        Real rmsError() const { return coeffs_->error_; }
        Real maxError() const { return coeffs_->maxError_; }
        const std::vector<Real>& interpolationWeights() const {
            return coeffs_->weights_;
        }
        EndCriteria::Type endCriteria() { return coeffs_->XABREndCriteria_; }

      private:
        boost::shared_ptr<detail::XABRCoeffHolder<detail::SABRSpecs> > coeffs_;
    };

    //! %SABR interpolation factory and traits
    class SABR {
      public:
        SABR(Time t, Real forward,
             Real alpha, Real beta, Real nu, Real rho,
             bool alphaIsFixed, bool betaIsFixed,
             bool nuIsFixed, bool rhoIsFixed,
             bool vegaWeighted = false,
             const boost::shared_ptr<EndCriteria> endCriteria
                 = boost::shared_ptr<EndCriteria>(),
             const boost::shared_ptr<OptimizationMethod> optMethod
                 = boost::shared_ptr<OptimizationMethod>(),
             const Real errorAccept = 0.0020, const bool useMaxError = false,
             const Size maxGuesses = 50)
        : t_(t), forward_(forward),
          alpha_(alpha), beta_(beta), nu_(nu), rho_(rho),
          alphaIsFixed_(alphaIsFixed), betaIsFixed_(betaIsFixed),
          nuIsFixed_(nuIsFixed), rhoIsFixed_(rhoIsFixed),
          vegaWeighted_(vegaWeighted),
          endCriteria_(endCriteria),
          optMethod_(optMethod), errorAccept_(errorAccept),
          useMaxError_(useMaxError), maxGuesses_(maxGuesses) {}
        template <class I1, class I2>
        Interpolation interpolate(const I1 &xBegin, const I1 &xEnd,
                                  const I2 &yBegin) const {
            return SABRInterpolation(
                xBegin, xEnd, yBegin, t_, forward_, alpha_, beta_, nu_, rho_,
                alphaIsFixed_, betaIsFixed_, nuIsFixed_, rhoIsFixed_, vegaWeighted_,
                endCriteria_, optMethod_, errorAccept_, useMaxError_, maxGuesses_);
        }
        static const bool global = true;

      private:
        Time t_;
        Real forward_;
        Real alpha_, beta_, nu_, rho_;
        bool alphaIsFixed_, betaIsFixed_, nuIsFixed_, rhoIsFixed_;
        bool vegaWeighted_;
        const boost::shared_ptr<EndCriteria> endCriteria_;
        const boost::shared_ptr<OptimizationMethod> optMethod_;
        const Real errorAccept_;
        const bool useMaxError_;
        const Size maxGuesses_;
    };

}

#endif


#ifndef __CONTROL_HH__
#define __CONTROL_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Describe the CONTROL Module On Board)
LIBRARY DEPENDENCY:
      ((../src/control.cpp))
PROGRAMMERS:
      (((Chung-Fan Yang) (Chun-Hsu Lai) () () ))
*******************************************************************************/
#include <armadillo>
#include "aux.hh"
#include "datadeck.hh"
#include "env/atmosphere76.hh"
#include "env/atmosphere_nasa2002.hh"
#include "matrix_tool.hh"
#include "cadac_constants.hh"
#include "integrate.hh"

class Control {
  TRICK_INTERFACE(Control);

 public:
  Control();

  void initialize();

  void control(double int_step);
  void set_close_loop_pole(double in1, double in2);
  void set_factor(double in1, double in2);
  void set_feedforward_gain(double in1);

  void set_aero_coffe(double in1, double in2, double in3);
  void set_IBBB0(double in1, double in2, double in3);
  void set_IBBB1(double in1, double in2, double in3);
  void set_controller_var(double in1, double in2, double in3, double in4,
                          double in5, double in6, double in7);
  void set_NO_CONTROL();
  void set_acc_control();
  void set_engnum(double in);
  void set_reference_point(double in);

  double get_theta_a_cmd();
  double get_theta_b_cmd();
  double get_theta_c_cmd();
  double get_theta_d_cmd();
  void load_aerotable(const char* filename);
  void atmosphere_use_nasa();
  void atmosphere_use_public();

  void set_ancomx(double in);
  void set_alcomx(double in);

  enum CONTROL_TYPE {
    NO_CONTROL = 0,
    S2_PITCH_DOWN_I,
    S2_PITCH_DOWN_II,
    S2_ROLL_CONTROL,
    S3_PITCH_DOWN,
    S2_AOA,
    S3_AOA,
    ACC_CONTROL_ON
  };

  std::function<int()> grab_thrust_state;
  std::function<double()> grab_dvbec;
  std::function<double()> grab_thtvdcx;
  std::function<double()> grab_thtbdcx;
  std::function<double()> grab_phibdcx;
  std::function<double()> grab_psibdcx;
  std::function<double()> grab_alphacx;
  std::function<double()> grab_altc;

  std::function<double()> grab_qqcx;
  std::function<double()> grab_rrcx;
  std::function<double()> grab_phipcx;
  std::function<double()> grab_alppcx;

  std::function<arma::vec3()> grab_FSPCB;

  std::function<arma::vec3()> grab_computed_WBIB;
  std::function<arma::vec4()> grab_TBDQ;
  std::function<arma::mat33()> grab_TBD;
  std::function<arma::mat33()> grab_TBICI;
  std::function<arma::mat33()> grab_TBIC;
  std::function<arma::vec3()> grab_WBECB;
  std::function<arma::vec3()> grab_ABICB;

  double get_delecx();
  double get_delrcx();

  void calculate_xcg_thrust(double int_step);

 private:
  cad::Atmosphere* atmosphere;

  double control_normal_accel(double ancomx_in, double int_step);
  double control_yaw_accel(double alcomx_in, double int_step);
  void aerodynamics_der();
  void Euler_Angle_Control(double Cmd_Input, double P_Gain_1, double P_Gain_2, double Angle_Feedback, double Rate_Feedback, double &Cmd_Output);

  double delecx; /* *io (d)      Pitch command deflection */  // n
  double delrcx; /* *io (d)      Yaw command deflection */    // n

  enum CONTROL_TYPE maut; /* *io (--)     maut=|mauty|mautp| see table */
  int mfreeze;   /* *io (--)     =0:Unfreeze; =1:Freeze; increment for more */
  double waclp;  /* *io (r/s)    Nat freq of accel close loop complex pole */
  double zaclp;  /* *io (--)     Damping of accel close loop complex pole */
  double paclp;  /* *io (--)     Close loop real pole */
  double yyd;    /* *io (m/s2)   Yaw feed-forward derivative variable */
  double yy;     /* *io (m/s)    Yaw feed-forward integration variable */
  double zzd;    /* *io (m/s2)   Pitch feed-forward derivative variable */
  double zz;     /* *io (m/s)    Pitch feed-forward integration variable */
  double alcomx_actual; /* *io (--)     Later accel com limited by 'betalimx' */
  double ancomx_actual; /* *io (--)     Normal accel com limited by 'alplimx' */
  arma::vec GAINFP;  /* *io (--)     Feedback gains of pitch accel controller */
  double _GAINFP[3]; /* *io (--)     Feedback gains of pitch accel controller */
  double gainp;  /* *io (s2/m)   Proportional gain in pitch acceleration loop */
  double gainl;  /* *io (--)     Gain in lateral acceleration loop */
  double gkp;    /* *io (s)      Gain of roll rate feedback */
  double gkphi;  /* *io (--)     Gain of roll angle feedback */
  double isetc2; /* *io (--)     Flag to print freeze variables */
  double wacly;  /* *io (r/s)    Nat freq of accel close loop pole, yaw */
  double zacly;  /* *io (--)     Damping of accel close loop pole, yaw */
  double pacly;  /* *io (--)     Close loop real pole, yaw */
  double gainy;  /* *io (--)     Gain in lateral acceleration loop */
  arma::vec GAINFY;  /* *io (--)     Feedback gains of yaw accel controller */
  double _GAINFY[3]; /* *io (--)     Feedback gains of yaw accel controller */
  double factwaclp; /* *io (--)     Factor to mod 'waclp': waclp*(1+factwacl) */
  double factwacly; /* *io (--)     Factor to mod 'wacly': wacly*(1+factwacl) */
  double alcomx;    /* *io (--)     Lateral (horizontal) acceleration command */
  double ancomx;    /* *io (--)     Pitch (normal) acceleration command */

  double fmasse;
  double mdot;
  double fmass0;
  double xcg_0;
  double xcg_1;
  double isp;
  double vmass;
  double vmass0;

  arma::vec IBBB0;
  double _IBBB0[3];

  arma::vec IBBB1;
  double _IBBB1[3];

  arma::vec IBBB2;
  double _IBBB2[3];

  double theta_a_cmd;
  double theta_b_cmd;
  double theta_c_cmd;
  double theta_d_cmd;
  double lx;

  double xcg;
  double thrust;
  double mass_ratio;

  double reference_point;
  double d;

  // Aerodynamics def()
  Datadeck aerotable;

  double dla;
  double dlde;
  double dma;
  double dmq;
  double dmde;
  double dyb;
  double dydr;
  double dnb;
  double dnr;
  double dndr;
  double dllp;
  double dllda;
  double dnd;
  double cla;
  double clde;
  double cyb;
  double cydr;
  double cllda;
  double cllp;
  double cma;
  double cmde;
  double cmq;
  double cnb;
  double cndr;
  double cnr;
  double cn0;
  // diagnostics
  double stmarg_yaw;
  double stmarg_pitch;
  double realp1;
  double realp2;
  double wnp;
  double zetp;
  double rpreal;
  double realy1;
  double realy2;
  double wny;
  double zety;
  double ryreal;
  double refa;
  double refd;
  double xcp;
  double pdynmc;
  double vmach;

  double eng_num;
};

#endif  // __CONTROL_HH__


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SINHCOSH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SINHCOSH_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-hyperbolic
    This function object computes simultaneously  and at lower cost
    the @c sinh and @c cosh of the input

    @par Header <boost/simd/function/sinhcosh.hpp>

    @see  sinh, cosh

    @par Example:

      @snippet sinhcosh.cpp sinhcosh

    @par Possible output:

      @snippet sinhcosh.txt sinhcosh

  **/
  std::pair<IEEEValue, IEEEValue> sinhcosh(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/sinhcosh.hpp>
#include <boost/simd/function/simd/sinhcosh.hpp>

#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef quantlib_binomial_loss_model_hpp
#define quantlib_binomial_loss_model_hpp

#include <ql/handle.hpp>
#include <ql/experimental/credit/basket.hpp>
#include <ql/experimental/credit/defaultlossmodel.hpp>
#include <ql/experimental/credit/constantlosslatentmodel.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/bind.hpp>
#include <algorithm>
#include <numeric>

namespace QuantLib {

    /*! Binomial Defaultable Basket Loss Model\par
    Models the portfolio loss distribution by approximatting it to an adjusted
    binomial. Fits the two moments of the loss distribution through an adapted
    binomial approximation. This simple model allows for portfolio inhomogeneity
    with no excesive cost over the LHP.\par
    See:\par
    Approximating Independent Loss Distributions with an Adjusted Binomial
    Distribution , Dominic O'Kane, 2007 EDHEC RISK AND ASSET MANAGEMENT
    RESEARCH CENTRE \par
    Modelling single name and multi-name credit derivatives Chapter
    18.5.2, Dominic O'Kane, Wiley Finance, 2008 \par
    The version presented here is adaptated to the multifactorial case
    by computing a conditional binomial approximation; notice that the Binomial
    is stable. This way the model can be used also in risk management models
    rather than only in pricing. The copula is also left
    undefined/arbitrary. \par
    LLM: Loss Latent Model template parameter able to model default and
    loss.\par
    The model is allowed and arbitrary copula, although initially designed for
    a Gaussian setup. If these exotic versions were not allowed the template
    parameter can then be dropped but the use of random recoveries should be
    added in some other way.

    \todo untested/wip for the random recovery models.
    \todo integrate with the previously computed probability inversions of
    the cumulative functions.
    */
    template<class LLM>
    class BinomialLossModel : public DefaultLossModel {
    public:
        typedef typename LLM::copulaType copulaType;
        BinomialLossModel(
            const boost::shared_ptr<LLM>& copula)
        : copula_(copula) { }
    private:
        void resetModel() {
            /* say there are defaults and these havent settled... and this is
            the engine to compute them.... is this the wrong place?:*/
            attachAmount_ = basket_->remainingAttachmentAmount();
            detachAmount_ = basket_->remainingDetachmentAmount();

            copula_->resetBasket(basket_.currentLink());// forces interface
        }
    protected:
        /*! Returns the probability of the default loss values given by the
            method lossPoints.
        */
        Disposable<std::vector<Real> >
            expectedDistribution(const Date& date) const {
            // precal date conditional magnitudes:
            std::vector<Real> notionals = basket_->remainingNotionals(date);
            std::vector<Probability> invProbs =
                basket_->remainingProbabilities(date);
            for(Size iName=0; iName<invProbs.size(); iName++)
                invProbs[iName] =
                    copula_->inverseCumulativeY(invProbs[iName], iName);

            return copula_->integratedExpectedValue(
                boost::function<Disposable<std::vector<Real> > (
                  const std::vector<Real>& v1)>(
                    boost::bind(
                        &BinomialLossModel<LLM>::lossProbability,
                        this,
                        boost::cref(date), //d,
                        boost::cref(notionals),
                        boost::cref(invProbs),
                        _1)
                    )
                );
        }
        //! attainable loss points this model provides
        Disposable<std::vector<Real> > lossPoints(const Date&) const;
        //! Returns the cumulative full loss distribution
        Disposable<std::map<Real, Probability> >
            lossDistribution(const Date& d) const;
        //! Loss level for this percentile
        Real percentile(const Date& d, Real percentile) const;
        Real expectedShortfall(const Date&d, Real percentile) const;
        Real expectedTrancheLoss(const Date& d) const;
    protected:
        // Model internal workings ----------------
        //! Average loss per credit.
        Real averageLoss(const Date&, const std::vector<Real>& reminingNots,
            const std::vector<Real>&) const;
        Real condTrancheLoss(const Date&, const std::vector<Real>& lossVals,
            const std::vector<Real>& bsktNots,
            const std::vector<Probability>& uncondDefProbs,
            const std::vector<Real>&) const;
        // expected as in time-value, not average, see literature
        Disposable<std::vector<Real> >
            expConditionalLgd(const Date& d,
                               const std::vector<Real>& mktFactors) const
        {
            std::vector<Real> condLgds;
            const std::vector<Size>& evalDateLives = basket_->liveList();
            for(Size i=0; i<evalDateLives.size(); i++)
                condLgds.push_back(1.-copula_->conditionalRecovery(d,
                    evalDateLives[i], mktFactors));
            return condLgds;
        }

        //! Loss probability density conditional on the market factor value.
        // Heres where the burden of the algorithm setup lies.
        Disposable<std::vector<Real> >
            lossProbability(
                const Date& date,
                // expected exposures at the passed date, no wrong way means
                //  no dependence of the exposure with the mkt factor
                const std::vector<Real>& bsktNots,
                const std::vector<Real>& uncondDefProbInv,
                            const std::vector<Real>&  mktFactor) const;
    protected:
        const boost::shared_ptr<LLM> copula_;

        // cached arguments:
        // remaining basket magnitudes:
        mutable Real attachAmount_, detachAmount_;
    };

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

    /* The algorithm to compute the prob. of n defaults in the basket is
        recursive. For this reason theres no sense in returning the prob
        distribution of a given number of defaults.
    */
    template< class LLM>
    Disposable<std::vector<Real> > BinomialLossModel<LLM>::lossProbability(
        const Date& date,
        const std::vector<Real>& bsktNots,
        const std::vector<Real>& uncondDefProbInv,
        const std::vector<Real>& mktFactors) const
    {   // the model as it is does not model the exposures conditional to the
        //   mkt factr, otherwise this needs revision
        /// model does not take the unconditional rr
        Size bsktSize = basket_->remainingSize();
        /* The conditional loss per unit notional of each name at time 'date'
            The spot recovery model is returning for all i's:
            \frac{\int_0^t  [1-rr_i(\tau; \xi)] P_{def-i}(0, \tau; \xi) d\tau}
                 {P_{def-i}(0,t;\xi)}
            and the constant recovery model is simply returning:
            1-RR_i
        */
        // conditional fractional LGD expected as given by the recovery model
        //   for the ramaining(live) names at the current eval date.
        std::vector<Real> fractionalEL = expConditionalLgd(date, mktFactors);
        std::vector<Real> lgdsLeft;
        std::transform(fractionalEL.begin(), fractionalEL.end(),
            bsktNots.begin(), std::back_inserter(lgdsLeft),
            std::multiplies<Real>());
        Real avgLgd =
            std::accumulate(lgdsLeft.begin(), lgdsLeft.end(), Real(0.)) /
                bsktSize;

        std::vector<Probability> condDefProb(bsktSize, 0.);
        for(Size j=0; j<bsktSize; j++)//transform
            condDefProb[j] =
                copula_->conditionalDefaultProbabilityInvP(uncondDefProbInv[j],
                    j, mktFactors);
        // of full portfolio:
        Real avgProb = avgLgd <= QL_EPSILON ? 0. : // only if all are 0
                std::inner_product(condDefProb.begin(),
                    condDefProb.end(), lgdsLeft.begin(), 0.)
                / (avgLgd * bsktSize);
        // model parameters:
        Real m = avgProb * bsktSize;
        Real floorAveProb = std::min(Real(bsktSize-1), std::floor(Real(m)));
        Real ceilAveProb = floorAveProb + 1.;
        // nu_A
        Real varianceBinom = avgProb * (1. - avgProb)/bsktSize;
        // nu_E
        std::vector<Probability> oneMinusDefProb;//: 1.-condDefProb[j]
        std::transform(condDefProb.begin(), condDefProb.end(),
            std::back_inserter(oneMinusDefProb),
            std::bind1st(std::minus<Real>(), 1.));

        //breaks condDefProb and lgdsLeft to spare memory
        std::transform(condDefProb.begin(), condDefProb.end(),
            oneMinusDefProb.begin(), condDefProb.begin(),
            std::multiplies<Real>());
        std::transform(lgdsLeft.begin(), lgdsLeft.end(),
            lgdsLeft.begin(), lgdsLeft.begin(), std::multiplies<Real>());
        Real variance = std::inner_product(condDefProb.begin(),
            condDefProb.end(), lgdsLeft.begin(), 0.);

        variance = avgLgd <= QL_EPSILON ? 0. :
            variance / (bsktSize * bsktSize * avgLgd * avgLgd );
        Real sumAves = -std::pow(ceilAveProb-m, 2)
            - (std::pow(floorAveProb-m, 2) - std::pow(ceilAveProb,2.))
                * (ceilAveProb-m);
        Real alpha = (variance * bsktSize + sumAves)
            / (varianceBinom * bsktSize + sumAves);
        // Full distribution:
        // ....DO SOMETHING CHEAPER at least go up to the loss tranche limit.
        std::vector<Probability> lossProbDensity(bsktSize+1, 0.);
        if(avgProb >= 1.-QL_EPSILON) {
           lossProbDensity[bsktSize] = 1.;
        }else if(avgProb <= QL_EPSILON) {
           lossProbDensity[0] = 1.;
        }else{
            /* FIX ME: With high default probabilities one only gets tiny values
            at the end and the sum of probabilities in the
            conditional distribution does not add up to one. It might be due to
            the fact that recursion should be done in the other direction as
            pointed out in the book. This is numerical.
            */
            Probability probsRatio = avgProb/(1.-avgProb);
            lossProbDensity[0] = std::pow(1.-avgProb,
                static_cast<Real>(bsktSize));
            for(Size i=1; i<bsktSize+1; i++) // recursive to avoid factorial
                lossProbDensity[i] = lossProbDensity[i-1] * probsRatio
                    * (bsktSize-i+1.)/i;
            // redistribute probability:
            for(Size i=0; i<bsktSize+1; i++)
                lossProbDensity[i] *= alpha;
            // adjust average
            Real epsilon = (1.-alpha)*(ceilAveProb-m);
            Real epsilonPlus = 1.-alpha-epsilon;
            lossProbDensity[static_cast<Size>(floorAveProb)] += epsilon;
            lossProbDensity[static_cast<Size>(ceilAveProb)]  += epsilonPlus;
        }
        return lossProbDensity;
    }

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

    template< class LLM>
    Real BinomialLossModel<LLM>::averageLoss(
        const Date& d,
        const std::vector<Real>& reminingNots,
        const std::vector<Real>& mktFctrs) const
    {
        Size bsktSize = basket_->remainingSize();
        /* The conditional loss per unit notional of each name at time 'date'
            The spot recovery model is returning for all i's:
            \frac{\int_0^t  [1-rr_i(\tau; \xi)] P_{def-i}(0, \tau; \xi) d\tau}
                 {P_{def-i}(0,t;\xi)}
            and the constant recovery model is simply returning:
            1-RR_i
        */
        std::vector<Real> fractionalEL = expConditionalLgd(d, mktFctrs);
        Real notBskt = std::accumulate(reminingNots.begin(),
            reminingNots.end(), Real(0.));
        std::vector<Real> lgdsLeft;
        std::transform(fractionalEL.begin(), fractionalEL.end(),
            reminingNots.begin(), std::back_inserter(lgdsLeft),
            boost::lambda::_1 * boost::lambda::_2 / notBskt);
        return std::accumulate(lgdsLeft.begin(), lgdsLeft.end(), Real(0.))
            / bsktSize;
    }

    template< class LLM>
    Disposable<std::vector<Real> >
        BinomialLossModel<LLM>::lossPoints(const Date& d) const
    {
        std::vector<Real> notionals = basket_->remainingNotionals(d);

        Real aveLossFrct = copula_->integratedExpectedValue(
            boost::function<Real (const std::vector<Real>& v1)>(
                boost::bind(
                    &BinomialLossModel<LLM>::averageLoss,
                    this,
                    boost::cref(d),
                    boost::cref(notionals),
                    _1)
                )
            );

        std::vector<Real> data;
        Size dataSize = basket_->remainingSize() + 1;
        data.reserve(dataSize);
        // use std::algorithm
        Real outsNot = basket_->remainingNotional(d);
        for(Size i=0; i<dataSize; i++)
            data.push_back(i * aveLossFrct * outsNot);
        return data;
    }

    template< class LLM>
    Real BinomialLossModel<LLM>::condTrancheLoss(
        const Date& d,
        const std::vector<Real>& lossVals,
        const std::vector<Real>& bsktNots,
        const std::vector<Real>& uncondDefProbsInv,
        const std::vector<Real>& mkf) const {

        std::vector<Real> condLProb =
            lossProbability(d, bsktNots, uncondDefProbsInv, mkf);
        // \to do: move to a do-while over attach to detach
        Real suma = 0.;
        for(Size i=0; i<lossVals.size(); i++) {
            suma += condLProb[i] *
                std::min(std::max(lossVals[i]
                 - attachAmount_, 0.), detachAmount_ - attachAmount_);
        }
        return suma;
    }

    template< class LLM>
    Real BinomialLossModel<LLM>::expectedTrancheLoss(const Date& d) const {
        std::vector<Real> lossVals  = lossPoints(d);
        std::vector<Real> notionals = basket_->remainingNotionals(d);
        std::vector<Probability> invProbs =
            basket_->remainingProbabilities(d);
        for(Size iName=0; iName<invProbs.size(); iName++)
            invProbs[iName] =
                copula_->inverseCumulativeY(invProbs[iName], iName);

        return copula_->integratedExpectedValue(
            boost::function<Real (const std::vector<Real>& v1)>(
                boost::bind(&BinomialLossModel<LLM>::condTrancheLoss,
                            this,
                            boost::cref(d),
                            boost::cref(lossVals),
                            boost::cref(notionals),
                            boost::cref(invProbs),
                            _1))
            );
    }

    template< class LLM>
    Disposable<std::map<Real, Probability> >
        BinomialLossModel<LLM>::lossDistribution(const Date& d) const
    {
        std::map<Real, Probability> distrib;
        std::vector<Real> lossPts = lossPoints(d);
        std::vector<Real> values  = expectedDistribution(d);
        Real sum = 0.;
        for(Size i=0; i<lossPts.size(); i++) {
            distrib.insert(std::make_pair(lossPts[i],
                //capped, some situations giving a very small probability over 1
                std::min(sum+values[i],1.)
                ));
            sum+= values[i];
        }
        return distrib;
    }

    template< class LLM>
    Real BinomialLossModel<LLM>::percentile(const Date& d, Real perc) const {
        std::map<Real, Probability> dist = lossDistribution(d);
        // \todo: Use some of the library interpolators instead
        if(// included in test below-> (dist.begin()->second >=1.) ||
            (dist.begin()->second >= perc))return dist.begin()->first;

        // deterministic case (e.g. date requested is todays date)
        if(dist.size() == 1) return dist.begin()->first;

        if(perc == 1.) return dist.rbegin()->first;
        if(perc == 0.) return dist.begin()->first;
        std::map<Real, Probability>::const_iterator itdist = dist.begin();
        while(itdist->second <= perc) itdist++;
        Real valPlus = itdist->second;
        Real xPlus   = itdist->first;
        itdist--;//we r never 1st or last, because of tests above
        Real valMin  = itdist->second;
        Real xMin    = itdist->first;

        Real portfLoss = xPlus-(xPlus-xMin)*(valPlus-perc)/(valPlus-valMin);

        return
            std::min(std::max(portfLoss - attachAmount_, 0.),
                detachAmount_ - attachAmount_);
    }

    template< class LLM>
    Real BinomialLossModel<LLM>::expectedShortfall(const Date&d,
        Real perctl) const
    {
        //taken from recursive since we have the distribution in both cases.
        if(d == Settings::instance().evaluationDate()) return 0.;
            std::map<Real, Probability> distrib = lossDistribution(d);

            std::map<Real, Probability>::iterator
                itNxt, itDist = distrib.begin();
            for(; itDist != distrib.end(); itDist++)
                if(itDist->second >= perctl) break;
            itNxt = itDist; itDist--;

            // \todo: I could linearly triangulate the exact point and get
            //    extra precission on the first(broken) period.
            if(itNxt != distrib.end()) {
                Real lossNxt = std::min(std::max(itNxt->first - attachAmount_,
                    0.), detachAmount_ - attachAmount_);
                Real lossHere = std::min(std::max(itDist->first - attachAmount_,
                    0.), detachAmount_ - attachAmount_);

                Real val =  lossNxt - (itNxt->second - perctl) *
                    (lossNxt - lossHere) / (itNxt->second - itDist->second);
                Real suma = (itNxt->second - perctl) * (lossNxt + val) * .5;
                itDist++;itNxt++;
                do{
                    lossNxt = std::min(std::max(itNxt->first - attachAmount_,
                        0.), detachAmount_ - attachAmount_);
                    lossHere = std::min(std::max(itDist->first - attachAmount_,
                        0.), detachAmount_ - attachAmount_);
                    suma += .5 * (lossHere + lossNxt)
                        * (itNxt->second - itDist->second);
                    itDist++;itNxt++;
                }while(itNxt != distrib.end());
                return suma / (1.-perctl);
            }
            QL_FAIL("Binomial model fails to calculate ESF.");
    }

    // The standard use:
    typedef BinomialLossModel<GaussianConstantLossLM> GaussianBinomialLossModel;
    typedef BinomialLossModel<TConstantLossLM> TBinomialLossModel;

}

#endif


#pragma once
#include <numbers>
#include <cmath>
#include <Eigen/Dense>
#include "Material.hpp"
#include "../../utils/random.hpp"

class MaterialDiffuse : public Material
{
public:
	MaterialDiffuse(Eigen::Vector3f k_d) : Material(MaterialType::Diffuse), k_d(k_d) {};

    Eigen::Vector3f calc_brdf(Eigen::Vector3f normal, Eigen::Vector3f in, Eigen::Vector3f out) const override
    {
        // calculate the contribution of diffuse model
        auto cosalpha = normal.dot(out);
        if (cosalpha <= 0.0f)
            return { 0.0f, 0.0f, 0.0f };

        auto diffuse = k_d / std::numbers::pi;
        return diffuse;
    };

    Eigen::Vector3f sample(Eigen::Vector3f normal, Eigen::Vector3f in) const override
    {
        // uniform sample on the hemisphere
        auto x_1 = get_random_float();
        auto x_2 = get_random_float();
        auto z = std::abs(1.0f - 2.0f * x_1);
        auto r = std::sqrt(1.0f - z * z);
        auto phi = static_cast<float>(2 * std::numbers::pi_v<float> * x_2);
        Eigen::Vector3f local{ r * std::cos(phi), r * std::sin(phi), z };
        return to_world(local, normal);
    };

    Eigen::Vector3f at(double u, double v) const override { return k_d; };

protected:
    Eigen::Vector3f k_d;

    Eigen::Vector3f to_world(const Eigen::Vector3f& a, const Eigen::Vector3f& N) const {
        Eigen::Vector3f C;
        if (std::abs(N.x()) > std::abs(N.y()))
        {
            auto lenInv = 1.0f / std::sqrt(N.x() * N.x() + N.z() * N.z());
            C = Eigen::Vector3f(N.z() * lenInv, 0.0f, -N.x() * lenInv);
        }
        else
        {
            auto lenInv = 1.0f / std::sqrt(N.y() * N.y() + N.z() * N.z());
            C = Eigen::Vector3f(0.0f, N.z() * lenInv, -N.y() * lenInv);
        }
        auto B = C.cross(N);
        return a.x() * B + a.y() * C + a.z() * N;
    }
};


#include "CandyTransform/Transform.h"
#include <iostream>
#include <string>
#include <boost/lexical_cast.hpp>

/*
        Given a prime, what is it's factorization
 */
namespace {
        using namespace CandyTransform;

        struct Factorization{
                friend std::ostream& operator<<(std::ostream& ostr,         Factorization const& self){
                        typedef std::vector<size_t>::const_iterator CI0;
                        const char* comma = "";
                        ostr << "numbers" << " = {";
                        for(CI0 iter= self.numbers.begin(), end=self.numbers.end();iter!=end;++iter){
                                ostr << comma << *iter;
                                comma = ", ";
                        }
                        ostr << "}";
                        typedef std::vector<std::string>::const_iterator CI1;
                        comma = "";
                        ostr << "tokens" << " = {";
                        for(CI1 iter= self.tokens.begin(), end=self.tokens.end();iter!=end;++iter){
                                ostr << comma << *iter;
                                comma = ", ";
                        }
                        ostr << "}";
                        ostr << ", target = " << self.target;
                        return ostr;
                }
                std::vector<size_t> numbers;
                std::vector<std::string> tokens;
                size_t target;
        };

        struct Operator{
                virtual ~Operator()=default;
                virtual void Emit(std::function<void(Factorization const&)> continuation, Factorization const& value)const=0;
        };
        enum OperatorArity{
                AR_Unary,
                AR_CommutativeBinary,
                AR_NonCommutativeBinary,
        };
        struct NonTerminalOperator : Operator{
                explicit NonTerminalOperator(OperatorArity arity)
                        :arity_(arity)
                {}
                virtual void Emit(std::function<void(Factorization const&)> continuation, Factorization const& value)const{
                        std::vector<size_t> p(value.numbers.size());
                        switch(arity_){
                                case AR_Unary:
                                {
                                        std::vector<size_t> v(1);
                                        for(size_t idx=0;idx!=value.numbers.size();++idx){
                                                v[0] = idx;
                                                EmitPerm(continuation, value, v);
                                        }
                                        break;
                                }
                                case AR_CommutativeBinary:
                                case AR_NonCommutativeBinary:
                                {
                                        std::vector<size_t> p(value.numbers.size());
                                        p[0] = 1;
                                        p[1] = 1;
                                        std::sort(p.begin(), p.end());
                                        std::vector<size_t> v(2);
                                        do{
                                                size_t* out = &v[0];
                                                for(size_t idx=0;idx!=value.numbers.size();++idx){
                                                        if( p[idx] ){
                                                                *out = idx;
                                                                ++out;
                                                                continue;
                                                        }
                                                }

                                                EmitPerm(continuation, value, v);
                                                if( arity_ == AR_NonCommutativeBinary ){
                                                        std::swap(v[0], v[1]);
                                                        EmitPerm(continuation, value, v);
                                                }
                                        }while(std::next_permutation(p.begin(), p.end()));

                                        break;
                                }
                        }
                }
                virtual void EmitPerm(std::function<void(Factorization const&)> continuation, Factorization const& value, std::vector<size_t> const perm)const=0;
        protected:
                static Factorization CopyExPerm(Factorization const& value, std::vector<size_t> const perm){
                        Factorization result;
                        result.target = value.target;
                        for(size_t idx=0;idx!=value.numbers.size();++idx){
                                if( perm[0] == idx || perm[1] == idx )
                                        continue;
                                result.numbers.push_back(value.numbers[idx]);
                                result.tokens.push_back(value.tokens[idx]);
                        }
                        return result;
                }
        private:
                OperatorArity arity_;
        };

        struct AddOperator : NonTerminalOperator{
                AddOperator():NonTerminalOperator(AR_CommutativeBinary){}
                virtual void EmitPerm(std::function<void(Factorization const&)> continuation, Factorization const& value, std::vector<size_t> const perm)const{
                        auto next = CopyExPerm(value, perm);
                        auto a = value.numbers[perm[0]];
                        auto a_s = value.tokens[perm[0]];
                        auto b = value.numbers[perm[1]];
                        auto b_s = value.tokens[perm[1]];
                        next.numbers.push_back( a + b);
                        next.tokens.push_back( "(" + a_s + "+" + b_s + ")");
                        continuation(next);
                }
        };
        struct MulOperator : NonTerminalOperator{
                MulOperator():NonTerminalOperator(AR_CommutativeBinary){}
                virtual void EmitPerm(std::function<void(Factorization const&)> continuation, Factorization const& value, std::vector<size_t> const perm)const{
                        auto next = CopyExPerm(value, perm);
                        auto a = value.numbers[perm[0]];
                        auto a_s = value.tokens[perm[0]];
                        auto b = value.numbers[perm[1]];
                        auto b_s = value.tokens[perm[1]];
                        next.numbers.push_back( a * b);
                        next.tokens.push_back( "("+ a_s + "*" + b_s + ")");
                        continuation(next);
                }
        };

        struct F : Transform<Factorization, Factorization>{
                std::vector<std::shared_ptr<Operator> > ops_;
                F(){
                        ops_.push_back(std::make_shared<AddOperator>());
                        ops_.push_back(std::make_shared<MulOperator>());
                }
                virtual void Apply(TransformControl* ctrl, ParamType in)override{

                        if( in.numbers.size() == 1){
                                if(in.numbers.back() == in.target ){
                                        std::cout << "in.numbers.back() => " << in.numbers.back() << "\n"; // __CandyPrint__(cxx-print-scalar,in.numbers.back())
                                        ctrl->Return(in.tokens.back());
                                }
                                return;
                        }

                        for(auto op : ops_ ){
                                op->Emit( [&](auto&& f){ ctrl->Emit(f); }, in);
                        }

                        ctrl->DeclPath()->Next(std::make_shared<F>());
                }
        };
} // end namespace anon

int main(){

        TransformContext ctx;

        auto path = ctx.Start();
        path->Next(std::make_shared<F>());

        Factorization init;

        /*

           2 + 4 = 6

         */
        #if 0
        init.numbers = std::vector<size_t>{2,4};
        init.target = 6;
        #endif

        /*

          ( 2 * (3 + 4) ) * 5 = 70

         */
        #if 1
        init.numbers = std::vector<size_t>{3,4,2,5};
        init.target = 70;
        #endif

        /*

          ( ( 19*97 % 2 ) * (3 + 4) ) * 5 = 35

         */
        #if 0
        init.numbers = std::vector<size_t>{3,4,19,5,2};
        init.target = 35;
        #endif
        for(auto _ : init.numbers ){
                init.tokens.push_back(boost::lexical_cast<std::string>(_));
        }

        for(auto const& result : ctx.Execute<std::string>(init) ){
                std::cout << "result => " << result << "\n"; // __CandyPrint__(cxx-print-scalar,result)
        }
}


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

int main(int , char**)
{
    using namespace mtl;
    typedef mtl::vec::parameters<tag::col_major, mtl::vec::fixed::dimension<2> > fvec_para;
    typedef mat::parameters<tag::row_major, mtl::index::c_index, mtl::fixed::dimensions<2, 2> > fmat_para;

    dense2D<float, fmat_para>        A, B; // dimension not needed here
    dense_vector<float, fvec_para>   v, w; // here neither

    A= 2., 3.,
       4., 5.;
    v= 3., 4.;

    w= A * v; // Same syntax as dynamic size
    B= A * A;

    std::cout << "A * v is " << w << "\n\n";
    std::cout << "A * A is\n" << B;

    return 0;
}


#include <boost/random/weibull_distribution.hpp>