/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2015 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef COARSENING_HH
#define COARSENING_HH

#include <boost/timer/timer.hpp>
#include <boost/type_traits/remove_pointer.hpp>
#include <boost/type_traits/remove_reference.hpp>

#include <boost/fusion/algorithm.hpp>
#include <boost/fusion/sequence.hpp>

#include "fem/errorest.hh"
#include "fem/embedded_errorest.hh"
#include "fem/fetransfer.hh"
#include "fem/firstless.hh"
#include "fem/fixfusion.hh"
#include "fem/iterate_grid.hh"
#include "utilities/power.hh"

#include "linalg/dynamicMatrix.hh"

namespace Kaskade
{
  namespace CoarseningDetail
  {

    using namespace boost::fusion;

    // A class representing a local projector for a given FE space
    template <class Sp>
    struct Projector
    {
      typedef Sp Space;

      Space const*                                                  space;  // pointer to FE space
      std::vector<size_t>                                           gidx;   // global indices
      DynamicMatrix<Dune::FieldMatrix<typename Space::Scalar,1,1>>  q;      // stores a factor of the projection matrix Q Q^T.

      // these are declared here to be used in GetLocalTransferProjection - avoid frequent reallocations
      DynamicMatrix<Dune::FieldMatrix<typename Space::Scalar,1,1>> pLocal;
      std::vector<int>                                             idx;
    };

    // A functor creating local projectors for a given FE space
    struct CreateProjection
    {
      template <class T> struct result {};

      template <class SpacePtr>
      struct result<CreateProjection(SpacePtr)> {
        typedef typename boost::remove_pointer<typename boost::remove_reference<SpacePtr>::type>::type Space;
        typedef Projector<Space> type;
      };

      template <class SpacePtr>
      typename result<CreateProjection(SpacePtr)>::type operator()(SpacePtr space) const
      {
        typename result<CreateProjection(SpacePtr)>::type p;
        p.space = space;
        return p;
      }
    };

    // A functor filling a local projector with actual data for a given cell
    template <class CellPointer>
    struct GetLocalTransferProjection
    {
      // father points to the father cell,
      // children contains pointer to its children
      GetLocalTransferProjection(CellPointer father_, std::vector<CellPointer> const& children_): father(father_), children(children_)
      { }

      template <class Projector>
      void operator()(Projector& p) const
      {
	typedef typename Projector::Space Space;
	// We could use LocalTransfer here, but this is conceptual and computational overkill. It supports different FE spaces (we need only one here)
	// and creates new shape function sets just in case the cell vanishes on coarsening. We do the projection here before coarsening...
	// Moreover we know here that all children of the father cell are leafs.

        // Following a direct implementation exploiting our a priori knowledge for simplicity and performance.
        // Obtain all global indices associated to leaf cells within this father cell
        p.gidx.clear();
        for (int i=0; i<children.size(); ++i)
        {
          auto gi = p.space->mapper().globalIndices(*children[i]);
          p.gidx.insert(p.gidx.end(),gi.begin(),gi.end());
        }
        // global indices can be duplicate - make unique
        std::sort(p.gidx.begin(),p.gidx.end());
        p.gidx.erase(std::unique(p.gidx.begin(),p.gidx.end()),p.gidx.end());

        // Step through all children and obtain the prolongation matrix. Scatter this to the
        // prolongation matrix P for all children's degrees of freedom.
        p.q.setSize(p.gidx.size(),p.space->mapper().shapefunctions(*father).size());
//        p.q = 0 // causes compilation error with dune-2.4.0 and clang++ on OS X (Darwin)
        p.q.fill(0);
        typedef typename Space::Grid Grid;
        typedef typename Space::Scalar   Scalar;
        int const sfComponents = Space::sfComponents;
        DynamicMatrix<Dune::FieldMatrix<Scalar,sfComponents,1>> afValues; // declare out of loop to prevent frequent reallocations
        std::vector<Dune::FieldVector<typename Grid::ctype,Grid::dimension> > iNodes; // declare out of loop to prevent frequent reallocations
        for (int i=0; i<children.size(); ++i)
        {
          // Compute the mapping of global indices on the child to the set of global indices on all children.
          auto gi = p.space->mapper().globalIndices(*children[i]);
          p.idx.resize(gi.size());
          for (int j=0; j<gi.size(); ++j)
            p.idx[j] = std::lower_bound(p.gidx.begin(),p.gidx.end(),gi[j]) - p.gidx.begin();

          // Compute local prolongation matrix p.pLocal. As father and child spaces are the same, we can use the same shape function set
          // (assuming that the type of cell doesn't change...).
          auto const& sfs = p.space->mapper().shapefunctions(*children[i]);
          localProlongationMatrix(*p.space,children[i],*p.space,father,p.pLocal,sfs,sfs);

          // Enter prolongation values on this child into the whole prolongation matrix for all children
          for(size_t k=0; k<p.idx.size(); ++k)
            for(int j=0; j<p.q.M(); ++j)
              p.q[p.idx[k]][j] = p.pLocal[k][j];
        }

        // Now the projection onto a coarser space is P P^+, where P^+, the pseudoinverse of P, is the restriction matrix.
        // Note that this formulation of the (not uniquely defined restriction) is different from the interpolation-based
        // restriction as obtained from LocalTransfer. Nevertheless, both make sense and both appear to work very similar.
        // In fact one might argue that the pseudoinverse formulation here makes even more sense, as it leads to a
        // least-squares approximation of the fine grid solution by the coarse grid restriction. "Least squares" is, however,
        // a norm-dependent concept, and the euclidean norm of FE coefficient vectors as implicitly used here might not
        // be the best suited.
        // With a QR decomposition of P = Q [R;0], the pseudoinverse is P^+ = [R^{-1} 0] Q^T and hence the projection
        // P P^+ = Q [I 0; 0 0] Q^T. If Q = [Q1 Q2], then P P^+ = Q1 Q1^T holds. The image space of Q1 is the same as that
        // of P. For computing Q1, we therefore do not need to compute a full QR decomposition, but only the first columns of
        // Q. The most simple method is Gram-Schmidt. Though it is numerically instable if the columns of P are almost linear
        // dependent, we use it here, as the columns of P are associated to the shape functions on the father cell and
        // should be comfortably linearly independent, though not orthogonal.
        // Instead of forming Q1 Q1^T, we store just Q1, and apply the product if needed. As Q1 is high and tall, this is
        // (slightly) more efficient.
        for (int j=0; j<p.q.M(); ++j) // step through all columns
        {
          Scalar tmp = 0;

          // Normalize column j.
          for (int i=0; i<p.q.N(); ++i)
            tmp += power<2>(p.q[i][j]);
          tmp = std::sqrt(tmp);
          for (int i=0; i<p.q.N(); ++i)
            p.q[i][j] /= tmp;

          // Orthogonalize remaining columns.
          for (int k=j+1; k<p.q.M(); ++k)
          {
            tmp = 0;
            for (int i=0; i<p.q.N(); ++i)
              tmp += p.q[i][j] * p.q[i][k];
            for (int i=0; i<p.q.N(); ++i)
              p.q[i][k] -= tmp*p.q[i][j];
          }
        }
      }

    private:
      CellPointer father;
      std::vector<CellPointer> const& children;
    };

    // A functor that applies the appropriate projector provided on construction to the
    // FE function that is given as argument.
    template <class Projectors>
    struct ProjectCoefficients
    {
      // Construct the functor, giving a list of (local) projectors indexed by the FE space index.
      ProjectCoefficients(Projectors const& projectors_): projectors(projectors_) {}

      // Apply the appropriate projector to the provided FE function. The pair consists of a variable description
      // (containing the space index) and the FE function itself.
      template <class Pair>
      void operator()(Pair const& pair) const
      {
        typedef typename boost::remove_reference<typename result_of::value_at_c<Pair,0>::type>::type VarDesc;
        typedef typename boost::remove_reference<typename result_of::value_at_c<Pair,1>::type>::type Function;

        int const sIdx = VarDesc::spaceIndex;
        auto const& proj = at_c<sIdx>(projectors);
        int const n = proj.gidx.size();

        // Multiplication of projector Q Q^T and coefficient vector. The
        // multiplication has to be done manually because of
        // type-incompatible entries. This results in the coefficients of
        // the projection residual.
        auto const& q = proj.q;
        int const m = q.M();
        typename Function::StorageType x(n);
        typename Function::StorageType y(m);

        // y <- Q^T v
        for (int i=0; i<m; ++i) {
          y[i] = 0;
          for (int j=0; j<n; ++j)
            y[i].axpy(q[j][i][0][0],(at_c<1>(pair).coefficients())[proj.gidx[j]]);
        }

        // x <- Q y
        for (int i=0; i<n; ++i) {
          typename Function::StorageValueType x(0);
          for (int j=0; j<m; ++j)
            x.axpy(q[i][j][0][0],y[j]);
          (at_c<1>(pair).coefficients())[proj.gidx[i]] = x;
        }
      }

    private:
      Projectors const& projectors;
    };

    // Convenience function for template type deduction.
    template <class Projectors>
    ProjectCoefficients<Projectors> getCoefficientProjectors(Projectors const& ps) { return ProjectCoefficients<Projectors>(ps); }

    class GroupByCell
    {
    public:
      GroupByCell(std::vector<size_t> const& groups_)
      : groups(&groups_), nGroups(*std::max_element(groups_.begin(),groups_.end())+1) {}

      size_t operator[](size_t idx) const { return (*groups)[idx]; }
      size_t nGroups;

    private:
      std::vector<size_t> const* groups;
    };

  } // End of namespace CoarseningDetail

  /** \ingroup adapt
   * In order to maintain compatibility with existing code,
   * this overload is needed, as non const reference parameters (coarsenedCells) can not
   * have default values. All it does is call the original method with a temporary
   * parameter.
   */
  template <class VariableSetDescription, class Scaling>
  void coarsening(VariableSetDescription const& varDesc,
                  typename VariableSetDescription::VariableSet const& sol,
                  Scaling const& scaling,
                  std::vector<std::pair<double,double> > const& tol,
                  GridManager<typename VariableSetDescription::Grid>& gridManager, int verbosity=1, int minRefLevel=0)
  {
    std::vector<bool> tmp;
    coarsening(varDesc, sol, scaling, tol, gridManager, tmp, verbosity, minRefLevel);
  }

  /** \ingroup adapt
   * \brief coarsening routine
   *
   * Perform a projection from a fine grid onto a coarse grid. If the error is small, locally
   * coarsen the grid there.
   *
   * \param varDesc
   * \param sol
   * \param scaling
   * \param tol
   * \param gridManager
   * \param coarsenedCells
   * \param verbosity
   * \param minRefLevel do not coarse cells that are on this level or below.
   */
  template <class VariableSetDescription, class Scaling>
  void coarsening(VariableSetDescription const& varDesc,
                  typename VariableSetDescription::VariableSet const& sol,
                  Scaling const& scaling,
                  std::vector<std::pair<double,double> > const& tol,
                  GridManager<typename VariableSetDescription::Grid>& gridManager,
                  std::vector<bool>& coarsenedCells, int verbosity=1, int minRefLevel=0)
  {
    using namespace boost::fusion;
    using namespace CoarseningDetail;

// boost::timer::cpu_timer timer;
// boost::timer::cpu_timer timerA, timerB;
// timerA.stop(); timerB.stop();

    typedef typename VariableSetDescription::Grid           Grid;
    typedef typename Grid::LocalIdSet    IdSet;
    typedef typename Grid::LeafIndexSet  IndexSet;
    typedef typename Grid::template Codim<0>::Entity        Cell;
    typedef typename Grid::template Codim<0>::EntityPointer CellPointer;
    typedef typename Cell::HierarchicIterator               HierarchicIterator;

    IndexSet const& indexSet = varDesc.indexSet;

    // A set of FE functions that will contain the projection.
    typename VariableSetDescription::VariableSet pSol(sol);

    // A container of local projectors - defined here to avoid multiple allocations inside the cell loop
    auto projectors = as_vector(transform(varDesc.spaces,CreateProjection()));

    // a container for caching computed cell pointers to children. Prevents doubled hierarchical grid traversal
    // and frequent reallocation
    std::vector<CellPointer> children;

    // A mapping from leaf cells to their coarsening group number. A
    // coarsening group is defined as follows. If all the direct
    // children of a non-leaf cell are leafs (i.e. none of the children
    // has been refined), these children form a coarsening group. Either
    // all or none cells of a coarsening group are marked for
    // coarsening. The special group number 0 collects all cells which
    // are not element of a regular coarsening group (and hence cannot be
    // removed during refinement).
    std::vector<size_t> coarseningGroup(indexSet.size(0),0);
    size_t coarseningGroupCount = 0;

    // Step through all leaf cells and process their fathers.
    typedef typename Grid::template Codim<0>::template Partition<Dune::All_Partition>::LeafIterator CellIterator;
    CellIterator end = gridManager.grid().template leafend<0>();
    for (CellIterator ci=gridManager.grid().template leafbegin<0>(); ci!=end; ++ci) // TODO: parallelize this loop
      // Father has already been processed if cell belongs to a regular
      // cell group. No father exists if the level is 0.
      if (coarseningGroup[indexSet.index(*ci)]==0 && ci->level()>0) { // not yet processed, not on coarse grid
        // Only process fathers which are suitable for coarsening.
        CellPointer father = ci->father();
        bool canBeCoarsened = true;
        children.clear();
        HierarchicIterator first = father->hbegin(father->level()+1), last = father->hend(father->level()+1);
        for (HierarchicIterator hi=first; canBeCoarsened && hi!=last; ++hi) // early termination in case we find a non-leaf child
        {
          canBeCoarsened = canBeCoarsened && hi->isLeaf();
          children.push_back(CellPointer(hi));
        }

        if (canBeCoarsened) {
// timerA.resume();
          // Gather all the children into a new coarsening group.
          ++coarseningGroupCount;
          for (auto const& c: children)
            coarseningGroup[indexSet.index(*c)] = coarseningGroupCount; // not coarseningGroupCount-1: group number 0 is for not coarsenable cells
          // Compute the projections to the locally coarser subspace.
          for_each(projectors,GetLocalTransferProjection<CellPointer>(father,children));
// timerA.stop();
// timerB.resume();

          // Apply the projectors to the FE functions.
          for_each2(typename VariableSetDescription::Variables(),pSol.data,getCoefficientProjectors(projectors));
// timerB.stop();
        }
      }

    // Create the projection residual (i.e. the error introduced by coarsening).
    pSol -= sol;

//     std::cerr << "Coarsening part Ia time: " << timer.format()
//               << "       A               : " << timerA.format()
//               << "       B               : " << timerB.format();
//     timer.start();

    // Compute scaled L2 norms of the function and difference,
    // attributing the cell contributions to its coarsening group.
    ErrorestDetail::GroupedSummationCollector<GroupByCell> sum{GroupByCell(coarseningGroup)};
    scaledTwoNormSquared(join(typename VariableSetDescription::Variables(),typename VariableSetDescription::Variables()),
                         join(pSol.data,sol.data),varDesc.spaces,scaling,sum);
//     std::cerr << "Coarsening part Ib time: " << timer.format();
//     timer.start();

    // For each variable, compute the (squared) relative error
    // contribution, that is e_i^2 = |p_i|^2/(atol^2+|f|^2*rtol^2),
    // where f is the function and p the difference to its hierarchic
    // projection.
    std::vector<double> norm2(varDesc.noOfVariables,0);
    for (int i=0; i<varDesc.noOfVariables; ++i)
      for (int j=0; j<=coarseningGroupCount; ++j)
        norm2[i] += sum.sums[j][i+varDesc.noOfVariables];
    if ( verbosity>0 )
    {
      std::cout << "coarsening solution norms2: ";
      std::copy(norm2.begin(),norm2.end(),
      std::ostream_iterator<double>(std::cout," ")); std::cout  << '\n';
    };

    for (int i=0; i<varDesc.noOfVariables; ++i) {
      double toli = power(tol[i].first,2) + norm2[i]*power(tol[i].second,2);
      for (int j=0; j<=coarseningGroupCount; ++j)
        sum.sums[j][i] /= toli;
    }

    // Select a maximal subset C of the coarsening groups, such that for
    // each variable the sum of C's relative errors is below 1.
    //
    // A greedy algorithm may work as follows: Maintain a vector E_i (of
    // size the number of variables) that contains the remaining allowed
    // relative error. Of the remaining coarsening groups j with
    // relative errors e_ji select the one for which max_i e_ji / E_i is
    // minimal.
    //
    // However, this has quadratic complexity. Thus we resort to the
    // following simplification.  First we assign to each coarsening
    // group j the maximal relative error e_j = max_i e_ji encountered
    // in any variable. Subsequently, these are sorted in ascending
    // order. Then the first k groups for which sum_{j=0}^k e_j <= 1
    // hold are selected. Non-coarsenable cells (in the irregular
    // coarsening group 0) are ignored.
    //
    // WARNING: Despite the fact that in principle we can compute the
    // projection error exactly, the value we obtain is just an
    // estimate. This is not only due to the heuristic suboptimal
    // selection of C, but also due to the fact that the coarsening
    // errors are modeled strictly locally. In conforming meshes, mesh
    // adaptation can lead to nonlocal effects, e.g. the removal of no
    // longer needed green closures (the introduced error is not taken
    // into account), the introduction of green closures on newly
    // coarsened cells (in which case the error could be below our
    // estimate, depending on the actual transfer - currently the
    // transfer is suboptimal and our error estimate should be exact),
    // or a cell group can actually be retained for mesh topology
    // reasons even if marked for coarsening (in which case the actual
    // local error would be zero).
    std::vector<std::pair<double,size_t> > e(coarseningGroupCount);
    for (int j=0; j<coarseningGroupCount; ++j) {
      e[j].second = j+1;                            // omit the irregular coarsening group 0
      for (int i=0; i<varDesc.noOfVariables; ++i)
        e[j].first = std::max(e[j].first,sum.sums[j+1][i]);
    }

    std::sort(e.begin(),e.end(),FirstLess());

    std::vector<size_t> selectedCoarseningGroups;
    double totalError = 0;
    for (int i=0; i<e.size(); ++i) {
      totalError += e[i].first;
      if (totalError<=1)
        selectedCoarseningGroups.push_back(e[i].second);
      else
        break;
    }

    // for compression: need to keep track of coarsening history
    coarsenedCells.clear() ;
    coarsenedCells.resize( indexSet.size(0), false ) ;
    // no coarsening: just stop here
    //   return ;

    // If no cell can be coarsened, we don't need to touch the grid.
    if (selectedCoarseningGroups.empty())
      return;

    // Now we sweep over the whole grid and mark all cells for
    // coarsening which belong to a selected coarsening group.
    std::sort(selectedCoarseningGroups.begin(),selectedCoarseningGroups.end());
    for (CellIterator ci=gridManager.grid().template leafbegin<0>(); ci!=gridManager.grid().template leafend<0>(); ++ci)
    {
      auto idx = indexSet.index(*ci);
      if (ci->level()>minRefLevel && std::binary_search(selectedCoarseningGroups.begin(),selectedCoarseningGroups.end(),
                                                        coarseningGroup[idx])) // for uniform refinement: just delete level
      {
        coarsenedCells[idx] = true ; // for compression: coarsening history
        gridManager.mark(-1,*ci);
      }
    }

//     std::cerr << "Coarsening part II time: " << timer.format();
//     timer.start();

    gridManager.adaptAtOnce();

//     std::cerr << "Coarsening part III time: " << timer.format();
  }
} /* end of namespace Kaskade */

#endif


#include <cstdio>
#include <cstdlib>
#include <cmath>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/stepper/dense_output_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include "ch_ethz_bhepp_ode_boost_FixedBoostOdeintSolver.h"

#include "Utilities.hpp"

using namespace boost::numeric::odeint;

typedef boost::numeric::ublas::vector<double> vector_type;
typedef vector_type state_type;
typedef result_of::make_dense_output<
    runge_kutta_dopri5< state_type > >::type dense_dopri5_stepper_type;

//typedef int (*CALLBACK_F_PTR)(double t, N_Vector y, N_Vector ydot, void* user_data);
//typedef int (*CALLBACK_G_PTR)(double t, N_Vector y, double* gout, void* user_data);

struct ode_system;

struct boost_solver_data {
//	dense_output_runge_kutta< runge_kutta_dopri5< state_type > > stepper;
//	runge_kutta_dopri5< state_type > stepper;
	ode_system* system;
	dense_dopri5_stepper_type stepper;
    JNIEnv* env;
    jobject ode;
    jmethodID vectorFieldMethodID;
    jdouble stepSize;
    jdouble t;
    jdouble t1;
    jdouble* x;
    jdouble* xTmp;
    jdouble* xDot;
    state_type y;
    state_type yDot;
};

struct ode_system {

    boost_solver_data* data;

	void operator()(const state_type &x, state_type &dxdt, double t )
    {
	    JNIEnv *env = data->env;
//	    for (state_type::const_iterator it=x.begin(); it != x.end(); ++it) {
		for (int i=0; i < x.size(); i++)
	    	data->xTmp[i] = x[i];
	    env->CallVoidMethod(data->ode, data->vectorFieldMethodID, t);
	    if (env->ExceptionCheck() == JNI_TRUE) {
	        fprintf(stderr, "f_direct_bridge: Failed to call computeVectorField!\n");
	        return;
	    }
//	    for (state_type::const_iterator it=x.begin(); it != x.end(); ++it)
		for (int i=0; i < dxdt.size(); i++)
	    	dxdt[i] = data->xDot[i];
    }

};

struct ode_system_wrapper {

	ode_system* system;

	ode_system_wrapper(ode_system* system)
		: system(system) { }
	void operator()(const state_type &x, state_type &dxdt, double t ) {
		system->operator ()(x, dxdt, t);
	}

};

/*int jac_bridge(long int N, double t, N Vector y, N Vector fy, DlsMat Jac,
               void *user data, N Vector tmp1, N Vector tmp2, N Vector tmp3) {
    return 0;
}*/

JNIEXPORT jlong JNICALL Java_ch_ethz_bhepp_ode_boost_FixedBoostOdeintSolver_jni_1initialize
	(JNIEnv *env, jobject obj, jobject ode, jobject xBuffer, jobject xTmpBuffer, jobject xDotBuffer,
			jdouble stepSize, jdouble relTol, jdouble absTol, jint stepperType) {
    jclass odeCls = env->GetObjectClass(ode);
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to get class of ode object\n");
        return 0;
    }
    jmethodID methodID = env->GetMethodID(odeCls, "getDimensionOfVectorField", "()I");
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to get method ID of getDimensionOfVectorField\n");
        return 0;
    }
    jint jdimension = env->CallIntMethod(ode, methodID);
    state_type::size_type dimension = jdimension;
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to call getDimensionOfVectorField\n");
        return 0;
    }

    // Allocate and initialize boost_solver_data
    boost_solver_data* data = new boost_solver_data();
    // FIXME
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to allocate boost_solver_data\n");
        return 0;
    }
    data->stepSize = stepSize;

    data->env = env;
    // Acquire method IDs of callbacks and put them into boost_solver_data
    data->ode = env->NewGlobalRef(ode);
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to create global reference to ode object\n");
        delete data;
        return 0;
    }
    data->vectorFieldMethodID = env->GetMethodID(odeCls, "computeVectorField", "(D)V");
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to get method ID of computeVectorField\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }

    // Check capacities of direct buffer objects
    if (env->GetDirectBufferCapacity(xBuffer) < dimension) {
        throw_java_exception(env, "jni_initialize: Direct buffer xBuffer is not big enough\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    if (env->GetDirectBufferCapacity(xTmpBuffer) < dimension) {
        throw_java_exception(env, "jni_initialize: Direct buffer xTmpBuffer is not big enough\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    if (env->GetDirectBufferCapacity(xDotBuffer) < dimension) {
        throw_java_exception(env, "jni_initialize: Direct buffer xDotBuffer is not big enough\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    // Get addresses of direct buffer objects
    data->x = static_cast<jdouble*>(env->GetDirectBufferAddress(xBuffer));
    if (data->x == NULL) {
        fprintf(stderr, "jni_initialize: Failed to get direct address of xBuffer\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    data->xTmp = static_cast<jdouble*>(env->GetDirectBufferAddress(xTmpBuffer));
    if (data->xTmp == NULL) {
        fprintf(stderr, "jni_initialize: Failed to get direct address of xTmpBuffer\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    data->xDot = static_cast<jdouble*>(env->GetDirectBufferAddress(xDotBuffer));
    if (data->xDot == NULL) {
        fprintf(stderr, "jni_initialize: Failed to get direct address of xDotBuffer\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }

    data->y.resize(dimension, false);
    data->yDot.resize(dimension, false);

    data->system = new ode_system;
    data->system->data = data;

    // Pass pointer-address of boost_solver_data back to Java
    jlong jni_pointer = (jlong)data;
    return jni_pointer;
}

JNIEXPORT void JNICALL Java_ch_ethz_bhepp_ode_boost_FixedBoostOdeintSolver_jni_1dispose
  (JNIEnv *env, jobject obj, jlong jni_pointer) {
    boost_solver_data* data = (boost_solver_data*)jni_pointer;
    if (data != NULL) {
        // Delete global java references
        if (data->ode != NULL)
            env->DeleteGlobalRef(data->ode);
        // Free data structure
        delete data;
    }
}

JNIEXPORT void JNICALL Java_ch_ethz_bhepp_ode_boost_FixedBoostOdeintSolver_jni_1prepareStep
	(JNIEnv *env, jobject obj, jlong jni_pointer, jdouble t0, jdouble t1) {
    boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
    data->t = t0;
    data->t1 = t1;
    for (int i=0; i < data->y.size(); i++)
    	data->y[i] = data->x[i];
    data->stepper.initialize(data->y, data->t, data->stepSize);
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_FixedBoostOdeintSolver
 * Method:    jni_integrateOneStep
 * Signature: (J)D
 */
JNIEXPORT jdouble JNICALL Java_ch_ethz_bhepp_ode_boost_FixedBoostOdeintSolver_jni_1integrateOneStep
(JNIEnv *env, jobject obj, jlong jni_pointer) {
    boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
    std::pair< double , double > times = data->stepper.do_step(ode_system_wrapper(data->system));
    const dense_dopri5_stepper_type::state_type& current_state = data->stepper.current_state();
    for (int i=0; i < data->y.size(); i++)
    	data->x[i] = current_state[i];
    return times.second;
}


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

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

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

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_BUFFER_HPP
#define BOOST_GEOMETRY_ALGORITHMS_BUFFER_HPP

#include <cstddef>

#include <boost/numeric/conversion/cast.hpp>

#include <boost/range.hpp>

#include <boost/variant/apply_visitor.hpp>
#include <boost/variant/static_visitor.hpp>
#include <boost/variant/variant_fwd.hpp>

#include <boost/geometry/algorithms/clear.hpp>
#include <boost/geometry/algorithms/envelope.hpp>
#include <boost/geometry/algorithms/is_empty.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/algorithms/detail/buffer/buffer_inserter.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace buffer
{

template <typename BoxIn, typename BoxOut, typename T, std::size_t C, std::size_t D, std::size_t N>
struct box_loop
{
    typedef typename coordinate_type<BoxOut>::type coordinate_type;

    static inline void apply(BoxIn const& box_in, T const& distance, BoxOut& box_out)
    {
        coordinate_type d = distance;
        set<C, D>(box_out, get<C, D>(box_in) + d);
        box_loop<BoxIn, BoxOut, T, C, D + 1, N>::apply(box_in, distance, box_out);
    }
};

template <typename BoxIn, typename BoxOut, typename T, std::size_t C, std::size_t N>
struct box_loop<BoxIn, BoxOut, T, C, N, N>
{
    static inline void apply(BoxIn const&, T const&, BoxOut&) {}
};

// Extends a box with the same amount in all directions
template<typename BoxIn, typename BoxOut, typename T>
inline void buffer_box(BoxIn const& box_in, T const& distance, BoxOut& box_out)
{
    assert_dimension_equal<BoxIn, BoxOut>();

    static const std::size_t N = dimension<BoxIn>::value;

    box_loop<BoxIn, BoxOut, T, min_corner, 0, N>::apply(box_in, -distance, box_out);
    box_loop<BoxIn, BoxOut, T, max_corner, 0, N>::apply(box_in, distance, box_out);
}

}} // namespace detail::buffer
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Input,
    typename Output,
    typename TagIn = typename tag<Input>::type,
    typename TagOut = typename tag<Output>::type
>
struct buffer: not_implemented<TagIn, TagOut>
{};

template <typename BoxIn, typename BoxOut>
struct buffer<BoxIn, BoxOut, box_tag, box_tag>
{
    template <typename Distance>
    static inline void apply(BoxIn const& box_in, Distance const& distance,
                Distance const& , BoxOut& box_out)
    {
        detail::buffer::buffer_box(box_in, distance, box_out);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace resolve_variant {

template <typename Geometry>
struct buffer
{
    template <typename Distance, typename GeometryOut>
    static inline void apply(Geometry const& geometry,
                             Distance const& distance,
                             Distance const& chord_length,
                             GeometryOut& out)
    {
        dispatch::buffer<Geometry, GeometryOut>::apply(geometry, distance, chord_length, out);
    }
};

template <BOOST_VARIANT_ENUM_PARAMS(typename T)>
struct buffer<boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> >
{
    template <typename Distance, typename GeometryOut>
    struct visitor: boost::static_visitor<void>
    {
        Distance const& m_distance;
        Distance const& m_chord_length;
        GeometryOut& m_out;

        visitor(Distance const& distance,
                Distance const& chord_length,
                GeometryOut& out)
        : m_distance(distance),
          m_chord_length(chord_length),
          m_out(out)
        {}

        template <typename Geometry>
        void operator()(Geometry const& geometry) const
        {
            buffer<Geometry>::apply(geometry, m_distance, m_chord_length, m_out);
        }
    };

    template <typename Distance, typename GeometryOut>
    static inline void apply(
        boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> const& geometry,
        Distance const& distance,
        Distance const& chord_length,
        GeometryOut& out
    )
    {
        boost::apply_visitor(visitor<Distance, GeometryOut>(distance, chord_length, out), geometry);
    }
};

} // namespace resolve_variant

/*!
\brief \brief_calc{buffer}
\ingroup buffer
\details \details_calc{buffer, \det_buffer}.
\tparam Input \tparam_geometry
\tparam Output \tparam_geometry
\tparam Distance \tparam_numeric
\param geometry_in \param_geometry
\param geometry_out \param_geometry
\param distance The distance to be used for the buffer
\param chord_length (optional) The length of the chord's in the generated arcs around points or bends

\qbk{[include reference/algorithms/buffer.qbk]}
 */
template <typename Input, typename Output, typename Distance>
inline void buffer(Input const& geometry_in, Output& geometry_out,
            Distance const& distance, Distance const& chord_length = -1)
{
    concepts::check<Input const>();
    concepts::check<Output>();

    resolve_variant::buffer<Input>::apply(geometry_in, distance, chord_length, geometry_out);
}

/*!
\brief \brief_calc{buffer}
\ingroup buffer
\details \details_calc{return_buffer, \det_buffer}. \details_return{buffer}.
\tparam Input \tparam_geometry
\tparam Output \tparam_geometry
\tparam Distance \tparam_numeric
\param geometry \param_geometry
\param distance The distance to be used for the buffer
\param chord_length (optional) The length of the chord's in the generated arcs
    around points or bends (RESERVED, NOT YET USED)
\return \return_calc{buffer}
 */
template <typename Output, typename Input, typename Distance>
Output return_buffer(Input const& geometry, Distance const& distance, Distance const& chord_length = -1)
{
    concepts::check<Input const>();
    concepts::check<Output>();

    Output geometry_out;

    resolve_variant::buffer<Input>::apply(geometry, distance, chord_length, geometry_out);

    return geometry_out;
}

/*!
\brief \brief_calc{buffer}
\ingroup buffer
\details \details_calc{buffer, \det_buffer}.
\tparam GeometryIn \tparam_geometry
\tparam MultiPolygon \tparam_geometry{MultiPolygon}
\tparam DistanceStrategy A strategy defining distance (or radius)
\tparam SideStrategy A strategy defining creation along sides
\tparam JoinStrategy A strategy defining creation around convex corners
\tparam EndStrategy A strategy defining creation at linestring ends
\tparam PointStrategy A strategy defining creation around points
\param geometry_in \param_geometry
\param geometry_out output multi polygon (or std:: collection of polygons),
    will contain a buffered version of the input geometry
\param distance_strategy The distance strategy to be used
\param side_strategy The side strategy to be used
\param join_strategy The join strategy to be used
\param end_strategy The end strategy to be used
\param point_strategy The point strategy to be used

\qbk{distinguish,with strategies}
\qbk{[include reference/algorithms/buffer_with_strategies.qbk]}
 */
template
<
    typename GeometryIn,
    typename MultiPolygon,
    typename DistanceStrategy,
    typename SideStrategy,
    typename JoinStrategy,
    typename EndStrategy,
    typename PointStrategy
>
inline void buffer(GeometryIn const& geometry_in,
                MultiPolygon& geometry_out,
                DistanceStrategy const& distance_strategy,
                SideStrategy const& side_strategy,
                JoinStrategy const& join_strategy,
                EndStrategy const& end_strategy,
                PointStrategy const& point_strategy)
{
    typedef typename boost::range_value<MultiPolygon>::type polygon_type;
    concepts::check<GeometryIn const>();
    concepts::check<polygon_type>();

    typedef typename point_type<GeometryIn>::type point_type;
    typedef typename rescale_policy_type<point_type>::type rescale_policy_type;

    geometry_out.clear();

    if (geometry::is_empty(geometry_in))
    {
        // Then output geometry is kept empty as well
        return;
    }

    model::box<point_type> box;
    geometry::envelope(geometry_in, box);
    geometry::buffer(box, box, distance_strategy.max_distance(join_strategy, end_strategy));

    typename strategy::intersection::services::default_strategy
        <
            typename cs_tag<GeometryIn>::type
        >::type intersection_strategy;

    rescale_policy_type rescale_policy
            = boost::geometry::get_rescale_policy<rescale_policy_type>(box);

    detail::buffer::buffer_inserter<polygon_type>(geometry_in, range::back_inserter(geometry_out),
                distance_strategy,
                side_strategy,
                join_strategy,
                end_strategy,
                point_strategy,
                intersection_strategy,
                rescale_policy);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_BUFFER_HPP


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

/// \file StereoSessionIsis.cc
///

// Vision Workbench
#include <vw/FileIO.h>
#include <vw/Math/Functors.h>
#include <vw/Math/Geometry.h>
#include <vw/Math/RANSAC.h>
#include <vw/InterestPoint.h>
#include <vw/Stereo/DisparityMap.h>
#include <vw/Cartography.h>

// Stereo Pipeline
#include <asp/Sessions/ISIS/StereoSessionIsis.h>
#include <asp/IsisIO/IsisCameraModel.h>
#include <asp/Core/StereoSettings.h>
#include <asp/IsisIO/IsisAdjustCameraModel.h>
#include <asp/IsisIO/DiskImageResourceIsis.h>
#include <asp/Sessions/ISIS/PhotometricOutlier.h>

// Boost
#include <boost/filesystem/operations.hpp>
#include <boost/shared_ptr.hpp>
namespace fs = boost::filesystem;

#include <algorithm>

using namespace vw;
using namespace vw::camera;
using namespace asp;

// Allows FileIO to correctly read/write these pixel types
namespace vw {
  template<> struct PixelFormatID<Vector3>   { static const PixelFormatEnum value = VW_PIXEL_GENERIC_3_CHANNEL; };
}

// Process a single ISIS image to find an ideal min max. The reason we
// need to do this, is that ASP is to get image intensity values in
// the range of 0-1. To some extent we are compressing the dynamic
// range, but we try to minimize that.
void find_ideal_isis_range( std::string const& in_file,
                            std::string const& tag,
                            float & isis_lo, float & isis_hi ) {

  boost::shared_ptr<DiskImageResourceIsis> isis_rsrc( new DiskImageResourceIsis(in_file) );
  DiskImageView<PixelGray<float> > disk_image(isis_rsrc);

  float isis_mean, isis_std;

  // Calculating statistics. We subsample the images so statistics
  // only does about a million samples.
  {
    vw_out(InfoMessage) << "\t--> Computing statistics for the "+tag+" image\n";
    int left_stat_scale = int(ceil(sqrt(float(disk_image.cols())*float(disk_image.rows()) / 1000000)));
    ChannelAccumulator<math::CDFAccumulator<float> > accumulator;
    for_each_pixel(
      subsample(create_mask( edge_extend(disk_image, ConstantEdgeExtension()),
                             isis_rsrc->valid_minimum(),
                             isis_rsrc->valid_maximum() ),
                left_stat_scale ),
      accumulator );
    isis_lo = accumulator.quantile(0);
    isis_hi = accumulator.quantile(1);
    isis_mean = accumulator.approximate_mean();
    isis_std  = accumulator.approximate_stddev();

    vw_out(InfoMessage) << "\t  "+tag+": [ lo:" << isis_lo << " hi:" << isis_hi
                        << " m: " << isis_mean << " s: " << isis_std <<  "]\n";
  }

  // Normalizing to -+2 sigmas around mean
  if ( stereo_settings().force_max_min == 0 ) {
    vw_out(InfoMessage) << "\t--> Adjusting hi and lo to -+2 sigmas around mean.\n";

    if ( isis_lo < isis_mean - 2*isis_std )
      isis_lo = isis_mean - 2*isis_std;
    if ( isis_hi > isis_mean + 2*isis_std )
      isis_hi = isis_mean + 2*isis_std;

    vw_out(InfoMessage) << "\t    "+tag+" changed: [ lo:"
                        << isis_lo << " hi:" << isis_hi << "]\n";
  }
}

// This actually modifies and writes the pre-processed image.
void write_preprocessed_isis_image( BaseOptions const& opt,
                                    std::string const& in_file,
                                    std::string const& out_file,
                                    std::string const& tag,
                                    float isis_lo, float isis_hi,
                                    float out_lo, float out_hi,
                                    Matrix<double> const& matrix,
                                    Vector2i const& crop_size ) {
  DiskImageView<PixelGray<float> > disk_image(in_file);
  ImageViewRef<PixelGray<float> > applied_image;
  if ( matrix == math::identity_matrix<3>() ) {
    applied_image =
      crop(edge_extend(clamp(normalize(remove_isis_special_pixels(disk_image,
                                                                  isis_lo, isis_hi, out_lo),
                           out_lo, out_hi, 0.0, 1.0)),
                       ZeroEdgeExtension()),
           0, 0, crop_size[0], crop_size[1]);
  } else {
    applied_image =
      transform(clamp(normalize(remove_isis_special_pixels(disk_image,
                                                           isis_lo, isis_hi,
                                                           out_lo),
                                out_lo, out_hi, 0.0, 1.0)),
                HomographyTransform(matrix),
                crop_size[0], crop_size[1]);
  }

  // Write the results to disk.
  vw_out() << "\t--> Writing normalized images.\n";
  block_write_gdal_image( out_file, applied_image, opt,
                          TerminalProgressCallback("asp", "\t  "+tag+":  ") );
}

void
asp::StereoSessionIsis::pre_preprocessing_hook(std::string const& input_file1,
                                               std::string const& input_file2,
                                               std::string & output_file1,
                                               std::string & output_file2) {
  output_file1 = m_out_prefix + "-L.tif";
  output_file2 = m_out_prefix + "-R.tif";

  if ( fs::exists(output_file1) && fs::exists(output_file2) ) {
    try {
      vw_log().console_log().rule_set().add_rule(-1,"fileio");
      DiskImageView<PixelGray<float32> > out1(output_file1);
      DiskImageView<PixelGray<float32> > out2(output_file2);
      vw_out(InfoMessage) << "\t--> Using cached normalized input images.\n";
      vw_settings().reload_config();
      return;
    } catch (vw::ArgumentErr const& e) {
      // This throws on a corrupted file.
      vw_settings().reload_config();
    } catch (vw::IOErr const& e) {
      vw_settings().reload_config();
    }
  }

  float left_lo, left_hi, right_lo, right_hi;
  find_ideal_isis_range( input_file1, "left",
                         left_lo, left_hi );
  find_ideal_isis_range( input_file2, "right",
                         right_lo, right_hi );

  // Working out alignment
  float lo = std::min (left_lo, right_lo);  // Finding global
  float hi = std::max (left_hi, right_hi);
  Matrix<double> align_matrix(3,3);
  align_matrix.set_identity();
  if ( stereo_settings().keypoint_alignment) {
    DiskImageView<PixelGray<float> > left_disk_image(input_file1);
    DiskImageView<PixelGray<float> > right_disk_image(input_file2);
    ImageViewRef<PixelGray<float> > left_view =
      normalize(remove_isis_special_pixels(left_disk_image, lo),
                lo, hi, 0, 1.0);
    ImageViewRef<PixelGray<float> > right_view =
      normalize(remove_isis_special_pixels(right_disk_image, lo),
                lo, hi, 0, 1.0);
    align_matrix = determine_image_align(input_file1, input_file2,
                                         left_view, right_view );
  }
  write_matrix( m_out_prefix + "-align.exr", align_matrix );

  // Getting left image size
  Vector2i left_size;
  {
    DiskImageView<PixelGray<float> > left_image(input_file1);
    left_size = Vector2i(left_image.cols(), left_image.rows());
  }

  // Apply alignment and normalization
  if (stereo_settings().individually_normalize == 0 ) {
    vw_out() << "\t--> Normalizing globally to: ["<<lo<<" "<<hi<<"]\n";
    write_preprocessed_isis_image( m_options, input_file1, output_file1, "left",
                                   left_lo, left_hi, lo, hi,
                                   math::identity_matrix<3>(), left_size );
    write_preprocessed_isis_image( m_options, input_file2, output_file2, "right",
                                   right_lo, right_hi, lo, hi,
                                   align_matrix, left_size );
  } else {
    vw_out() << "\t--> Individually normalizing.\n";
    write_preprocessed_isis_image( m_options, input_file1, output_file1, "left",
                                   left_lo, left_hi, left_lo, left_hi,
                                   math::identity_matrix<3>(), left_size );
    write_preprocessed_isis_image( m_options, input_file2, output_file2, "right",
                                   right_lo, right_hi, right_lo, right_hi,
                                   align_matrix, left_size );
  }
}

inline std::string write_shadow_mask( BaseOptions const& opt,
                                      std::string const& output_prefix,
                                      std::string const& input_image,
                                      std::string const& mask_postfix ) {
  // This thresholds at -25000 as the input sub4s for Apollo that I've
  // processed have a range somewhere between -32000 and +32000. -ZMM
  DiskImageView<PixelGray<float> > disk_image( input_image );
  DiskImageView<uint8> disk_mask( output_prefix + mask_postfix );
  ImageViewRef<uint8> mask =
    apply_mask(intersect_mask(create_mask(disk_mask),
                              create_mask(threshold(disk_image,-25000,0,1.0))));
  std::string output_mask =
    output_prefix+mask_postfix.substr(0,mask_postfix.size()-4)+"Debug.tif";

  block_write_gdal_image( output_mask, mask, opt,
                          TerminalProgressCallback("asp","\t  Shadow:") );
  return output_mask;
}

// Stage 2: Correlation
//
// Pre file is a pair of grayscale images.  ( ImageView<PixelGray<float> > )
// Post file is a disparity map.            ( ImageView<PixelMask<Vector2f> > )
void
asp::StereoSessionIsis::pre_filtering_hook(std::string const& input_file,
                                           std::string & output_file) {
  output_file = input_file;

  // ****************************************************
  // The following code is for Apollo Metric Camera ONLY!
  // (use at your own risk)
  // ****************************************************
  if (stereo_settings().mask_flatfield) {
    vw_out() << "\t--> Masking pixels that are less than 0.0.  (NOTE: Use this option with Apollo Metric Camera frames only!)\n";
    output_file = m_out_prefix + "-R-masked.exr";

    std::string shadowLmask_name =
      write_shadow_mask( m_options, m_out_prefix, m_left_image_file,
                         "-lMask.tif" );
    std::string shadowRmask_name =
      write_shadow_mask( m_options, m_out_prefix, m_right_image_file,
                         "-rMask.tif" );

    DiskImageView<uint8> shadowLmask( shadowLmask_name );
    DiskImageView<uint8> shadowRmask( shadowRmask_name );

    DiskImageView<PixelMask<Vector2f> > disparity_disk_image(input_file);
    ImageViewRef<PixelMask<Vector2f> > disparity_map =
      stereo::disparity_mask(disparity_disk_image,
                             shadowLmask, shadowRmask );

    DiskImageResourceOpenEXR disparity_map_rsrc(output_file, disparity_map.format() );
    Vector2i block_size(std::min<size_t>(vw_settings().default_tile_size(),
                                         disparity_map.cols()),
                        std::min<size_t>(vw_settings().default_tile_size(),
                                         disparity_map.rows()));
    disparity_map_rsrc.set_block_write_size(block_size);
    block_write_image( disparity_map_rsrc, disparity_map,
                       TerminalProgressCallback( "asp", "\t--> Saving Mask :") );
  }
}

// Reverse any pre-alignment that was done to the images.
void
asp::StereoSessionIsis::pre_pointcloud_hook(std::string const& input_file,
                                            std::string & output_file) {

  // ****************************************************
  // The following code is for Apollo Metric Camera ONLY!
  // (use at your own risk)
  // ****************************************************
  std::string dust_result = input_file;
  if ( stereo_settings().mask_flatfield ) {
    vw_out() << "\t--> Masking pixels that appear to be dust.  (NOTE: Use this option with Apollo Metric Camera frames only!)\n";
    photometric_outlier_rejection( m_out_prefix, input_file,
                                   dust_result, stereo_settings().h_kern );
  }

  DiskImageView<PixelMask<Vector2f> > disparity_map(dust_result);
  output_file = m_out_prefix + "-F-corrected.tif";

  // We used a homography to line up the images, we may want
  // to generate pre-alignment disparities before passing this information
  // onto the camera model in the next stage of the stereo pipeline.
  Matrix<double> align_matrix;
  try {
    read_matrix(align_matrix, m_out_prefix + "-align.exr");
    vw_out(DebugMessage) << "Alignment Matrix: " << align_matrix << "\n";
  } catch (vw::IOErr const& e) {
    vw_out() << "\nCould not read in aligment matrix: " << m_out_prefix
             << "-align.exr.  Exiting. \n\n";
    exit(1);
  }

  // Remove pixels that are outside the bounds of the secondary image.
  DiskImageView<PixelGray<float> > right_disk_image(m_right_image_file);
  ImageViewRef<PixelMask<Vector2f> > result =
    stereo::disparity_range_mask(stereo::transform_disparities(disparity_map,
                                          HomographyTransform(align_matrix)),
                                 Vector2f(0,0),
                                 Vector2f( right_disk_image.cols(),
                                           right_disk_image.rows() ) );

  block_write_gdal_image( output_file, result, m_options,
                          TerminalProgressCallback("asp", "\t    Processing:") );
}

boost::shared_ptr<vw::camera::CameraModel>
asp::StereoSessionIsis::camera_model(std::string const& image_file,
                                     std::string const& camera_file) {

  if (boost::ends_with(boost::to_lower_copy(camera_file), ".isis_adjust")){
    // Creating Equations for the files
    std::ifstream input( camera_file.c_str() );
    boost::shared_ptr<asp::BaseEquation> posF = read_equation( input );
    boost::shared_ptr<asp::BaseEquation> poseF = read_equation( input );
    input.close();

    // Finally creating camera model
    return boost::shared_ptr<camera::CameraModel>(new IsisAdjustCameraModel( image_file, posF, poseF ));

  } else {
    return boost::shared_ptr<camera::CameraModel>(new IsisCameraModel(image_file));
  }

}


/**
 * icn-360 - 360 degree video streaming producer/consumer on NDN
 *
 * Copyright (c) 2019 KDDI Research, Inc.
 *
 * This software is released under the MIT License.
 * http://opensource.org/licenses/mit-license.php
 */
#include <unistd.h>
#include <sys/types.h>
#include <pwd.h>

#include <ndn-cxx/security/signing-helpers.hpp>
#include <ndn-cxx/security/signing-info.hpp>
#include <ndn-cxx/security/tpm/tpm.hpp>
#include <ndn-cxx/encoding/buffer-stream.hpp>
#include <ndn-cxx/encoding/buffer.hpp>
#include <ndn-cxx/security/transform/hmac-filter.hpp>
#include <ndn-cxx/security/transform/buffer-source.hpp>
#include <ndn-cxx/security/transform/stream-sink.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>

#include <iostream>
#include "Signature.h"

static const uint8_t HMAC_KEY[] = {
  0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
  0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f
};

Signature::Signature(const char* prefix)
    : sign_type_(ParametersProducer::signatureType())
{
    has_key_ = sign_type_.hasType(SignatureType::RSA) || sign_type_.hasType(SignatureType::ECDSA);
    if(has_key_){
        identity_ = key_chain_.createIdentity(prefix);
        if(sign_type_.hasType(SignatureType::RSA)){
            key_ = key_chain_.createKey(identity_, ndn::RsaKeyParams(sign_type_.keySize()));
        }else if(sign_type_.hasType(SignatureType::ECDSA)){
            key_ = key_chain_.createKey(identity_, ndn::EcKeyParams(sign_type_.keySize()));
        }
        signature_info_ = ndn::security::signingByKey(key_);
    }else if(sign_type_ == SignatureType::SHA_256){
        signature_info_ = ndn::security::signingWithSha256();
    }
}

Signature::~Signature()
{
    if(has_key_){
        key_chain_.deleteKey(identity_, key_);
        key_chain_.deleteIdentity(identity_);
    }
}

void
Signature::sign(ndn::Data& data)
{
    // Digest
    if(sign_type_ == SignatureType::NO_SIGN){
        return;
    }else{
        key_chain_.sign(data, signature_info_);
    }
}

void
Signature::signWithHmac(ndn::Data& data)
{
    data.setSignature(ndn::Signature(signature_info_.getSignatureInfo()));
    ndn::OBufferStream os;

    ndn::EncodingBuffer encoder;
    data.wireEncode(encoder, true);

    ndn::security::transform::bufferSource(encoder.buf(), encoder.size()) >>
        ndn::security::transform::hmacFilter(ndn::DigestAlgorithm::SHA256, HMAC_KEY, sizeof(HMAC_KEY)) >>
        ndn::security::transform::streamSink(os);

    data.wireEncode(encoder, ndn::Block(ndn::tlv::SignatureValue, os.buf()));
}


//
//  equality.hpp
//  solver
//
//  Created by Alexandre Martin on 13/05/2016.
//  Copyright © 2016 scalexm. All rights reserved.
//

#ifndef EQUALITY_HPP
#define EQUALITY_HPP

#include "curry.hpp"
#include <boost/functional/hash/hash.hpp>
#include <vector>
#include <unordered_map>

class equality_solver {
private:
    struct dummy { };
    using edge = boost::variant<const_equality, std::pair<curry_equality, curry_equality>, dummy>;

    struct record {
        std::vector<std::vector<curry_equality>> use_lists;

        std::unordered_map<
            std::pair<int, int>,
            curry_equality,
            boost::hash<std::pair<int, int>>
        > lookup_table;

        std::vector<int> representative;
        std::vector<std::vector<int>> class_lists;
        std::vector<std::unordered_set<int>> disequalities;
        std::vector<std::pair<int, edge>> proof_forest;

        record(int max_const) : representative(max_const),
                                class_lists(max_const),
                                use_lists(max_const),
                                disequalities(max_const),
                                proof_forest(max_const, std::make_pair(-1, dummy { })) { }

        record(const record &) = default;
    };

    std::vector<edge> m_pending;
    std::vector<curry_atom> m_atoms;
    std::vector<record> m_db;

    bool propagate(std::vector<int> &);
public:
    equality_solver(int, std::vector<curry_atom>);

    std::pair<std::vector<int>, bool> set_true(int);
    void backtrack_one(int);
};

#endif


#pragma once

#include <crest/util/timer.hpp>
#include <crest/util/eigen_extensions.hpp>
#include <crest/basis/basis.hpp>
#include <crest/wave/constraints.hpp>
#include <crest/wave/integrators.hpp>
#include <crest/wave/transformers.hpp>
#include <crest/wave/load.hpp>

#include <utility>

#include <Eigen/Sparse>

namespace crest
{
    namespace wave
    {
        namespace detail
        {
            template <typename Scalar>
            InitialConditions<Scalar> constrain_initial_conditions(
                    const ConstrainedSystem<Scalar> & system,
                    const InitialConditions<Scalar> & full)
            {
                InitialConditions<Scalar> constrained;
                constrained.u0_h = system.constrain_solution(full.u0_h);
                constrained.v0_h = system.constrain_velocity(full.v0_h);
                constrained.u0_tt_h = system.constrain_acceleration(full.u0_tt_h);
                return constrained;
            }
        }

        struct SolveTiming {
            double assembly_time;
            double load_time;
            double initializer_time;
            double integrator_setup_time;
            double integrator_solve_time;
            double transform_time;
            double total_time;

            SolveTiming() : assembly_time(NAN),
                            load_time(0.0),
                            initializer_time(0.0),
                            integrator_setup_time(0.0),
                            integrator_solve_time(0.0),
                            transform_time(0.0),
                            total_time(0.0) {}
        };

        template <typename Scalar, typename TransformedResult>
        struct SolveResult
        {
            std::vector<TransformedResult> result;
            SolveTiming timing;
            bool converged;
        };

        template <typename Scalar>
        struct Parameters {
            Scalar dt;
            uint64_t num_samples;
        };

        template <typename Scalar, typename TransformedResult>
        SolveResult<Scalar, TransformedResult>
        solve(const ConstrainedSystem<Scalar> & system,
              const InitialConditions<Scalar> & initial_conditions,
              Integrator<Scalar> & integrator,
              const Initializer<Scalar> & initializer,
              const Parameters<Scalar> & parameters,
              const ResultTransformer<Scalar, TransformedResult> & transformer)
        {
            SolveResult<Scalar, TransformedResult> sol;
            SolveTiming timing;
            Timer total_timer;

            try {
                if (parameters.num_samples == 0) {
                    throw std::invalid_argument("Number of samples must be greater or equal to 1.");
                }

                // TODO: Rewrite constrained system so that it does not take ownership of system matrices?
                const auto constrained_ic = detail::constrain_initial_conditions(system, initial_conditions);

                const auto dt = parameters.dt;
                inspect_timing(timing.integrator_setup_time, [&] {
                    integrator.setup(dt, system.assembly().stiffness, system.assembly().mass);
                });

                VectorX<Scalar> x_prev = constrained_ic.u0_h;
                VectorX<Scalar> x_current = inspect_timing(timing.initializer_time, [&] {
                    return initializer.initialize(constrained_ic, dt);
                });
                VectorX<Scalar> x_next = VectorX<Scalar>(system.num_free_nodes());

                VectorX<Scalar> load_prev = inspect_timing(timing.load_time, [&] {
                    return system.load(Scalar(0));
                });
                VectorX<Scalar> load_current = inspect_timing(timing.load_time, [&] {
                    return system.load(dt);
                });
                VectorX<Scalar> load_next = inspect_timing(timing.load_time, [&] {
                    return system.load(Scalar(2) * dt);
                });

                {
                    const auto expanded = system.expand_solution(Scalar(0), x_prev);
                    const auto transformed = inspect_timing(timing.transform_time, [&] {
                        return transformer.transform(0u, Scalar(0), expanded);
                    });
                    sol.result.emplace_back(std::move(transformed));
                }

                if (parameters.num_samples > 1) {
                    const auto expanded = system.expand_solution(Scalar(dt), x_current);
                    const auto transformed = inspect_timing(timing.transform_time, [&] {
                        return transformer.transform(1u, dt, expanded);
                    });
                    sol.result.emplace_back(std::move(transformed));
                }

                for (uint64_t i = 2; i < parameters.num_samples; ++i)
                {
                    const auto t = Scalar(i) * dt;
                    load_next = inspect_timing(timing.load_time, [&] {
                        return system.load(Scalar(i) * dt);
                    });
                    x_next = inspect_timing(timing.integrator_solve_time, [&] {
                        return integrator.next(i, dt, x_current, x_prev, load_next, load_current, load_prev);
                    });

                    {
                        const auto expanded = system.expand_solution(t, x_next);
                        const auto transformed = inspect_timing(timing.transform_time, [&] {
                            return transformer.transform(i, t, expanded);
                        });
                        sol.result.emplace_back(std::move(transformed));
                    }

                    // TODO: Replace this with Eigen's swap for efficiency? For now keep std::swap until we
                    // have confirmed working code

                    // Shuffle results of previous computations, such that x_next and load_next immediately
                    // get overwritten upon the next iteration
                    std::swap(x_prev, x_current);
                    std::swap(x_current, x_next);
                    std::swap(load_prev, load_current);
                    std::swap(load_current, load_next);
                }

                if (sol.result.size() != parameters.num_samples) {
                    throw std::logic_error("Internal error: result size is not equal to number of samples.");
                }

                sol.converged = true;
            } catch (const ConvergenceError & e) {
                sol.converged = false;
            }

            timing.total_time = total_timer.elapsed();
            sol.timing = timing;

            return sol;
        }
    }
}


#include <soft_body_demo/physics_plugin.h>
#include <gazebo/physics/PhysicsIface.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/Link.hh>
#include <gazebo/physics/World.hh>
#include <gazebo/physics/PhysicsEngine.hh>
#include <Eigen/Geometry>

template <typename T>
inline T read(sdf::ElementPtr sdf, std::string key, T fallback)
{
  if(sdf->HasElement(key))
    return sdf->Get<T>(key);
  return fallback;
}

namespace gazebo
{

void SpringModelPlugin::Load(physics::ModelPtr model, sdf::ElementPtr sdf)
{
  std::cout << "Loading soft body physics" << std::endl;

  ee = model->GetLinks()[0];
  auto base1 = model->GetWorld()->ModelByName("base1")->GetLinks()[0];
  auto base2 = model->GetWorld()->ModelByName("base2")->GetLinks()[0];

  // model params are passed through plugin definition in SDF
  spring_model.initParams(ee->GetWorld()->Physics()->GetMaxStepSize(),  // dt from Gazebo engine
                          read(sdf, "fxy", 10.),
                          read(sdf, "fz", 10.),
                          read(sdf, "mz", 10.),
                          read(sdf, "damping", 0.01),
                          read(sdf, "length", 1.));

  attach = read(sdf, "attach_point", Vector3d());

  // connect update function
  update_event = event::Events::ConnectWorldUpdateBegin(std::bind(&SpringModelPlugin::Update, this));
}

void SpringModelPlugin::Update()
{
  // get pose of end-effector wrt attach point
  auto pose(ee->WorldPose());
  pose.Pos() -= attach;

  // to Eigen pose 3D
  state.updateTranslation(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
  state.updateRotation(pose.Rot().W(), pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z());

  // get absolute velocity
  auto vel = ee->WorldLinearVel();
  state.twist.linear << vel.X(), vel.Y(), vel.Z();
  vel = ee->WorldAngularVel();
  state.twist.angular << vel.X(), vel.Y(), vel.Z();

  // call model to get wrench
  const auto wrench(spring_model.update(state));

  ee->AddForce({wrench.force.x(), wrench.force.y(), wrench.force.z()});
  ee->AddTorque({wrench.torque.x(), wrench.torque.y(), wrench.torque.z()});
}

}


#pragma once
#include <array>
#include "_python.hpp"
#include <string>
#include <functional>
#include <Eigen/Dense>
#include <iomanip>
#include <fstream>
#include <complex>
#include <iostream>
#include <cmath>

/* Docstrings */
static char module_docstring[] =
"pyoscode: this module provides an interface for oscode, for solving oscillatory ordinary differential equations with the RKWKB method.";

static char solve_docstring[] =
"Runs the solver";

/* Available functions */
static PyObject *_pyoscode_solve(PyObject *self, PyObject *args, PyObject *kwargs);

/* Module interface */
static PyMethodDef module_methods[] = {
    {"solve", (PyCFunction) _pyoscode_solve, METH_VARARGS | METH_KEYWORDS, solve_docstring},
    {NULL, NULL, 0, NULL}
};

#ifdef PYTHON3
static struct PyModuleDef _pyoscodemodule = {
    PyModuleDef_HEAD_INIT,
    "_pyoscode",
    module_docstring,
    -1,
    module_methods
};
#endif


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ScalarMultiplyTest

#include <algorithm>
#include <iostream>
#include <numeric>

#include "./codelets/CodeletsTestsCommon.hpp"
#include "poplibs_test/Util.hpp"
#include "popops/ElementWise.hpp"
#include "popops/Expr.hpp"
#include "popops/ScalarMultiply.hpp"
#include "popops/codelets.hpp"
#include "poputil/TileMapping.hpp"
#include "poputil/exceptions.hpp"
#include <boost/cstdfloat.hpp>
#include <boost/multi_array.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/test/unit_test.hpp>
#include <poplar/CSRFunctions.hpp>
#include <poplar/Graph.hpp>
#include <poplar/Program.hpp>
#include <poplar/Tensor.hpp>
#include <poplibs_support/TestDevice.hpp>

using namespace poplar;
using namespace poplar::program;
using namespace poplibs_test::util;
using namespace popops;

std::vector<float> clipData(const std::vector<float> &data,
                            const float &clipValue) {
  std::vector<float> clippedData(data.size());
  for (unsigned i = 0; i < clippedData.size(); i++) {
    clippedData[i] = std::min(data[i], clipValue);
  }
  return clippedData;
}

struct Tensor_ {
  Tensor_() = default;
  Tensor_(const std::vector<float> &data_,
          const std::vector<std::size_t> &shape_, const Type &poplar_type_)
      : data(data_), shape(shape_), poplar_type(poplar_type_) {}

  std::vector<float> data;
  std::vector<std::size_t> shape;
  Type poplar_type;

  static float getFP16ClipValue() { return 65504; }

  std::vector<float> getData(bool clip = true) const {
    if (clip && poplar_type == HALF) {
      return clipData(data, getFP16ClipValue());
    } else {
      return data;
    }
  }
};

Tensor addVariable(Graph &graph, const Type &type,
                   const std::vector<size_t> &shape, const std::string &name,
                   bool singleRegion) {
  unsigned size = std::accumulate(shape.begin(), shape.end(), 0);
  Tensor t;
  if (!singleRegion && size > 1) {
    Tensor t1 = graph.addVariable(type, {size / 2}, name + "1");
    Tensor t2 = graph.addVariable(type, {size - size / 2}, name + "2");
    poputil::mapTensorLinearly(graph, t1);
    poputil::mapTensorLinearly(graph, t2);
    t = concat(t1, t2);
    t = t.reshape(shape);
  } else {
    t = graph.addVariable(type, shape, name);
    poputil::mapTensorLinearly(graph, t);
  }
  return t;
}

struct TestResult {
  TestResult(poputil::poplibs_error error_) { errorMsg = error_.what(); }
  TestResult(Tensor_ c_) { c = c_; }

  std::string errorMsg;
  Tensor_ c;
};

struct Fixture {
  Fixture() {
    namespace po = boost::program_options;
    po::options_description poDesc("Test a specialisation of multiplication "
                                   "between a float scalar and a half tensor.");

    ShapeOption<size_t> aShape;
    ShapeOption<size_t> bShape;
    Type aType;
    Type bType;
    VectorOption<float> aData;
    VectorOption<float> bData;

    poDesc.add_options()                                                    //
        ("a-shape",                                                         //
         po::value<ShapeOption<size_t>>(&aShape)->multitoken()->required(), //
         "The shape of `a` in `a * b` (e.g. `{2,2}`).")                     //
        ("a-type",                                                          //
         po::value<Type>(&aType)->multitoken()->required(),                 //
         "The type of `a` in `a * b` (e.g. `half` or `float`).")            //
        ("a-data",                                                          //
         po::value<VectorOption<float>>(&aData)->multitoken()->required(),  //
         "The data of `a` in `a * b` (e.g. `{1,2,3,4}`).")                  //
        ("b-shape",                                                         //
         po::value<ShapeOption<size_t>>(&bShape)->multitoken()->required(), //
         "The shape of `b` in `a * b` (e.g. `{2,2}`).")                     //
        ("b-type",                                                          //
         po::value<Type>(&bType)->multitoken()->required(),                 //
         "The type of `b` in `a * b` (e.g. `half` or `float`).")            //
        ("b-data",                                                          //
         po::value<VectorOption<float>>(&bData)->multitoken()->required(),  //
         "The data of `b` in `a * b` (e.g. `{1,2,3,4}`).")                  //
        ("inplace",                                                         //
         po::value<bool>(&inplace)->multitoken()->required(),               //
         "Perfoms the operation inplace if `true`.")                        //
        ("device-type",                                                     //
         po::value<DeviceType>(&dt)->required(),                            //
         "Device type.")                                                    //
        ;

    int argc = boost::unit_test::framework::master_test_suite().argc;
    char **argv = boost::unit_test::framework::master_test_suite().argv;
    parseOptions(argc, argv, poDesc);

    a = Tensor_(aData.val, aShape.val, aType);
    b = Tensor_(bData.val, bShape.val, bType);
  };

  bool inplace;
  Tensor_ a;
  Tensor_ b;
  DeviceType dt;
};

enum class BuildGraphUsing {
  Map = 0,
  Codelet,
};

Tensor buildGraphUsingMap(Graph &graph, Tensor &a, Tensor &b, Sequence &prog,
                          bool inplace) {
  if (inplace) {
    mapInPlace(graph, expr::BinaryOpType::MULTIPLY, a, b, prog, "", {});
    return a;
  } else {
    Tensor c = map(graph, expr::BinaryOpType::MULTIPLY, a, b, prog, "", {});
    return c;
  }
}

Tensor buildGraphUsingCodelet(Graph &graph, Tensor &a, Tensor &b,
                              Sequence &prog, bool inplace) {
  poputil::PoplibsOpDebugInfo di("");
  if (inplace) {
    scalarMultiplyInplace(graph, a, b, prog, di);
    return a;
  } else {
    auto c = scalarMultiply(graph, a, b, prog, di);
    return c;
  }
}

TestResult runTest(BuildGraphUsing buildGraphMethod, const Tensor_ &a_,
                   const Tensor_ &b_, const DeviceType &dt, bool inplace) {
  auto device = createTestDevice(dt);
  const auto &target = device.getTarget();

  Graph graph(target);
  popops::addCodelets(graph);

  Sequence uploadProg, prog, downloadProg;

  setFloatingPointBehaviour(graph, prog,
                            {
                                false, // exceptOnInv
                                true,  // exceptOnDiv0
                                false, // exceptOnOflo
                                false, // enableStochasticRounding
                                false, // nanOnOverflow
                            },
                            "");

  Tensor a = addVariable(graph, a_.poplar_type, a_.shape, "a", true);
  Tensor b = addVariable(graph, b_.poplar_type, b_.shape, "b", true);

  Tensor c;
  try {
    switch (buildGraphMethod) {
    case BuildGraphUsing::Map:
      c = buildGraphUsingMap(graph, a, b, prog, inplace);
      break;
    case BuildGraphUsing::Codelet:
      c = buildGraphUsingCodelet(graph, a, b, prog, inplace);
      break;
    default:
      throw std::runtime_error("Invalid `buildGraphMethod`.");
      break;
    }
  } catch (const poputil::poplibs_error &e) {
    return TestResult(e);
  }

  std::vector<std::pair<std::string, char *>> tmap;
  std::vector<float> aHost{a_.getData()};
  std::vector<float> bHost{b_.getData()};
  std::vector<float> cHost(c.numElements());
  auto aRaw = allocateHostMemoryForTensor(a, "a", graph, uploadProg,
                                          downloadProg, tmap);
  auto bRaw = allocateHostMemoryForTensor(b, "b", graph, uploadProg,
                                          downloadProg, tmap);
  std::unique_ptr<char[]> cRaw;
  char *cRawPtr = nullptr;

  if (inplace) {
    cRawPtr = aRaw.get();
  } else {
    cRaw = allocateHostMemoryForTensor(c, "c", graph, uploadProg, downloadProg,
                                       tmap);
    cRawPtr = cRaw.get();
  }

  copy(target, aHost, a.elementType(), aRaw.get());
  copy(target, bHost, b.elementType(), bRaw.get());

  Engine engine(graph, Sequence{uploadProg, prog, downloadProg}, {});
  device.bind([&](const Device &d) {
    engine.load(d);
    attachStreams(engine, tmap);
    engine.run(0);
  });

  copy(target, c.elementType(), cRawPtr, cHost);

  return TestResult(Tensor_(cHost, c.shape(), c.elementType()));
}

void verifyResult(BuildGraphUsing buildGraphMethod, const Tensor_ &a,
                  const Tensor_ &b, bool inplace, const TestResult &result) {

  auto isScalar = [](const Tensor_ &t) {
    return t.shape == std::vector<size_t>{1};
  };

  auto isAnyScalar = [&isScalar](const Tensor_ &t0, const Tensor_ &t1) {
    return isScalar(t0) || isScalar(t1);
  };

  auto isFloatScalar = [&isScalar](const Tensor_ &t) {
    return isScalar(t) && t.poplar_type == FLOAT;
  };

  auto isAnyFloatScalar = [&isFloatScalar](const Tensor_ &t0,
                                           const Tensor_ &t1) {
    return isFloatScalar(t0) || isFloatScalar(t1);
  };

  // Can't have both a valid result and an exception.
  BOOST_TEST((result.errorMsg == "" || result.c.data.size() == 0));

  if (buildGraphMethod == BuildGraphUsing::Map) {
    if ((a.poplar_type != b.poplar_type) &&
        (!(inplace && isFloatScalar(b)) &&
         !(!inplace && isAnyFloatScalar(a, b)))) {
      BOOST_TEST(result.c.data.size() == 0);
      std::string expectedMsg = "Error inferring types in expression:";
      BOOST_TEST(boost::algorithm::starts_with(result.errorMsg, expectedMsg));
      return;
    }
  } else if (buildGraphMethod == BuildGraphUsing::Codelet) {
    auto argsMatchSignature = [&isFloatScalar](const Tensor_ &t0,
                                               const Tensor_ &t1) {
      return t0.poplar_type == poplar::HALF && isFloatScalar(t1);
    };
    if (!((inplace && argsMatchSignature(a, b)) ||
          (!inplace &&
           (argsMatchSignature(a, b) || argsMatchSignature(b, a))))) {
      BOOST_TEST(result.c.data.size() == 0);
      std::string expectedMsg = "Invalid operands of shape and type";
      BOOST_TEST(boost::algorithm::starts_with(result.errorMsg, expectedMsg));
      return;
    }
  }

  // Numerical test
  // --------------
  // Only cases without broadcasting are tested here.
  BOOST_REQUIRE(a.shape == b.shape || isAnyScalar(a, b));

  BOOST_TEST(result.errorMsg == "");

  std::vector<float> expectedData{};
  Type expectedType = a.poplar_type == b.poplar_type ? a.poplar_type : HALF;
  std::vector<std::size_t> expectedShape = isScalar(a) ? b.shape : a.shape;

  auto aData = a.getData();
  auto bData = b.getData();

  if (a.data.size() < b.data.size()) {
    std::swap(aData, bData);
  }

  if (bData.size() == 1) {
    for (unsigned i = 1; i < aData.size(); i++) {
      bData.push_back(bData[0]);
    }
  }

  for (unsigned i = 0; i < aData.size(); i++) {
    float value = aData[i] * bData[i];
    value = expectedType == HALF ? std::min(value, Tensor_::getFP16ClipValue())
                                 : value;
    expectedData.push_back(value);
  }

  BOOST_TEST(expectedData == result.c.data);
  BOOST_TEST(expectedType == result.c.poplar_type);
  BOOST_TEST(expectedShape == result.c.shape);
}

BOOST_FIXTURE_TEST_CASE(IntegrationTest, Fixture) {
  // TODO(T45374): Remove the short circuit below. In addition, cover the extra
  // cases inside verifyResult().
  if (inplace && a.shape == std::vector<std::size_t>{1} &&
      b.shape != std::vector<std::size_t>{1}) {
    return;
  }

  auto result = runTest(BuildGraphUsing::Map, a, b, dt, inplace);
  verifyResult(BuildGraphUsing::Map, a, b, inplace, result);
}

BOOST_FIXTURE_TEST_CASE(GraphBuilderTest, Fixture) {
  auto result = runTest(BuildGraphUsing::Codelet, a, b, dt, inplace);
  verifyResult(BuildGraphUsing::Codelet, a, b, inplace, result);
}


/**
 * @file goal_guided_multivariate_gaussian.cpp
 * @brief This class generates noisy trajectories to an under-constrained cartesian goal pose
 *
 * @author Jorge Nicho
 * @date Jun 14, 2016
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2016, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * 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
 * @par
 * 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 "stomp_plugins/noise_generators/goal_guided_multivariate_gaussian.h"
#include <stomp_moveit/utils/multivariate_gaussian.h>
#include <XmlRpcException.h>
#include <pluginlib/class_list_macros.h>
#include <ros/package.h>
#include <ros/console.h>
#include <boost/filesystem.hpp>
#include <fstream>

PLUGINLIB_EXPORT_CLASS(stomp_moveit::noise_generators::GoalGuidedMultivariateGaussian,
                       stomp_moveit::noise_generators::StompNoiseGenerator);

static int const IK_ITERATIONS = 40;
static double const JOINT_UPDATE_RATE = 0.5f;
static double const CARTESIAN_POS_CONVERGENCE = 0.01;
static double const CARTESIAN_ROT_CONVERGENCE = 0.01;
static const std::vector<double> ACC_MATRIX_DIAGONAL_VALUES = {-1.0/12.0, 16.0/12.0, -30.0/12.0, 16.0/12.0, -1.0/12.0};
static const std::vector<int> ACC_MATRIX_DIAGONAL_INDICES = {-2, -1, 0 ,1, 2};
static const int CARTESIAN_DOF_SIZE = 6;

namespace stomp_moveit
{
namespace noise_generators
{

GoalGuidedMultivariateGaussian::GoalGuidedMultivariateGaussian():
  name_("GoalGuidedMultivariateGaussian"),
  goal_rand_generator_(new RandomGenerator(RGNType(),boost::uniform_real<>(-1,1)))
{

}

GoalGuidedMultivariateGaussian::~GoalGuidedMultivariateGaussian()
{

}

bool GoalGuidedMultivariateGaussian::initialize(moveit::core::RobotModelConstPtr robot_model_ptr,
                        const std::string& group_name,const XmlRpc::XmlRpcValue& config)
{
  using namespace moveit::core;

  group_ = group_name;
  robot_model_ = robot_model_ptr;
  const JointModelGroup* joint_group = robot_model_ptr->getJointModelGroup(group_name);
  if(!joint_group)
  {
    ROS_ERROR("Invalid joint group %s",group_name.c_str());
    return false;
  }

  stddev_.resize(joint_group->getActiveJointModelNames().size());
  goal_stddev_.resize(CARTESIAN_DOF_SIZE);

  // goal kinematics parameters
  kc_.joint_update_rates = JOINT_UPDATE_RATE*Eigen::VectorXd::Ones(stddev_.size());
  kc_.cartesian_convergence_thresholds << CARTESIAN_POS_CONVERGENCE, CARTESIAN_POS_CONVERGENCE, CARTESIAN_POS_CONVERGENCE,
      CARTESIAN_ROT_CONVERGENCE, CARTESIAN_ROT_CONVERGENCE, CARTESIAN_ROT_CONVERGENCE;
  kc_.constrained_dofs << 1, 1, 1, 1, 1, 1;
  kc_.max_iterations = IK_ITERATIONS;

  return configure(config);
}

bool GoalGuidedMultivariateGaussian::configure(const XmlRpc::XmlRpcValue& config)
{
  using namespace XmlRpc;

  try
  {
    XmlRpcValue params = config;

    // noise generation parameters
    XmlRpcValue stddev_param = params["stddev"];
    XmlRpcValue goal_stddev_param = params["goal_stddev"];

    // check  stddev
    if(stddev_param.size() < stddev_.size())
    {
      ROS_ERROR("%s the 'stddev' parameter has fewer elements than the number of joints",getName().c_str());
      return false;
    }

    // check goal stddev
    if(goal_stddev_param.size() != CARTESIAN_DOF_SIZE)
    {
      ROS_ERROR("%s the 'goal_stddev' parameter must have 6 entries [x y z rx ry rz] for the tool cartesian goal",getName().c_str());
      return false;
    }

    // parsing parameters
    for(auto i = 0u; i < stddev_param.size(); i++)
    {
      stddev_[i] = static_cast<double>(stddev_param[i]);
    }

    // goal stddev
    for(auto i = 0u; i < goal_stddev_param.size(); i++)
    {
      goal_stddev_[i] = static_cast<double>(goal_stddev_param[i]);
    }

    // goal constraint parameters
    XmlRpcValue dof_nullity_param = params["constrained_dofs"];
    if((dof_nullity_param.getType() != XmlRpcValue::TypeArray) ||
        dof_nullity_param.size() < CARTESIAN_DOF_SIZE )
    {
      ROS_ERROR("UnderconstrainedGoal received invalid array parameters");
      return false;
    }

    for(auto i = 0u; i < dof_nullity_param.size(); i++)
    {
      kc_.constrained_dofs(i) = static_cast<int>(dof_nullity_param[i]);
    }

  }
  catch(XmlRpc::XmlRpcException& e)
  {
    ROS_ERROR("%s failed to load parameters",getName().c_str());
    return false;
  }

  return true;
}

bool GoalGuidedMultivariateGaussian::setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& planning_scene,
                 const moveit_msgs::MotionPlanRequest &req,
                 const stomp_core::StompConfiguration &config,
                 moveit_msgs::MoveItErrorCodes& error_code)
{
  bool succeed = setNoiseGeneration(planning_scene,req,config,error_code) &&
      setGoalConstraints(planning_scene,req,config,error_code);

  return succeed;
}

bool GoalGuidedMultivariateGaussian::setNoiseGeneration(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                               const moveit_msgs::MotionPlanRequest &req,
                                               const stomp_core::StompConfiguration &config,
                                               moveit_msgs::MoveItErrorCodes& error_code)
{
  using namespace Eigen;

  // convenience lambda function to fill matrix
  auto fill_diagonal = [](Eigen::MatrixXd& m,double coeff,int diag_index)
  {
    std::size_t size = m.rows() - std::abs(diag_index);
    m.diagonal(diag_index) = VectorXd::Constant(size,coeff);
  };

  // creating finite difference acceleration matrix
  std::size_t num_timesteps = config.num_timesteps;
  Eigen::MatrixXd A = MatrixXd::Zero(num_timesteps,num_timesteps);
  int num_elements = (int((ACC_MATRIX_DIAGONAL_INDICES.size() -1)/2.0) + 1)* num_timesteps ;
  for(auto i = 0u; i < ACC_MATRIX_DIAGONAL_INDICES.size() ; i++)
  {
    fill_diagonal(A,ACC_MATRIX_DIAGONAL_VALUES[i],ACC_MATRIX_DIAGONAL_INDICES[i]);
  }

  // create and scale covariance matrix
  Eigen::MatrixXd covariance = MatrixXd::Identity(num_timesteps,num_timesteps);
  covariance = A.transpose() * A;
  covariance = covariance.fullPivLu().inverse();
  double max_val = covariance.array().abs().matrix().maxCoeff();
  covariance /= max_val;

  // create random generators
  traj_noise_generators_.resize(stddev_.size());
  for(auto& r: traj_noise_generators_)
  {
    r.reset(new utils::MultivariateGaussian(VectorXd::Zero(num_timesteps),covariance));
  }

  // preallocating noise data
  raw_noise_.resize(config.num_timesteps);
  raw_noise_.setZero();

  error_code.val = error_code.SUCCESS;

  return true;
}

bool GoalGuidedMultivariateGaussian::setGoalConstraints(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                               const moveit_msgs::MotionPlanRequest &req,
                                               const stomp_core::StompConfiguration &config,
                                               moveit_msgs::MoveItErrorCodes& error_code)
{
  using namespace Eigen;
  using namespace moveit::core;
  using namespace utils::kinematics;

  const JointModelGroup* joint_group = robot_model_->getJointModelGroup(group_);
  int num_joints = joint_group->getActiveJointModels().size();
  tool_link_ = joint_group->getLinkModelNames().back();
  state_.reset(new RobotState(robot_model_));
  robotStateMsgToRobotState(req.start_state,*state_);

  ROS_DEBUG("%s using '%s' tool link",getName().c_str(),tool_link_.c_str());
  error_code.val = error_code.SUCCESS;

  return true;
}

bool GoalGuidedMultivariateGaussian::generateNoise(const Eigen::MatrixXd& parameters,
                                     std::size_t start_timestep,
                                     std::size_t num_timesteps,
                                     int iteration_number,
                                     int rollout_number,
                                     Eigen::MatrixXd& parameters_noise,
                                     Eigen::MatrixXd& noise)
{
  using namespace Eigen;
  using namespace stomp_moveit::utils;

  VectorXd goal_joint_pose, goal_joint_noise;
  if(parameters.rows() != stddev_.size())
  {
    ROS_ERROR("Number of rows in parameters %i differs from expected number of joints",int(parameters.rows()));
    return false;
  }

  if(generateRandomGoal(parameters.rightCols(1),goal_joint_pose))
  {
    goal_joint_noise = goal_joint_pose - parameters.rightCols(1);
  }
  else
  {
    ROS_WARN("%s failed to generate random goal pose in the task space, not applying noise at goal",getName().c_str());
    goal_joint_noise = VectorXd::Zero(parameters.rows());
  }

  // generating noise
  int sign;
  for(auto d = 0u; d < parameters.rows() ; d++)
  {
    traj_noise_generators_[d]->sample(raw_noise_,true);

    // shifting data towards goal
    sign = goal_joint_noise(d) > 0 ? 1 : -1;
    noise.row(d).transpose() = stddev_[d] * raw_noise_+ sign*Eigen::VectorXd::LinSpaced(
        raw_noise_.size(),0,std::abs(goal_joint_noise(d)));
  }

  parameters_noise = parameters + noise;

  return true;
}

bool GoalGuidedMultivariateGaussian::generateRandomGoal(const Eigen::VectorXd& seed_joint_pose,Eigen::VectorXd& goal_joint_pose)
{
  using namespace Eigen;
  using namespace moveit::core;
  using namespace stomp_moveit::utils;

  // generate noise
  Eigen::VectorXd noise = Eigen::VectorXd::Zero(CARTESIAN_DOF_SIZE);
  for(auto d = 0u; d < noise.size(); d++)
  {
    noise(d) = goal_stddev_[d]*(*goal_rand_generator_)();
  }

  // applying noise onto tool pose
  state_->setJointGroupPositions(group_,seed_joint_pose);
  state_->updateLinkTransforms();
  Affine3d tool_pose = state_->getGlobalLinkTransform(tool_link_);
  auto& n = noise;
  kc_.tool_goal_pose = tool_pose * Translation3d(Vector3d(n(0),n(1),n(2)))*
      AngleAxisd(n(3),Vector3d::UnitX())*AngleAxisd(n(4),Vector3d::UnitY())*AngleAxisd(n(5),Vector3d::UnitZ());
  kc_.init_joint_pose = seed_joint_pose;

  if(!kinematics::solveIK(state_,group_,kc_,goal_joint_pose))
  {
    ROS_DEBUG("%s 'solveIK(...)' failed, returning noiseless goal pose",getName().c_str());
    goal_joint_pose= seed_joint_pose;
    return false;
  }

  return true;
}

} /* namespace noise_generators */
} /* namespace stomp_moveit */