#include "CompositeTECPetal.h"

#include "FWCore/MessageLogger/interface/MessageLogger.h"

#include "ForwardDiskSectorBuilderFromWedges.h"
#include "LayerCrossingSide.h"
#include "DetGroupMerger.h"
#include "CompatibleDetToGroupAdder.h"

#include "TrackingTools/DetLayers/interface/DetLayerException.h"
#include "TrackingTools/DetLayers/interface/MeasurementEstimator.h"
#include "TrackingTools/GeomPropagators/interface/HelixForwardPlaneCrossing.h"

#include "TkDetUtil.h"
#include "DataFormats/GeometryVector/interface/VectorUtil.h"

#include <boost/function.hpp>
#include <boost/bind.hpp>
#include<algorithm>
#include<numeric>
#include<iterator>

using namespace std;

typedef GeometricSearchDet::DetWithState DetWithState;

namespace {
  namespace details {

    struct Mean {
      float operator()(const GeometricSearchDet*  a, const GeometricSearchDet* b) const {
	return 0.5*(b->position().perp()+a->position().perp());
      }
      float operator()(float  a, float b) const {
	return 0.5*(b+a);
      }
    };

    void fillBoundaries(std::vector<const TECWedge*> const & dets,
			std::vector<float> & boundaries) {
      boundaries.resize(dets.size());
      std::transform(dets.begin(), dets.end(),  boundaries.begin(),
		   boost::bind(&GlobalPoint::perp,boost::bind(&GeometricSearchDet::position,_1))
		     );
      std::adjacent_difference(boundaries.begin(),boundaries.end(),  boundaries.begin(), Mean());
    }

    inline
    int findBin(std::vector<float> const & boundaries, float r) {
      return
	std::lower_bound(boundaries.begin()+1,boundaries.end(),r)
	-boundaries.begin()-1;
    }

    void fillPars(std::vector<const TECWedge*> const & dets,   std::vector<CompositeTECPetal::WedgePar> & pars) {

      for (auto gsdet : dets) {
	const BoundDiskSector& wedgeSector = static_cast<const BoundDiskSector&>( gsdet->surface());
	float wedgeMinZ = std::abs( wedgeSector.position().z()) - 0.5*wedgeSector.bounds().thickness();
	float wedgeMaxZ = std::abs( wedgeSector.position().z()) + 0.5*wedgeSector.bounds().thickness();
	float thetaWedgeMin =  wedgeSector.innerRadius()/ wedgeMaxZ;
	float thetaWedgeMax =  wedgeSector.outerRadius()/ wedgeMinZ;
	CompositeTECPetal::WedgePar apar = {gsdet->position().perp(),thetaWedgeMin,thetaWedgeMax};
	pars.push_back(apar);
      }
    }

    inline
    bool overlap( const GlobalPoint& gpos, const CompositeTECPetal::WedgePar & wpar, float ymax)
    {
      // this method is just a duplication of overlapInR
      // adapeted for groupedCompatibleDets() needs

      // assume "fixed theta window", i.e. margin in local y = r is changing linearly with z
      auto tsRadius = gpos.perp();
      auto rmin = std::max(0.f,tsRadius-ymax);
      auto zmax = std::abs(gpos.z())+10.f; // add 10 cm contingency
      auto rmax = (tsRadius + ymax);
      auto zmin = std::abs(gpos.z())-10.f;

      // do the theta regions overlap ?

      return  !(  (rmin > zmax*wpar.thetaMax) | ( zmin*wpar.thetaMin > rmax) );

    }

  }
}

CompositeTECPetal::CompositeTECPetal(vector<const TECWedge*>& innerWedges,
				     vector<const TECWedge*>& outerWedges) :
  GeometricSearchDet(true),
  theFrontComps(innerWedges),
  theBackComps(outerWedges)
{
  theComps.assign(theFrontComps.begin(),theFrontComps.end());
  theComps.insert(theComps.end(),theBackComps.begin(),theBackComps.end());

  details::fillBoundaries( theFrontComps, theFrontBoundaries);
  details::fillBoundaries( theBackComps, theBackBoundaries);
  details::fillPars(theFrontComps, theFrontPars);
  details::fillPars(theBackComps, theBackPars);

  for(vector<const GeometricSearchDet*>::const_iterator it=theComps.begin();
      it!=theComps.end();it++){
    theBasicComps.insert(theBasicComps.end(),
			 (**it).basicComponents().begin(),
			 (**it).basicComponents().end());
  }

  //the Wedge are already R ordered
  //sort( theWedges.begin(), theWedges.end(), DetLessR());
  //sort( theFrontWedges.begin(), theFrontWedges.end(), DetLessR() );
  //sort( theBackWedges.begin(), theBackWedges.end(), DetLessR() );
  vector<const TECWedge*> allWedges;
  allWedges.assign(innerWedges.begin(),innerWedges.end());
  allWedges.insert(allWedges.end(),outerWedges.begin(),outerWedges.end());

  theDiskSector  = ForwardDiskSectorBuilderFromWedges()( allWedges );
  theFrontSector = ForwardDiskSectorBuilderFromWedges()( innerWedges);
  theBackSector  = ForwardDiskSectorBuilderFromWedges()( outerWedges);

  //--------- DEBUG INFO --------------
  LogDebug("TkDetLayers") << "DEBUG INFO for CompositeTECPetal" ;

  for(auto it=theFrontComps.begin();
      it!=theFrontComps.end(); it++){
    LogDebug("TkDetLayers") << "frontWedge phi,z,r: "
			    << (*it)->surface().position().phi() << " , "
			    << (*it)->surface().position().z() <<   " , "
			    << (*it)->surface().position().perp() ;
  }

  for(auto it=theBackComps.begin();
      it!=theBackComps.end(); it++){
    LogDebug("TkDetLayers") << "backWedge phi,z,r: "
			    << (*it)->surface().position().phi() << " , "
			    << (*it)->surface().position().z() <<   " , "
			    << (*it)->surface().position().perp() ;
  }
  //-----------------------------------

}

CompositeTECPetal::~CompositeTECPetal(){
  vector<const GeometricSearchDet*>::const_iterator i;
  for (i=theComps.begin(); i!=theComps.end(); i++) {
    delete *i;
  }
}

pair<bool, TrajectoryStateOnSurface>
CompositeTECPetal::compatible( const TrajectoryStateOnSurface& ts, const Propagator&,
		  const MeasurementEstimator&) const{
  edm::LogError("TkDetLayers") << "temporary dummy implementation of CompositeTECPetal::compatible()!!" ;
  return pair<bool,TrajectoryStateOnSurface>();
}

void
CompositeTECPetal::groupedCompatibleDetsV( const TrajectoryStateOnSurface& tsos,
					   const Propagator& prop,
					   const MeasurementEstimator& est,
					   std::vector<DetGroup> & result) const {

  vector<DetGroup> closestResult;
  SubLayerCrossings  crossings;
  crossings = computeCrossings( tsos, prop.propagationDirection());
  if(! crossings.isValid()) return;

  addClosest( tsos, prop, est, crossings.closest(), closestResult);
  LogDebug("TkDetLayers") << "in TECPetal, closestResult.size(): "<< closestResult.size();

  if (closestResult.empty()){
    vector<DetGroup> nextResult;
    addClosest( tsos, prop, est, crossings.other(), nextResult);
    LogDebug("TkDetLayers") << "in TECPetal, nextResult.size(): "<< nextResult.size() ;
    if(nextResult.empty())    return;

    DetGroupElement nextGel( nextResult.front().front());
    int crossingSide = LayerCrossingSide::endcapSide( nextGel.trajectoryState(), prop);
    DetGroupMerger::orderAndMergeTwoLevels( std::move(closestResult), std::move(nextResult), result,
					    crossings.closestIndex(), crossingSide);
  } else {

    DetGroupElement closestGel( closestResult.front().front());
    float window = computeWindowSize( closestGel.det(), closestGel.trajectoryState(), est);

    searchNeighbors( tsos, prop, est, crossings.closest(), window,
		     closestResult, false);

    vector<DetGroup> nextResult;
    searchNeighbors( tsos, prop, est, crossings.other(), window,
		     nextResult, true);

    int crossingSide = LayerCrossingSide::endcapSide( closestGel.trajectoryState(), prop);
    DetGroupMerger::orderAndMergeTwoLevels( std::move(closestResult), std::move(nextResult), result,
					    crossings.closestIndex(), crossingSide);
  }
}

SubLayerCrossings
CompositeTECPetal::computeCrossings(const TrajectoryStateOnSurface& startingState,
				   PropagationDirection propDir) const
{

  HelixPlaneCrossing::PositionType startPos( startingState.globalPosition() );
  HelixPlaneCrossing::DirectionType startDir( startingState.globalMomentum() );

  auto rho = startingState.transverseCurvature();

  HelixForwardPlaneCrossing crossing(startPos,startDir,rho,propDir);

  pair<bool,double> frontPath = crossing.pathLength( *theFrontSector);
  if (!frontPath.first) return SubLayerCrossings();

  pair<bool,double> backPath = crossing.pathLength(*theBackSector);
  if (!backPath.first) return SubLayerCrossings();

  GlobalPoint gFrontPoint(crossing.position(frontPath.second));
  GlobalPoint gBackPoint( crossing.position(backPath.second));

  LogDebug("TkDetLayers")
    << "in TECPetal,front crossing : r,z,phi: ("
    << gFrontPoint.perp() << ","
    << gFrontPoint.z() << ","
    << gFrontPoint.phi() << ")";

  LogDebug("TkDetLayers")
    << "in TECPetal,back crossing r,z,phi: ("
    << gBackPoint.perp() << ","
    << gBackPoint.z() << ","
    << gBackPoint.phi() << ")" ;

  int frontIndex = findBin(gFrontPoint.perp(),0);
  SubLayerCrossing frontSLC( 0, frontIndex, gFrontPoint);

  int backIndex = findBin(gBackPoint.perp(),1);
  SubLayerCrossing backSLC( 1, backIndex, gBackPoint);

  auto frontDist = std::abs( theFrontPars[frontIndex].theR - gFrontPoint.perp());
  auto backDist = std::abs( theBackPars[backIndex].theR - gBackPoint.perp());

  // 0ss: frontDisk has index=0, backDisk has index=1
  if (frontDist < backDist) {
    return SubLayerCrossings( frontSLC, backSLC, 0);
  }
  else {
    return SubLayerCrossings( backSLC, frontSLC, 1);
  }
}

bool CompositeTECPetal::addClosest( const TrajectoryStateOnSurface& tsos,
				    const Propagator& prop,
				    const MeasurementEstimator& est,
				    const SubLayerCrossing& crossing,
				    vector<DetGroup>& result) const
{

  auto det = subLayer( crossing.subLayerIndex())[crossing.closestDetIndex()];

  LogDebug("TkDetLayers")
    << "in TECPetal, adding Wedge at r,z,phi: ("
    << det->position().perp() << ","
    << det->position().z() << ","
    << det->position().phi() << ")" ;
  LogDebug("TkDetLayers")
    << "wedge comps size: "
    << det->basicComponents().size();

  return CompatibleDetToGroupAdder::add( *det, tsos, prop, est, result);
}

void
CompositeTECPetal::searchNeighbors( const TrajectoryStateOnSurface& tsos,
				    const Propagator& prop,
				    const MeasurementEstimator& est,
				    const SubLayerCrossing& crossing,
				    float window,
				    vector<DetGroup>& result,
				    bool checkClosest) const
{
  const GlobalPoint& gCrossingPos = crossing.position();

  int closestIndex = crossing.closestDetIndex();
  int negStartIndex = closestIndex-1;
  int posStartIndex = closestIndex+1;

  float detR = findPar(closestIndex,crossing.subLayerIndex()).theR;

  if (checkClosest) { // must decide if the closest is on the neg or pos side
    if ( gCrossingPos.perp2() < detR*detR ) {
      posStartIndex = closestIndex;
    }
    else {
      negStartIndex = closestIndex;
    }
  }

  const std::vector<const TECWedge*>& sLayer = subLayer(crossing.subLayerIndex() );

  //const BinFinderType& binFinder = (crossing.subLayerIndex()==0 ? theFrontBinFinder : theBackBinFinder);
  int theSize = crossing.subLayerIndex()==0 ? theFrontComps.size() : theBackComps.size();

  typedef CompatibleDetToGroupAdder Adder;
  for (int idet=negStartIndex; idet >= 0; idet--) {
    //if(idet<0 || idet>= theSize) {edm::LogInfo(TkDetLayers) << "===== error! gone out vector bounds.idet: " << idet ;exit;}
    const GeometricSearchDet & neighborWedge = *sLayer[idet];
    WedgePar const & wpar = findPar(idet, crossing.subLayerIndex());
    if (!details::overlap( gCrossingPos, wpar, window)) break;  // --- to check
    if (!Adder::add( neighborWedge, tsos, prop, est, result)) break;
    // maybe also add shallow crossing angle test here???
  }
  for (int idet=posStartIndex; idet <theSize; idet++) {
    //if(idet<0 || idet>= theSize) {edm::LogInfo(TkDetLayers) << "===== error! gone out vector bounds.idet: " << idet ;exit;}
    const GeometricSearchDet & neighborWedge = *sLayer[idet];
    WedgePar const & wpar = findPar(idet, crossing.subLayerIndex());
    if (!details::overlap( gCrossingPos, wpar, window)) break;  // ---- to check
    if (!Adder::add( neighborWedge, tsos, prop, est, result)) break;
    // maybe also add shallow crossing angle test here???
  }
}

float CompositeTECPetal::computeWindowSize( const GeomDet* det,
					    const TrajectoryStateOnSurface& tsos,
					    const MeasurementEstimator& est)
{
  return est.maximalLocalDisplacement(tsos, det->surface()).y();
}

int CompositeTECPetal::findBin( float R, int diskSectorType) const
{
  return details::findBin(diskSectorType==0 ? theFrontBoundaries : theBackBoundaries,R);
}


// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//
// Contributors: Kirsten Weber

#ifndef DUNE_GDT_EVALUATION_ELLIPTIC_HH
#define DUNE_GDT_EVALUATION_ELLIPTIC_HH

#include <tuple>

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

#include <dune/common/dynmatrix.hh>
#include <dune/common/typetraits.hh>

#include <dune/stuff/functions/interfaces.hh>

#include "interface.hh"

namespace Dune {
namespace GDT {
namespace LocalEvaluation {

// forward
template< class DiffusionFactorImp, class DiffusionTensorImp = void >
class Elliptic;

namespace internal {

/**
 * \brief Traits for the Elliptic evaluation (variant for given diffusion factor and tensor).
 * \sa    EllipticTraits (below) for a variant if only a diffusion is given.
 */
template< class DiffusionFactorType, class DiffusionTensorType >
class EllipticTraits
{
  static_assert(Stuff::is_localizable_function< DiffusionFactorType >::value,
                "DiffusionFactorType has to be a localizable function!");
  static_assert(Stuff::is_localizable_function< DiffusionTensorType >::value,
                "DiffusionTensorType has to be a localizable function!");
  static_assert(std::is_same< typename DiffusionFactorType::EntityType,
                              typename DiffusionTensorType::EntityType >::value,
                "EntityTypes have to agree!");
  static_assert(std::is_same< typename DiffusionFactorType::DomainFieldType,
                              typename DiffusionTensorType::DomainFieldType >::value,
                "DomainFieldTypes have to agree!");
  static_assert(DiffusionFactorType::dimDomain == DiffusionTensorType::dimDomain,
                "Dimensions have to agree!");
public:
  typedef Elliptic< DiffusionFactorType, DiffusionTensorType > derived_type;
  typedef std::tuple< std::shared_ptr< typename DiffusionFactorType::LocalfunctionType >,
                      std::shared_ptr< typename DiffusionTensorType::LocalfunctionType > > LocalfunctionTupleType;
  typedef typename DiffusionFactorType::EntityType      EntityType;
  typedef typename DiffusionFactorType::DomainFieldType DomainFieldType;
  static const size_t                                   dimDomain = DiffusionFactorType::dimDomain;
}; // class EllipticTraits

/**
 * \brief Traits for the Elliptic evaluation (variant for a given diffusion).
 * \note  It does not matter if that function plays the role of the diffusion factor (scalar) or the diffusion
 *        tensor (matrix).
 * \sa    EllipticTraits (above) for a variant if a diffusion factor and a diffusion tensor is given.
 */
template< class DiffusionType >
class EllipticTraits< DiffusionType, void >
{
  static_assert(Stuff::is_localizable_function< DiffusionType >::value,
                "DiffusionType has to be a localizable function!");
public:
  typedef Elliptic< DiffusionType, void >         derived_type;
  typedef typename DiffusionType::EntityType      EntityType;
  typedef typename DiffusionType::DomainFieldType DomainFieldType;
  typedef std::tuple< std::shared_ptr< typename DiffusionType::LocalfunctionType > > LocalfunctionTupleType;
  static const size_t                             dimDomain = DiffusionType::dimDomain;
}; // class EllipticTraits< ..., void >

} // namespace internal

/**
 * \brief Computes an elliptic evaluation (variant for given diffusion factor and tensor).
 * \sa    Elliptic (below) for a variant if only a diffusion is given.
 */
template< class DiffusionFactorImp, class DiffusionTensorImp >
class Elliptic
  : public LocalEvaluation::Codim0Interface< internal::EllipticTraits< DiffusionFactorImp, DiffusionTensorImp >, 2 >
{
public:
  typedef DiffusionFactorImp                                                   DiffusionFactorType;
  typedef DiffusionTensorImp                                                   DiffusionTensorType;
  typedef internal::EllipticTraits< DiffusionFactorType, DiffusionTensorType > Traits;
  typedef typename Traits::LocalfunctionTupleType                              LocalfunctionTupleType;
  typedef typename Traits::EntityType                                          EntityType;
  typedef typename Traits::DomainFieldType                                     DomainFieldType;
  static const size_t                                                          dimDomain = Traits::dimDomain;

  Elliptic(const DiffusionFactorType& diffusion_factor,
           const DiffusionTensorType& diffusion_tensor)
    : diffusion_factor_(diffusion_factor)
    , diffusion_tensor_(diffusion_tensor)
  {}

  LocalfunctionTupleType localFunctions(const EntityType& entity) const
  {
    return std::make_tuple(diffusion_factor_.local_function(entity),
                           diffusion_tensor_.local_function(entity));
  }

  /**
   * \brief extracts the local functions and calls the correct order() method
   */
  template< class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  size_t order(const LocalfunctionTupleType& local_functions_tuple,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBase,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBase) const
  {
    const auto local_diffusion_factor = std::get< 0 >(local_functions_tuple);
    const auto local_diffusion_tensor = std::get< 1 >(local_functions_tuple);
    return order(*local_diffusion_factor, *local_diffusion_tensor, testBase, ansatzBase);
  } // ... order(...)

  /**
   * \brief extracts the local functions and calls the correct evaluate() method
   */
  template< class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  void evaluate(const LocalfunctionTupleType& local_functions_tuple,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBase,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBase,
                const Dune::FieldVector< DomainFieldType, dimDomain >& localPoint,
                Dune::DynamicMatrix< R >& ret) const
  {
    const auto local_diffusion_factor = std::get< 0 >(local_functions_tuple);
    const auto local_diffusion_tensor = std::get< 1 >(local_functions_tuple);
    evaluate(*local_diffusion_factor, *local_diffusion_tensor, testBase, ansatzBase, localPoint, ret);
  }

private:
  template< class R, size_t rDF, size_t rCDF, size_t rDT, size_t rCDT, size_t rT, size_t rCT, size_t rA, size_t rCA >
  size_t order(const Stuff::LocalfunctionInterface
                   < EntityType, DomainFieldType, dimDomain, R, rDF, rCDF >& local_diffusion_factor,
               const Stuff::LocalfunctionInterface
                   < EntityType, DomainFieldType, dimDomain, R, rDT, rCDT >& local_diffusion_tensor,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBase,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBase) const
  {
    return local_diffusion_factor.order()
        + local_diffusion_tensor.order()
        + std::max(ssize_t(testBase.order()) - 1, ssize_t(0))
        + std::max(ssize_t(ansatzBase.order()) - 1, ssize_t(0));
  } // ... order(...)

  template< class R, size_t r >
  void evaluate(const Stuff::LocalfunctionInterface
                    < EntityType, DomainFieldType, dimDomain, R, 1, 1 >& local_diffusion_factor,
                const Stuff::LocalfunctionInterface
                    < EntityType, DomainFieldType, dimDomain, R, 1, 1 >& local_diffusion_tensor,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, r, 1 >& testBase,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, r, 1 >& ansatzBase,
                const Dune::FieldVector< DomainFieldType, dimDomain >& localPoint,
                Dune::DynamicMatrix< R >& ret) const
  {
    // evaluate local functions
    const auto local_diffusion_factor_value = local_diffusion_factor.evaluate(localPoint);
    const auto local_diffusion_tensor_value = local_diffusion_tensor.evaluate(localPoint);
    // evaluate test gradient
    const auto rows = testBase.size();
    const auto testGradients = testBase.jacobian(localPoint);
    // evaluate ansatz gradient
    const auto cols = ansatzBase.size();
    const auto ansatzGradients = ansatzBase.jacobian(localPoint);
    // compute products
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    for (size_t ii = 0; ii < rows; ++ii) {
      auto& retRow = ret[ii];
      for (size_t jj = 0; jj < cols; ++jj) {
        retRow[jj] = local_diffusion_factor_value * local_diffusion_tensor_value
                     * (ansatzGradients[jj][0] * testGradients[ii][0]);
      }
    }
  } // ... evaluate(...)

  template< class R >
  void evaluate(const Stuff::LocalfunctionInterface
                    < EntityType, DomainFieldType, dimDomain, R, 1, 1 >& local_diffusion_factor,
                const Stuff::LocalfunctionInterface
                    < EntityType, DomainFieldType, dimDomain, R, 2, 2 >& local_diffusion_tensor,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, 1, 1 >& testBase,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, 1, 1 >& ansatzBase,
                const Dune::FieldVector< DomainFieldType, dimDomain >& localPoint,
                Dune::DynamicMatrix< R >& ret) const
  {
    evaluate_matrix_valued_(local_diffusion_factor, local_diffusion_tensor, testBase, ansatzBase, localPoint, ret);
  }

  template< class R >
  void evaluate(const Stuff::LocalfunctionInterface
                    < EntityType, DomainFieldType, dimDomain, R, 1, 1 >& local_diffusion_factor,
                const Stuff::LocalfunctionInterface
                    < EntityType, DomainFieldType, dimDomain, R, 3, 3 >& local_diffusion_tensor,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, 1, 1 >& testBase,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, 1, 1 >& ansatzBase,
                const Dune::FieldVector< DomainFieldType, dimDomain >& localPoint,
                Dune::DynamicMatrix< R >& ret) const
  {
    evaluate_matrix_valued_(local_diffusion_factor, local_diffusion_tensor, testBase, ansatzBase, localPoint, ret);
  }

  template< class R >
  void evaluate_matrix_valued_(const Stuff::LocalfunctionInterface
                                   < EntityType, DomainFieldType, dimDomain, R, 1, 1 >& local_diffusion_factor,
                               const Stuff::LocalfunctionInterface
                                   < EntityType, DomainFieldType, dimDomain, R, dimDomain, dimDomain >& local_diffusion_tensor,
                               const Stuff::LocalfunctionSetInterface
                                   < EntityType, DomainFieldType, dimDomain, R, 1, 1 >& testBase,
                               const Stuff::LocalfunctionSetInterface
                                   < EntityType, DomainFieldType, dimDomain, R, 1, 1 >& ansatzBase,
                               const Dune::FieldVector< DomainFieldType, dimDomain >& localPoint,
                               Dune::DynamicMatrix< R >& ret) const
  {
    typedef typename Stuff::LocalfunctionSetInterface
        < EntityType, DomainFieldType, dimDomain, R, 1, 1 >::JacobianRangeType JacobianRangeType;
    // evaluate local functions
    const auto local_diffusion_factor_value = local_diffusion_factor.evaluate(localPoint);
    auto local_diffusion_tensor_value = local_diffusion_tensor.evaluate(localPoint);
    local_diffusion_tensor_value *= local_diffusion_factor_value[0];
    // evaluate test gradient
    const size_t rows = testBase.size();
    std::vector< JacobianRangeType > testGradients(rows, JacobianRangeType(0));
    testBase.jacobian(localPoint, testGradients);
    // evaluate ansatz gradient
    const size_t cols = ansatzBase.size();
    std::vector< JacobianRangeType > ansatzGradients(cols, JacobianRangeType(0));
    ansatzBase.jacobian(localPoint, ansatzGradients);
    // compute products
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    FieldVector< DomainFieldType, dimDomain > product(0.0);
    for (size_t ii = 0; ii < rows; ++ii) {
      auto& retRow = ret[ii];
      for (size_t jj = 0; jj < cols; ++jj) {
        local_diffusion_tensor_value.mv(ansatzGradients[jj][0], product);
        retRow[jj] = product * testGradients[ii][0];
      }
    }
  } // ... evaluate_matrix_valued_(...)

  const DiffusionFactorType& diffusion_factor_;
  const DiffusionTensorType& diffusion_tensor_;
}; // class Elliptic

/**
 * \brief Computes an elliptic evaluation (variant for a given diffusion).
 * \note  It does not matter if that function plays the role of the diffusion factor (scalar) or the diffusion
 *        tensor (matrix).
 * \sa    Elliptic (above) for a variant if a diffusion factor and a diffusion tensor is given.
 */
template< class DiffusionImp >
class Elliptic< DiffusionImp, void >
  : public LocalEvaluation::Codim0Interface< internal::EllipticTraits< DiffusionImp, void >, 2 >
{
public:
  typedef DiffusionImp                                    DiffusionType;
  typedef internal::EllipticTraits< DiffusionType, void > Traits;
  typedef typename Traits::LocalfunctionTupleType         LocalfunctionTupleType;
  typedef typename Traits::EntityType                     EntityType;
  typedef typename Traits::DomainFieldType                DomainFieldType;
  static const size_t                                     dimDomain = Traits::dimDomain;

  explicit Elliptic(const DiffusionType& inducingFunction)
    : diffusion_(inducingFunction)
  {}

  /// \name Required by LocalEvaluation::Codim0Interface< ..., 2 >
  /// \{

  LocalfunctionTupleType localFunctions(const EntityType& entity) const
  {
    return std::make_tuple(diffusion_.local_function(entity));
  }

  /**
   * \brief extracts the local functions and calls the correct order() method
   */
  template< class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  size_t order(const LocalfunctionTupleType& localFuncs,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBase,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBase) const
  {
    return order(*std::get< 0 >(localFuncs), testBase, ansatzBase);
  }

  /**
   * \brief extracts the local functions and calls the correct evaluate() method
   */
  template< class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  void evaluate(const LocalfunctionTupleType& localFuncs,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBase,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBase,
                const Dune::FieldVector< DomainFieldType, dimDomain >& localPoint,
                Dune::DynamicMatrix< R >& ret) const
  {
    evaluate(*std::get< 0 >(localFuncs), testBase, ansatzBase, localPoint, ret);
  }

  /// \}
  /// \name Actual implementations of order
  /// \{

  /**
   *  \return localFunction.order() + (testBase.order() - 1) + (ansatzBase.order() - 1)
   */
  template< class R, size_t rL, size_t rCL, size_t rT, size_t rCT, size_t rA, size_t rCA >
  size_t order(const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, dimDomain, R, rL, rCL >& localFunction,
               const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBase,
               const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBase)
  const
  {
    return localFunction.order()
        + boost::numeric_cast< size_t >(std::max(ssize_t(testBase.order())   - 1, ssize_t(0)))
        + boost::numeric_cast< size_t >(std::max(ssize_t(ansatzBase.order()) - 1, ssize_t(0)));
  } // ... order( ... )

  /// \}
  /// \name Actual implementations of evaluate
  /// \{

  /**
   *  \brief  Computes an elliptic evaluation for a scalar local function and scalar or vector valued basefunctionsets.
   *  \tparam R RangeFieldType
   */
  template< class R, size_t r >
  void evaluate(const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, dimDomain, R, 1, 1 >& localFunction,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, r, 1 >& testBase,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, r, 1 >& ansatzBase,
                const Dune::FieldVector< DomainFieldType, dimDomain >& localPoint,
                Dune::DynamicMatrix< R >& ret) const
  {
    typedef typename Stuff::LocalfunctionSetInterface
        < EntityType, DomainFieldType, dimDomain, R, r, 1 >::JacobianRangeType JacobianRangeType;
    // evaluate local function
    const auto functionValue = localFunction.evaluate(localPoint);
    // evaluate test gradient
    const size_t rows = testBase.size();
    std::vector< JacobianRangeType > testGradients(rows, JacobianRangeType(0));
    testBase.jacobian(localPoint, testGradients);
    // evaluate ansatz gradient
    const size_t cols = ansatzBase.size();
    std::vector< JacobianRangeType > ansatzGradients(cols, JacobianRangeType(0));
    ansatzBase.jacobian(localPoint, ansatzGradients);
    // compute products
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    for (size_t ii = 0; ii < rows; ++ii) {
      auto& retRow = ret[ii];
      for (size_t jj = 0; jj < cols; ++jj) {
        retRow[jj] = functionValue * (ansatzGradients[jj][0] * testGradients[ii][0]);
      }
    }
  } // ... evaluate< ..., 1, ... >(...)

  /**
   *  \brief  Computes an elliptic evaluation for a 2x2 matrix-valued local function and matrix-valued basefunctionsets.
   *  \tparam R RangeFieldType
   *  \note   Unfortunately we need this explicit specialization, otherwise the compiler will complain for 1d grids.
   */
  template< class R >
  void evaluate(const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, dimDomain, R, 2, 2 >& localFunction,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, 1, 1 >& testBase,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, 1, 1 >& ansatzBase,
                const Dune::FieldVector< DomainFieldType, dimDomain >& localPoint,
                Dune::DynamicMatrix< R >& ret) const
  {
    evaluate_matrix_valued_(localFunction, testBase, ansatzBase, localPoint, ret);
  }

  /**
   *  \brief  Computes an elliptic evaluation for a 3x3 matrix-valued local function and matrix-valued basefunctionsets.
   *  \tparam R RangeFieldType
   *  \note   Unfortunately we need this explicit specialization, otherwise the compiler will complain for 1d grids.
   */
  template< class R >
  void evaluate(const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, dimDomain, R, 3, 3 >& localFunction,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, 1, 1 >& testBase,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, 1, 1 >& ansatzBase,
                const Dune::FieldVector< DomainFieldType, dimDomain >& localPoint,
                Dune::DynamicMatrix< R >& ret) const
  {
    evaluate_matrix_valued_(localFunction, testBase, ansatzBase, localPoint, ret);
  }

  /// \}

private:
  template< class R >
  void evaluate_matrix_valued_(const Stuff::LocalfunctionInterface
                                   < EntityType, DomainFieldType, dimDomain, R, dimDomain, dimDomain >& localFunction,
                               const Stuff::LocalfunctionSetInterface
                                   < EntityType, DomainFieldType, dimDomain, R, 1, 1 >& testBase,
                               const Stuff::LocalfunctionSetInterface
                                   < EntityType, DomainFieldType, dimDomain, R, 1, 1 >& ansatzBase,
                               const Dune::FieldVector< DomainFieldType, dimDomain >& localPoint,
                               Dune::DynamicMatrix< R >& ret) const
  {
    // evaluate local function
    const auto functionValue = localFunction.evaluate(localPoint);
    // evaluate test gradient
    const size_t rows = testBase.size();
    const auto testGradients = testBase.jacobian(localPoint);
    // evaluate ansatz gradient
    const size_t cols = ansatzBase.size();
    const auto ansatzGradients = ansatzBase.jacobian(localPoint);
    // compute products
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    FieldVector< DomainFieldType, dimDomain > product(0.0);
    for (size_t ii = 0; ii < rows; ++ii) {
      auto& retRow = ret[ii];
      for (size_t jj = 0; jj < cols; ++jj) {
        functionValue.mv(ansatzGradients[jj][0], product);
        retRow[jj] = product * testGradients[ii][0];
      }
    }
  } // ... evaluate_matrix_valued_(...)

  const DiffusionType& diffusion_;
}; // class Elliptic< ...., void >

} // namespace LocalEvaluation
} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_EVALUATION_ELLIPTIC_HH


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

#include <boost/hana/identity.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/functor/laws.hpp>
#include <boost/hana/list/instance.hpp>

#include "unique_comparable.hpp"
#include "unique_function.hpp"
using namespace boost::hana;

auto x = unique_comparable([]{});
auto y = unique_comparable([]{});
auto z = unique_comparable([]{});

auto f = unique_function([]{});
auto g = unique_function([]{});
auto h = unique_function([]{});
auto i = unique_function([]{});

BOOST_HANA_CONSTANT_ASSERT(
    equal(fmap(f, identity(x)), identity(f(x)))
);

BOOST_HANA_CONSTANT_ASSERT(Functor::laws::check(
    list(identity(x), identity(y), identity(z)),
    list(f, g),
    list(h, i)
));

int main() { }


//
// Copyright (c) 2016-2019 CNRS INRIA
//

#ifndef __pinocchio_joint_composite_hpp__
#define __pinocchio_joint_composite_hpp__

#include "pinocchio/multibody/joint/fwd.hpp"
#include "pinocchio/multibody/joint/joint-collection.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
#include "pinocchio/container/aligned-vector.hpp"
#include "pinocchio/spatial/act-on-set.hpp"

#include "pinocchio/serialization/fwd.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct JointCompositeTpl;

  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
  struct traits< JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
  {
    typedef _Scalar Scalar;

    enum {
      Options = _Options,
      NQ = Eigen::Dynamic,
      NV = Eigen::Dynamic
    };

    typedef JointCollectionTpl<Scalar,Options> JointCollection;
    typedef JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> JointDataDerived;
    typedef JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> JointModelDerived;
    typedef ConstraintTpl<Eigen::Dynamic,Scalar,Options> Constraint_t;
    typedef SE3Tpl<Scalar,Options> Transformation_t;
    typedef MotionTpl<Scalar,Options> Motion_t;
    typedef MotionTpl<Scalar,Options> Bias_t;

    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> F_t;
    // [ABA]
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;

    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE

    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
  };

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct traits< JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
  { typedef JointCompositeTpl<Scalar,Options,JointCollectionTpl> JointDerived; };

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct traits< JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
  { typedef JointCompositeTpl<Scalar,Options,JointCollectionTpl> JointDerived; };

  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
  struct JointDataCompositeTpl
  : public JointDataBase< JointDataCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef JointDataBase<JointDataCompositeTpl> Base;
    typedef JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE;
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR

    typedef JointCollectionTpl<Scalar,Options> JointCollection;
    typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataVariant;

    typedef container::aligned_vector<JointDataVariant> JointDataVector;

    // JointDataComposite()  {} // can become necessary if we want a vector of JointDataComposite ?

    JointDataCompositeTpl()
    : joints()
    , iMlast(0), pjMi(0)
    , S(0)
    , M(),v(),c()
    , U(6,0), Dinv(0,0), UDinv(6,0)
    , StU(0,0)
    {}

    JointDataCompositeTpl(const JointDataVector & joint_data, const int /*nq*/, const int nv)
    : joints(joint_data), iMlast(joint_data.size()), pjMi(joint_data.size())
    , S(nv)
    , M(), v(), c()
    , U(6,nv), Dinv(nv,nv), UDinv(6,nv)
    , StU(nv,nv)
    {}

    /// \brief Vector of joints
    JointDataVector joints;

    /// \brief Transforms from previous joint to last joint
    container::aligned_vector<Transformation_t> iMlast;

    /// \brief Transforms from previous joint to joint i
    container::aligned_vector<Transformation_t> pjMi;

    Constraint_t S;
    Transformation_t M;
    Motion_t v;
    Bias_t c;

    // // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

    D_t StU;

    static std::string classname() { return std::string("JointDataComposite"); }
    std::string shortname() const { return classname(); }

  };

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline std::ostream & operator <<(std::ostream & os, const JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> & jdata)
  {
    typedef typename JointDataCompositeTpl<Scalar,Options,JointCollectionTpl>::JointDataVector JointDataVector;

    os << "JointDataComposite containing following models:\n" ;
    for (typename JointDataVector::const_iterator it = jdata.joints.begin();
         it != jdata.joints.end(); ++it)
      os << "  " << shortname(*it) << std::endl;
    return os;
  }

  template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct CastType< NewScalar, JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
  {
    typedef JointModelCompositeTpl<NewScalar,Options,JointCollectionTpl> type;
  };

  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
  struct JointModelCompositeTpl
  : public JointModelBase< JointModelCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef JointModelBase<JointModelCompositeTpl> Base;
    typedef JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE;

    typedef JointCollectionTpl<Scalar,Options> JointCollection;
    typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModelVariant;

    typedef SE3Tpl<Scalar,Options> SE3;
    typedef MotionTpl<Scalar,Options> Motion;
    typedef InertiaTpl<Scalar,Options> Inertia;

    typedef container::aligned_vector<JointModelVariant> JointModelVector;

    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;
    using Base::nq;
    using Base::nv;

    /// \brief Default contructor
    JointModelCompositeTpl()
    : joints()
    , jointPlacements()
    , m_nq(0)
    , m_nv(0)
    , njoints(0)
    {}

    /// \brief Default contructor with a defined size
    JointModelCompositeTpl(const size_t size)
    : joints()
    , jointPlacements()
    , m_nq(0)
    , m_nv(0)
    , njoints(0)
    {
      joints.reserve(size); jointPlacements.reserve(size);
      m_idx_q.reserve(size); m_idx_v.reserve(size);
      m_nqs.reserve(size); m_nvs.reserve(size);
    }

    ///
    /// \brief Constructor with one joint.
    ///
    /// \param jmodel Model of the first joint.
    /// \param placement Placement of the first joint w.r.t. the joint origin.
    ///
    template<typename JointModel>
    JointModelCompositeTpl(const JointModelBase<JointModel> & jmodel,
                           const SE3 & placement = SE3::Identity())
    : joints(1,(JointModelVariant)jmodel.derived())
    , jointPlacements(1,placement)
    , m_nq(jmodel.nq())
    , m_nv(jmodel.nv())
    , m_idx_q(1,0), m_nqs(1,jmodel.nq())
    , m_idx_v(1,0), m_nvs(1,jmodel.nv())
    , njoints(1)
    {}

    ///
    /// \brief Copy constructor.
    ///
    /// \param other JointModel to copy.
    ///
    JointModelCompositeTpl(const JointModelCompositeTpl & other)
    : Base(other)
    , joints(other.joints)
    , jointPlacements(other.jointPlacements)
    , m_nq(other.m_nq)
    , m_nv(other.m_nv)
    , m_idx_q(other.m_idx_q), m_nqs(other.m_nqs)
    , m_idx_v(other.m_idx_v), m_nvs(other.m_nvs)
    , njoints(other.njoints)
    {}

    ///
    /// \brief Add a joint to the vector of joints.
    ///
    /// \param jmodel Model of the joint to add.
    /// \param placement Placement of the joint relatively to its predecessor.
    ///
    /// \return A reference to *this
    ///
    template<typename JointModel>
    JointModelDerived & addJoint(const JointModelBase<JointModel> & jmodel,
                                 const SE3 & placement = SE3::Identity())
    {
      joints.push_back((JointModelVariant)jmodel.derived());
      jointPlacements.push_back(placement);

      m_nq += jmodel.nq(); m_nv += jmodel.nv();

      updateJointIndexes();
      njoints++;

      return *this;
    }

    JointDataDerived createData() const
    {
      typename JointDataDerived::JointDataVector jdata(joints.size());
      for (int i = 0; i < (int)joints.size(); ++i)
        jdata[(size_t)i] = ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(joints[(size_t)i]);
      return JointDataDerived(jdata,nq(),nv());
    }

    template<typename, int, template<typename S, int O> class, typename>
    friend struct JointCompositeCalcZeroOrderStep;

    template<typename ConfigVectorType>
    void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const;

    template<typename, int, template<typename S, int O> class, typename, typename>
    friend struct JointCompositeCalcFirstOrderStep;

    template<typename ConfigVectorType, typename TangentVectorType>
    void calc(JointDataDerived & data,
              const Eigen::MatrixBase<ConfigVectorType> & qs,
              const Eigen::MatrixBase<TangentVectorType> & vs) const;

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
    {
      data.U.noalias() = I * data.S.matrix();
      data.StU.noalias() = data.S.matrix().transpose() * data.U;

      // compute inverse
      data.Dinv.setIdentity();
      data.StU.llt().solveInPlace(data.Dinv);
      data.UDinv.noalias() = data.U * data.Dinv;

      if (update_I)
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
    }

    Scalar finiteDifferenceIncrement() const
    {
      using std::max;
      Scalar eps = 0;
      for(typename JointModelVector::const_iterator it = joints.begin();
          it != joints.end(); ++it)
        eps = max((Scalar)::pinocchio::finiteDifferenceIncrement(*it),eps);

      return eps;
    }

    int nv_impl() const { return m_nv; }
    int nq_impl() const { return m_nq; }

    /**
     * @brief      Update the indexes of subjoints in the stack
     */
    void setIndexes_impl(JointIndex id, int q, int v)
    {
      Base::setIndexes_impl(id, q, v);
      updateJointIndexes();
    }

    static std::string classname() { return std::string("JointModelComposite"); }
    std::string shortname() const { return classname(); }

    JointModelCompositeTpl & operator=(const JointModelCompositeTpl & other)
    {
      Base::operator=(other);
      m_nq = other.m_nq;
      m_nv = other.m_nv;
      m_idx_q = other.m_idx_q;
      m_idx_v = other.m_idx_v;
      m_nqs = other.m_nqs;
      m_nvs = other.m_nvs;
      joints = other.joints;
      jointPlacements = other.jointPlacements;
      njoints = other.njoints;

      return *this;
    }

    using Base::isEqual;
    bool isEqual(const JointModelCompositeTpl & other) const
    {
      return Base::isEqual(other)
      && nq() == other.nq()
      && nv() == other.nv()
      && m_idx_q == other.m_idx_q
      && m_idx_v == other.m_idx_v
      && m_nqs == other.m_nqs
      && m_nvs == other.m_nvs
      && joints == other.joints
      && jointPlacements == other.jointPlacements
      && njoints == other.njoints;
    }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelCompositeTpl<NewScalar,Options,JointCollectionTpl> cast() const
    {
      typedef JointModelCompositeTpl<NewScalar,Options,JointCollectionTpl> ReturnType;
      ReturnType res((size_t)njoints);
      res.setIndexes(id(),idx_q(),idx_v());
      res.m_nq = m_nq;
      res.m_nv = m_nv;
      res.m_idx_q = m_idx_q;
      res.m_idx_v = m_idx_v;
      res.m_nqs = m_nqs;
      res.m_nvs = m_nvs;
      res.njoints = njoints;

      res.joints.resize(joints.size());
      res.jointPlacements.resize(jointPlacements.size());
      for(size_t k = 0; k < jointPlacements.size(); ++k)
      {
        res.joints[k] = joints[k].template cast<NewScalar>();
        res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
      }

      return res;
    }

    /// \brief Vector of joints contained in the joint composite.
    JointModelVector joints;
    /// \brief Vector of joint placements. Those placements correspond to the origin of the joint relatively to their parent.
    container::aligned_vector<SE3> jointPlacements;

    template<typename D>
    typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
    jointConfigSelector(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
    template<typename D>
    typename SizeDepType<NQ>::template SegmentReturn<D>::Type
    jointConfigSelector( Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }

    template<typename D>
    typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
    jointVelocitySelector(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv());  }
    template<typename D>
    typename SizeDepType<NV>::template SegmentReturn<D>::Type
    jointVelocitySelector( Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv());  }

    template<typename D>
    typename SizeDepType<NV>::template ColsReturn<D>::ConstType
    jointCols(const Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv());  }
    template<typename D>
    typename SizeDepType<NV>::template ColsReturn<D>::Type
    jointCols(Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv());  }

    template<typename D>
    typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
    jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
    template<typename D>
    typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
    jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
    template<typename D>
    typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
    jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
    template<typename D>
    typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
    jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }

    template<typename D>
    typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
    jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
    template<typename D>
    typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
    jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }

  protected:

    friend struct Serialize<JointModelCompositeTpl>;

    template<typename, int, template<typename,int> class>
    friend struct JointModelCompositeTpl;

    /// \brief Update the indexes of the joints contained in the composition according
    /// to the position of the joint composite.
    void updateJointIndexes()
    {
      int idx_q = this->idx_q();
      int idx_v = this->idx_v();

      m_idx_q.resize(joints.size());
      m_idx_v.resize(joints.size());
      m_nqs.resize(joints.size());
      m_nvs.resize(joints.size());

      for(size_t i = 0; i < joints.size(); ++i)
      {
        JointModelVariant & joint = joints[i];

        m_idx_q[i] = idx_q; m_idx_v[i] = idx_v;
        ::pinocchio::setIndexes(joint,i,idx_q,idx_v);
        m_nqs[i] = ::pinocchio::nq(joint);
        m_nvs[i] = ::pinocchio::nv(joint);
        idx_q += m_nqs[i]; idx_v += m_nvs[i];
      }
    }

    /// \brief Dimensions of the config and tangent space of the composite joint.
    int m_nq, m_nv;

    /// Keep information of both the dimension and the position of the joints in the composition.

    /// \brief Index in the config vector
    std::vector<int> m_idx_q;
    /// \brief Dimension of the segment in the config vector
    std::vector<int> m_nqs;
    /// \brief Index in the tangent vector
    std::vector<int> m_idx_v;
    /// \brief Dimension of the segment in the tangent vector
    std::vector<int> m_nvs;

  public:
    /// \brief Number of joints contained in the JointModelComposite
    int njoints;
  };

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline std::ostream & operator <<(std::ostream & os, const JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> & jmodel)
  {
    typedef typename JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>::JointModelVector JointModelVector;

    os << "JointModelComposite containing following models:\n" ;
    for (typename JointModelVector::const_iterator it = jmodel.joints.begin();
         it != jmodel.joints.end(); ++it)
      os << "  " << shortname(*it) << std::endl;

    return os;
  }

} // namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct has_nothrow_constructor< ::pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct has_nothrow_copy< ::pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct has_nothrow_constructor< ::pinocchio::JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct has_nothrow_copy< ::pinocchio::JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
  : public integral_constant<bool,true> {};
}

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/joint/joint-composite.hxx"

#endif // ifndef __pinocchio_joint_composite_hpp__


//=======================================================================
// Copyright 2014-2015 David Simmons-Duffin.
// Distributed under the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

#include "SDPB_Parameters.hxx"

#include <El.hpp>

#include <boost/date_time/posix_time/posix_time.hpp>

Timers solve(const Block_Info &block_info, const SDPB_Parameters &parameters);

void write_timing(const boost::filesystem::path &checkpoint_out,
                  const Block_Info &block_info, const Timers &timers,
                  const bool &debug, El::Matrix<int32_t> &block_timings);

int main(int argc, char **argv)
{
  El::Environment env(argc, argv);

  try
    {
      SDPB_Parameters parameters(argc, argv);
      if(!parameters.is_valid())
        {
          return 0;
        }

      El::gmp::SetPrecision(parameters.solver.precision);
      if(parameters.verbosity >= Verbosity::regular && El::mpi::Rank() == 0)
        {
          std::cout << "SDPB started at "
                    << boost::posix_time::second_clock::local_time() << '\n'
                    << parameters << '\n';
        }

      Block_Info block_info(parameters.sdp_path,
                            parameters.solver.checkpoint_in,
                            parameters.procs_per_node,
                            parameters.proc_granularity, parameters.verbosity);
      // Only generate a block_timings file if
      // 1) We are running in parallel
      // 2) We did not load a block_timings file
      // 3) We are not going to load a checkpoint.
      if(El::mpi::Size(El::mpi::COMM_WORLD) > 1
         && block_info.block_timings_filename.empty()
         && !exists(parameters.solver.checkpoint_in / "checkpoint.0"))
        {
          if(parameters.verbosity >= Verbosity::regular
             && El::mpi::Rank() == 0)
            {
              std::cout << "Performing a timing run\n";
            }
          SDPB_Parameters timing_parameters(parameters);
          timing_parameters.solver.max_iterations = 2;
          timing_parameters.no_final_checkpoint = true;
          timing_parameters.solver.checkpoint_interval
            = std::numeric_limits<int64_t>::max();
          timing_parameters.solver.max_runtime
            = std::numeric_limits<int64_t>::max();
          timing_parameters.solver.duality_gap_threshold = 0;
          timing_parameters.solver.primal_error_threshold = 0;
          timing_parameters.solver.dual_error_threshold = 0;
          timing_parameters.solver.min_primal_step = 0;
          timing_parameters.solver.min_dual_step = 0;
          if(timing_parameters.verbosity != Verbosity::debug)
            {
              timing_parameters.verbosity = Verbosity::none;
            }
          Timers timers(solve(block_info, timing_parameters));

          El::Matrix<int32_t> block_timings(block_info.dimensions.size(), 1);
          write_timing(timing_parameters.solver.checkpoint_out, block_info,
                       timers, timing_parameters.verbosity >= Verbosity::debug,
                       block_timings);
          El::mpi::Barrier(El::mpi::COMM_WORLD);
          Block_Info new_info(
            parameters.sdp_path, block_timings, parameters.procs_per_node,
            parameters.proc_granularity, parameters.verbosity);
          std::swap(block_info, new_info);

          parameters.solver.max_runtime
            -= timers.front().second.elapsed_seconds();
        }
      else if(!block_info.block_timings_filename.empty()
              && block_info.block_timings_filename
                   != (parameters.solver.checkpoint_out / "block_timings"))
        {
          if(El::mpi::Rank() == 0)
            {
              create_directories(parameters.solver.checkpoint_out);
              copy_file(block_info.block_timings_filename,
                        parameters.solver.checkpoint_out / "block_timings",
                        boost::filesystem::copy_option::overwrite_if_exists);
            }
        }
      solve(block_info, parameters);
    }
  catch(std::exception &e)
    {
      El::ReportException(e);
      El::mpi::Abort(El::mpi::COMM_WORLD, 1);
    }
  catch(...)
    {
      El::mpi::Abort(El::mpi::COMM_WORLD, 1);
    }
}


#include <boost/container_hash/hash.hpp>

#include <fmt/core.h>

#include <cbag/common/transformation_util.h>
#include <cbag/layout/grid_object.h>
#include <cbag/layout/routing_grid_util.h>
#include <cbag/layout/track_info_util.h>
#include <cbag/util/math.h>

namespace cbag {
namespace layout {

track_id::track_id() = default;

track_id::track_id(level_t level, htr_t htr, cnt_t ntr, cnt_t num, htr_t pitch)
    : level(level), htr(htr), ntr(ntr), num(num), pitch((num == 1) ? 0 : pitch) {}

bool track_id::operator==(const track_id &rhs) const noexcept {
    return level == rhs.level && htr == rhs.htr && ntr == rhs.ntr && num == rhs.num &&
           pitch == rhs.pitch;
}

htr_t track_id::operator[](std::size_t idx) const noexcept { return htr + idx * pitch; }

std::size_t track_id::get_hash() const noexcept {
    auto seed = static_cast<std::size_t>(0);
    boost::hash_combine(seed, level);
    boost::hash_combine(seed, htr);
    boost::hash_combine(seed, ntr);
    boost::hash_combine(seed, num);
    boost::hash_combine(seed, pitch);
    return seed;
}

std::string track_id::to_string() const noexcept {
    return fmt::format("TrackID(layer={}, htr={}, wdth={}, num={}, htr_pitch={})", level, htr, ntr,
                       num, pitch);
}

level_t track_id::get_level() const noexcept { return level; }

htr_t track_id::get_htr() const noexcept { return htr; }

cnt_t track_id::get_ntr() const noexcept { return ntr; }

cnt_t track_id::get_num() const noexcept { return num; }

htr_t track_id::get_pitch() const noexcept { return pitch; }

std::array<offset_t, 2> track_id::get_bounds(const routing_grid &grid) const {
    auto ans = get_wire_bounds(grid, level, htr, ntr);
    auto delta = static_cast<offset_t>(get_num() - 1) * get_pitch() * grid[level].get_pitch() / 2;
    ans[delta > 0] += delta;
    return ans;
}

void track_id::set_htr(htr_t val) noexcept { htr = val; }

void track_id::set_pitch(htr_t val) noexcept { pitch = (num == 1) ? 0 : val; }

wire_array::wire_array() = default;

wire_array::wire_array(std::shared_ptr<track_id> tid, offset_t lower, offset_t upper)
    : tid_ptr(std::move(tid)), coord({lower, upper}) {}

bool wire_array::operator==(const wire_array &rhs) const noexcept {
    auto is_null = tid_ptr == nullptr;
    auto rhs_is_null = rhs.tid_ptr == nullptr;
    return (is_null && rhs_is_null) ||
           (!is_null && !rhs_is_null && *tid_ptr == *(rhs.tid_ptr) && coord == rhs.coord);
}

std::size_t wire_array::get_hash() const {
    auto seed = static_cast<std::size_t>(0);
    boost::hash_combine(seed, tid_ptr->get_hash());
    boost::hash_combine(seed, coord[0]);
    boost::hash_combine(seed, coord[1]);
    return seed;
}

std::string wire_array::to_string() const {
    return fmt::format("WireArray({}, {}, {})", tid_ptr->to_string(), coord[0], coord[1]);
}

const std::array<offset_t, 2> &wire_array::get_coord() const noexcept { return coord; }

offset_t wire_array::get_coord(direction dir) const noexcept { return coord[to_int(dir)]; }

std::shared_ptr<track_id> wire_array::get_track_id() const noexcept { return tid_ptr; }

const track_id &wire_array::get_track_id_ref() const { return *tid_ptr; }

offset_t wire_array::get_middle() const noexcept { return util::floor2(coord[0] + coord[1]); }

void wire_array::set_track_id(std::shared_ptr<track_id> tid) noexcept { tid_ptr = std::move(tid); }

void wire_array::set_coord(offset_t lower, offset_t upper) noexcept {
    coord[0] = lower;
    coord[1] = upper;
}

warr_rect_iterator wire_array::begin_rect(const routing_grid &grid) const {
    return cbag::layout::begin_rect(grid, *tid_ptr, coord);
}

warr_rect_iterator wire_array::end_rect(const routing_grid &grid) const {
    return cbag::layout::end_rect(grid, *tid_ptr, coord);
}

warr_rect_iterator::warr_rect_iterator() = default;

warr_rect_iterator::warr_rect_iterator(const routing_grid &grid, const track_id &tid,
                                       std::array<offset_t, 2> coord, bool begin)
    : grid_ptr(&grid), tr_ptr(&tid), coord(coord), tr_idx(begin ? 0 : tid.get_num()) {
    auto &tinfo = grid.track_info_at(tid.get_level());
    wire_w = tinfo.get_wire_width(tid.get_ntr());
    ww_idx = 0;
}

bool warr_rect_iterator::operator==(const warr_rect_iterator &rhs) const {
    return tr_idx == rhs.tr_idx && ww_idx == rhs.ww_idx;
}

bool warr_rect_iterator::operator!=(const warr_rect_iterator &rhs) const { return !(*this == rhs); }

std::tuple<layer_t, box_t> warr_rect_iterator::operator*() const {
    auto level = tr_ptr->get_level();

    auto &tinfo = (*grid_ptr)[level];
    auto htr = (*tr_ptr)[tr_idx];
    auto &[rel_htr, w] = wire_w[ww_idx];
    auto c = htr_to_coord(tinfo, rel_htr + htr);
    auto half_w = w / 2;
    return {get_layer_t(*grid_ptr, level, htr + rel_htr),
            box_t(tinfo.get_direction(), coord[0], coord[1], c - half_w, c + half_w)};
}

warr_rect_iterator &warr_rect_iterator::operator++() {
    ++ww_idx;
    if (ww_idx == wire_w.size()) {
        ww_idx = 0;
        ++tr_idx;
    }
    return *this;
}

const routing_grid &warr_rect_iterator::get_grid() const { return *grid_ptr; }

warr_rect_iterator begin_rect(const routing_grid &grid, const track_id &tid,
                              std::array<offset_t, 2> coord) {
    return {grid, tid, coord, true};
}

warr_rect_iterator end_rect(const routing_grid &grid, const track_id &tid,
                            std::array<offset_t, 2> coord) {
    return {grid, tid, coord, false};
}

} // namespace layout
} // namespace cbag


//
// Created by pengpeng on 1/11/21.
//

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/OpticalFlowRad.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <trajectory_msgs/JointTrajectoryPoint.h>

#include <iostream>
#include <string>
#include <fstream>
#include <cmath>
#include <Eigen/Eigen>
#include <algorithm>
#include <chrono>
#include <cstdint>

#define PI 3.1415926
#define number 10
using namespace Eigen;
using namespace std;

mavros_msgs::State current_state;

ros::Time last_time;

Vector3d current_p;
Vector3d current_v,last_current_v;
Vector3d current_a,last_a;

Vector3d planned_p;
Vector3d planned_v;
Vector3d planned_a;

Vector3d last_planned_p(0,0,0);
Vector3d last_planned_v(0,0,0);
Vector3d last_planned_a(0,0,0);

double planned_yaw;
Vector4d set_orientation;
double set_thrust;

std::ofstream outFile;
//
//void state_cb(const mavros_msgs::State::ConstPtr& msg){
//    current_state = *msg;
//}

Eigen::Quaterniond euler2quaternion_eigen(float roll, float pitch, float yaw)
{
    Eigen::Quaterniond temp;
    temp.x() = sin(roll/2)*cos(pitch/2)*cos(yaw/2) - cos(roll/2)*sin(pitch/2)*sin(yaw/2);
    temp.y() = cos(roll/2)*sin(pitch/2)*cos(yaw/2) + sin(roll/2)*cos(pitch/2)*sin(yaw/2);
    temp.z() = cos(roll/2)*cos(pitch/2)*sin(yaw/2) - sin(roll/2)*sin(pitch/2)*cos(yaw/2);
    temp.w() = cos(roll/2)*cos(pitch/2)*cos(yaw/2) + sin(roll/2)*sin(pitch/2)*sin(yaw/2);

    return temp;
}

Eigen::Vector3d quaternion2euler_eigen(float x, float y, float z, float w)
{
    Eigen::Vector3d temp;//roll pitch yaw
    temp.x() = atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));
    temp.y() = asin(2.0 * (-z * x + w * y));
    temp.z() = atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
    return temp;
}

void positionCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    current_p << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
}

void velocity_sub_cb(const geometry_msgs::TwistStamped::ConstPtr& msg)
{
    //ROS_INFO("velocity sub cb!!!");
    current_v << msg->twist.linear.x, msg->twist.linear.y, msg->twist.linear.z;
    //ROS_INFO("VX %f VY %f VZ  %f",current_v(0),current_v(1),current_v(2));
    current_a=(current_v-last_current_v)/(ros::Time::now().toSec()-last_time.toSec());

    if(current_a(0)>100||current_a(0)<-100)
    {
        //ROS_INFO("A TOO BIG");
        current_a=last_a;
    }
//    ROS_INFO("QQQQQ  a0  :%f   a1:%f    a2:%f ",current_a(0),current_a(1),current_a(2));
    last_time=ros::Time::now();
    last_current_v=current_v;
    last_a=current_a;

}

void pvaCallback(const trajectory_msgs::JointTrajectoryPoint::ConstPtr& msg)
{

    planned_p << msg->positions[0], msg->positions[1], msg->positions[2];
    planned_yaw = msg->positions[3];
    planned_v << msg->velocities[0], msg->velocities[1], msg->velocities[2];
    planned_a << msg->accelerations[0], msg->accelerations[1], msg->accelerations[2];
}
void att_ctrl_cb(const mavros_msgs::AttitudeTarget::ConstPtr &msg)
{

    set_orientation <<msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w;
    //roll pitch yaw
    Eigen::Vector3d euler=quaternion2euler_eigen(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w);
    set_thrust=msg->thrust;
//    ROS_INFO("VX %f VY %f VZ  %f",current_v(0),current_v(1),current_v(2));
//规划位置1*3，规划速度1*3，规划加速度1*3，实际位置1*3，实际速度1*3，实际加速度1*3，
// 位置差值1*3，速度差值1*3，加速度差值1*3， 给定si yuan shu 1*4，gei ding ou la jiao1*3,给定推力1*1
    outFile <<planned_p(0)<< ','<< planned_p(1) << ','<<planned_p(2)
            << ','<<planned_v(0)<< ','<< planned_v(1) << ','<<planned_v(2)
            << ','<<planned_a(0)<< ','<< planned_a(1) << ','<<planned_a(2)
            << ','<<current_p(0) << ','<<current_p(1)<<','<<current_p(2)
            << ','<<current_v(0)<< ','<<current_v(1)<< ','<<current_v(2)
            << ','<<current_a(0)<< ','<<current_a(1)<< ','<<current_a(2)

            << ','<<current_p(0)-last_planned_p(0)<< ','<<current_p(1)-last_planned_p(1)<<','<<current_p(2)-last_planned_p(2)
            << ','<<current_v(0)-last_planned_v(0)<< ','<<current_v(1)-last_planned_v(1)<< ','<<current_v(2)-last_planned_v(2)
            << ','<<current_a(0)-last_planned_a(0)<< ','<<current_a(1)-last_planned_a(1)<< ','<<current_a(2)-last_planned_a(2)

            << ','<<set_orientation(0)<< ','<<set_orientation(1)<< ','<<set_orientation(2)<<','<<set_orientation(3)

            << ','<<euler(0)<< ','<<euler(1)<< ','<<euler(2)
            << ','<<set_thrust
            << '\n';
    last_planned_p=planned_p;
    last_planned_v=planned_v;
    last_planned_a=planned_a;

}

/*
void set_zuan_quan_point(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw=0.0)
{

    trajectory_msgs::JointTrajectoryPoint zuan_quan_setpoint;

    zuan_quan_setpoint.positions.push_back(p(0)); //x
    zuan_quan_setpoint.positions.push_back(p(1)); //y
    zuan_quan_setpoint.positions.push_back(p(2)); //z
    zuan_quan_setpoint.positions.push_back(yaw);
    //ROS_INFO("P(0)  %f P(1) %f P(2) %f",p(0),p(1),p(2));

    zuan_quan_setpoint.velocities.push_back(v(0));
    zuan_quan_setpoint.velocities.push_back(v(1));
    zuan_quan_setpoint.velocities.push_back(v(2));

    zuan_quan_setpoint.accelerations.push_back(a(0));
    zuan_quan_setpoint.accelerations.push_back(a(1));
    zuan_quan_setpoint.accelerations.push_back(a(2));

    zuan_quan_point_pub.publish(zuan_quan_setpoint);
}

*/

int main(int argc, char **argv)
{
    ros::init(argc, argv, "mpc_record");
    ros::NodeHandle nh;

    std::string file="random_fly_new/mpc_record_square_fly.csv";

    outFile.open(file, std::ios::out);
    cout<<"record data to "<<file<<endl;

    outFile.trunc;

/*    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 1, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 1);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");*/

//    zuan_quan_point_pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/zuan_quan_setpoint", 1);
    ros::Rate rate(30.0);
    //ROS_INFO("1111");
    ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 1, positionCallback);
    ros::Subscriber pva_sub = nh.subscribe("/pva_setpoint", 1, pvaCallback);
    ros::Subscriber velocity_sub = nh.subscribe<geometry_msgs::TwistStamped>("mavros/local_position/velocity_local", 1, velocity_sub_cb);
    ros::Subscriber att_ctrl_sub = nh.subscribe<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 1,att_ctrl_cb);

    // wait for FCU connection
    while(ros::ok() )
    {

        ros::spinOnce();
        rate.sleep();
    }

}