// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MATRIX_UMFPACK_SOLVE_INCLUDE
#define MTL_MATRIX_UMFPACK_SOLVE_INCLUDE

#ifdef MTL_HAS_UMFPACK

#include <iostream>

#include <cassert>
#include <algorithm>
#include <boost/mpl/bool.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/make_copy_or_reference.hpp>
#include <boost/numeric/mtl/operation/merge_complex_vector.hpp>
#include <boost/numeric/mtl/operation/split_complex_vector.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

extern "C" {
#  include <umfpack.h>
}

namespace mtl { namespace matrix {

    /// Namespace for Umfpack solver
    namespace umfpack {

	// conversion for value_type needed if not double or complex<double> (where possible)
	template <typename Value> struct value         	    {};
	template<> struct value<long double>           	    { typedef double               type; };
	template<> struct value<double>                	    { typedef double               type; };
	template<> struct value<float>                 	    { typedef double               type; };
	template<> struct value<std::complex<long double> > { typedef std::complex<double> type; };
	template<> struct value<std::complex<double> > 	    { typedef std::complex<double> type; };
	template<> struct value<std::complex<float> >  	    { typedef std::complex<double> type; };

	template <typename Value> struct use_long { static const bool value= sizeof(Value) > sizeof(int); };

	template <bool Larger> struct index_aux   { typedef int     type; };
#if defined(UF_long)
	template<> struct index_aux<true>         { typedef UF_long type; };
#elif defined(SuiteSparse_long)
	template<> struct index_aux<true>         { typedef SuiteSparse_long type; };
#else
	template<> struct index_aux<true>         { typedef long type; };
#endif

	template <typename Value> struct index
          : index_aux<use_long<Value>::value> {};

	template <typename Matrix, typename Value, typename Orientation>
	struct matrix_copy {};

	// If arbitrary compressed matrix -> copy
	template <typename Value, typename Parameters, typename Orientation>
	struct matrix_copy<compressed2D<Value, Parameters>, Value, Orientation>
	{
	    typedef typename value<Value>::type                      value_type;
	    typedef compressed2D<value_type, parameters<col_major> > matrix_type;
	    typedef compressed2D<Value, Parameters>                  in_matrix_type;

	    matrix_copy(const in_matrix_type& A) : matrix(A) {}
	    matrix_type matrix;
	};

	struct error : public domain_error
	{
	    error(const char *s, int code) : domain_error(s), code(code) {}
	    int code;
	};

	inline void check(int res, const char* s)
	{
	    MTL_THROW_IF(res != UMFPACK_OK, error(s, res));
	}

	/// Class for repeated Umfpack solutions
	/** Keeps symbolic and numeric preprocessing. Numeric part can be updated.
	    Only defined for compressed2D<double> and compressed2D<complex<double> >. **/
	template <typename T>
	class solver {
	  public:
	    /// Constructor referring to matrix \p A (not changed) and optionally Umfpack's strategy and alloc_init (look for the specializations)
	    // \ref solver<compressed2D<double, Parameters> > and \ref solver<compressed2D<std::complex<double>, Parameters> >)
	    explicit solver(const T& /*A*/) {}

	    /// Update numeric part, for matrices that kept the sparsity and changed the values
	    void update_numeric() {}

	    /// Update symbolic and numeric part
	    void update() {}

	    /// Solve system A*x == b with matrix passed in constructor
	    /** Please note that the order of b and x is different than in solve() !!! **/
	    template <typename VectorX, typename VectorB>
	    int operator()(VectorX& /*x*/, const VectorB& /*b*/) const {return 0;}

	    /// Solve system A*x == b with matrix passed in constructor
	    /** Please note that the order of b and x is different than in operator() !!! **/
	    template <typename VectorB, typename VectorX>
	    int operator()(const VectorB& /*b*/, VectorX& /*x*/) const {return 0;}
	};

	/// Speciatization of solver for \ref matrix::compressed2D with double values
	template <typename Parameters>
	class solver<compressed2D<double, Parameters> >
	{
	    typedef double                                    value_type;
	    typedef compressed2D<value_type, Parameters>      matrix_type;
	    typedef typename matrix_type::size_type           size_type;
	    typedef typename index<size_type>::type           index_type;

	    static const bool copy_indices= sizeof(index_type) != sizeof(size_type),
		              long_indices= use_long<size_type>::value;
	    typedef boost::mpl::bool_<long_indices>           blong;
	    typedef boost::mpl::true_                         true_;
	    typedef boost::mpl::false_                        false_;

	    // typedef parameters<col_major>     Parameters;

	    void assign_pointers()
	    {
		if (copy_indices) {
		    if (Apc == 0) Apc= new index_type[n + 1];
		    if (my_nnz != A.nnz() && Aic) { delete[] Aic; Aic= 0; }
		    if (Aic == 0) Aic= new index_type[A.nnz()];
		    std::copy(A.address_major(), A.address_major() + n + 1, Apc);
		    std::copy(A.address_minor(), A.address_minor() + A.nnz(), Aic);
		    Ap= Apc;
		    Ai= Aic;
		} else {
		    Ap= reinterpret_cast<const index_type*>(A.address_major());
		    Ai= reinterpret_cast<const index_type*>(A.address_minor());
		}
		Ax= A.address_data();
	    }

	    void init_aux(true_)
	    {
	      check(umfpack_dl_symbolic(n, n, Ap, Ai, Ax, &Symbolic, Control, Info), "Error in dl_symbolic");
	      check(umfpack_dl_numeric(Ap, Ai, Ax, Symbolic, &Numeric, Control, Info), "Error in dl_numeric");
	    }

	    void init_aux(false_)
	    {
		check(umfpack_di_symbolic(n, n, Ap, Ai, Ax, &Symbolic, Control, Info), "Error in di_symbolic");
#if 0
		std::cout << "=== INFO of umfpack_*_symbolic ===\n";
		std::cout << "    UMFPACK_STATUS: " << (Info[UMFPACK_STATUS] == UMFPACK_OK ? "OK" : "ERROR") << "\n";
		std::cout << "    UMFPACK_NROW: " << Info[UMFPACK_NROW] << "\n";
		std::cout << "    UMFPACK_NCOL: " << Info[UMFPACK_NCOL] << "\n";
		std::cout << "    UMFPACK_NZ: " << Info[UMFPACK_NZ] << "\n";
		std::cout << "    UMFPACK_SIZE_OF_UNIT: " << Info[UMFPACK_SIZE_OF_UNIT] << "\n";
		std::cout << "    UMFPACK_NDENSE_ROW: " << Info[UMFPACK_NDENSE_ROW] << "\n";
		std::cout << "    UMFPACK_NEMPTY_ROW: " << Info[UMFPACK_NEMPTY_ROW] << "\n";
		std::cout << "    UMFPACK_NDENSE_COL: " << Info[UMFPACK_NDENSE_COL] << "\n";
		std::cout << "    UMFPACK_NEMPTY_COL: " << Info[UMFPACK_NEMPTY_COL] << "\n";
		std::cout << "    UMFPACK_SYMBOLIC_DEFRAG: " << Info[UMFPACK_SYMBOLIC_DEFRAG] << "\n";
		std::cout << "    UMFPACK_SYMBOLIC_PEAK_MEMORY: " << Info[UMFPACK_SYMBOLIC_PEAK_MEMORY] << "\n";
		std::cout << "    UMFPACK_SYMBOLIC_SIZE: " << Info[UMFPACK_SYMBOLIC_SIZE] << "\n";
		std::cout << "    UMFPACK_VARIABLE_PEAK_ESTIMATE: " << Info[UMFPACK_VARIABLE_PEAK_ESTIMATE]  << "\n";
		std::cout << "    UMFPACK_NUMERIC_SIZE_ESTIMATE: " << Info[UMFPACK_NUMERIC_SIZE_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_PEAK_MEMORY_ESTIMATE: " << Info[UMFPACK_PEAK_MEMORY_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_FLOPS_ESTIMATE: " << Info[UMFPACK_FLOPS_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_LNZ_ESTIMATE: " << Info[UMFPACK_LNZ_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_UNZ_ESTIMATE: " << Info[UMFPACK_UNZ_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_MAX_FRONT_SIZE_ESTIMATE: " << Info[UMFPACK_MAX_FRONT_SIZE_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_SYMBOLIC_TIME: " << Info[UMFPACK_SYMBOLIC_TIME] << "\n";
		std::cout << "    UMFPACK_SYMBOLIC_WALLTIME: " << Info[UMFPACK_SYMBOLIC_WALLTIME] << "\n";

		if (Info[UMFPACK_STRATEGY_USED] == UMFPACK_STRATEGY_SYMMETRIC)
		  std::cout << "    UMFPACK_STRATEGY_USED: SYMMETRIC\n";
		else {
		  if(Info[UMFPACK_STRATEGY_USED] == UMFPACK_STRATEGY_UNSYMMETRIC)
		    std::cout << "    UMFPACK_STRATEGY_USED: UNSYMMETRIC\n";
		  else {
		    if (Info[UMFPACK_STRATEGY_USED] == UMFPACK_STRATEGY_2BY2)
		      std::cout << "    UMFPACK_STRATEGY_USED: 2BY2\n";
		    else
		      std::cout << "    UMFPACK_STRATEGY_USED: UNKOWN STRATEGY " << Info[UMFPACK_STRATEGY_USED] << "\n";
		  }
		}

		std::cout << "    UMFPACK_ORDERING_USED: " << Info[UMFPACK_ORDERING_USED] << "\n";
		std::cout << "    UMFPACK_QFIXED: " << Info[UMFPACK_QFIXED] << "\n";
		std::cout << "    UMFPACK_DIAG_PREFERRED: " << Info[UMFPACK_DIAG_PREFERRED] << "\n";
		std::cout << "    UMFPACK_ROW_SINGLETONS: " << Info[UMFPACK_ROW_SINGLETONS] << "\n";
		std::cout << "    UMFPACK_COL_SINGLETONS: " << Info[UMFPACK_COL_SINGLETONS] << "\n";
		std::cout << "    UMFPACK_PATTERN_SYMMETRY: " << Info[UMFPACK_PATTERN_SYMMETRY] << "\n";
		std::cout << "    UMFPACK_NZ_A_PLUS_AT: " << Info[UMFPACK_NZ_A_PLUS_AT] << "\n";
		std::cout << "    UMFPACK_NZDIAG: " << Info[UMFPACK_NZDIAG] << "\n";
		std::cout << "    UMFPACK_N2: " << Info[UMFPACK_N2] << "\n";
		std::cout << "    UMFPACK_S_SYMMETRIC: " << Info[UMFPACK_S_SYMMETRIC] << "\n";
		std::cout << "    UMFPACK_MAX_FRONT_NROWS_ESTIMATE: " << Info[UMFPACK_MAX_FRONT_NROWS_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_MAX_FRONT_NCOLS_ESTIMATE: " << Info[UMFPACK_MAX_FRONT_NCOLS_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_SYMMETRIC_LUNZ: " << Info[UMFPACK_SYMMETRIC_LUNZ] << "\n";
		std::cout << "    UMFPACK_SYMMETRIC_FLOPS: " << Info[UMFPACK_SYMMETRIC_FLOPS] << "\n";
		std::cout << "    UMFPACK_SYMMETRIC_NDENSE: " << Info[UMFPACK_SYMMETRIC_NDENSE] << "\n";
		std::cout << "    UMFPACK_SYMMETRIC_DMAX: " << Info[UMFPACK_SYMMETRIC_DMAX] << "\n";
#endif

		check(umfpack_di_numeric(Ap, Ai, Ax, Symbolic, &Numeric, Control, Info), "Error in di_numeric");

#if 0
		std::cout << "=== INFO of umfpack_*_numeric ===\n";
		std::cout << "    UMFPACK_STATUS: " << (Info[UMFPACK_STATUS] == UMFPACK_OK ? "OK" : "ERROR") << "\n";
		std::cout << "    UMFPACK_VARIABLE_PEAK: " << Info[UMFPACK_VARIABLE_PEAK]  << "\n";
		std::cout << "    UMFPACK_PEAK_MEMORY: " << Info[UMFPACK_PEAK_MEMORY] << "\n";
		std::cout << "    UMFPACK_FLOPS: " << Info[UMFPACK_FLOPS] << "\n";
		std::cout << "    UMFPACK_LNZ: " << Info[UMFPACK_LNZ] << "\n";
		std::cout << "    UMFPACK_UNZ: " << Info[UMFPACK_UNZ] << "\n";
		std::cout << "    UMFPACK_NUMERIC_DEFRAG: " << Info[UMFPACK_NUMERIC_DEFRAG] << "\n";
		std::cout << "    UMFPACK_NUMERIC_REALLOC: " << Info[UMFPACK_NUMERIC_REALLOC] << "\n";
		std::cout << "    UMFPACK_NUMERIC_COSTLY_REALLOC: " << Info[UMFPACK_NUMERIC_COSTLY_REALLOC] << "\n";
		std::cout << "    UMFPACK_COMPRESSED_PATTERN: " << Info[UMFPACK_COMPRESSED_PATTERN] << "\n";
		std::cout << "    UMFPACK_LU_ENTRIES: " << Info[UMFPACK_LU_ENTRIES] << "\n";
		std::cout << "    UMFPACK_NUMERIC_TIME: " << Info[UMFPACK_NUMERIC_TIME] << "\n";
		std::cout << "    UMFPACK_RCOND: " << Info[UMFPACK_RCOND] << "\n";
		std::cout << "    UMFPACK_UDIAG_NZ: " << Info[UMFPACK_UDIAG_NZ] << "\n";
		std::cout << "    UMFPACK_UMIN: " << Info[UMFPACK_UMIN] << "\n";
		std::cout << "    UMFPACK_UMAX: " << Info[UMFPACK_UMAX] << "\n";
		std::cout << "    UMFPACK_MAX_FRONT_NROWS: " << Info[UMFPACK_MAX_FRONT_NROWS] << "\n";
		std::cout << "    UMFPACK_MAX_FRONT_NCOLS: " << Info[UMFPACK_MAX_FRONT_NCOLS] << "\n";
		std::cout << "    UMFPACK_ALL_LNZ: " << Info[UMFPACK_ALL_LNZ] << "\n";
		std::cout << "    UMFPACK_ALL_UNZ: " << Info[UMFPACK_ALL_UNZ] << "\n";
#endif
	    }

	    void init()
	    {
		MTL_THROW_IF(num_rows(A) != num_cols(A), matrix_not_square());
		n= num_rows(A);
		assign_pointers();
		init_aux(blong());
	    }

	  public:
	    /// Constructor referring to matrix \p A (not changed) and optionally Umfpack's strategy and alloc_init
	    solver(const matrix_type& A, int strategy = UMFPACK_STRATEGY_AUTO, double alloc_init = 0.7)
	      : A(A), Apc(0), Aic(0), my_nnz(0), Symbolic(0), Numeric(0)
	    {
		vampir_trace<5060> trace;
		// Use default setings.
		if (long_indices)
		    umfpack_dl_defaults(Control);
		else
		    umfpack_di_defaults(Control);

		Control[UMFPACK_STRATEGY] = strategy;
		Control[UMFPACK_ALLOC_INIT] = alloc_init;
		init();
	    }

	    ~solver()
	    {
		vampir_trace<5061> trace;
		if (long_indices) {
		    umfpack_dl_free_numeric(&Numeric);
		    umfpack_dl_free_symbolic(&Symbolic);
		} else {
		    umfpack_di_free_numeric(&Numeric);
		    umfpack_di_free_symbolic(&Symbolic);
		}
		if (Apc) delete[] Apc;
		if (Aic) delete[] Aic;
	    }

	    void update_numeric_aux(true_)
	    {
		umfpack_dl_free_numeric(&Numeric);
		check(umfpack_dl_numeric(Ap, Ai, Ax, Symbolic, &Numeric, Control, Info), "Error in dl_numeric");
	    }

	    void update_numeric_aux(false_)
	    {
		umfpack_di_free_numeric(&Numeric);
		check(umfpack_di_numeric(Ap, Ai, Ax, Symbolic, &Numeric, Control, Info), "Error in di_numeric");
	    }

	    /// Update numeric part, for matrices that kept the sparsity and changed the values
	    void update_numeric()
	    {
		assign_pointers();
		update_numeric_aux(blong());
	    }

	    /// Update symbolic and numeric part
	    void update()
	    {
		if (long_indices) {
		    umfpack_dl_free_numeric(&Numeric);
		    umfpack_dl_free_symbolic(&Symbolic);
		} else {
		    umfpack_di_free_numeric(&Numeric);
		    umfpack_di_free_symbolic(&Symbolic);
		}
		init();
	    }

	    template <typename VectorX, typename VectorB>
	    void solve_aux(int sys, VectorX& xx, const VectorB& bb, true_)
	    {
	      check(umfpack_dl_solve(sys, Ap, Ai, Ax, &xx.value[0], &bb.value[0], Numeric, Control, Info), "Error in dl_solve");
	    }

	    template <typename VectorX, typename VectorB>
	    void solve_aux(int sys, VectorX& xx, const VectorB& bb, false_)
	    {
		check(umfpack_di_solve(sys, Ap, Ai, Ax, &xx.value[0], &bb.value[0], Numeric, Control, Info), "Error in di_solve");
	    }

	    /// Solve double system
	    template <typename VectorX, typename VectorB>
	    int operator()(VectorX& x, const VectorB& b)
	    {
		vampir_trace<5062> trace;
		MTL_THROW_IF(num_rows(A) != size(x) || num_rows(A) != size(b), incompatible_size());
		make_in_out_copy_or_reference<dense_vector<value_type>, VectorX> xx(x);
		make_in_copy_or_reference<dense_vector<value_type>, VectorB>     bb(b);
		int sys= mtl::traits::is_row_major<Parameters>::value ? UMFPACK_At : UMFPACK_A;
		solve_aux(sys, xx, bb, blong());
		return UMFPACK_OK;
	    }

	    /// Solve double system
	    template <typename VectorB, typename VectorX>
	    int solve(const VectorB& b, VectorX& x) const
	    {
		// return (*this)(x, b);
		return const_cast<solver&>(*this)(x, b); // evil hack because Umfpack has no const
	    }

	  private:
	    const matrix_type&  A;
	    int                 n;
	    const index_type    *Ap, *Ai;
	    index_type          *Apc, *Aic;
	    size_type           my_nnz;
	    const double        *Ax;
	    double              Control[UMFPACK_CONTROL], Info[UMFPACK_INFO];
	    void                *Symbolic, *Numeric;
	};

	/// Speciatization of solver for \ref matrix::compressed2D with double values
	template <typename Parameters>
	class solver<compressed2D<std::complex<double>, Parameters> >
	{
	    typedef std::complex<double>                      value_type;
	    typedef compressed2D<value_type, Parameters>      matrix_type;
	    typedef typename matrix_type::size_type           size_type;
	    typedef typename index<size_type>::type           index_type;

	    static const bool copy_indices= sizeof(index_type) != sizeof(size_type),
		              long_indices= use_long<size_type>::value;

	    typedef boost::mpl::bool_<long_indices>           blong;
	    typedef boost::mpl::true_                         true_;
	    typedef boost::mpl::false_                        false_;

	    void assign_pointers()
	    {
		if (copy_indices) {
		    if (Apc == 0) Apc= new index_type[n + 1];
		    if (Aic == 0) Aic= new index_type[A.nnz()];
		    std::copy(A.address_major(), A.address_major() + n + 1, Apc);
		    std::copy(A.address_minor(), A.address_minor() + A.nnz(), Aic);
		    Ap= Apc;
		    Ai= Aic;
		} else {
		    Ap= reinterpret_cast<const index_type*>(A.address_major());
		    Ai= reinterpret_cast<const index_type*>(A.address_minor());
		}
		split_complex_vector(A.data, Ax, Az);
	    }

	    void init_aux(true_)
	    {
		check(umfpack_zl_symbolic(n, n, Ap, Ai, &Ax[0], &Az[0], &Symbolic, Control, Info), "Error in zl_symbolic");
		check(umfpack_zl_numeric(Ap, Ai, &Ax[0], &Az[0], Symbolic, &Numeric, Control, Info), "Error in zl_numeric");
	    }

	    void init_aux(false_)
	    {
		check(umfpack_zi_symbolic(n, n, Ap, Ai, &Ax[0], &Az[0], &Symbolic, Control, Info), "Error in zi_symbolic");
		check(umfpack_zi_numeric(Ap, Ai, &Ax[0], &Az[0], Symbolic, &Numeric, Control, Info), "Error in zi_numeric");
	    }

	    void initialize()
	    {
		MTL_THROW_IF(num_rows(A) != num_cols(A), matrix_not_square());
		n= num_rows(A);
		assign_pointers();
		init_aux(blong());
	    }
	public:
	    /// Constructor referring to matrix \p A (not changed) and optionally Umfpack's strategy and alloc_init (look for the specializations)
	    explicit solver(const compressed2D<value_type, Parameters>& A, int strategy = UMFPACK_STRATEGY_AUTO, double alloc_init = 0.7)
	      : A(A), Apc(0), Aic(0)
	    {
		vampir_trace<5060> trace;
		// Use default setings.
		if (long_indices)
		    umfpack_zl_defaults(Control);
		else
		    umfpack_zi_defaults(Control);
		// umfpack_zi_defaults(Control);

		Control[UMFPACK_STRATEGY] = strategy;
		Control[UMFPACK_ALLOC_INIT] = alloc_init;
		initialize();
	    }

	    ~solver()
	    {
		vampir_trace<5061> trace;
		if (long_indices) {
		    umfpack_zl_free_numeric(&Numeric);
		    umfpack_zl_free_symbolic(&Symbolic);
		} else {
		    umfpack_zi_free_numeric(&Numeric);
		    umfpack_zi_free_symbolic(&Symbolic);
		}
		if (Apc) delete[] Apc;
		if (Aic) delete[] Aic;
	    }

	    void update_numeric_aux(true_)
	    {
		umfpack_zl_free_numeric(&Numeric);
		check(umfpack_zl_numeric(Ap, Ai, &Ax[0], &Az[0], Symbolic, &Numeric, Control, Info), "Error in dl_numeric D");
	    }

	    void update_numeric_aux(false_)
	    {
		umfpack_zi_free_numeric(&Numeric);
		check(umfpack_zi_numeric(Ap, Ai, &Ax[0], &Az[0], Symbolic, &Numeric, Control, Info), "Error in di_numeric");
	    }

	    /// Update numeric part, for matrices that kept the sparsity and changed the values
	    void update_numeric()
	    {
		assign_pointers();
		update_numeric_aux(blong());
	    }

	    /// Update symbolic and numeric part
	    void update()
	    {
		Ax.change_dim(0); Az.change_dim(0);
		if (long_indices) {
		    umfpack_zl_free_numeric(&Numeric);
		    umfpack_zl_free_symbolic(&Symbolic);
		} else {
		    umfpack_zi_free_numeric(&Numeric);
		    umfpack_zi_free_symbolic(&Symbolic);
		}
		initialize();
	    }

	    template <typename VectorX, typename VectorB>
	    void solve_aux(int sys, VectorX& Xx, VectorX& Xz, const VectorB& Bx, const VectorB& Bz, true_)
	    {
		check(umfpack_zl_solve(sys, Ap, Ai, &Ax[0], &Az[0], &Xx[0], &Xz[0], &Bx[0], &Bz[0], Numeric, Control, Info),
		      "Error in zi_solve");
	    }

	    template <typename VectorX, typename VectorB>
	    void solve_aux(int sys, VectorX& Xx, VectorX& Xz, const VectorB& Bx, const VectorB& Bz, false_)
	    {
		check(umfpack_zi_solve(sys, Ap, Ai, &Ax[0], &Az[0], &Xx[0], &Xz[0], &Bx[0], &Bz[0], Numeric, Control, Info),
		      "Error in zi_solve");
	    }

	    /// Solve complex system
	    template <typename VectorX, typename VectorB>
	    int operator()(VectorX& x, const VectorB& b)
	    {
		vampir_trace<5062> trace;
		MTL_THROW_IF(num_rows(A) != size(x) || num_rows(A) != size(b), incompatible_size());
		dense_vector<double> Xx(size(x)), Xz(size(x)), Bx, Bz;
		split_complex_vector(b, Bx, Bz);
		int sys= mtl::traits::is_row_major<Parameters>::value ? UMFPACK_Aat : UMFPACK_A;
		solve_aux(sys, Xx, Xz, Bx, Bz, blong());
		merge_complex_vector(Xx, Xz, x);
		return UMFPACK_OK;
	    }

	    /// Solve complex system
	    template <typename VectorB, typename VectorX>
	    int solve(const VectorB& b, VectorX& x)
	    {
		return (*this)(x, b);
	    }

	private:
	    const matrix_type&   A;
	    int                  n;
	    const index_type     *Ap, *Ai;
	    index_type          *Apc, *Aic;
	    dense_vector<double> Ax, Az;
	    double               Control[UMFPACK_CONTROL], Info[UMFPACK_INFO];
	    void                 *Symbolic, *Numeric;
	};

	template <typename Value, typename Parameters>
	class solver<compressed2D<Value, Parameters> >
	  : matrix_copy<compressed2D<Value, Parameters>, Value, typename Parameters::orientation>,
	    public solver<typename matrix_copy<compressed2D<Value, Parameters>, Value, typename Parameters::orientation>::matrix_type >
	{
	    typedef matrix_copy<compressed2D<Value, Parameters>, Value, typename Parameters::orientation> copy_type;
	    typedef solver<typename matrix_copy<compressed2D<Value, Parameters>, Value, typename Parameters::orientation>::matrix_type > solver_type;
	public:
	    explicit solver(const compressed2D<Value, Parameters>& A)
		: copy_type(A), solver_type(copy_type::matrix), A(A)
	    {}

	    void update()
	    {
		copy_type::matrix= A;
		solver_type::update();
	    }

	    void update_numeric()
	    {
		copy_type::matrix= A;
		solver_type::update_numeric();
	    }
	private:
	    const compressed2D<Value, Parameters>& A;
	};
    } // umfpack

/// Solve A*x == b with umfpack
/** Only available when compiled with enabled macro MTL_HAS_UMFPACK.
    Uses classes umfpack::solver internally.
    If you want more control on single operations or to keep umfpack's
    internal factorization, use this class.
 **/
template <typename Value, typename Parameters, typename VectorX, typename VectorB>
int umfpack_solve(const compressed2D<Value, Parameters>& A, VectorX& x, const VectorB& b)
{
    umfpack::solver<compressed2D<Value, Parameters> > solver(A);
    return solver(x, b);
}

}} // namespace mtl::matrix

#endif

#endif // MTL_MATRIX_UMFPACK_SOLVE_INCLUDE


#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <mmtbx/rotamer/fit.h>

#include <boost/python.hpp>

#include <cctbx/boost_python/flex_fwd.h>
#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
//#include <boost/python/args.hpp>
#include <scitbx/array_family/boost_python/shared_wrapper.h>
#include <scitbx/boost_python/is_polymorphic_workaround.h>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>

namespace mmtbx { namespace rotamer {
namespace {

  boost::python::tuple
  getinitargs(xyzrad<> const& self)
  {
    return boost::python::make_tuple(self.sites_cart, self.radii);
  }

  void init_module()
  {
    using namespace boost::python;

    //
    typedef return_value_policy<return_by_value> rbv;
    class_<xyzrad<> >("xyzrad")
      .def(init<af::shared<scitbx::vec3<double> > const&,
                af::shared<double> const&,
                af::shared<double> const& >((arg("sites_cart"),
                                             arg("radii"),
                                             arg("weights"))))
      .add_property("sites_cart", make_getter(&xyzrad<>::sites_cart, rbv()))
      .add_property("radii",      make_getter(&xyzrad<>::radii, rbv()))
      .add_property("weights",    make_getter(&xyzrad<>::weights, rbv()))
      .enable_pickling()
      .def("__getinitargs__", getinitargs)

      .def(init<af::shared<scitbx::vec3<double> > const&,
                af::shared<double> const& >((arg("sites_cart"),
                                             arg("radii"))))
      .add_property("sites_cart", make_getter(&xyzrad<>::sites_cart, rbv()))
      .add_property("radii",    make_getter(&xyzrad<>::weights, rbv()))
      .enable_pickling()
      .def("__getinitargs__", getinitargs)
    ;
    //

    class_<fit<> >("fit")

     .def(init<       double,
                      xyzrad<double> const&,
                      //af::shared<scitbx::vec3<double> >,
                      boost::python::list const&,
                      boost::python::list const&,
                      boost::python::list const&,
                      af::const_ref<double, af::c_grid_padded<3> > const&,
                      //af::shared<scitbx::vec3<double> >,
                      xyzrad<double> const&,

                      cctbx::uctbx::unit_cell const&,
                      af::const_ref<std::size_t> const& ,
                      af::const_ref<std::size_t> const& ,
                      af::const_ref<double> const&,
                      af::const_ref<double> const&,
                      double,
                      int >
                       ((arg("target_value"),
                         arg("xyzrad_bumpers"),
                         //arg("sites_cart_bumpers"),
                         arg("axes"),
                         arg("rotatable_points_indices"),
                         arg("angles_array"),
                         arg("density_map"),
                         arg("all_points"),
                         arg("unit_cell"),
                         arg("selection_clash"),
                         arg("selection_rsr"),
                         arg("sin_table"),
                         arg("cos_table"),
                         arg("step"),
                         arg("n"))))

     .def(init<double,
                      boost::python::list const&,
                      boost::python::list const&,
                      boost::python::list const&,
                      af::const_ref<double, af::c_grid_padded<3> > const&,
                      af::shared<scitbx::vec3<double> >,
                      cctbx::uctbx::unit_cell const&,
                      af::const_ref<std::size_t> const& ,
                      af::const_ref<double> const&,
                      af::const_ref<double> const&,
                      double,
                      int >
                       ((arg("target_value"),
                         arg("axes"),
                         arg("rotatable_points_indices"),
                         arg("angles_array"),
                         arg("density_map"),
                         arg("all_points"),
                         arg("unit_cell"),
                         arg("selection"),
                         arg("sin_table"),
                         arg("cos_table"),
                         arg("step"),
                         arg("n"))))

     .def(init<af::shared<scitbx::vec3<double> > const&,
                      boost::python::list const&,
                      boost::python::list const&,
                      boost::python::list const&,
                      af::shared<scitbx::vec3<double> >,
                      af::const_ref<double> const&,
                      af::const_ref<double> const&,
                      double,
                      int >
                       ((arg("sites_cart_start"),
                         arg("axes"),
                         arg("rotatable_points_indices"),
                         arg("angles_array"),
                         arg("all_points"),
                         arg("sin_table"),
                         arg("cos_table"),
                         arg("step"),
                         arg("n"))))

      .def("result", &fit<>::result)
      .def("score", &fit<>::score)
    ;

  }

} // namespace <anonymous>
}} // namespace mmtbx::rotamer

BOOST_PYTHON_MODULE(mmtbx_rotamer_fit_ext)
{
  mmtbx::rotamer::init_module();
}


/// @file
///
/// XXX Notes on program XXX
///
/// @copyright (c) 2010 CSIRO
/// Australia Telescope National Facility (ATNF)
/// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
/// PO Box 76, Epping NSW 1710, Australia
/// atnf-enquiries@csiro.au
///
/// This file is part of the ASKAP software distribution.
///
/// The ASKAP software distribution 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
///
/// @author XXX XXX <XXX.XXX@csiro.au>
///

#include <askap_analysis.h>

#include <askap/AskapLogging.h>
#include <askap/AskapError.h>

#include <sourcefitting/SubThresholder.h>
#include <sourcefitting/RadioSource.h>
#include <sourcefitting/SubComponent.h>

#include <duchamp/Cubes/cubes.hh>
#include <casacore/casa/Arrays/Slicer.h>
#include <casacore/casa/Arrays/Vector.h>
#include <casacore/casa/Arrays/Matrix.h>

#include <boost/scoped_ptr.hpp>

#include <math.h>
#include <vector>

///@brief Where the log messages go.
ASKAP_LOGGER(logger, ".subthresholder");

namespace askap {

namespace analysis {

namespace sourcefitting {

SubThresholder::~SubThresholder()
{
}

// SubThresholder::SubThresholder(const SubThresholder &s) {
//     operator=(s);
// }

// SubThresholder& SubThresholder::operator=(const SubThresholder &s) {
//     if(this == &s) return *this;
//     this->itsFirstGuess = s.itsFirstGuess;
//     this->itsSourceBox = s.itsSourceBox;
//     this->itsBaseThreshold = s.itsBaseThreshold;
//     this->itsThreshIncrement = s.itsThreshIncrement;
//     this->itsPeakFlux = s.itsPeakFlux;
//     this->itsSourceSize = s.itsSourceSize;
//     this->itsDim = s.itsDim;
//     this->itsFluxArray = s.itsFluxArray;
//     this->itsCurrentThreshold = s.itsCurrentThreshold;
//     this->itsFitParams = s.itsFitParams;
//     return *this;

// }

void SubThresholder::define(RadioSource &src,
                            casa::Matrix<casa::Double> pos,
                            casa::Vector<casa::Double> &array)
{
    this->saveArray(src, pos, array);
    this->define(src);
}

void SubThresholder::saveArray(RadioSource &src,
                               casa::Matrix<casa::Double> pos,
                               casa::Vector<casa::Double> &f)
{
    int xmin = src.boxXmin();
    int ymin = src.boxYmin();
    int xsize = src.boxXsize();
    int ysize = src.boxYsize();
    size_t size = xsize * ysize;
    itsFluxArray = std::vector<float>(size, 0.);
    PixelInfo::Object2D spatMap = src.getSpatialMap();
    for (size_t i = 0; i < f.size(); i++) {
        int x = int(pos(i, 0));
        int y = int(pos(i, 1));

        if (spatMap.isInObject(x, y)) {
            int loc = (x - xmin) + xsize * (y - ymin);
            itsFluxArray[loc] = float(f(i));
        }
    }

}

void SubThresholder::define(RadioSource &src)
{

    itsPeakFlux = src.getPeakFlux();
    itsSourceSize = src.getSize();

    itsDim = std::vector<size_t>(2);
    itsDim[0] = src.boxXsize();
    itsDim[1] = src.boxYsize();

    this->setFirstGuess(src);
    itsFitParams = src.fitparams();
    itsSourceBox = src.box();

    if (itsFitParams.flagLogarithmicIncrements()) {
        itsBaseThreshold = src.detectionThreshold() > 0 ?
                           log10(src.detectionThreshold()) : -6.;
        itsThreshIncrement = (log10(itsPeakFlux) - itsBaseThreshold) /
                             float(itsFitParams.numSubThresholds() + 1);
        itsCurrentThreshold = pow(10., itsBaseThreshold + itsThreshIncrement);
    } else {
        itsBaseThreshold = src.detectionThreshold();
        itsThreshIncrement = (itsPeakFlux - itsBaseThreshold) /
                             float(itsFitParams.numSubThresholds() + 1);
        itsCurrentThreshold = itsBaseThreshold + itsThreshIncrement;
    }

}

void SubThresholder::setFirstGuess(RadioSource &src)
{

    itsFirstGuess.setPeak(src.getPeakFlux());
    itsFirstGuess.setX(src.getXPeak());
    itsFirstGuess.setY(src.getYPeak());
    double a, b, c;

    if (src.getSize() < 3) {
        itsFirstGuess.setPA(0);
        itsFirstGuess.setMajor(1.);
        itsFirstGuess.setMinor(1.);
    } else {
        src.getFWHMestimate(itsFluxArray, a, b, c);
        itsFirstGuess.setPA(a);
        itsFirstGuess.setMajor(b);
        itsFirstGuess.setMinor(c);
    }

}

void SubThresholder::keepObject(PixelInfo::Object2D &obj)
{

    for (size_t i = 0; i < itsDim[0]*itsDim[1]; i++) {
        int xbox = i % itsDim[0];
        int ybox = i / itsDim[0];

        if (!obj.isInObject(xbox, ybox)) {
            itsFluxArray[i] = 0.;
        }
    }

}

void SubThresholder::incrementThreshold()
{

    if (itsFitParams.flagLogarithmicIncrements()) {
        itsCurrentThreshold *= pow(10., itsThreshIncrement);
    } else {
        itsCurrentThreshold += itsThreshIncrement;
    }

}

std::vector<SubComponent> SubThresholder::find()
{

    std::vector<SubComponent> fullList;

    if (itsSourceSize < 3) {
        fullList.push_back(itsFirstGuess);
        return fullList;
    }

    std::vector<PixelInfo::Object2D> objlist;
    std::vector<PixelInfo::Object2D>::iterator obj;
    bool keepGoing = true;

    boost::scoped_ptr<duchamp::Image> theImage(new duchamp::Image(itsDim.data()));

    if (itsFluxArray.size() > 0) {
        ASKAPCHECK(itsFluxArray.size() == (itsDim[0]*itsDim[1]),
                   "Size of flux array (" << itsFluxArray.size() <<
                   ") doesn't match dimension (" << itsDim[0] <<
                   "x" << itsDim[1] << "=" << itsDim[0]*itsDim[1] << ")!");
        theImage->saveArray(&(itsFluxArray[0]), itsFluxArray.size());
    }
    theImage->setMinSize(1);
    theImage->pars().setFlagUserThreshold(true);

    while (itsCurrentThreshold <= itsPeakFlux && keepGoing) {
        theImage->stats().setThreshold(itsCurrentThreshold);
        theImage->pars().setThreshold(itsCurrentThreshold);
        objlist = theImage->findSources2D();
        keepGoing = (objlist.size() == 1);
        this->incrementThreshold();
    }

    if (!keepGoing) {

        if (objlist.size() == 0) {
            fullList.push_back(itsFirstGuess);
        } else {

            for (obj = objlist.begin(); obj < objlist.end(); obj++) {

                RadioSource src;
                src.addChannel(0, *obj);
                src.setFitParams(itsFitParams);
                src.setDetectionThreshold(itsCurrentThreshold);
                src.setBox(itsSourceBox);
                src.calcFluxes(&(itsFluxArray[0]), &(itsDim[0]));
                duchamp::Param par;
                par.setXOffset(itsSourceBox.start()[0]);
                par.setYOffset(itsSourceBox.start()[1]);
                src.setOffsets(par);
                src.addOffsets();
                SubThresholder newthresher(*this);
                newthresher.setFirstGuess(src);
                newthresher.keepObject(*obj);
                std::vector<SubComponent> newlist = newthresher.find();
                for (uInt i = 0; i < newlist.size(); i++) {
                    fullList.push_back(newlist[i]);
                }
            }
        }
    } else {
        fullList.push_back(itsFirstGuess);
    }

    if (fullList.size() > 1) {
        std::sort(fullList.begin(), fullList.end());
        std::reverse(fullList.begin(), fullList.end());
    }

    return fullList;

}

}
}

}


/**
 * Copyright (c) 2011-2017 libbitcoin developers (see AUTHORS)
 *
 * This file is part of libbitcoin.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 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 Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <bitcoin/bitcoin/wallet/ec_private.hpp>

#include <cstdint>
#include <iostream>
#include <string>
#include <boost/program_options.hpp>
#include <bitcoin/bitcoin/formats/base_58.hpp>
#include <bitcoin/bitcoin/math/checksum.hpp>
#include <bitcoin/bitcoin/math/elliptic_curve.hpp>
#include <bitcoin/bitcoin/math/hash.hpp>
#include <bitcoin/bitcoin/utility/data.hpp>
#include <bitcoin/bitcoin/wallet/ec_public.hpp>
#include <bitcoin/bitcoin/wallet/hd_private.hpp>
#include <bitcoin/bitcoin/wallet/payment_address.hpp>

namespace libbitcoin {
namespace wallet {

const uint8_t ec_private::compressed_sentinel = 0x01;

const uint8_t ec_private::mainnet_wif = 0x80;
const uint8_t ec_private::mainnet_p2kh = 0x00;
const uint16_t ec_private::mainnet = to_version(mainnet_p2kh, mainnet_wif);

const uint8_t ec_private::testnet_wif = 0xef;
const uint8_t ec_private::testnet_p2kh = 0x6f;
const uint16_t ec_private::testnet = to_version(testnet_p2kh, testnet_wif);

ec_private::ec_private()
  : valid_(false), compress_(true), version_(0), secret_(null_hash)
{
}

ec_private::ec_private(const ec_private& other)
  : valid_(other.valid_), compress_(other.compress_), version_(other.version_),
    secret_(other.secret_)
{
}

ec_private::ec_private(const data_chunk& seed, uint8_t address_version)
  : ec_private(from_seed(seed, address_version))
{
}

ec_private::ec_private(const std::string& wif, uint8_t address_version)
  : ec_private(from_string(wif, address_version))
{
}

ec_private::ec_private(const wif_compressed& wif, uint8_t address_version)
  : ec_private(from_compressed(wif, address_version))
{
}

ec_private::ec_private(const wif_uncompressed& wif, uint8_t address_version)
  : ec_private(from_uncompressed(wif, address_version))
{
}

ec_private::ec_private(const ec_secret& secret, uint16_t version, bool compress)
  : valid_(true), compress_(compress), version_(version), secret_(secret)
{
}

// Validators.
// ----------------------------------------------------------------------------

bool ec_private::is_wif(data_slice decoded)
{
    const auto size = decoded.size();
    if (size != wif_compressed_size && size != wif_uncompressed_size)
        return false;

    if (!verify_checksum(decoded))
        return false;

    return (size == wif_uncompressed_size) ||
        decoded.data()[1 + ec_secret_size] == compressed_sentinel;
}

// Factories.
// ----------------------------------------------------------------------------

ec_private ec_private::from_seed(const data_chunk& seed,
    uint8_t address_version)
{
    // This technique ensures consistent secrets with BIP32 from a given seed.
    const hd_private key(seed);

    // The key is invalid if parse256(IL) >= n or 0:
    return key ? ec_private{ key.secret(), address_version } : ec_private{};
}

ec_private ec_private::from_string(const std::string& wif,
    uint8_t address_version)
{
    data_chunk decoded;
    if (!decode_base58(decoded, wif) || !is_wif(decoded))
        return ec_private();

    const auto compressed = decoded.size() == wif_compressed_size;
    return compressed ?
        ec_private(to_array<wif_compressed_size>(decoded), address_version) :
        ec_private(to_array<wif_uncompressed_size>(decoded), address_version);
}

ec_private ec_private::from_compressed(const wif_compressed& wif,
    uint8_t address_version)
{
    if (!is_wif(wif))
        return ec_private();

    const uint16_t version = to_version(address_version, wif.front());
    const auto secret = slice<1, ec_secret_size + 1>(wif);
    return ec_private(secret, version, true);
}

ec_private ec_private::from_uncompressed(const wif_uncompressed& wif,
    uint8_t address_version)
{
    if (!is_wif(wif))
        return ec_private();

    const uint16_t version = to_version(address_version, wif.front());
    const auto secret = slice<1, ec_secret_size + 1>(wif);
    return ec_private(secret, version, false);
}

// Cast operators.
// ----------------------------------------------------------------------------

ec_private::operator const bool() const
{
    return valid_;
}

ec_private::operator const ec_secret&() const
{
    return secret_;
}

// Serializer.
// ----------------------------------------------------------------------------

// Conversion to WIF loses payment address version info.
std::string ec_private::encoded() const
{
    if (compressed())
    {
        wif_compressed wif;
        const auto prefix = to_array(wif_version());
        const auto compressed = to_array(compressed_sentinel);
        build_checked_array(wif, { prefix, secret_, compressed });
        return encode_base58(wif);
    }

    wif_uncompressed wif;
    const auto prefix = to_array(wif_version());
    build_checked_array(wif, { prefix, secret_ });
    return encode_base58(wif);
}

// Accessors.
// ----------------------------------------------------------------------------

const ec_secret& ec_private::secret() const
{
    return secret_;
}

const uint16_t ec_private::version() const
{
    return version_;
}

const uint8_t ec_private::payment_version() const
{
    return to_address_prefix(version_);
}

const uint8_t ec_private::wif_version() const
{
    return to_wif_prefix(version_);
}

const bool ec_private::compressed() const
{
    return compress_;
}

// Methods.
// ----------------------------------------------------------------------------

// Conversion to ec_public loses all version information.
// In the case of failure the key is always compressed (ec_compressed_null).
ec_public ec_private::to_public() const
{
    ec_compressed point;
    return valid_ && secret_to_public(point, secret_) ?
        ec_public(point, compressed()) : ec_public();
}

payment_address ec_private::to_payment_address() const
{
    return payment_address(*this);
}

// Operators.
// ----------------------------------------------------------------------------

ec_private& ec_private::operator=(const ec_private& other)
{
    valid_ = other.valid_;
    compress_ = other.compress_;
    version_ = other.version_;
    secret_ = other.secret_;
    return *this;
}

bool ec_private::operator<(const ec_private& other) const
{
    return encoded() < other.encoded();
}

bool ec_private::operator==(const ec_private& other) const
{
    return valid_ == other.valid_ && compress_ == other.compress_ &&
        version_ == other.version_ && secret_ == other.secret_;
}

bool ec_private::operator!=(const ec_private& other) const
{
    return !(*this == other);
}

std::istream& operator>>(std::istream& in, ec_private& to)
{
    std::string value;
    in >> value;
    to = ec_private(value);

    if (!to)
    {
        using namespace boost::program_options;
        BOOST_THROW_EXCEPTION(invalid_option_value(value));
    }

    return in;
}

std::ostream& operator<<(std::ostream& out, const ec_private& of)
{
    out << of.encoded();
    return out;
}

} // namespace wallet
} // namespace libbitcoin


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_StandardCompleteDopplerBroadenedPhotonEnergyDistribution.hpp
//! \author Alex Robinson
//! \brief  The standard complete Doppler broadened photon energy dist. decl.
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_STANDARD_COMPLETE_DOPPLER_BROADENED_PHOTON_ENERGY_DISTRIBUTION_HPP
#define MONTE_CARLO_STANDARD_COMPLETE_DOPPLER_BROADENED_PHOTON_ENERGY_DISTRIBUTION_HPP

// Std Lib Includes
#include <memory>

// Boost Includes
#include <boost/bimap.hpp>

// FRENSIE Includes
#include "MonteCarlo_CompleteDopplerBroadenedPhotonEnergyDistribution.hpp"
#include "Data_SubshellType.hpp"
#include "MonteCarlo_ComptonProfileSubshellConverter.hpp"
#include "MonteCarlo_ComptonProfilePolicy.hpp"
#include "Utility_TabularUnivariateDistribution.hpp"

namespace MonteCarlo{

//! The standard complete (all subshells) Doppler broadened photon energy dist.
template<typename ComptonProfilePolicy>
class StandardCompleteDopplerBroadenedPhotonEnergyDistribution : public CompleteDopplerBroadenedPhotonEnergyDistribution
{

public:

  //! The trials counter type
  typedef CompleteDopplerBroadenedPhotonEnergyDistribution::Counter Counter;

  //! Constructor
  StandardCompleteDopplerBroadenedPhotonEnergyDistribution(
               const std::vector<double>& endf_subshell_occupancies,
               const std::vector<Data::SubshellType>& endf_subshell_order,
               const std::shared_ptr<const ComptonProfileSubshellConverter>&
               subshell_converter,
               const ComptonProfileArray& compton_profile_array );

  //! Destructor
  virtual ~StandardCompleteDopplerBroadenedPhotonEnergyDistribution()
  { /* .. */ }

  //! Check if the subshell is valid
  bool isValidSubshell( const Data::SubshellType subshell ) const override;

  //! Return the occupancy of a subshell (default is the ENDF occupancy)
  virtual double getSubshellOccupancy( const Data::SubshellType subshell ) const override;

  //! Evaluate the distribution with electron momentum projection
  virtual double evaluateWithElectronMomentumProjection(
                              const double incoming_energy,
                              const double electron_momentum_projection,
                              const double scattering_angle_cosine ) const override;

  //! Evaluate the exact distribution
  virtual double evaluateExact( const double incoming_energy,
                                const double outgoing_energy,
                                const double scattering_angle_cosine ) const override;

  //! Evaluate the subshell with the electron momentum projection
  double evaluateSubshellWithElectronMomentumProjection(
                                     const double incoming_energy,
                                     const double electron_momentum_projection,
                                     const double scattering_angle_cosine,
                                     const Data::SubshellType subshell ) const override;

  //! Evaluate the exact subshell distribution
  double evaluateSubshellExact( const double incoming_energy,
                                const double outgoing_energy,
                                const double scattering_angle_cosine,
                                const Data::SubshellType subshell ) const override;

  //! Evaluate the PDF with electron momentum projection
  double evaluatePDFWithElectronMomentumProjection(
                                   const double incoming_energy,
                                   const double electron_momentum_projection,
                                   const double scattering_angle_cosine,
                                   const double precision ) const override;

  //! Evaluate the exact PDF
  double evaluatePDFExact( const double incoming_energy,
                           const double outgoing_energy,
                           const double scattering_angle_cosine,
                           const double precision ) const override;

  //! Evaluate the subshell PDF with electron momentum projection
  double evaluateSubshellPDFWithElectronMomentumProjection(
                                     const double incoming_energy,
                                     const double electron_momentum_projection,
                                     const double scattering_angle_cosine,
                                     const Data::SubshellType subshell,
                                     const double precision ) const override;

  //! Evaluate the exact subshell PDF
  double evaluateSubshellPDFExact( const double incoming_energy,
                                   const double outgoing_energy,
                                   const double scattering_angle_cosine,
                                   const Data::SubshellType subshell,
                                   const double precision ) const override;

  //! Evaluate the integrated cross section (b/mu)
  virtual double evaluateIntegratedCrossSection(
                                          const double incoming_energy,
                                          const double scattering_angle_cosine,
                                          const double precision ) const override;

  //! Evaluate the exact integrated cross section (b/mu)
  virtual double evaluateIntegratedCrossSectionExact(
                                          const double incoming_energy,
                                          const double scattering_angle_cosine,
                                          const double precision ) const override;

  //! Evaluate the subshell integrated cross section (b/mu)
  double evaluateSubshellIntegratedCrossSection(
				          const double incoming_energy,
					  const double scattering_angle_cosine,
					  const Data::SubshellType subshell,
					  const double precision ) const override;

  //! Evaluate the exact subshell integrated cross section (b/mu)
  double evaluateSubshellIntegratedCrossSectionExact(
                                          const double incoming_energy,
					  const double scattering_angle_cosine,
					  const Data::SubshellType subshell,
					  const double precision ) const override;

  //! Sample an outgoing energy from the distribution
  void sample( const double incoming_energy,
	       const double scattering_angle_cosine,
	       double& outgoing_energy,
	       Data::SubshellType& shell_of_interaction ) const override;

  //! Sample an outgoing energy and record the number of trials
  void sampleAndRecordTrials( const double incoming_energy,
			      const double scattering_angle_cosine,
			      double& outgoing_energy,
			      Data::SubshellType& shell_of_interaction,
			      Counter& trials ) const override;

  //! Sample an electron momentum from the distribution
  void sampleMomentumAndRecordTrials(
                                    const double incoming_energy,
                                    const double scattering_angle_cosine,
                                    double& electron_momentum,
                                    Data::SubshellType& shell_of_interaction,
                                    Counter& trials ) const override;

  //! Sample an electron momentum from the subshell distribution
  double sampleSubshellMomentum( const double incoming_energy,
                                 const double scattering_angle_cosine,
                                 const Data::SubshellType subshell ) const override;

protected:

  //! Return the old subshell index corresponding to the subshell
  unsigned getOldSubshellIndex( const Data::SubshellType subshell ) const;

  //! Return the endf subshell index corresponding to the subshell
  unsigned getENDFSubshellIndex( const Data::SubshellType subshell ) const;

  //! Return the subshell corresponding to the endf subshell index
  Data::SubshellType getSubshell( const size_t endf_subshell_index ) const;

  //! Return the Compton profile for a subshell
  const ComptonProfile& getComptonProfile( const Data::SubshellType& subshell) const;

  //! Return the Compton profile for an old subshell index
  const ComptonProfile& getComptonProfile(
                                    const unsigned& old_subshell_index ) const;

  //! Sample an ENDF subshell
  Data::SubshellType sampleENDFInteractionSubshell() const;

  //! Sample an interaction subshell
  virtual void sampleInteractionSubshell( size_t& old_subshell_index,
                                          double& subshell_binding_energy,
                                          Data::SubshellType& subshell ) const = 0;

private:

  // Sample an electron momentum from the subshell distribution
  double sampleSubshellMomentum( const double incoming_energy,
                                 const double scattering_angle_cosine,
                                 const double subshell_binding_energy,
                                 const ComptonProfile& compton_profile ) const;

  // The ENDF subshell interaction probabilities
  std::unique_ptr<const Utility::TabularUnivariateDistribution>
  d_endf_subshell_occupancy_distribution;

  // The ENDF subshell order
  typedef boost::bimap<unsigned,Data::SubshellType> SubshellOrderMapType;
  boost::bimap<unsigned,Data::SubshellType> d_endf_subshell_order;

  // The ENDF subshell occupancies
  std::vector<double> d_endf_subshell_occupancies;

  // The Compton profile subshell converter
  std::shared_ptr<const ComptonProfileSubshellConverter> d_subshell_converter;

  // The electron momentum dist array
  ComptonProfileArray d_compton_profile_array;
};

} // end MonteCarlo namespace

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "MonteCarlo_StandardCompleteDopplerBroadenedPhotonEnergyDistribution_def.hpp"

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

#endif // end MONTE_CARLO_STANDARD_COMPLETE_DOPPLER_BROADENED_PHOTON_ENERGY_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_StandardCompleteDopplerBroadenedPhotonEnergyDistribution.hpp
//---------------------------------------------------------------------------//


#pragma once

#include <scorum/chain/database/block_tasks/block_tasks.hpp>

#include <scorum/chain/services/comment.hpp>
#include <scorum/chain/services/comment_statistic.hpp>
#include <scorum/chain/services/reward_funds.hpp>
#include <scorum/chain/services/comment_vote.hpp>
#include <scorum/chain/services/dynamic_global_property.hpp>
#include <scorum/chain/services/account.hpp>
#include <scorum/chain/services/account_blogging_statistic.hpp>
#include <scorum/chain/services/hardfork_property.hpp>

#include <scorum/chain/schema/scorum_objects.hpp>
#include <scorum/chain/schema/comment_objects.hpp>
#include <scorum/chain/schema/account_objects.hpp>
#include <scorum/chain/schema/reward_objects.hpp>

#include <scorum/rewards_math/curve.hpp>
#include <scorum/rewards_math/formulas.hpp>

#include <boost/range/adaptor/reversed.hpp>
#include <map>

namespace scorum {
namespace chain {
namespace database_ns {

using scorum::rewards_math::shares_vector_type;
using comment_refs_type = scorum::chain::comment_service_i::comment_refs_type;

class process_comments_cashout_impl
{
public:
    struct comment_payout_result
    {
        /// amount of tokens distributed within particular comment across author, beneficiars and curators
        asset total_claimed_reward;
        asset parent_comment_reward;
    };

    struct curators_author_rewards
    {
        asset curators_reward;
        asset author_reward;
    };

    explicit process_comments_cashout_impl(block_task_context& ctx);

    template <typename FundService> void update_decreasing_total_claims(FundService& fund_service)
    {
        using fund_object_type = typename FundService::object_type;

        const auto& rf = fund_service.get();

        auto now = dgp_service.head_block_time();

        fc::uint128_t total_claims = rewards_math::calculate_decreasing_total_claims(
            rf.recent_claims, now, rf.last_update, SCORUM_RECENT_RSHARES_DECAY_RATE);

        fund_service.update([&](fund_object_type& rfo) {
            rfo.recent_claims = total_claims;
            rfo.last_update = now;
        });
    }

    template <typename TFundService> void reward(TFundService& fund_service, const comment_refs_type& comments);

    asset pay_for_comments(const comment_refs_type& comments, const std::vector<asset>& fund_rewards);

    /// These two methods distribute comments reward across all parent comments
    asset pay_for_comments_legacy(const comment_refs_type& comments, const std::vector<asset>& fund_rewards);
    comment_payout_result pay_for_comment_legacy(const comment_object& comment,
                                                 const asset& publication_reward,
                                                 const asset& parent_payout_value);

    void close_comment_payout(const comment_object& comment);

private:
    std::vector<asset> calculate_comments_payout(const comment_refs_type& comments,
                                                 const asset& reward_fund_balance,
                                                 fc::uint128_t total_claims,
                                                 curve_id reward_curve) const;

    fc::uint128_t
    get_total_claims(const comment_refs_type& comments, curve_id reward_curve, fc::uint128_t recent_claims) const;

    curators_author_rewards pay_curators(const comment_object& comment, const asset& fund_reward);
    asset pay_beneficiaries(const comment_object& comment, const asset& author_reward);

    void pay_account(const account_object& recipient, const asset& reward);

    template <class CommentStatisticService>
    void accumulate_comment_statistic(CommentStatisticService& stat_service,
                                      const comment_object& comment,
                                      const asset& fund_reward,
                                      const asset& total_payout,
                                      const asset& author_payout,
                                      const asset& curation_payout,
                                      const asset& payout_from_children,
                                      const asset& payout_to_parent,
                                      const asset& beneficiary_payout)
    {
        using comment_object_type = typename CommentStatisticService::object_type;

        const auto& stat = stat_service.get(comment.id);
        stat_service.update(stat, [&](comment_object_type& c) {
            c.fund_reward_value += fund_reward;
            c.total_payout_value += total_payout;
            c.author_payout_value += author_payout;
            c.curator_payout_value += curation_payout;
            c.beneficiary_payout_value += beneficiary_payout;
            c.from_children_payout_value += payout_from_children;
            c.to_parent_payout_value += payout_to_parent;
        });
    }

    void accumulate_statistic(const comment_object& comment,
                              const account_object& author,
                              const asset& fund_reward,
                              const asset& author_payout,
                              const asset& curation_payout,
                              const asset& payout_from_children,
                              const asset& payout_to_parent,
                              const asset& beneficiary_payout,
                              asset_symbol_type reward_symbol);

    void accumulate_statistic(const account_object& voter, const asset& curation_payout);

    comment_refs_type collect_parents(const comment_refs_type& comments);

    fc::shared_string get_permlink(const fc::shared_string& str) const;

private:
    block_task_context& _ctx;
    dynamic_global_property_service_i& dgp_service;
    account_service_i& account_service;
    account_blogging_statistic_service_i& account_blogging_statistic_service;
    comment_service_i& comment_service;
    comment_statistic_scr_service_i& comment_statistic_scr_service;
    comment_statistic_sp_service_i& comment_statistic_sp_service;
    comment_vote_service_i& comment_vote_service;
    hardfork_property_service_i& hardfork_service;
};
}
}
}


/*
 * Copyright (C) 2019-2020 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */

#include <ros/ros.h>
#include <string>
#include <algorithm>
#include <memory>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <lanelet2_core/geometry/Point.h>
#include <trajectory_utils/trajectory_utils.h>
#include <trajectory_utils/conversions/conversions.h>
#include <sstream>
#include <carma_utils/containers/containers.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <unordered_set>
#include <vector>
#include <cav_msgs/Trajectory.h>
#include <cav_msgs/StopAndWaitManeuver.h>
#include <lanelet2_core/primitives/Lanelet.h>
#include <lanelet2_core/geometry/LineString.h>
#include <carma_wm/CARMAWorldModel.h>
#include <carma_utils/containers/containers.h>
#include <carma_wm/Geometry.h>
#include <cav_msgs/TrajectoryPlanPoint.h>
#include <cav_msgs/TrajectoryPlan.h>
#include <math.h>
#include <std_msgs/Float64.h>
#include <math.h>
#include <stop_controlled_intersection_plugin.h>

using oss = std::ostringstream;

namespace stop_controlled_intersection_transit_plugin
{
StopControlledIntersectionTacticalPlugin::StopControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm,const StopControlledIntersectionTacticalPluginConfig& config,
                                    const PublishPluginDiscoveryCB& plugin_discovery_publisher)
  : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher)
  {
    plugin_discovery_msg_.name = "StopControlledIntersectionTacticalPlugin";
    plugin_discovery_msg_.versionId = "v1.0";
    plugin_discovery_msg_.available = true;
    plugin_discovery_msg_.activated = false;
    plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL;
    plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory";
  }

bool StopControlledIntersectionTacticalPlugin::onSpin()
{
    plugin_discovery_publisher_(plugin_discovery_msg_);
    return true;
}

bool StopControlledIntersectionTacticalPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req, cav_srvs::PlanTrajectoryResponse& resp)
{
    ROS_DEBUG_STREAM("Starting stop controlled intersection trajectory planning");

    if(req.maneuver_index_to_plan >= req.maneuver_plan.maneuvers.size())
    {
    throw std::invalid_argument(
        "Stop Control Intersection Plugin asked to plan invalid maneuver index: " + std::to_string(req.maneuver_index_to_plan) +
        " for plan of size: " + std::to_string(req.maneuver_plan.maneuvers.size()));
    }
    std::vector<cav_msgs::Maneuver> maneuver_plan;
    for(size_t i = req.maneuver_index_to_plan; i < req.maneuver_plan.maneuvers.size(); i++){

        if((req.maneuver_plan.maneuvers[i].type == cav_msgs::Maneuver::LANE_FOLLOWING || req.maneuver_plan.maneuvers[i].type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT
        || req.maneuver_plan.maneuvers[i].type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN || req.maneuver_plan.maneuvers[i].type ==cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN)
        && GET_MANEUVER_PROPERTY(req.maneuver_plan.maneuvers[i], parameters.string_valued_meta_data.front()) == stop_controlled_intersection_strategy_)
        {
            maneuver_plan.push_back(req.maneuver_plan.maneuvers[i]);
            resp.related_maneuvers.push_back(req.maneuver_plan.maneuvers[i].type);
        }
        else
        {
            break;
        }
    }

    lanelet::BasicPoint2d veh_pos(req.vehicle_state.X_pos_global, req.vehicle_state.Y_pos_global);
    ROS_DEBUG_STREAM("Planning state x:"<<req.vehicle_state.X_pos_global <<" , y: " << req.vehicle_state.Y_pos_global);

    double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack;
    ROS_DEBUG_STREAM("Current_downtrack"<< current_downtrack);

    std::vector<PointSpeedPair> points_and_target_speeds = maneuvers_to_points( maneuver_plan, wm_, req.vehicle_state);

    //Trajectory Plan
    cav_msgs::TrajectoryPlan trajectory;
    trajectory.header.frame_id = "map";
    trajectory.header.stamp = req.header.stamp;
    trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());

    //Add compose trajectory from centerline
    trajectory.trajectory_points = compose_trajectory_from_centerline(points_and_target_speeds, req.vehicle_state, req.header.stamp);
    trajectory.initial_longitudinal_velocity = req.vehicle_state.longitudinal_vel;

    resp.trajectory_plan = trajectory;

    resp.maneuver_status.push_back(cav_srvs::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);

    return true;
}

std::vector<PointSpeedPair> StopControlledIntersectionTacticalPlugin::maneuvers_to_points(const std::vector<cav_msgs::Maneuver>& maneuvers,
                                                            const carma_wm::WorldModelConstPtr& wm, const cav_msgs::VehicleState& state)
{
    std::vector<PointSpeedPair> points_and_target_speeds;
    std::unordered_set<lanelet::Id> visited_lanelets;

    lanelet::BasicPoint2d veh_pos(state.X_pos_global, state.Y_pos_global);
    double max_starting_downtrack = wm_->routeTrackPos(veh_pos).downtrack; //The vehicle position
    double starting_speed = state.longitudinal_vel;

    bool first = true;
    double starting_downtrack;
    for (const auto& maneuver : maneuvers)
    {
        if(maneuver.type != cav_msgs::Maneuver::LANE_FOLLOWING && maneuver.type != cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT && maneuver.type != cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN
        && maneuver.type !=cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ){
            throw std::invalid_argument("Stop Controlled Intersection Tactical Plugin does not support this maneuver type");
        }

        if(first)
        {
            starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist);
            if (starting_downtrack > max_starting_downtrack)
            {
                starting_downtrack = max_starting_downtrack;
            }
            first = false;
        }

        // Sample the lanelet centerline at fixed increments.
        // std::min call here is a guard against starting_downtrack being within 1m of the maneuver end_dist
        // in this case the sampleRoutePoints method will return a single point allowing execution to continue
        std::vector<lanelet::BasicPoint2d> route_points = wm->sampleRoutePoints(
            std::min(starting_downtrack + config_.centerline_sampling_spacing, GET_MANEUVER_PROPERTY(maneuver,end_dist)),
            GET_MANEUVER_PROPERTY(maneuver, end_dist), config_.centerline_sampling_spacing);

        route_points.insert(route_points.begin(), veh_pos);

        //get case num from maneuver parameters
        if(GET_MANEUVER_PROPERTY(maneuver,parameters.int_valued_meta_data).empty()){
            throw std::invalid_argument("No case number specified for stop controlled intersection maneuver");
        }

        int case_num = GET_MANEUVER_PROPERTY(maneuver,parameters.int_valued_meta_data[0]);
        if(case_num == 1){
            points_and_target_speeds = create_case_one_speed_profile(wm, maneuver, route_points, starting_speed);
        }
        else if(case_num == 2){
            points_and_target_speeds = create_case_two_speed_profile(wm, maneuver, route_points, starting_speed);
        }
        else if(case_num == 3)
        {
            points_and_target_speeds = create_case_three_speed_profile(wm, maneuver, route_points, starting_speed);
        }
        else{
            throw std::invalid_argument("The stop controlled intersection tactical plugin doesn't handle the case number requested");
        }

    }

    return points_and_target_speeds;
}

std::vector<PointSpeedPair> StopControlledIntersectionTacticalPlugin::create_case_one_speed_profile(const carma_wm::WorldModelConstPtr& wm,
const cav_msgs::Maneuver& maneuver, std::vector<lanelet::BasicPoint2d>& route_geometry_points, double starting_speed){
    //Derive meta data values from maneuver message - Using order in sci_strategic_plugin
    double a_acc = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]);
    double a_dec = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[1]); //a_dec is a -ve value
    double t_acc = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[2]);
    double t_dec = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[3]);
    double speed_before_decel = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[4]);

    //Derive start and end dist from maneuver
    double start_dist = GET_MANEUVER_PROPERTY(maneuver, start_dist);
    double end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist);

    //Checking route geometry start against start_dist and adjust profile
    double route_starting_downtrack = wm->routeTrackPos(route_geometry_points[0]).downtrack;  //Starting downtrack based on geometry points
    double dist_acc;        //Distance for which acceleration lasts

    if(route_starting_downtrack < start_dist){
        //Update parameters
        //Keeping the deceleration part the same
        double dist_decel = pow(speed_before_decel, 2)/(2*std::abs(a_dec));

        dist_acc = end_dist - dist_decel;
        a_acc = (pow(speed_before_decel, 2) - pow(starting_speed,2))/(2*dist_acc);
    }
    else{
        //Use parameters from maneuver message
        dist_acc = (pow(speed_before_decel, 2) - pow(starting_speed, 2))/(2*a_acc);
    }

    std::vector<PointSpeedPair> points_and_target_speeds;
    PointSpeedPair first_point;
    first_point.point = route_geometry_points[0];
    first_point.speed = starting_speed;
    points_and_target_speeds.push_back(first_point);

    lanelet::BasicPoint2d prev_point = route_geometry_points[0];
    double total_dist_covered = 0;                  //Starting dist for maneuver treated as 0.0

    for(size_t i = 1; i < route_geometry_points.size(); i++){
        lanelet::BasicPoint2d current_point = route_geometry_points[i];
        double delta_d = lanelet::geometry::distance2d(prev_point, current_point);
        total_dist_covered += delta_d;
        //Find speed at dist covered
        double speed_i;
        if(total_dist_covered <= dist_acc){
            //Acceleration part
            speed_i = sqrt(pow(starting_speed,2) + 2*a_acc*total_dist_covered);
        }
        else{
            //Deceleration part
            speed_i = sqrt(std::max(pow(speed_before_decel,2) + 2*a_dec*(total_dist_covered - dist_acc),0.0)); //std::max to ensure negative value is not sqrt
            if(speed_i < epsilon_){
                speed_i = 0.0;
            }
        }

        PointSpeedPair p;
        p.point = route_geometry_points[i];
        p.speed = speed_i;
        points_and_target_speeds.push_back(p);

        prev_point = route_geometry_points[i];
    }

    return points_and_target_speeds;

}

std::vector<PointSpeedPair> StopControlledIntersectionTacticalPlugin::create_case_two_speed_profile(const carma_wm::WorldModelConstPtr& wm,
const cav_msgs::Maneuver& maneuver, std::vector<lanelet::BasicPoint2d>& route_geometry_points, double starting_speed){

    //Derive meta data values from maneuver message - Using order in sci_strategic_plugin
    double a_acc = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]);
    double a_dec = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[1]); //a_dec is a -ve value
    double t_acc = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[2]);
    double t_dec = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[3]);
    double t_cruise = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[4]);
    double speed_before_decel = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[5]);

    //Derive start and end dist from maneuver
    double start_dist = GET_MANEUVER_PROPERTY(maneuver, start_dist);
    double end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist);

    //Checking route geometry start against start_dist and adjust profile
    double route_starting_downtrack = wm->routeTrackPos(route_geometry_points[0]).downtrack;  //Starting downtrack based on geometry points
    double dist_acc;        //Distance over which acceleration happens
    double dist_cruise;     //Distance over which cruising happens
    double dist_decel;      //Distance over which deceleration happens

    if(route_starting_downtrack < start_dist){
        //update parameters
        //Keeping acceleration and deceleration part same as planned in strategic plugin
        dist_acc = starting_speed*t_acc + 0.5 * a_acc * pow(t_acc,2);
        dist_decel = speed_before_decel*t_dec + 0.5 * a_dec * pow(t_dec,2);
        dist_cruise = end_dist - route_starting_downtrack - (dist_acc + dist_decel);
    }
    else{
        //Use maneuver parameters to create speed profile
        dist_acc = starting_speed*t_acc + 0.5 * a_acc * pow(t_acc,2);
        dist_cruise = speed_before_decel*t_cruise;
        dist_decel = speed_before_decel*t_dec + 0.5 * a_dec * pow(t_dec,2);
    }

    //Check calculated total dist against maneuver limits
    double total_distance_needed = dist_acc + dist_cruise + dist_decel;
    if(total_distance_needed - (end_dist - start_dist) > epsilon_ ){
        //Requested maneuver needs to be modified to meet start and end dist req
        //Sacrifice on cruising and then acceleration if needed
        double delta_total_dist = total_distance_needed - (end_dist - start_dist);
        dist_cruise -= delta_total_dist;
        if(dist_cruise < 0){
            dist_acc += dist_cruise;
            dist_cruise = 0;
        }
        //Not considering dist_acc < 0 after this.
    }

    std::vector<PointSpeedPair> points_and_target_speeds;
    PointSpeedPair first_point;
    first_point.point = route_geometry_points[0];
    first_point.speed = starting_speed;
    points_and_target_speeds.push_back(first_point);

    lanelet::BasicPoint2d prev_point = route_geometry_points.front();
    double total_dist_planned = 0;                  //Starting dist for maneuver treated as 0.0
    double prev_speed = starting_speed;
    for(auto route_point : route_geometry_points){
        lanelet::BasicPoint2d current_point = route_point;
        double delta_d = lanelet::geometry::distance2d(prev_point, current_point);
        total_dist_planned += delta_d;

        //Find speed at dist covered
        double speed_i;
        if(total_dist_planned < dist_acc){
            //Acceleration part
            speed_i = sqrt(pow(starting_speed,2) + 2*a_acc*total_dist_planned);
        }
        else if(dist_cruise > 0 && total_dist_planned >= dist_acc && total_dist_planned <= (dist_acc + dist_cruise)){
            //Cruising part
            speed_i = prev_speed;
        }
        else{
            //Deceleration part
            speed_i = sqrt(std::max(pow(speed_before_decel,2) + 2*a_dec*(total_dist_planned - dist_acc - dist_cruise),0.0));//std::max to ensure negative value is not sqrt
        }

        PointSpeedPair p;
        p.point = route_point;
        p.speed = std::min(speed_i,speed_before_decel);
        points_and_target_speeds.push_back(p);

        prev_point = route_point;
        prev_speed = speed_i;
    }

    return points_and_target_speeds;

}

std::vector<PointSpeedPair> StopControlledIntersectionTacticalPlugin::create_case_three_speed_profile(const carma_wm::WorldModelConstPtr& wm,
const cav_msgs::Maneuver& maneuver, std::vector<lanelet::BasicPoint2d>& route_geometry_points, double starting_speed){
    //Derive meta data values from maneuver message - Using order in sci_strategic_plugin
    double a_dec = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]);

    //Derive start and end dist from maneuver
    double start_dist = GET_MANEUVER_PROPERTY(maneuver, start_dist);
    double end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist);

    //Checking route geometry start against start_dist and adjust profile
    double route_starting_downtrack = wm->routeTrackPos(route_geometry_points[0]).downtrack;  //Starting downtrack based on geometry points

    if(route_starting_downtrack < start_dist){
        //update parameter
        a_dec = pow(starting_speed, 2)/(2*(end_dist - route_starting_downtrack));
    }

    std::vector<PointSpeedPair> points_and_target_speeds;
    PointSpeedPair first_point;
    first_point.point = route_geometry_points[0];
    first_point.speed = starting_speed;
    points_and_target_speeds.push_back(first_point);

    lanelet::BasicPoint2d prev_point = route_geometry_points[0];
    double total_dist_covered = 0;          //Starting dist for maneuver treated as 0.0

    for(size_t i = 0;i < route_geometry_points.size(); i++){
        lanelet::BasicPoint2d current_point = route_geometry_points[i];
        double delta_d = lanelet::geometry::distance2d(prev_point, current_point);
        total_dist_covered +=delta_d;
        //Find speed at dist covered
        double speed_i = sqrt(std::max(pow(starting_speed,2) + 2 * a_dec * total_dist_covered, 0.0)); //std::max to ensure negative value is not sqrt

        if(speed_i < epsilon_){
            speed_i = 0.0;
        }

        PointSpeedPair p;
        p.point = route_geometry_points[i];
        p.speed = speed_i;
        points_and_target_speeds.push_back(p);

        prev_point = current_point;
    }

    return points_and_target_speeds;
}

std::vector<cav_msgs::TrajectoryPlanPoint> StopControlledIntersectionTacticalPlugin::compose_trajectory_from_centerline(
    const std::vector<PointSpeedPair>& points, const cav_msgs::VehicleState& state, const ros::Time& state_time){

    std::vector<cav_msgs::TrajectoryPlanPoint> trajectory;
    ROS_DEBUG_STREAM("VehicleState: "
                        << " x: " << state.X_pos_global << " y: " << state.Y_pos_global << " yaw: " << state.orientation
                        << " speed: " << state.longitudinal_vel);

    int nearest_pt_index = basic_autonomy::waypoint_generation::get_nearest_point_index(points, state);

    std::vector<PointSpeedPair> future_points(points.begin() + nearest_pt_index + 1, points.end()); //Points in front of current vehicle position
    auto time_bound_points = basic_autonomy::waypoint_generation::constrain_to_time_boundary(future_points, config_.trajectory_time_length);

    ROS_DEBUG_STREAM("Got time bound points with size:" << time_bound_points.size());

    //Attach past points
    std::vector<PointSpeedPair> back_and_future = attach_past_points(points, time_bound_points, nearest_pt_index, config_.back_distance);
    ROS_DEBUG_STREAM("Got back_and_future points with size: "<<back_and_future.size());

    std::vector<double> speed_limits;
    std::vector<lanelet::BasicPoint2d> curve_points;
    split_point_speed_pairs(time_bound_points, &curve_points, &speed_limits);

    std::unique_ptr<basic_autonomy::smoothing::SplineI> fit_curve = basic_autonomy::waypoint_generation::compute_fit(curve_points); //Compute splines based on curve points
    if(!fit_curve)
    {
        throw std::invalid_argument("Could not fit a spline curve along the trajectory!");
    }

    ROS_DEBUG("Got fit");
    ROS_DEBUG_STREAM("Speed_limits.size(): "<<speed_limits.size());

    std::vector<lanelet::BasicPoint2d> all_sampling_points;
    all_sampling_points.reserve(1 + curve_points.size() * 2);

    std::vector<double> distributed_speed_limits;
    distributed_speed_limits.reserve(1+ curve_points.size() * 2);

    //Compute total length of the trajectory to get correct number of points
    // we expect using curve resample step size
    std::vector<double> downtracks_raw = carma_wm::geometry::compute_arc_lengths(curve_points);

    auto total_step_along_curve = static_cast<int>(downtracks_raw.back() / config_.curve_resample_step_size);

    int current_speed_index = 0;
    size_t total_point_size = curve_points.size();

    double step_threshold_for_next_speed = (double)total_step_along_curve / (double)total_point_size;
    double scaled_steps_along_curve = 0.0; // from 0 (start) to 1 (end) for the whole trajectory
    std::vector<double> better_curvature;
    better_curvature.reserve(1 + curve_points.size() * 2);

    for (size_t steps_along_curve = 0; steps_along_curve < total_step_along_curve; steps_along_curve++) // Resample curve at tighter resolution
    {
        lanelet::BasicPoint2d p = (*fit_curve)(scaled_steps_along_curve);
        all_sampling_points.push_back(p);
        double c = basic_autonomy::waypoint_generation::compute_curvature_at((*fit_curve), scaled_steps_along_curve);
        better_curvature.push_back(c);

        if((double) steps_along_curve > step_threshold_for_next_speed)
        {
            step_threshold_for_next_speed += (double)total_step_along_curve / (double)total_point_size;
            current_speed_index++;
        }
        distributed_speed_limits.push_back(speed_limits[current_speed_index]);  //Identify speed limits for resampled points
        scaled_steps_along_curve += 1.0 / total_step_along_curve;               //adding steps_along_curve_step_size
    }

    ROS_DEBUG_STREAM("Got sampled points with size:" << all_sampling_points.size());

    std::vector<double> final_yaw_values = carma_wm::geometry::compute_tangent_orientations(all_sampling_points);

    std::vector<double> curvatures = basic_autonomy::smoothing::moving_average_filter(better_curvature, config_.curvature_moving_average_window_size, false);
    std::vector<double> ideal_speeds =
        trajectory_utils::constrained_speeds_for_curvatures(curvatures, config_.lateral_accel_limit);

    std::vector<double> constrained_speed_limits = basic_autonomy::waypoint_generation::apply_speed_limits(ideal_speeds, distributed_speed_limits); //Speed min(ideal, calculated)
    ROS_DEBUG("Processed all points in computed fit");
    std::vector<double> final_actual_speeds = constrained_speed_limits;

    if (all_sampling_points.empty())
    {
        ROS_WARN_STREAM("No trajectory points could be generated");
        return {};
    }

    //Drop Past points
    nearest_pt_index = basic_autonomy::waypoint_generation::get_nearest_index_by_downtrack(all_sampling_points, wm_, state);
    std::vector<lanelet::BasicPoint2d> future_basic_points(all_sampling_points.begin() + nearest_pt_index + 1,
                                                            all_sampling_points.end());
    std::vector<double> future_speeds(final_actual_speeds.begin() + nearest_pt_index + 1,
                                    final_actual_speeds.end());
    std::vector<double> future_yaw(final_yaw_values.begin() + nearest_pt_index + 1,
                                    final_yaw_values.end());

    // Add current vehicle point to front of the trajectory
    lanelet::BasicPoint2d cur_veh_point(state.X_pos_global, state.Y_pos_global);

    future_basic_points.insert(future_basic_points.begin(),
                                cur_veh_point); // Add current vehicle position to front of sample points
    future_speeds.insert(future_speeds.begin(), state.longitudinal_vel);
    future_yaw.insert(future_yaw.begin(), state.orientation);

    // Compute points to local downtracks
    std::vector<double> downtracks = carma_wm::geometry::compute_arc_lengths(future_basic_points);

    final_actual_speeds = basic_autonomy::smoothing::moving_average_filter(future_speeds, config_.speed_moving_average_window_size);

    // Convert speeds to times
    std::vector<double> times;

    //Force last point speed to 0.0 if close to end
    if(lanelet::geometry::distance2d(future_basic_points.back(), points.back().point) < epsilon_){
        final_actual_speeds.back() = 0.0;
    }

    trajectory_utils::conversions::speed_to_time(downtracks, final_actual_speeds, &times);

    // Build trajectory points
    std::vector<cav_msgs::TrajectoryPlanPoint> traj_points =
        basic_autonomy::waypoint_generation::trajectory_from_points_times_orientations(future_basic_points, times, future_yaw, state_time);

    return traj_points;
}

}


#ifdef USE_CUDNN
#include <algorithm>
#include <vector>

#include "caffe/filler.hpp"
#include "caffe/layer.hpp"
#include "caffe/util/im2col.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/vision_layers.hpp"

#include <boost/unordered_map.hpp>
#include <cudnn.h>

using boost::unordered_map;

namespace caffe {

// Set to three for the benefit of the backward pass, which
// can use separate streams for calculating the gradient w.r.t.
// bias, filter weights, and bottom data for each group independently
#define CUDNN_FWD_STREAMS_PER_GROUP 1
#define CUDNN_BWD_STREAMS_PER_GROUP 2

template <typename Dtype>
shared_ptr<SyncedMemory> CuDNNConvolutionLayer<Dtype>::workspaceData_fwd;
template <typename Dtype>
shared_ptr<SyncedMemory> CuDNNConvolutionLayer<Dtype>::workspaceData_bwd_filter;
template <typename Dtype>
shared_ptr<SyncedMemory> CuDNNConvolutionLayer<Dtype>::workspaceData_bwd_data;

template <typename Dtype>
size_t CuDNNConvolutionLayer<Dtype>::conv_layer_count = 0;

/**
 * TODO(dox) explain cuDNN interface
 */
template <typename Dtype>
void CuDNNConvolutionLayer<Dtype>::LayerSetUp(
    const vector<Blob<Dtype>*>& bottom, const vector<Blob<Dtype>*>& top) {
  ConvolutionLayer<Dtype>::LayerSetUp(bottom, top);

  if (conv_layer_count == 0){
    workspaceData_fwd = shared_ptr<SyncedMemory>(new SyncedMemory());
    workspaceData_bwd_filter = shared_ptr<SyncedMemory>(new SyncedMemory());
    workspaceData_bwd_data = shared_ptr<SyncedMemory>(new SyncedMemory());
  }
  conv_layer_count++;

  // Initialize CUDA streams and cuDNN.
  int total_streams_per_group = CUDNN_FWD_STREAMS_PER_GROUP + CUDNN_BWD_STREAMS_PER_GROUP;
  stream_         = new cudaStream_t[this->group_ * total_streams_per_group];
  handle_         = new cudnnHandle_t[this->group_ * total_streams_per_group];

  // initialize size arrays
  workspace_fwd_offsets_ = new size_t[bottom.size()];
  workspace_bwd_filter_offsets_ = new size_t[bottom.size()];
  workspace_bwd_data_offsets_ = new size_t[bottom.size()];

  for (size_t i = 0; i < bottom.size(); ++i) {
    // initialize all to default algorithms
    fwd_algo_.push_back((cudnnConvolutionFwdAlgo_t)0);
    bwd_filter_algo_.push_back((cudnnConvolutionBwdFilterAlgo_t)0);
    bwd_data_algo_.push_back((cudnnConvolutionBwdDataAlgo_t)0);
    // default algorithms don't require workspace
    workspace_fwd_offsets_[i] = 0;
    workspace_bwd_filter_offsets_[i] = 0;
    workspace_bwd_data_offsets_[i] = 0;
  }

  for (int g = 0; g < this->group_ * total_streams_per_group; g++) {
    CUDA_CHECK(cudaStreamCreate(&stream_[g]));
    CUDNN_CHECK(cudnnCreate(&handle_[g]));
    CUDNN_CHECK(cudnnSetStream(handle_[g], stream_[g]));
  }

  // Set the indexing parameters.

  bias_offset_ = (this->num_output_ / this->group_);

  std::vector<int> kernel_shape;
  kernel_shape.push_back(this->num_output_ / this->group_);
  kernel_shape.push_back(this->channels_ / this->group_);
  for (unsigned int i = 0; i < this->num_spatial_axes_; ++i)
    kernel_shape.push_back(this->kernel_shape_.cpu_data()[i]);

  cudnn::createNdFilterDesc<Dtype>(&filter_desc_, kernel_shape);

  // Create tensor descriptor(s) for data and corresponding convolution(s).
  for (int i = 0; i < bottom.size(); i++) {
    cudnnTensorDescriptor_t bottom_desc;
    cudnn::createTensorDesc<Dtype>(&bottom_desc);
    bottom_descs_.push_back(bottom_desc);
    cudnnTensorDescriptor_t top_desc;
    cudnn::createTensorDesc<Dtype>(&top_desc);
    top_descs_.push_back(top_desc);
    cudnnConvolutionDescriptor_t conv_desc;
    cudnn::createConvolutionDesc<Dtype>(&conv_desc);
    conv_descs_.push_back(conv_desc);
  }

  // Tensor descriptor for bias.
  if (this->bias_term_) {
    cudnn::createTensorDesc<Dtype>(&bias_desc_);
  }

  handles_setup_ = true;
  need_benchmark_ = true;

}

template <typename Dtype>
void CuDNNConvolutionLayer<Dtype>::Reshape(
    const vector<Blob<Dtype>*>& bottom, const vector<Blob<Dtype>*>& top) {
  ConvolutionLayer<Dtype>::Reshape(bottom, top);

  bottom_offset_ = this->bottom_dim_ / this->group_;
  top_offset_ = this->top_dim_ / this->group_;

  std::vector<int> bottom_tensor_shape(bottom[0]->shape());
  bottom_tensor_shape[1] /= this->group_;
  std::vector<int> bottom_tensor_stride(bottom[0]->shape().size(), 1);
  for (int i = bottom[0]->shape().size() - 2; i >= 0; --i) {
    bottom_tensor_stride[i] =
        bottom[0]->shape(i + 1) * bottom_tensor_stride[i + 1];
  }

  std::vector<int> top_tensor_shape(top[0]->shape());
  top_tensor_shape[1] /= this->group_;
  std::vector<int> top_tensor_stride(top[0]->shape().size(), 1);
  for (int i = top[0]->shape().size() - 2; i >= 0; --i) {
    top_tensor_stride[i] = top[0]->shape(i + 1) * top_tensor_stride[i + 1];
  }

  std::vector<int> pad, stride;
  for (unsigned int i = 0; i < this->num_spatial_axes_; ++i) {
    pad.push_back(this->pad_.cpu_data()[i]);
    stride.push_back(this->stride_.cpu_data()[i]);
  }
  // Specify workspace limit for kernels directly until we have a
  // planning strategy and a rewrite of Caffe's GPU memory mangagement.
  //
  // However this can be tuned by the "richness" parameter in the solver protobuf
  // By setting richness, you can increase the memory available to cuDNN and thus
  // let it choose fast but space consuming algorithms.
  for (int i = 0; i < bottom.size(); i++) {

    cudnn::setTensorNdDesc<Dtype>(&bottom_descs_[i],
        bottom_tensor_shape, bottom_tensor_stride);
    cudnn::setTensorNdDesc<Dtype>(&top_descs_[i],
        top_tensor_shape, top_tensor_stride);
    cudnn::setNdConvolutionDesc<Dtype>(&conv_descs_[i], bottom_descs_[i],
        filter_desc_, pad, stride);

  if (need_benchmark_){
      // choose forward and backward algorithms + workspace(s)
      const int kRequestedForwardAlgoCount = 6;
      vector<cudnnConvolutionFwdAlgoPerf_t> fwd_perf;
      fwd_perf.resize(kRequestedForwardAlgoCount);
      int returnedAlgoCount;
      size_t mem_limit = 200*1024*1024;
      CUDNN_CHECK(cudnnGetConvolutionForwardAlgorithm (handle_[0], bottom_descs_[i], filter_desc_, conv_descs_[i], top_descs_[i],CUDNN_CONVOLUTION_FWD_SPECIFY_WORKSPACE_LIMIT,mem_limit, &fwd_algo_[i]));
/*
      CUDNN_CHECK(cudnnGetConvolutionForwardWorkspaceSize(handle_[0],
                                                       bottom_descs_[i],
                                                       filter_desc_,
                                                       conv_descs_[i],
                                                       top_descs_[i],
                                                       kRequestedForwardAlgoCount,
                                                       &returnedAlgoCount,
                                                       &fwd_perf[0]));

      // choose the fastest within limit
      // if all algorithms exceed memory limit, we will use the 0 algorithm with no workspace
      for (int a = 0; a < kRequestedForwardAlgoCount; ++a){
        if (fwd_perf[a].memory * this->group_ < (Caffe::cudnn_mem_richness() * 1024 * 1024)
            || Caffe::cudnn_mem_richness() == 0){
          fwd_algo_[i] = fwd_perf[a].algo;
          break;
        }
      }
*/

      // choose backward algorithm for filter
      const int kRequestedBackwardFilterAlgoCount = 4;
      vector<cudnnConvolutionBwdFilterAlgoPerf_t> bwd_filter_perf;
      bwd_filter_perf.resize(kRequestedBackwardFilterAlgoCount);
      CUDNN_CHECK(cudnnGetConvolutionBackwardFilterAlgorithm(handle_[0], bottom_descs_[i], top_descs_[i], conv_descs_[i], filter_desc_,
                                                                   CUDNN_CONVOLUTION_BWD_FILTER_SPECIFY_WORKSPACE_LIMIT,
                                                                   mem_limit,
                                                                   &bwd_filter_algo_[i]));

/*
      CUDNN_CHECK(cudnnFindConvolutionBackwardFilterAlgorithm(handle_[0],
                                                              bottom_descs_[i],
                                                              top_descs_[i],
                                                              conv_descs_[i],
                                                              filter_desc_,
                                                              kRequestedBackwardFilterAlgoCount,
                                                              &returnedAlgoCount,
                                                              &bwd_filter_perf[0]));

      // choose the fastest within limit
      // if all algorithms exceed memory limit, we will use the 0 algorithm with no workspace
      for (int a = 0; a < kRequestedBackwardFilterAlgoCount; ++a){
        if (bwd_filter_perf[a].memory * this->group_ < (Caffe::cudnn_mem_richness() * 1024 * 1024)
            || Caffe::cudnn_mem_richness() == 0){
          bwd_filter_algo_[i] = bwd_filter_perf[a].algo;
          break;
        }
      }

*/
      // choose backward algo for data
      const int kRequestedBackwardDataAlgoCount = 4;
      vector<cudnnConvolutionBwdDataAlgoPerf_t> bwd_data_perf;
      bwd_data_perf.resize(kRequestedBackwardDataAlgoCount);

           //backward data
            CUDNN_CHECK(cudnnGetConvolutionBackwardDataAlgorithm(handle_[0],
                                                                 filter_desc_, top_descs_[i], conv_descs_[i], bottom_descs_[i],
                                                                 CUDNN_CONVOLUTION_BWD_DATA_SPECIFY_WORKSPACE_LIMIT,
                                                                 mem_limit,
                                                                 &bwd_data_algo_[i]));
/*
      CUDNN_CHECK(cudnnFindConvolutionBackwardDataAlgorithm(handle_[0],
                                                            filter_desc_,
                                                            top_descs_[i],
                                                            conv_descs_[i],
                                                            bottom_descs_[i],
                                                            kRequestedBackwardDataAlgoCount,
                                                            &returnedAlgoCount,
                                                            &bwd_data_perf[0]));

      // choose the fastest within limit
      // if all algorithms exceed memory limit, we will use the 0 algorithm with no workspace
      for (int a = 0; a < kRequestedBackwardDataAlgoCount; ++a){
        if (bwd_data_perf[a].memory * this->group_ <(Caffe::cudnn_mem_richness() * 1024 * 1024)
            || Caffe::cudnn_mem_richness() == 0){
          bwd_data_algo_[i] = bwd_data_perf[a].algo;
          break;
        }
      }
*/

      need_benchmark_ = false;
    }
  }

  // Tensor descriptor for bias.
  if (this->bias_term_) {

    vector<int> bias_shape(bottom[0]->shape().size(), 1);
    bias_shape[1] = this->num_output_ / this->group_;
    cudnn::setTensorNdDesc<Dtype>(&bias_desc_, bias_shape);
  }

  AdjustWorkSpaces();
}

template<typename Dtype>
void CuDNNConvolutionLayer<Dtype>::AdjustWorkSpaces() {

  size_t workspace_size_fwd = 0;
  size_t workspace_size_bwd_data = 0;
  size_t workspace_size_bwd_filter = 0;

  for (int i = 0; i < fwd_algo_.size(); ++i){
    size_t workspace_size;
    cudnnGetConvolutionForwardWorkspaceSize(handle_[0],
                                            bottom_descs_[i], filter_desc_,
                                            conv_descs_[i],
                                            top_descs_[i],
                                            fwd_algo_[i], &workspace_size);
    workspace_fwd_offsets_[i] = workspace_size;
    workspace_size_fwd = std::max(workspace_size * this->group_, workspace_size_fwd);

    cudnnGetConvolutionBackwardFilterWorkspaceSize(handle_[1],
                                                   bottom_descs_[i], top_descs_[i],
                                                   conv_descs_[i],
                                                   filter_desc_,
                                                   bwd_filter_algo_[i], &workspace_size);
    workspace_bwd_filter_offsets_[i] = workspace_size;
    workspace_size_bwd_filter = std::max(workspace_size * this->group_, workspace_size_bwd_filter);

    cudnnGetConvolutionBackwardDataWorkspaceSize(handle_[2],
                                                 filter_desc_,
                                                 top_descs_[i],
                                                 conv_descs_[i],
                                                 bottom_descs_[i],
                                                 bwd_data_algo_[i], &workspace_size);
    workspace_bwd_data_offsets_[i] = workspace_size;
    workspace_size_bwd_data = std::max(workspace_size * this->group_, workspace_size_bwd_data);
  }

  workspaceData_fwd->Resize(workspace_size_fwd);
  workspaceData_bwd_filter->Resize(workspace_size_bwd_filter);
  workspaceData_bwd_data->Resize(workspace_size_bwd_data);
}

template <typename Dtype>
CuDNNConvolutionLayer<Dtype>::~CuDNNConvolutionLayer() {
  // Check that handles have been setup before destroying.
  if (!handles_setup_) { return; }

  for (int i = 0; i < bottom_descs_.size(); i++) {
    cudnnDestroyTensorDescriptor(bottom_descs_[i]);
    cudnnDestroyTensorDescriptor(top_descs_[i]);
    cudnnDestroyConvolutionDescriptor(conv_descs_[i]);
  }
  if (this->bias_term_) {
    cudnnDestroyTensorDescriptor(bias_desc_);
  }
  cudnnDestroyFilterDescriptor(filter_desc_);

  int total_stream_per_group = CUDNN_FWD_STREAMS_PER_GROUP + CUDNN_BWD_STREAMS_PER_GROUP;
  for (int g = 0; g < this->group_ * total_stream_per_group; g++) {
    cudaStreamDestroy(stream_[g]);
    cudnnDestroy(handle_[g]);
  }

  --conv_layer_count;
  if (conv_layer_count == 0){
    workspaceData_fwd.reset();
    workspaceData_bwd_filter.reset();
    workspaceData_bwd_data.reset();
  }

  delete [] stream_;
  delete [] handle_;
  delete [] workspace_fwd_offsets_;
  delete [] workspace_bwd_data_offsets_;
  delete [] workspace_bwd_filter_offsets_;

}

INSTANTIATE_CLASS(CuDNNConvolutionLayer);

}   // namespace caffe
#endif