#ifndef DCM_PLANNER_H
#define DCM_PLANNER_H

#include <PnC/PlannerSet/DCMPlanner/Footstep.hpp>
#include <Utils/Math/minjerk_vec.hpp>
#include <Utils/Math/hermite_quaternion_curve.hpp>
#include <Utils/IO/IOUtilities.hpp>
#include <Eigen/Dense>
#include <cmath>
#include <string>
#include <iostream>
#include <map>

class DCMPlanner{
public:
  DCMPlanner();
  ~DCMPlanner();

  static int const DCM_RL_SWING_VRP_TYPE;
  static int const DCM_LL_SWING_VRP_TYPE;
  static int const DCM_MIDSTEP_TRANSFER_VRP_TYPE;
  static int const DCM_TRANSFER_VRP_TYPE;
  static int const DCM_END_VRP_TYPE;

  std::vector<int> rvrp_type_list; // List of type of virtual repelant point

  std::vector<Footstep> footstep_list; // Footstep list to generate pattern

  std::vector<Eigen::Vector3d> rvrp_list; // List of virtual repelant points.
  std::vector<Eigen::Vector3d> dcm_ini_list; // List of initial DCM states
  std::vector<Eigen::Vector3d> dcm_eos_list; // List of end-of-step DCM states

  // Initial DCM states
  Eigen::Vector3d ini_dcm_pos;
  Eigen::Vector3d ini_dcm_vel;

  // map containing the rvrp index that have a corresponding footstep swing
  std::map<int, int> rvrp_index_to_footstep_index;
  // Vector containing hermite quaternion curve objects for the pelvis orientation.
  // one curve per RVRP.
  std::vector<HermiteQuaternionCurve> pelvis_ori_quat_curves;

  // Initial and Boundary Conditions for the continuous DS trajectory
  std::vector<Eigen::Vector3d> dcm_ini_DS_list;
  std::vector<Eigen::Vector3d> dcm_vel_ini_DS_list;
  std::vector<Eigen::Vector3d> dcm_acc_ini_DS_list;
  std::vector<Eigen::Vector3d> dcm_end_DS_list;
  std::vector<Eigen::Vector3d> dcm_vel_end_DS_list;
  std::vector<Eigen::Vector3d> dcm_acc_end_DS_list;
  std::vector<Eigen::MatrixXd> dcm_P; // polynomial matrices for polynomial interpolation
  std::vector<MinJerkCurveVec> dcm_minjerk; // minjerk curves for interpolation

  // DCM walking parameters
  double t_transfer = 0.1; //0.125 ; //0.1; // exponential interpolation transfer time during initial transfer or same step transfer
  double t_ds = 0.05; // double support polynomial transfer time
  double t_ss = 0.3; // single support exponential interpolation  time
  double percentage_settle = 0.99;//0.999; // percent to converge at the end of the trajectory
  double alpha_ds = 0.5; // value between 0.0 and 1.0 for double support DCM interpolation

  void paramInitialization(const YAML::Node& node);
  void setRobotMass(double mass);
  void setCoMHeight(double z_vrp_in); // Sets the desired CoM Height
  void setInitialTime(double t_start_in); // Sets the initial offset time.
  void setInitialOri(const Eigen::Quaterniond initial_ori_in);

  double getInitialTime(); // Returns t_start;

  // DCM trajectory calculation
  // input: input_footstep_list - a list of footsteps to take not including the current stance configuration.
  //        initial_footstance  - a footstep object describing the stance leg.
  void initialize_footsteps_rvrp(const std::vector<Footstep> & input_footstep_list, const Footstep & initial_footstance, bool clear_list=false);

  // input: input_footstep_list - a list of footsteps to take not including the current stance configuration.
  //        initial_footstance  - a footstep object describing the stance leg.
  //        initial_rvrp        - an initial virtual repelant point (eg: average of the stance feet's rvrp).
  void initialize_footsteps_rvrp(const std::vector<Footstep> & input_footstep_list, const Footstep & initial_footstance, const Eigen::Vector3d & initial_rvrp);

  // input: input_footstep_list - a list of footsteps to take not including the current stance configuration.
  //        left_footstance        - a footstep object describing the left stance feet
  //        right_footstance       - a footstep object describing the right stance feet
  void initialize_footsteps_rvrp(const std::vector<Footstep> & input_footstep_list, const Footstep & left_footstance, const Footstep & right_footstance);

  // input: input_footstep_list - a list of footsteps to take not including the current stance configuration.
  //        left_footstance        - a footstep object describing the left stance feet
  //        right_footstance       - a footstep object describing the right stance feet
  //        initial_com            - the initial location of the com
  void initialize_footsteps_rvrp(const std::vector<Footstep> & input_footstep_list, const Footstep & left_footstance, const Footstep & right_footstance, const Eigen::Vector3d & initial_com);

  // input: input_footstep_list - a list of footsteps to take not including the current stance configuration.
  //        left_footstance        - a footstep object describing the left stance feet
  //        right_footstance       - a footstep object describing the right stance feet
  //        initial_dcm            - the initial starting position of the dcm
  //        initial_dcm_vel        - the initial starting velocity of the dcm
  void initialize_footsteps_rvrp(const std::vector<Footstep> & input_footstep_list,
                                 const Footstep & left_footstance, const Footstep & right_footstance,
                                 const Eigen::Vector3d & initial_dcm, const Eigen::Vector3d & initial_dcm_vel);

  // Compute: DCM, DCM vel, CoM, CoM Vel, given time, t.
  // t is a global time.
  void get_ref_dcm(const double t, Eigen::Vector3d & dcm_out);
  void get_ref_dcm_vel(const double t, Eigen::Vector3d & dcm_vel_out);
  void get_ref_com(const double t, Eigen::Vector3d & com_out);
  void get_ref_com_vel(const double t, Eigen::Vector3d & com_vel_out);
  void get_ref_r_vrp(const double t, Eigen::Vector3d & r_vrvp_out);
  void get_ref_reaction_force(const double t, Eigen::Vector3d & f_out);

  // Global reference quat, ang vel and ang acc
  void get_ref_ori_ang_vel_acc(const double t, Eigen::Quaterniond & quat_out,
                                               Eigen::Vector3d & ang_vel_out,
                                               Eigen::Vector3d & ang_acc_out);

  // computes the CoM velocity given the current CoM position and DCM velocity state.
  void get_com_vel(const Eigen::Vector3d & com_pos, const Eigen::Vector3d & dcm, Eigen::Vector3d & com_vel_out);
  // computes the CoM reaction force / leg reaction force given mass, CoM position and the r_vrp
  void get_reaction_force(const double mass, const Eigen::Vector3d & com_pos, const Eigen::Vector3d & r_vrp, Eigen::Vector3d fr_out);
  // computes the current r_vrp given
  //   -the DCM dynamics time constant b,
  //  - the current dcm
  //  - and the current dcm_vel
  void get_r_vrp(const double b_in, const Eigen::Vector3d & dcm, const Eigen::Vector3d & dcm_vel, Eigen::Vector3d & r_vrp_out);

  // prints the boundary conditions of the DCM
  void printBoundaryConditions();

  // Helper Functions
  // Returns the exponential step index the current time falls in.
  int which_step_index(const double t);
  // Returns the polynomial step index to use given the input time from t_start.
  int which_step_index_to_use(const double t);

  // returns the starting and ending time of the step_index from t_start.
  double get_t_step_start(const int step_index);
  double get_t_step_end(const int step_index);

  // returns the double support starting and ending time of the step_index from t_start.
  double get_double_support_t_start(const int step_index);
  double get_double_support_t_end(const int step_index);

  // if the step_index is a swing type, returns true an populates the swing start time and end
  bool get_t_swing_start_end(const int step_index, double & swing_start_time, double & swing_end_time);

  // returns the polynomial duration for the given step index
  double get_polynomial_duration(const int step_index);

  // Get the initial and end of double support transition times
  double get_eoDS_transition_time();
  double get_iniDS_transition_time();

  // Returns the type of VRP. Clamps the index to valid values.
  int get_r_vrp_type(const int step_index);

  // Returns t_end
  double get_total_trajectory_time();

  // Returns t_settle;
  double get_settle_time(){
    double t_settle = -b*log(1.0 - percentage_settle);
    return t_settle;
  }

private:
  // DCM parameters:
  double robot_mass = 50; // kg
  double gravity = 9.81;
  double z_vrp = 0.75; // desired VRP height / CoM height
  double b = std::sqrt(z_vrp/gravity); // time constant of DCM dynamics

  double t_start = 0.0; // the starting time for the DCM Walking reference

   // Outputs the average r_vrp location given two footstances
  void get_average_rvrp(const Footstep & footstance_1, const Footstep & footstance_2, Eigen::Vector3d & average_rvrp);

    // computes all the dcm states. Computation properly populates the dcm_ini_list and dcm_eos_list
  void computeDCM_states();

  // Sums up the total trajectory time and stores the result in t_end
  void compute_total_trajectory_time();
  double t_end = 0.0;

  // computes the reference com trajectories by integration
  void compute_reference_com();
  double dt_local = 1e-3; // discretization factor used
  // containers for the integrated reference CoM position and velocites
  std::vector<Eigen::Vector3d> ref_com_pos;
  std::vector<Eigen::Vector3d> ref_com_vel;

  // computes the reference pelvis orientation.
  void compute_reference_pelvis_ori();
  Footstep initial_leftfoot_stance;
  Footstep initial_rightfoot_stance;
  Eigen::Quaterniond initial_ori;

  // input: r_vrp_d_i - the desired virtual repelant point for the i-th step.
  //        t_step    - the time interval to use for backwards integration
  //        dcm_eos_i - the DCM state at the end of the i-th step.
  // computes the step i's initial DCM state and the end-of-step i-1's dcm state.
  // The computation is stored in the dcm_ini_list and dcm_eos_list.
  Eigen::Vector3d computeDCM_ini_i(const Eigen::Vector3d & r_vrp_d_i, const double & t_step, const Eigen::Vector3d & dcm_eos_i);

  // Computes the double support DCM boundary conditions for continuous double support trajectories
  // step_index the boundary condition for the step index
  // t_DS_{ini, end} the interpolation duration away from a Virtual repelant point.
  Eigen::Vector3d computeDCM_iniDS_i(const int & step_index, const double t_DS_ini);
  Eigen::Vector3d computeDCM_eoDS_i(const int & step_index, const double t_DS_end);
  Eigen::Vector3d computeDCMvel_iniDS_i(const int & step_index, const double t_DS_ini);
  Eigen::Vector3d computeDCMvel_eoDS_i(const int & step_index, const double t_DS_end);
  Eigen::Vector3d computeDCMacc_iniDS_i(const int & step_index, const double t_DS_ini);
  Eigen::Vector3d computeDCMacc_eoDS_i(const int & step_index, const double t_DS_end);

  // Returns the DCM exponential interpolation for the requested step_index.
  // time, t, is clamped between 0.0 and t_step.
  Eigen::Vector3d get_DCM_exp(const int & step_index, const double & t);

  // Returns the DCM velocity exponential interpolation for the requested step_index.
  // time, t, is clamped between 0.0 and t_step.
  Eigen::Vector3d get_DCM_vel_exp(const int & step_index, const double & t);

  // Returns the DCM acceleration exponential interpolation for the requested step_index.
  // time, t, is clamped between 0.0 and t_step.
  Eigen::Vector3d get_DCM_acc_exp(const int & step_index, const double & t);

  // Returns the DCM double support polynomial interpolation for the requested step_index.
  // time, t, is clamped between 0.0 and t_step.
  Eigen::Vector3d get_DCM_DS_poly(const int & step_index, const double & t);

  // Returns the DCM double support velocity polynomial interpolation for the requested step_index.
  // time, t, is clamped between 0.0 and t_step.
  Eigen::Vector3d get_DCM_DS_vel_poly(const int & step_index, const double & t);

  // Returns the DCM double support min jerk interpolation for the requested step_index.
  // time, t, is clamped between 0.0 and t_step.
  Eigen::Vector3d get_DCM_DS_minjerk(const int & step_index, const double & t);

  // Returns the DCM double support velocity min jerk interpolation for the requested step_index.
  // time, t, is clamped between 0.0 and t_step.
  Eigen::Vector3d get_DCM_DS_vel_minjerk(const int & step_index, const double & t);

  // Get the t_step for step i.
  double get_t_step(const int & step_i);

  // returns matrix P \in R^{4x3}
  // P = mat_coeff * [dcm_ini^T; \dot{dcm}_ini^T; dcm_end^T; \dot{dcm}_end]
  // where mat_coeff \in R^{4x4} and ^T denotes a vector transpose transpose.
  // Ts is the duration in seconds
  Eigen::MatrixXd polynomialMatrix(const double Ts,
                                   const Eigen::Vector3d & dcm_ini, const Eigen::Vector3d & dcm_vel_ini,
                                   const Eigen::Vector3d & dcm_end, const Eigen::Vector3d & dcm_vel_end);

  int clampINT(int input, int low_bound, int upper_bound);
  double clampDOUBLE(double input, double low_bound, double upper_bound);

};

#endif


/* orthogonal_test.cc
   Jeremy Barnes, 22 March 2006
   Copyright (c) Jeremy Barnes, 2006.  All rights reserved.
   $Source$

   Test of the orthogonal array.
*/

#include <iostream>
#include "jml/boosting/config_impl.h"
#include <boost/multi_array.hpp>
#include "jml/stats/distribution.h"

using namespace std;

typedef boost::multi_array<unsigned char, 2> Array;

/** Calculate the Hamming distance between two points. */

int distance(const Array & array, int n1, int n2)
{
    int result = 0;
    int l = array.shape()[1];

    for (unsigned i = 0;  i < l;  ++i)
        result += (array[n1][i] != array[n2][i]);

    return result;
}

void test(const Array & array, const distribution<float> & weights)
{
    int n = array.shape()[0];
    int l = array.shape()[1];

    int min_dist = 1000;

    for (unsigned n1 = 0;  n1 < n;  ++n1) {
        for (unsigned n2 = 0;  n2 < n;  ++n2) {
            int dist = distance(array, n1, n2);
            if (dist < min_dist) min_dist = dist;

        }
    }
}

/** Algorithm to generate a maximally distant encoding to expand a set
    of values into a higher coded representation.

    We start with a part of the best hamming code.
*/

int main(int argc, char ** argv)
{
    return 0;
}


/* vim: set sw=4 sts=4 et foldmethod=syntax : */

#ifndef GLASGOW_SUBGRAPH_SOLVER_TEMPLATE_VOODOO_HH
#define GLASGOW_SUBGRAPH_SOLVER_TEMPLATE_VOODOO_HH 1

#include "fixed_bit_set.hh"

#include <array>
#include <type_traits>
#include <utility>
#include <vector>

#include <boost/dynamic_bitset.hpp>

template <template <typename, typename> class Algorithm_, typename Result_, typename Graph_, unsigned size_, unsigned... other_sizes_, typename... Params_>
auto select_graph_size(const std::integer_sequence<unsigned, size_, other_sizes_...> &, const Graph_ & graph, Params_ && ... params) -> Result_
{
    if (graph.size() < int(size_ * bits_per_word)) {
        Algorithm_<FixedBitSet<size_>, std::array<int, size_ * bits_per_word + 1> > algorithm{ graph, std::forward<Params_>(params)... };
        return algorithm.run();
    }
    else {
        if constexpr (0 == sizeof...(other_sizes_)) {
            Algorithm_<boost::dynamic_bitset<>, std::vector<int> > algorithm{ graph, std::forward<Params_>(params)... };
            return algorithm.run();
        }
        else
            return select_graph_size<Algorithm_, Result_, Graph_>(std::integer_sequence<unsigned, other_sizes_...>{}, graph, std::forward<Params_>(params)...);
    }
}

using AllGraphSizes = std::integer_sequence<unsigned, 1, 2, 3, 4, 5, 6, 7, 8, 16, 20, 24, 28, 32, 64, 128, 256, 512, 1024>;

#endif


/*
 *  Sparse linear solver.
 *  Copyright (C) 2004 Bruno Levy
 *  Copyright (C) 2005-2015 Blender Foundation
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  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
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 *  If you modify this software, you should include a notice giving the
 *  name of the person performing the modification, the date of modification,
 *  and the reason for such modification.
 */

#include "linear_solver.h"

#include <Eigen/Sparse>

#include <algorithm>
#include <cassert>
#include <cstdlib>
#include <iostream>
#include <vector>

/* Eigen data structures */

typedef Eigen::SparseMatrix<double, Eigen::ColMajor> EigenSparseMatrix;
typedef Eigen::SparseLU<EigenSparseMatrix> EigenSparseLU;
typedef Eigen::VectorXd EigenVectorX;
typedef Eigen::Triplet<double> EigenTriplet;

/* Linear Solver data structure */

struct LinearSolver
{
	struct Coeff
	{
		Coeff()
		{
			index = 0;
			value = 0.0;
		}

		int index;
		double value;
	};

	struct Variable
	{
		Variable()
		{
			memset(value, 0, sizeof(value));
			locked = false;
			index = 0;
		}

		double value[4];
		bool locked;
		int index;
		std::vector<Coeff> a;
	};

	enum State
	{
		STATE_VARIABLES_CONSTRUCT,
		STATE_MATRIX_CONSTRUCT,
		STATE_MATRIX_SOLVED
	};

	LinearSolver(int num_rows_, int num_variables_, int num_rhs_, bool lsq_)
	{
		assert(num_variables_ > 0);
		assert(num_rhs_ <= 4);

		state = STATE_VARIABLES_CONSTRUCT;
		m = 0;
		n = 0;
		sparseLU = NULL;
		num_variables = num_variables_;
		num_rhs = num_rhs_;
		num_rows = num_rows_;
		least_squares = lsq_;

		variable.resize(num_variables);
	}

	~LinearSolver()
	{
		delete sparseLU;
	}

	State state;

	int n;
	int m;

	std::vector<EigenTriplet> Mtriplets;
	EigenSparseMatrix M;
	EigenSparseMatrix MtM;
	std::vector<EigenVectorX> b;
	std::vector<EigenVectorX> x;

	EigenSparseLU *sparseLU;

	int num_variables;
	std::vector<Variable> variable;

	int num_rows;
	int num_rhs;

	bool least_squares;
};

LinearSolver *EIG_linear_solver_new(int num_rows, int num_columns, int num_rhs)
{
	return new LinearSolver(num_rows, num_columns, num_rhs, false);
}

LinearSolver *EIG_linear_least_squares_solver_new(int num_rows, int num_columns, int num_rhs)
{
	return new LinearSolver(num_rows, num_columns, num_rhs, true);
}

void EIG_linear_solver_delete(LinearSolver *solver)
{
	delete solver;
}

/* Variables */

void EIG_linear_solver_variable_set(LinearSolver *solver, int rhs, int index, double value)
{
	solver->variable[index].value[rhs] = value;
}

double EIG_linear_solver_variable_get(LinearSolver *solver, int rhs, int index)
{
	return solver->variable[index].value[rhs];
}

void EIG_linear_solver_variable_lock(LinearSolver *solver, int index)
{
	if (!solver->variable[index].locked) {
		assert(solver->state == LinearSolver::STATE_VARIABLES_CONSTRUCT);
		solver->variable[index].locked = true;
	}
}

void EIG_linear_solver_variable_unlock(LinearSolver *solver, int index)
{
	if (solver->variable[index].locked) {
		assert(solver->state == LinearSolver::STATE_VARIABLES_CONSTRUCT);
		solver->variable[index].locked = false;
	}
}

static void linear_solver_variables_to_vector(LinearSolver *solver)
{
	int num_rhs = solver->num_rhs;

	for (int i = 0; i < solver->num_variables; i++) {
		LinearSolver::Variable* v = &solver->variable[i];
		if (!v->locked) {
			for (int j = 0; j < num_rhs; j++)
				solver->x[j][v->index] = v->value[j];
		}
	}
}

static void linear_solver_vector_to_variables(LinearSolver *solver)
{
	int num_rhs = solver->num_rhs;

	for (int i = 0; i < solver->num_variables; i++) {
		LinearSolver::Variable* v = &solver->variable[i];
		if (!v->locked) {
			for (int j = 0; j < num_rhs; j++)
				v->value[j] = solver->x[j][v->index];
		}
	}
}

/* Matrix */

static void linear_solver_ensure_matrix_construct(LinearSolver *solver)
{
	/* transition to matrix construction if necessary */
	if (solver->state == LinearSolver::STATE_VARIABLES_CONSTRUCT) {
		int n = 0;

		for (int i = 0; i < solver->num_variables; i++) {
			if (solver->variable[i].locked)
				solver->variable[i].index = ~0;
			else
				solver->variable[i].index = n++;
		}

		int m = (solver->num_rows == 0)? n: solver->num_rows;

		solver->m = m;
		solver->n = n;

		assert(solver->least_squares || m == n);

		/* reserve reasonable estimate */
		solver->Mtriplets.clear();
		solver->Mtriplets.reserve(std::max(m, n)*3);

		solver->b.resize(solver->num_rhs);
		solver->x.resize(solver->num_rhs);

		for (int i = 0; i < solver->num_rhs; i++) {
			solver->b[i].setZero(m);
			solver->x[i].setZero(n);
		}

		linear_solver_variables_to_vector(solver);

		solver->state = LinearSolver::STATE_MATRIX_CONSTRUCT;
	}
}

void EIG_linear_solver_matrix_add(LinearSolver *solver, int row, int col, double value)
{
	if (solver->state == LinearSolver::STATE_MATRIX_SOLVED)
		return;

	linear_solver_ensure_matrix_construct(solver);

	if (!solver->least_squares && solver->variable[row].locked);
	else if (solver->variable[col].locked) {
		if (!solver->least_squares)
			row = solver->variable[row].index;

		LinearSolver::Coeff coeff;
		coeff.index = row;
		coeff.value = value;
		solver->variable[col].a.push_back(coeff);
	}
	else {
		if (!solver->least_squares)
			row = solver->variable[row].index;
		col = solver->variable[col].index;

		/* direct insert into matrix is too slow, so use triplets */
		EigenTriplet triplet(row, col, value);
		solver->Mtriplets.push_back(triplet);
	}
}

/* Right hand side */

void EIG_linear_solver_right_hand_side_add(LinearSolver *solver, int rhs, int index, double value)
{
	linear_solver_ensure_matrix_construct(solver);

	if (solver->least_squares) {
		solver->b[rhs][index] += value;
	}
	else if (!solver->variable[index].locked) {
		index = solver->variable[index].index;
		solver->b[rhs][index] += value;
	}
}

/* Solve */

bool EIG_linear_solver_solve(LinearSolver *solver)
{
	/* nothing to solve, perhaps all variables were locked */
	if (solver->m == 0 || solver->n == 0)
		return true;

	bool result = true;

	assert(solver->state != LinearSolver::STATE_VARIABLES_CONSTRUCT);

	if (solver->state == LinearSolver::STATE_MATRIX_CONSTRUCT) {
		/* create matrix from triplets */
		solver->M.resize(solver->m, solver->n);
		solver->M.setFromTriplets(solver->Mtriplets.begin(), solver->Mtriplets.end());
		solver->Mtriplets.clear();

		/* create least squares matrix */
		if (solver->least_squares)
			solver->MtM = solver->M.transpose() * solver->M;

		/* convert M to compressed column format */
		EigenSparseMatrix& M = (solver->least_squares)? solver->MtM: solver->M;
		M.makeCompressed();

		/* perform sparse LU factorization */
		EigenSparseLU *sparseLU = new EigenSparseLU();
		solver->sparseLU = sparseLU;

		sparseLU->compute(M);
		result = (sparseLU->info() == Eigen::Success);

		solver->state = LinearSolver::STATE_MATRIX_SOLVED;
	}

	if (result) {
		/* solve for each right hand side */
		for (int rhs = 0; rhs < solver->num_rhs; rhs++) {
			/* modify for locked variables */
			EigenVectorX& b = solver->b[rhs];

			for (int i = 0; i < solver->num_variables; i++) {
				LinearSolver::Variable *variable = &solver->variable[i];

				if (variable->locked) {
					std::vector<LinearSolver::Coeff>& a = variable->a;

					for (int j = 0; j < a.size(); j++)
						b[a[j].index] -= a[j].value*variable->value[rhs];
				}
			}

			/* solve */
			if (solver->least_squares) {
				EigenVectorX Mtb = solver->M.transpose() * b;
				solver->x[rhs] = solver->sparseLU->solve(Mtb);
			}
			else {
				EigenVectorX& b = solver->b[rhs];
				solver->x[rhs] = solver->sparseLU->solve(b);
			}

			if (solver->sparseLU->info() != Eigen::Success)
				result = false;
		}

		if (result)
			linear_solver_vector_to_variables(solver);
	}

	/* clear for next solve */
	for (int rhs = 0; rhs < solver->num_rhs; rhs++)
		solver->b[rhs].setZero(solver->m);

	return result;
}

/* Debugging */

void EIG_linear_solver_print_matrix(LinearSolver *solver)
{
	std::cout << "A:" << solver->M << std::endl;

	for (int rhs = 0; rhs < solver->num_rhs; rhs++)
		std::cout << "b " << rhs << ":" << solver->b[rhs] << std::endl;

	if (solver->MtM.rows() && solver->MtM.cols())
		std::cout << "AtA:" << solver->MtM << std::endl;
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/monad/bind_mcd.hpp>
#include <boost/hana/monad/flatten_mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/minimal/comparable.hpp>
#include <boost/hana/detail/minimal/monad.hpp>
using namespace boost::hana;

template <int i>
constexpr auto x = detail::minimal::comparable<>(i);
struct invalid { };

template <template <typename ...> class mcd>
void test() {
    constexpr auto monad = detail::minimal::monad<mcd>;

    BOOST_HANA_CONSTEXPR_ASSERT(then(monad(invalid{}), monad(x<1>)) == monad(x<1>));
}

int main() {
    test<Monad::flatten_mcd>();
    test<Monad::bind_mcd>();
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include <catch.hpp>

#include <cmath>

#include <Eigen/Core>

#include "cavity/GePolCavity.hpp"
#include "utils/Molecule.hpp"
#include "utils/Symmetry.hpp"
#include "TestingMolecules.hpp"

SCENARIO("GePol cavity for the H3+ molecule in C2v symmetry", "[gepol][gepol_H3+_C2v]")
{
    GIVEN("The H3+ molecule in C2v symmetry")
    {
        Molecule molec = H3<5>();

        WHEN("the addition of spheres is enabled")
        {
            double area = 0.2 / bohr2ToAngstrom2();
            double probeRadius = 1.385 / bohrToAngstrom();
            double minRadius = 0.2 / bohrToAngstrom();
            GePolCavity cavity = GePolCavity(molec, area, probeRadius, minRadius, "c2v");
            cavity.saveCavity("h3+_c2v.npz");

            /*! \class GePolCavity
             *  \test \b GePolCavityC2vAddTest_size tests GePol cavity size for H3+ in C2v symmetry with added spheres
             */
            THEN("the size of the cavity is")
            {
                int size = 312;
                int actualSize = cavity.size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC2vAddTest_irreducible_size tests GePol cavity irreducible size for H3+ in C2v symmetry with added spheres
             */
            AND_THEN("the irreducible size of the cavity is")
            {
                int size = 78;
                int actualSize = cavity.irreducible_size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC2vAddTest_area tests GePol cavity surface area for H3+ in C2v symmetry with added spheres
             */
            AND_THEN("the surface area of the cavity is")
            {
                double area = 178.74700256128352;
                double actualArea = cavity.elementArea().sum();
                REQUIRE(area == Approx(actualArea));
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC2vAddTest_volume tests GePol cavity volume for H3+ in C2v symmetry with added spheres
             */
            AND_THEN("the volume of the cavity is")
            {
                double volume = 196.47360294559090;
                Eigen::Matrix3Xd elementCenter = cavity.elementCenter();
                Eigen::Matrix3Xd elementNormal = cavity.elementNormal();
                double actualVolume = 0;
                for ( int i = 0; i < cavity.size(); ++i ) {
                    actualVolume += cavity.elementArea(i) * elementCenter.col(i).dot(elementNormal.col(
                                i));
                }
                actualVolume /= 3;
                REQUIRE(volume == Approx(actualVolume));
            }
        }

        WHEN("the addition of spheres is disabled")
        {
            double area = 0.2 / bohr2ToAngstrom2();
            double probeRadius = 1.385 / bohrToAngstrom();
            double minRadius = 100.0 / bohrToAngstrom();
            GePolCavity cavity = GePolCavity(molec, area, probeRadius, minRadius, "c2v_noadd");
            cavity.saveCavity("h3+_c2v_noadd.npz");

            /*! \class GePolCavity
             *  \test \b GePolCavityC2vTest_size tests GePol cavity size for H3+ in C2v symmetry without added spheres
             */
            THEN("the size of the cavity is")
            {
                int size = 288;
                int actualSize = cavity.size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC2vTest_irreducible_size tests GePol cavity irreducible size for H3+ in C2v symmetry without added spheres
             */
            AND_THEN("the irreducible size of the cavity is")
            {
                int size = 72;
                int actualSize = cavity.irreducible_size();
                REQUIRE(size == actualSize);
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC2vTest_area tests GePol cavity surface area for H3+ in C2v symmetry without added spheres
             */
            AND_THEN("the surface area of the cavity is")
            {
                double area = 181.87043332808548;
                double actualArea = cavity.elementArea().sum();
                REQUIRE(area == Approx(actualArea));
            }
            /*! \class GePolCavity
             *  \test \b GePolCavityC2vTest_volume tests GePol cavity volume for H3+ in C2v symmetry without added spheres
             */
            AND_THEN("the volume of the cavity is")
            {
                double volume = 192.48281460140359;
                Eigen::Matrix3Xd elementCenter = cavity.elementCenter();
                Eigen::Matrix3Xd elementNormal = cavity.elementNormal();
                double actualVolume = 0;
                for ( int i = 0; i < cavity.size(); ++i ) {
                    actualVolume += cavity.elementArea(i) * elementCenter.col(i).dot(elementNormal.col(
                                i));
                }
                actualVolume /= 3;
                REQUIRE(volume == Approx(actualVolume));
            }
        }
    }
}


#include "metric.hpp"

#include <core/metric.hpp>

#include <boost/python.hpp>

#include "../uniform.hpp"
#include "../rigid_mass.hpp"

#include "numpy.hpp"

namespace python {

  template<class Metric>
  static void bind_uniform(const char* name) {
	using namespace boost::python;
	class_< Metric, std::shared_ptr< Metric>,
			bases< metric_base >, boost::noncopyable >(name)
	  .def_readwrite("value", &Metric::value)
	  ;
  }

  void metric::module() {
	using namespace boost::python;
	using ::metric;

	class_<metric_base, std::shared_ptr<metric_base>,
		   boost::noncopyable >("metric_base", no_init);

    //
	class_< mass<vec3>, std::shared_ptr< mass<vec3>>,
			boost::noncopyable, bases<metric_base> >("mass_vec3", no_init);

	class_< mass<rigid>, std::shared_ptr< mass<rigid>>,
			boost::noncopyable, bases<metric_base> >("mass_rigid", no_init);

	// TODO: put this in uniform.cpp
	bind_uniform< uniform_mass<vec3> >("uniform_mass_vec3");
    bind_uniform< uniform_mass<rigid> >("uniform_mass_rigid");

	bind_uniform< uniform_stiffness<vec3> >("uniform_stiffness_vec3");
	bind_uniform< uniform_stiffness<real> >("uniform_stiffness_real");

	bind_uniform< uniform_compliance<vec3> >("uniform_compliance_vec3");
	bind_uniform< uniform_compliance<real> >("uniform_compliance_real");

    static auto get_mass = +[](rigid_mass<real>* self) {
      return numpy::ndarray(self->mass.begin(), { (npy_intp)self->size() } );
    };

    static auto set_mass = +[](rigid_mass<real>* self, object obj) {
      get_mass(self) = obj;
    };

    static auto get_inertia = +[](rigid_mass<real>* self) {
      return numpy::ndarray(self->inertia.begin()->data(), { (npy_intp)self->size(), 3 } );
    };

    static auto set_inertia = +[](rigid_mass<real>* self, object obj) {
      get_inertia(self) = obj;
    };

    class_< rigid_mass<real>, std::shared_ptr< rigid_mass<real> >,
            boost::noncopyable, bases< mass<rigid> > > ("mass_rigid")
      .add_property("mass", get_mass, set_mass)
      .add_property("inertia", get_inertia, set_inertia)
      ;

  }
}


/* --------------------------------------------------------------------------
*
* (C) Copyright …
*
* ---------------------------------------------------------------------------
*/

/*!
 * @file ModelBasedDetection.cpp
 * @date 08/01/2019
 * @author Souriya Trinh
 */

/*!
 * @addtogroup DFNsTest
 *
 * Unit Test for the DFN Model-based detection.
 *
 * @{
 */

/* --------------------------------------------------------------------------
 *
 * Includes
 *
 * --------------------------------------------------------------------------
 */
#include <catch.hpp>
#include <Types/CPP/Pose.hpp>
#include <Eigen/Geometry>
#include <ModelBasedDetection/Linemod.hpp>
#include <opencv2/imgcodecs.hpp>

static void cvMat2ASN1(const cv::Mat& mat, asn1SccFrame * asn)
{
    asn1SccArray3D &imageOnly = asn->data;
    imageOnly.msgVersion = array3D_Version;
    imageOnly.rows = static_cast<asn1SccT_UInt32>(mat.rows);
    imageOnly.cols = static_cast<asn1SccT_UInt32>(mat.cols);
    imageOnly.channels = static_cast<asn1SccT_UInt32>(mat.channels());
    imageOnly.depth = static_cast<asn1SccArray3D_depth_t>(mat.depth());
    imageOnly.rowSize = mat.step[0];
    imageOnly.data.nCount = static_cast<int>(imageOnly.rows * imageOnly.rowSize);
    memcpy(imageOnly.data.arr, mat.data, static_cast<size_t>(imageOnly.data.nCount));
}

/* --------------------------------------------------------------------------
 *
 * Test Cases
 *
 * --------------------------------------------------------------------------
 */
TEST_CASE( "Call to process (Linemod detection color image)", "[process]" )
{
    //Detect on an image generated during the training using intrinsic parameters
    //that correspond to the DLR cameras
    int index = 60;

    // Load color and depth image
    cv::Mat cvColorImage = cv::imread(cv::format("../tests/Data/Images/Linemod/mod1/Linemod_color_%04d.png", index));

    // Convert to ASN1 format
    asn1SccFrame *colorImage = new asn1SccFrame;
    asn1SccFrame_Initialize(colorImage);

    // Copy image data
    cvMat2ASN1(cvColorImage, colorImage);

    // Instantiate DFN
    CDFF::DFN::ModelBasedDetection::Linemod *linemod = new CDFF::DFN::ModelBasedDetection::Linemod;

    // Configure
    linemod->parameters.useDepthModality = false;
    linemod->parameters.cadObjectName = "../tests/Data/Images/Linemod/mod1/oos";
    linemod->configure();

    // Send input data to DFN
    linemod->imageInput(*colorImage);

    // Run DFN
    linemod->process();

    // Get results
    float similarity = 0.0;
    cv::Rect detection;
    std::string class_id;
    int template_id = -1;
    cv::Vec3d vec_R, vec_T;
    cv::Mat cameraPose;
    bool retDetection = linemod->getDetection(similarity, detection, class_id, template_id, vec_R, vec_T, cameraPose);

    REQUIRE(retDetection);
    REQUIRE(similarity == 100.0f);
    REQUIRE(template_id == index);

    // Query output data from DFN
    const PoseWrapper::Pose3D& camera = linemod->cameraOutput();
    const asn1SccMatrix2d& boundingBox = linemod->detectionBoundingBoxOutput();
    bool success = linemod->successOutput();

    REQUIRE(success);
    Eigen::Quaternionf q(camera.orient.arr[3], camera.orient.arr[0], camera.orient.arr[1], camera.orient.arr[2]);
    Eigen::Matrix3f R = q.toRotationMatrix();
    for (int i = 0; i < 3; i++)
    {
        REQUIRE(camera.pos.arr[i] == Approx(cameraPose.at<double>(i,3)).epsilon(std::numeric_limits<float>::epsilon()));
        for (int j = 0; j < 3; j++)
        {
            REQUIRE(R(i,j) == Approx(cameraPose.at<double>(i,j)).epsilon(1e-5));
        }
    }

    REQUIRE(detection.tl().x == Approx(boundingBox.arr[0].arr[0]).epsilon(std::numeric_limits<double>::epsilon()));
    REQUIRE(detection.tl().y == Approx(boundingBox.arr[0].arr[1]).epsilon(std::numeric_limits<double>::epsilon()));
    REQUIRE(detection.width == Approx(boundingBox.arr[1].arr[0]).epsilon(std::numeric_limits<double>::epsilon()));
    REQUIRE(detection.height == Approx(boundingBox.arr[1].arr[1]).epsilon(std::numeric_limits<double>::epsilon()));

    // Cleanup
    delete colorImage;
    delete linemod;
}

TEST_CASE( "Call to process (Linemod detection color + depth images)", "[process]" )
{
    //Detect on image + depth generated during the training using intrinsic parameters
    //that correspond to the DLR cameras
    int index = 509;

    // Load color and depth image
    cv::Mat cvColorImage = cv::imread(cv::format("../tests/Data/Images/Linemod/mod2/Linemod_color_%04d.png", index));
    cv::Mat cvDepthImageRaw = cv::imread(cv::format("../tests/Data/Images/Linemod/mod2/Linemod_depth_%04d.exr", index), cv::IMREAD_ANYCOLOR | cv::IMREAD_ANYDEPTH);
    cv::Mat cvDepthImage;
    cvDepthImageRaw.convertTo(cvDepthImage, CV_16U);

    // Convert to ASN1 format
    asn1SccFrame *colorImage = new asn1SccFrame;
    asn1SccFrame *depthImage = new asn1SccFrame;
    asn1SccFrame_Initialize(colorImage);
    asn1SccFrame_Initialize(depthImage);

    // Copy image data
    cvMat2ASN1(cvColorImage, colorImage);
    cvMat2ASN1(cvDepthImage, depthImage);

    // Instantiate DFN
    CDFF::DFN::ModelBasedDetection::Linemod *linemod = new CDFF::DFN::ModelBasedDetection::Linemod;

    // Configure
    linemod->parameters.useDepthModality = true;
    linemod->parameters.cadObjectName = "../tests/Data/Images/Linemod/mod2/oos";
    linemod->configure();

    // Send input data to DFN
    linemod->imageInput(*colorImage);
    linemod->depthInput(*depthImage);

    // Run DFN
    linemod->process();

    // Get results
    float similarity = 0.0;
    cv::Rect detection;
    std::string class_id;
    int template_id = -1;
    cv::Vec3d vec_R, vec_T;
    cv::Mat cameraPose;
    bool retDetection = linemod->getDetection(similarity, detection, class_id, template_id, vec_R, vec_T, cameraPose);

    REQUIRE(retDetection);
    REQUIRE(similarity == 100.0f);
    REQUIRE(template_id == index);

    // Query output data from DFN
    const PoseWrapper::Pose3D& camera = linemod->cameraOutput();
    const asn1SccMatrix2d& boundingBox = linemod->detectionBoundingBoxOutput();
    bool success = linemod->successOutput();

    REQUIRE(success);
    Eigen::Quaternionf q(camera.orient.arr[3], camera.orient.arr[0], camera.orient.arr[1], camera.orient.arr[2]);
    Eigen::Matrix3f R = q.toRotationMatrix();
    for (int i = 0; i < 3; i++)
    {
        REQUIRE(camera.pos.arr[i] == Approx(cameraPose.at<double>(i,3)).epsilon(std::numeric_limits<float>::epsilon()));
        for (int j = 0; j < 3; j++)
        {
            REQUIRE(R(i,j) == Approx(cameraPose.at<double>(i,j)).epsilon(1e-5));
        }
    }

    REQUIRE(detection.tl().x == Approx(boundingBox.arr[0].arr[0]).epsilon(std::numeric_limits<double>::epsilon()));
    REQUIRE(detection.tl().y == Approx(boundingBox.arr[0].arr[1]).epsilon(std::numeric_limits<double>::epsilon()));
    REQUIRE(detection.width == Approx(boundingBox.arr[1].arr[0]).epsilon(std::numeric_limits<double>::epsilon()));
    REQUIRE(detection.height == Approx(boundingBox.arr[1].arr[1]).epsilon(std::numeric_limits<double>::epsilon()));

    // Cleanup
    delete colorImage;
    delete depthImage;
    delete linemod;
}

TEST_CASE( "Call to process (Linemod detection color + depth images Blender)", "[process]" )
{
    //Detect on image + depth generated by Blender using a camera pose generated during the training
    //Training is done using intrinsic parameters for Blender camera
    int index = 530;

    // Load color and depth image
    cv::Mat cvColorImage = cv::imread(cv::format("../tests/Data/Images/Linemod/mod2_Blender/Linemod_color_%04d.png", index));
    cv::Mat cvDepthImageRaw = cv::imread(cv::format("../tests/Data/Images/Linemod/mod2_Blender/Linemod_depth_%04d.exr", index), cv::IMREAD_ANYCOLOR | cv::IMREAD_ANYDEPTH);
    cv::Mat cvDepthImage;
    cvDepthImageRaw.convertTo(cvDepthImage, CV_16U);

    // Convert to ASN1 format
    asn1SccFrame *colorImage = new asn1SccFrame;
    asn1SccFrame *depthImage = new asn1SccFrame;
    asn1SccFrame_Initialize(colorImage);
    asn1SccFrame_Initialize(depthImage);

    // Copy image data
    cvMat2ASN1(cvColorImage, colorImage);
    cvMat2ASN1(cvDepthImage, depthImage);

    // Instantiate DFN
    CDFF::DFN::ModelBasedDetection::Linemod *linemod = new CDFF::DFN::ModelBasedDetection::Linemod;

    // Configure
    linemod->parameters.useDepthModality = true;
    linemod->parameters.cadObjectName = "../tests/Data/Images/Linemod/mod2_Blender/oos";
    linemod->configure();

    // Send input data to DFN
    linemod->imageInput(*colorImage);
    linemod->depthInput(*depthImage);

    // Run DFN
    linemod->process();

    // Get results
    float similarity = 0.0;
    cv::Rect detection;
    std::string class_id;
    int template_id = -1;
    cv::Vec3d vec_R, vec_T;
    cv::Mat cameraPose;
    bool retDetection = linemod->getDetection(similarity, detection, class_id, template_id, vec_R, vec_T, cameraPose);

    REQUIRE(retDetection);
    REQUIRE(similarity >= 99.0f);
    REQUIRE(template_id == index);

    // Query output data from DFN
    const PoseWrapper::Pose3D& camera = linemod->cameraOutput();
    const asn1SccMatrix2d& boundingBox = linemod->detectionBoundingBoxOutput();
    bool success = linemod->successOutput();

    REQUIRE(success);
    Eigen::Quaternionf q(camera.orient.arr[3], camera.orient.arr[0], camera.orient.arr[1], camera.orient.arr[2]);
    Eigen::Matrix3f R = q.toRotationMatrix();
    for (int i = 0; i < 3; i++)
    {
        REQUIRE(camera.pos.arr[i] == Approx(cameraPose.at<double>(i,3)).epsilon(std::numeric_limits<float>::epsilon()));
        for (int j = 0; j < 3; j++)
        {
            REQUIRE(R(i,j) == Approx(cameraPose.at<double>(i,j)).epsilon(1e-5));
        }
    }

    REQUIRE(detection.tl().x == Approx(boundingBox.arr[0].arr[0]).epsilon(std::numeric_limits<double>::epsilon()));
    REQUIRE(detection.tl().y == Approx(boundingBox.arr[0].arr[1]).epsilon(std::numeric_limits<double>::epsilon()));
    REQUIRE(detection.width == Approx(boundingBox.arr[1].arr[0]).epsilon(std::numeric_limits<double>::epsilon()));
    REQUIRE(detection.height == Approx(boundingBox.arr[1].arr[1]).epsilon(std::numeric_limits<double>::epsilon()));

    // Cleanup
    delete colorImage;
    delete depthImage;
    delete linemod;
}

/** @} */


/*******************************************************************************
*
*  Filename    : PlotMCRes.cc
*  Description : Plotting control flow
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "TstarAnalysis/Common/interface/PlotStyle.hpp"
#include "TstarAnalysis/SignalMCStudy/interface/Common.hpp"
#include "TstarAnalysis/SignalMCStudy/interface/MCResMgr.hpp"

#include "ManagerUtils/PlotUtils/interface/Common.hpp"

#include <boost/format.hpp>
#include <string>
using namespace std;

void
MakeMCResPlot( MCResMgr* mgr )
{
  boost::format legentry( "%s (s.d. %.1lf)" );
  {// w mass plots
    TCanvas* c = mgr::NewCanvas();

    TH1D* hist = mgr->Hist( "HadWMass" );
    hist->Draw( "HIST" );
    mgr::SetSinglePad( c );
    mgr::SetAxis( hist );
    hist->SetMaximum( mgr::GetYmax( hist ) * 1.6 );

    const string wentry = boost::str(legentry % "Hadronic W boson" % hist->GetRMS());
    TLegend* leg        = mgr::NewLegend( 0.45, 0.7 );
    leg->AddEntry( hist, wentry.c_str(), "l" );
    leg->Draw();

    mgr::DrawCMSLabel( SIMULATION );
    mgr::SaveToPDF( c, PlotFileName( "wres_" + mgr->Name() ) );

    delete c;
  }
  {// top resolution
    TCanvas* c  = mgr::NewCanvas();
    TH1D* hist1 = mgr->Hist( "HadTopMass" );
    TH1D* hist2 = mgr->Hist( "LepTopMass" );
    hist1->Draw( "HIST" );
    hist2->Draw( "HISTSAME" );
    mgr::SetSinglePad( c );
    mgr::SetAxis( hist1 );

    hist1->GetXaxis()->SetTitle( "Top mass (truth reconstruction) (GeV/c^{2})" );
    hist1->SetMaximum( mgr::GetYmax( hist1, hist2 ) *1.6 );

    hist1->SetLineColor( KBLUE );
    hist2->SetLineColor( KRED  );

    const string hadtop = boost::str(legentry % "Hadronic top" % hist1->GetRMS());
    const string leptop = boost::str(legentry % "Leptonic top" % hist2->GetRMS());
    TLegend* leg = mgr::NewLegend( 0.5, 0.7 );
    leg->AddEntry( hist1, hadtop.c_str(), "l" );
    leg->AddEntry( hist2, leptop.c_str(), "l" );
    leg->Draw();

    mgr::DrawCMSLabel( SIMULATION );
    mgr::SaveToPDF( c, PlotFileName( "topres_" + mgr->Name() ) );
    delete leg;
    delete c;
  }
  {// tstar resolution
    TCanvas* c = mgr::NewCanvas();

    TH1D* hist1 = mgr->Hist( "HadTstarMass" );
    TH1D* hist2 = mgr->Hist( "LepTstarMass" );
    hist1->Draw( "HIST" );
    hist2->Draw( "HISTSAME" );

    mgr::SetSinglePad( c );
    mgr::SetAxis( hist1 );
    hist1->GetXaxis()->SetTitle( "Top mass (truth reconstruction) (GeV/c^{2})" );
    hist1->SetMaximum( mgr::GetYmax( hist1, hist2 ) * 1.6 );

    hist1->SetLineColor( KBLUE );
    hist2->SetLineColor( KRED  );

    const string hadtstar = boost::str(legentry % "Hadronic t*" % hist1->GetRMS());
    const string leptstar = boost::str(legentry % "Leptonic t*" % hist2->GetRMS());

    TLegend* leg = mgr::NewLegend( 0.5, 0.7 );
    leg->AddEntry( hist1, hadtstar.c_str(), "l" );
    leg->AddEntry( hist2, leptstar.c_str(), "l" );
    leg->Draw();

    mgr::DrawCMSLabel( SIMULATION );
    mgr::SaveToPDF( c, PlotFileName( "tstarres_" + mgr->Name() ) );
    delete leg;
    delete c;
  }
  {// tstar resolution by difference
    TCanvas* c = mgr::NewCanvas();

    TH1D* hist = mgr->Hist( "TstarMassDiff" );
    hist->Draw( "HIST" );
    mgr::SetSinglePad( c );
    mgr::SetAxis( hist );
    hist->SetMaximum( mgr::GetYmax( hist ) * 1.2 );

    const string diffentry = boost::str(legentry % "(Had t*)-(Lep t*)" % hist->GetRMS());
    TLegend* leg        = mgr::NewLegend( 0.5, 0.7 );
    leg->AddEntry( hist, diffentry.c_str(), "l" );
    leg->Draw();

    mgr::DrawCMSLabel( SIMULATION );
    mgr::SaveToPDF( c, PlotFileName( "tstardiffres_" + mgr->Name() ) );
    delete c;
  }
}


#include <boost/units/systems/si/angular_momentum.hpp>


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

#include <boost/hana/ext/boost/tuple.hpp>

#include <boost/hana/bool.hpp>
#include <boost/hana/functional/always.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/monad_plus.hpp>

#include <boost/tuple/tuple.hpp>
using namespace boost::hana;

template <int i>
using eq = test::ct_eq<i>;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // Setup for the laws below
    //////////////////////////////////////////////////////////////////////////
    auto eq_tuples = make<tuple_tag>(
          ::boost::make_tuple(eq<0>{})
        , ::boost::make_tuple(eq<0>{}, eq<1>{})
        , ::boost::make_tuple(eq<0>{}, eq<1>{}, eq<2>{})
    );

    auto eq_values = make<tuple_tag>(eq<0>{}, eq<1>{}, eq<2>{});
    auto predicates = make<tuple_tag>(
        equal.to(eq<0>{}), equal.to(eq<1>{}), equal.to(eq<2>{}),
        always(false_c), always(true_c)
    );

    //////////////////////////////////////////////////////////////////////////
    // MonadPlus
    //////////////////////////////////////////////////////////////////////////
    test::TestMonadPlus<ext::boost::tuple_tag>{eq_tuples, predicates, eq_values};
}


/*
    This file is part of Mitsuba, a physically based rendering system.

    Copyright (c) 2007-2012 by Wenzel Jakob and others.

    Mitsuba is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

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

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <mitsuba/render/scene.h>
#include <mitsuba/render/renderproc.h>
#include <mitsuba/core/autodiff.h>
#include <mitsuba/core/statistics.h>
#include <boost/algorithm/string.hpp>

DECLARE_DIFFSCALAR_BASE();

MTS_NAMESPACE_BEGIN

static StatsCounter statsConverged(
		" manifold", "Converged manifold walks", EPercentage);

/*!\plugin{motion}{Motion and specular motion vector integrator}
 * \parameters{
 *     \parameter{time}{\Float}{
 *       Denotes the time stamp of the target frame of the motion vectors.
 *       The current frame is specified via the sensor's \code{shutterOpen}
 *       and \code{shutterClose} parameters, which should both be set to
 *       the same value. \default{0}
 *     }
 *     \parameter{time}{\String}{
 *        Path configuration of the desired motion vectors:
 *        \begin{enumerate}[(i)]
 *           \item \textbf{d}: Primary (non-specular) hit points\vspace{-1mm}
 *           \item \textbf{rd}: A non-specular surface seen through a specular reflection\vspace{-1mm}
 *           \item \textbf{ttd}: A non-specular surface seen through a pair of specular refractions\vspace{-1mm}
 *           \item \textbf{trtd}: A non-specular surface seen through a sequence of refraction, reflection, and refraction events.\vspace{-1mm}
 *        \end{enumerate}
 *        etc.
 *     }
 *     \parameter{derivativesOnly}{\Boolean}{
 *       By default, the Manifold Exploration technique is used to accurately
 *       solve the underlying specular flow problem. When this parameter is set to \code{true},
 *       the nonlinear solver is deactivated, and only first-order extrapolations are provided.
 *       \default{\code{false}}
 *     }
 *     \parameter{glossyThreshold}{\Float}{
 *        Threshold on the roughness parameter of reflectance models
 *        to be classified as specular.
 *        \default{\texttt{0}, i.e.~only perfectly specular materials are classified as specular}
 *     }
 *     \parameter{maxTimeSteps}{\Integer}{
 *        Maximum number of temporal sub-steps \default{5}
 *     }
 *     \parameter{maxSpaceSteps}{\Integer}{
 *        Maximum number of spatial sub-steps \default{10}
 *     }
 * }
 * This integrator extracts motion vectors for animated input scenes, alternatively
 * at primary hit points or at hit points observed through sequences of reflective
 * and refractive objects. The first two color components (R and G) of the resulting
 * rendering specify the screen-space motion in 2D pixel coordinates, and the last
 * component (B) denotes the change in distance of the observed 3D point to the
 * camera position. Sometimes a specular path cannot be tracked from one frame to the other,
 * e.g. because it does not exist, or because the solver did not converge. In this case,
 * the pixel color is set to infinity. The images on the following page show
 * motion vectors obtained for a sphere that is moving from the left to the right.
 *
 * \renderings{
 *     \rendering{Input scene at time $t=0$}{integrator_motion_sphere_1}
 *     \rendering{Input scene at time $t=1$}{integrator_motion_sphere_2}
 * }
 * \renderings{
 *     \medrendering{\code{config="d"}}{integrator_motion_path_d}
 *     \medrendering{\code{config="rd"}}{integrator_motion_path_rd}
 *     \medrendering{\code{config="ttd"}}{integrator_motion_path_ttd}
 * }
 * \renderings{
 *     \rendering{\code{config="trtd"}}{integrator_motion_path_trtd}
 *     \rendering{\code{config="trrtd"}}{integrator_motion_path_trrtd}
 * }
 * \clearpage
 *
 * \begin{xml}[caption={Exemplary scene configuration for computing specular motion vectors}]
 * <scene>
 *     <integrator type="motion">
 *         <string name="config" value="ttd"/>
 *         <float name="time" value="1"/>
 *     </integrator>
 *
 *     <shape type="serialized">
 *         <string name="filename" value="..."/>
 *         <animation name="toWorld">
 *             <transform time="0">
 *                 <!-- Transformation at time zero -->
 *             </transform>
 *             <transform time="1">
 *                 <!-- Transformation at time one -->
 *             </transform>
 *         </animation>
 *          <bsdf type="dielectric"/>
 *     </shape>
 *
 *     <sensor type="perspective">
 *         <float name="shutterOpen" value="0"/>
 *         <float name="shutterClose" value="0"/>
 *
 *         <sampler type="ldsampler">
 *             <integer name="sampleCount" value="1"/>
 *             <boolean name="pixelCenters" value="true"/>
 *         </sampler>
 *
 *         <film type="hdrfilm" id="film">
 *             <string name="pixelFormat" value="rgb"/>
 *             <boolean name="banner" value="false"/>
 *             <rfilter type="box"/>
 *         </film>
 *     </sensor>
 * </scene>
 * \end{xml}
 */

class MotionIntegrator : public SamplingIntegrator {
public:
	typedef Eigen::Matrix<Float, Eigen::Dynamic, Eigen::Dynamic> EMatrix;
	typedef Eigen::Matrix<Float, Eigen::Dynamic, 1> EVector;
	typedef Eigen::Matrix<Float, 1, 7> Gradient;
	typedef DScalar1<Float, Gradient> DScalar;
	typedef DScalar::DVector3 DVector;

	MotionIntegrator(const Properties &props) : SamplingIntegrator(props) {
		m_time = props.getFloat("time");
		m_config = boost::to_lower_copy(props.getString("config", "d"));
		if (m_config.length() == 0)
			Log(EError, "Path configuration string must have at least one entry!");
		if (m_config[m_config.length()-1] != 'd')
			Log(EError, "Configuration string must end with 'd'!");

		m_derivativesOnly = props.getBoolean("derivativesOnly", false);
		m_maxTimeSteps = props.getInteger("maxTimeSteps", 5);
		m_maxSpaceSteps = props.getInteger("maxSpaceSteps", 10);
		m_glossyThreshold = props.getFloat("glossyThreshold", 0);
		m_subSteps = props.getInteger("subSteps", 1);
	}

	MotionIntegrator(Stream *stream, InstanceManager *manager)
	 : SamplingIntegrator(stream, manager) {
		 m_time = stream->readFloat();
		 m_config = stream->readString();
		 m_derivativesOnly = stream->readBool();
		 m_maxTimeSteps = stream->readInt();
		 m_maxSpaceSteps = stream->readInt();
		 m_glossyThreshold = stream->readFloat();
		 m_subSteps = stream->readInt();
	}

	void serialize(Stream *stream, InstanceManager *manager) const {
		SamplingIntegrator::serialize(stream, manager);
		stream->writeFloat(m_time);
		stream->writeString(m_config);
		stream->writeBool(m_derivativesOnly);
		stream->writeInt(m_maxTimeSteps);
		stream->writeInt(m_maxSpaceSteps);
		stream->writeFloat(m_glossyThreshold);
		stream->writeInt(m_subSteps);
	}

	Spectrum Li(const RayDifferential &r, RadianceQueryRecord &rRec) const {
		const Point2 apertureSample(0.5f);
		Point p0, p1;

		if (m_config.length() == 1) {
			Intersection &its = rRec.its;

			// compute motion of environment
			if (!rRec.rayIntersect(r)) {
				BSphere sphere = rRec.scene->getBSphere();
				Float nearT, farT;
				sphere.radius = 1e6;
				sphere.rayIntersect(r, nearT, farT);
				p0 = r(nearT);
			} else {
				p0 = its.p;
			}

			p1 = p0;

			// reproject point according to the motion of the intersected shape
			if (its.isValid() && its.instance != NULL)  {
				Intersection its2(its);
				its2.adjustTime(m_time);
				p1 = its2.p;
			}
		} else {
			std::vector<Intersection> source, target, temp, temp2;
			Ray ray(r);

			/* Trace an initial light path with the given configuration*/
			if (!tracePath(rRec, ray, source))
				return Spectrum(0.0f);

			m_scene = rRec.scene;
			p0 = source[1].p;

			int timeIteration = 0;

			Float stepSizeReduction = 1.0f;
			while (true) {
				if (++timeIteration > m_maxTimeSteps)
					return Spectrum(std::numeric_limits<Float>::infinity());

				Float maxStepSize = (m_time-r.time) / m_subSteps;
				Float timeStepSize = std::min((Float) 1.0f, maxStepSize / (m_time-source[0].time));
				timeStepSize *= stepSizeReduction;

				/* Compute updated intersection records for time 'm_time' */
				adjustTime(rRec, apertureSample, source, target, timeStepSize);

				if (timeIteration == 1) {
					bool moved = false;
					for (size_t i=0; i<source.size(); ++i)
						if ((source[i].p-target[i].p).length() > 1e-4f)
							moved = true;

					if (!moved)
						return Spectrum(0.0f);

					if (m_derivativesOnly) {
						p1 = extrapolateTimePoint(source, target);
						break;
					} else {
						statsConverged.incrementBase();
					}
				}

				if (!timeStep(rRec, source, target, temp, temp2)) {
					stepSizeReduction *= 0.5f;
				} else {
					p1 = source[1].p;
					stepSizeReduction = std::min((Float) 1.0f, stepSizeReduction * 2);

					if (std::abs(source[0].time-m_time) < 1e-5f) {
						++statsConverged;
						break;
					}
				}
			}
		}

		const Sensor *sensor = rRec.scene->getSensor();
		DirectSamplingRecord dRec0(p0, r.time), dRec1(p1, m_time);
		sensor->sampleDirect(dRec0, apertureSample);
		sensor->sampleDirect(dRec1, apertureSample);

		/* Step 4: Compute depth difference */
		Float dDelta = dRec1.dist - dRec0.dist;
		Spectrum result(0.0f);
		result.fromLinearRGB(dRec1.uv.x-dRec0.uv.x,
				dRec1.uv.y-dRec0.uv.y,
				std::isfinite(dDelta) ? dDelta : (Float) 0);
		return result;
	}

	bool timeStep(RadianceQueryRecord &rRec, std::vector<Intersection> &source, const std::vector<Intersection> &target, std::vector<Intersection> &temp, std::vector<Intersection> &temp2) const {
		Ray ray = extrapolateTimeRay(source, target);

		if (!tracePath(rRec, ray, temp))
			return false;

		Float error = computeError(temp, target);

		Float spaceStepSize = 1.0f;
		int spaceIteration = 0;
		while (error > 1e-5f) {
			++spaceIteration;

			if (spaceIteration > m_maxSpaceSteps) {
				return false;
			}

			Ray candidateRay = extrapolateSpaceRay(temp, target, spaceStepSize);

			Float candidateError = 0;
			if (!tracePath(rRec, candidateRay, temp2))
				candidateError = std::numeric_limits<Float>::infinity();
			else
				candidateError = computeError(temp2, target);

			if (candidateError < error) {
				temp = temp2;
				error = candidateError;
				spaceStepSize = std::min((Float) 1.0f, spaceStepSize * 2);
			} else {
				spaceStepSize *= 0.5f;
			}
		}

		source = temp;
		return true;
	}

	bool tracePath(RadianceQueryRecord &rRec, Ray ray, std::vector<Intersection> &intersections) const {
		int depth = 0;

		Intersection its;
		memset(&its, 0, sizeof(Intersection));
		its.p = ray.o;
		its.time = ray.time;

		intersections.clear();
		intersections.push_back(its);

		while (depth < (int) m_config.size()) {
			rRec.scene->rayIntersect(ray, its);

			char interactionType = m_config[depth++];
			if (interactionType == 'd') {
				if (!its.isValid()) {
					BSphere sphere = rRec.scene->getBSphere();
					Float nearT, farT;
					sphere.radius *= 1000;
					bool success = sphere.rayIntersect(ray, nearT, farT);
					Assert(success && nearT < 0 && farT > 0);
					its.p = ray(farT);
					its.geoFrame = its.shFrame = Frame(-ray.d);
					its.dpdu = its.geoFrame.s;
					its.dpdv = its.geoFrame.t;
					its.time = ray.time;
					its.uv = Point2(0.0f);
					its.shape = its.instance = NULL;
				}

				if (its.isValid() && !(its.shape->getBSDF()->getType() & BSDF::EDiffuseReflection) && !its.isEmitter())
					return false;

				intersections.push_back(its);

				break;
			}

			if (!its.isValid())
				return false;

			intersections.push_back(its);

			/* Sample BSDF * cos(theta) */
			const BSDF *bsdf = its.shape->getBSDF();

			BSDFSamplingRecord bRec(its, rRec.sampler, ERadiance);

			if (interactionType == 'r') {
				if (bsdf->getType() & BSDF::EDeltaReflection) {
					bRec.typeMask = BSDF::EDeltaReflection;
				} else if (bsdf->getType() & BSDF::EGlossyReflection) {
					for (int i=0; i<bsdf->getComponentCount(); ++i)
						if ((bsdf->getType(i) & BSDF::EGlossyReflection) && bsdf->getRoughness(its, i) < m_glossyThreshold)
							bRec.typeMask = BSDF::EGlossyReflection;
				}
			} else if (interactionType == 't') {
				if (bsdf->getType() & BSDF::EDeltaTransmission) {
					bRec.typeMask = BSDF::EDeltaTransmission;
				} else if (bsdf->getType() & BSDF::EGlossyTransmission) {
					for (int i=0; i<bsdf->getComponentCount(); ++i)
						if ((bsdf->getType(i) & BSDF::EGlossyTransmission) && bsdf->getRoughness(its, i) < m_glossyThreshold)
							bRec.typeMask = BSDF::EGlossyTransmission;
				}
			}

			if (bRec.typeMask == BSDF::EAll)
				return false;

			Spectrum bsdfWeight = bsdf->sample(bRec, Point2(0.0f));
			if (bsdfWeight.isZero())
				return false;

			const Vector wo = its.toWorld(bRec.wo);

			ray = Ray(its.p, wo, ray.time);
		}

		return true;
	}

	void adjustTime(const RadianceQueryRecord &rRec, const Point2 &apertureSample, const std::vector<Intersection> &source, std::vector<Intersection> &target, Float timeStepSize) const {
		target = source;

		Float targetTime = (1-timeStepSize) * source[0].time + timeStepSize * m_time;

		const Sensor *sensor = rRec.scene->getSensor();
		DirectSamplingRecord dRec0(Point(0.0f), source[0].time),
							 dRec1(Point(0.0f), targetTime);
		sensor->sampleDirect(dRec0, apertureSample);
		sensor->sampleDirect(dRec1, apertureSample);
		Assert((source[0].p-dRec0.p).lengthSquared() < 1e-6);

		target[0].p = dRec1.p;
		target[0].time = targetTime;

		for (size_t i=1; i<target.size(); ++i)
			target[i].adjustTime(targetTime);
	}

	DVector getVertexPosition(const std::vector<Intersection> &source, const std::vector<Intersection> &target, int i, int rel) const {
		DScalar u(rel*2, 0), v(rel*2+1, 0), time(6, 0);

		return DScalar::vector(source[i].p)
		     + DScalar::vector(source[i].dpdu) * u
		     + DScalar::vector(source[i].dpdv) * v
		     + DScalar::vector(target[i].p-source[i].p) * time;
	}

	void getVertexFrame(const std::vector<Intersection> &source, const std::vector<Intersection> &target, int i, DVector &s, DVector &t, DVector &n) const {
		DScalar u(2, 0), v(3, 0), time(6, 0);
		const BSDF *bsdf = source[i].shape->getBSDF();

		Frame frame, du, dv;
		frame = bsdf->getFrame(source[i]);
		bsdf->getFrameDerivative(source[i], du, dv);

		DVector n0 = DScalar::vector(frame.n)
			+ DScalar::vector(du.n) * u
			+ DScalar::vector(dv.n) * v;
		DVector s0 = DScalar::vector(frame.s)
			+ DScalar::vector(du.s) * u
			+ DScalar::vector(dv.s) * v;
		DVector t0 = DScalar::vector(frame.t)
			+ DScalar::vector(du.t) * u
			+ DScalar::vector(dv.t) * v;

		frame = bsdf->getFrame(target[i]);
		bsdf->getFrameDerivative(target[i], du, dv);

		DVector n1 = DScalar::vector(frame.n)
			+ DScalar::vector(du.n) * u
			+ DScalar::vector(dv.n) * v;
		DVector s1 = DScalar::vector(frame.s)
			+ DScalar::vector(du.s) * u
			+ DScalar::vector(dv.s) * v;
		DVector t1 = DScalar::vector(frame.t)
			+ DScalar::vector(du.t) * u
			+ DScalar::vector(dv.t) * v;

		n = (1-time)*n0 + time*n1;
		s = (1-time)*s0 + time*s1;
		t = (1-time)*t0 + time*t1;
	}

	void assembleMatrix(const std::vector<Intersection> &source, const std::vector<Intersection> &target, EMatrix &M) const {
		DScalar::setVariableCount(7);
		M.resize(2*(source.size()-2), 2*source.size()+1);
		M.setZero();

		for (int i=1; i < (int) source.size()-1; ++i) {
			DVector p_pred = getVertexPosition(source, target, i-1, 0);
			DVector p_cur  = getVertexPosition(source, target, i, 1);
			DVector p_succ = getVertexPosition(source, target, i+1, 2);

			DVector s, t, n;
			getVertexFrame(source, target, i, s, t, n);

			DVector wi = p_pred - p_cur, wo = p_succ - p_cur;
			wi *= inverse(sqrt(wi.dot(wi)));
			wo *= inverse(sqrt(wo.dot(wo)));

			Float eta = source[i].shape->getBSDF()->getEta();

			if (m_config[i-1] == 'r')
				eta = 1;
			else if (wi.dot(n).getValue() < 0)
				eta = 1/eta;

			DVector H = wi + wo * DScalar(eta);
			H *= inverse(sqrt(H.dot(H)));

			Gradient H_s = H.dot(s).getGradient(),
			         H_t = H.dot(t).getGradient();

			M.block<1, 6>((i-1)*2+0, (i-1)*2) = H_s.segment<6>(0);
			M.block<1, 6>((i-1)*2+1, (i-1)*2) = H_t.segment<6>(0);
			M((i-1)*2+0, M.cols()-1) = H_s(6);
			M((i-1)*2+1, M.cols()-1) = H_t(6);
		}
	}

	Point extrapolateTimePoint(const std::vector<Intersection> &source, const std::vector<Intersection> &target) const {
		EMatrix M;
		assembleMatrix(source, target, M);
		EVector b = -M.block(0, 2, (source.size()-2)*2, (source.size()-2)*2).lu().solve(M.col(M.cols()-1));
		return target[1].p + target[1].dpdu * b[0] + target[1].dpdv * b[1];
	}

	Ray extrapolateTimeRay(const std::vector<Intersection> &source, const std::vector<Intersection> &target) const {
		EMatrix M;
		assembleMatrix(source, target, M);

		EVector b = -M.block(0, 2, (source.size()-2)*2, (source.size()-2)*2).lu().solve(M.col(M.cols()-1));
		Point rayOrigin = target[0].p;
		Point rayTarget = target[1].p + target[1].dpdu * b[0] + target[1].dpdv * b[1];
		return Ray(rayOrigin, normalize(rayTarget-rayOrigin), target[1].time);
	}

	Float computeError(const std::vector<Intersection> &source, const std::vector<Intersection> &target) const {
		int last = source.size()-1;
		Float scale = std::max(Epsilon,std::max(std::abs(target[last].p.x), std::max(std::abs(target[last].p.y), std::abs(target[last].p.z))));
		return (target[last].p-source[last].p).length() / scale;
	}

	Ray extrapolateSpaceRay(const std::vector<Intersection> &source, const std::vector<Intersection> &target, Float stepSize) const {
		EMatrix M;
		assembleMatrix(source, target, M);

		Eigen::PartialPivLU<EMatrix> lu = M.block(0, 2, (source.size()-2)*2, (source.size()-2)*2).lu();

		int last = source.size()-1;
		Vector rel = target[last].p-source[last].p,
		       dpdu = source[last].dpdu,
			   dpdv = source[last].dpdv;

		Float b1 = dot(rel, dpdu),
			  b2 = dot(rel, dpdv),
			  a11 = dot(dpdu, dpdu), a12 = dot(dpdu, dpdv),
			  a22 = dot(dpdv, dpdv),
			  det = a11 * a22 - a12 * a12;

		Float invDet = 1.0f / det,
		      du = ( a22 * b1 - a12 * b2) * invDet,
		      dv = (-a12 * b1 + a11 * b2) * invDet;

		EVector b = -(du*lu.solve(M.col(M.cols()-3)) + dv*lu.solve(M.col(M.cols()-2)));

		Point rayTarget = source[1].p + stepSize * (source[1].dpdu * b[0] + source[1].dpdv * b[1]);

		return Ray(source[0].p, normalize(rayTarget-source[0].p), source[1].time);
	}

	std::string toString() const {
		return "MotionIntegrator[]";
	}

	MTS_DECLARE_CLASS()
private:
	Float m_time;
	std::string m_config;
	bool m_derivativesOnly;
	int m_maxSpaceSteps;
	int m_maxTimeSteps;
	int m_subSteps;
	Float m_glossyThreshold;
	mutable const Scene *m_scene;
};

MTS_IMPLEMENT_CLASS_S(MotionIntegrator, false, SamplingIntegrator)
MTS_EXPORT_PLUGIN(MotionIntegrator, " motion vector integrator");
MTS_NAMESPACE_END


/*
 *
 * Copyright (c) Kresimir Fresl 2002
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_TRAITS_DETAIL_UBLAS_BANDED_ORDERING_H
#define BOOST_NUMERIC_BINDINGS_TRAITS_DETAIL_UBLAS_BANDED_ORDERING_H

#include <boost/numeric/ublas/fwd.hpp>

namespace boost { namespace numeric { namespace bindings { namespace traits {

  namespace detail {

    template <typename StOrdTag>
    struct ublas_banded_ordering {};

    template<>
    struct ublas_banded_ordering<boost::numeric::ublas::row_major_tag> {
      // When orientation_category==row_major_tag then the ublas banded format corresponds to
      // the LAPACK band format, which really is a column_major format.
      typedef column_major_t                        type;

      template <typename M>
      static typename M::size_type leading_dimension( M const& m ) {
        return m.lower() + m.upper() + 1 ;
      }

      template <typename M>
      static typename M::size_type stride1( M const& m ) {
        return 1 ;
      }

      template <typename M>
      static typename M::size_type stride2( M const& m ) {
        return leading_dimension(m)-1 ;
      }
    };

    template<>
    struct ublas_banded_ordering<boost::numeric::ublas::column_major_tag> {
      // The type row_major_t is just used to indicate that this is not a column_major format.
      typedef row_major_t                        type;

      template <typename M>
      static typename M::size_type leading_dimension( M const& m ) {
        return m.size2() ;
      }

      template <typename M>
      static typename M::size_type stride1( M const& m ) {
        return leading_dimension(m) ;
      }

      template <typename M>
      static typename M::size_type stride2( M const& m ) {
        return 1-leading_dimension(m) ;
      }
    };
  }

}}}}

#endif


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "armnnOnnxParser/IOnnxParser.hpp"
#include  "ParserPrototxtFixture.hpp"

BOOST_AUTO_TEST_SUITE(OnnxParser)

struct ReshapeMainFixture : public armnnUtils::ParserPrototxtFixture<armnnOnnxParser::IOnnxParser>
{
    ReshapeMainFixture(const std::string& dataType)
    {
        m_Prototext = R"(
                   ir_version: 3
                   producer_name:  "CNTK"
                   producer_version:  "2.5.1"
                   domain:  "ai.cntk"
                   model_version: 1
                   graph {
                     name:  "CNTKGraph"
                     input {
                        name: "Input"
                        type {
                          tensor_type {
                            elem_type: )" + dataType + R"(
                            shape {
                              dim {
                                dim_value: 4
                              }
                            }
                          }
                        }
                      }
                      input {
                         name: "Shape"
                         type {
                           tensor_type {
                             elem_type: 7
                             shape {
                               dim {
                                 dim_value: 2
                               }
                             }
                           }
                         }
                       }
                     node {
                         input: "Input"
                         input: "Shape"
                         output: "Output"
                         name: "reshape"
                         op_type: "Reshape"

                      }
                      initializer {
                        dims: 2
                        data_type: 7
                        int64_data: 2
                        int64_data: 2
                        name: "Shape"
                     }
                      output {
                          name: "Output"
                          type {
                             tensor_type {
                               elem_type: 1
                               shape {
                                   dim {
                                       dim_value: 2
                                   }
                                   dim {
                                       dim_value: 2
                                   }
                               }
                            }
                          }
                       }
                    }
                   opset_import {
                      version: 7
                    })";
    }
};

struct ReshapeValidFixture : ReshapeMainFixture
{
    ReshapeValidFixture() : ReshapeMainFixture("1") {
        Setup();
    }
};

struct ReshapeInvalidFixture : ReshapeMainFixture
{
    ReshapeInvalidFixture() : ReshapeMainFixture("10") { }
};

BOOST_FIXTURE_TEST_CASE(ValidReshapeTest, ReshapeValidFixture)
{
    RunTest<2>({{"Input", { 0.0f, 1.0f, 2.0f, 3.0f }}}, {{"Output", { 0.0f, 1.0f, 2.0f, 3.0f }}});
}

BOOST_FIXTURE_TEST_CASE(IncorrectDataTypeReshape, ReshapeInvalidFixture)
{
   BOOST_CHECK_THROW(Setup(), armnn::ParseException);
}

BOOST_AUTO_TEST_SUITE_END()