/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/
#include "NewtonEulerDS.hpp"
#include "BlockVector.hpp"
#include "BlockMatrix.hpp"
#include <boost/math/quaternion.hpp>

#include <iostream>
// #define DEBUG_NOCOLOR
//#define DEBUG_BEGIN_END_ONLY
// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include <debug.h>

void computeMObjToAbs(SP::SiconosVector q, SP::SimpleMatrix mObjToAbs)
{
  DEBUG_BEGIN("computeMObjToAbs(SP::SiconosVector q, SP::SimpleMatrix mObjToAbs)\n");
  double q0 = q->getValue(3);
  double q1 = q->getValue(4);
  double q2 = q->getValue(5);
  double q3 = q->getValue(6);

  ::boost::math::quaternion<double>    quatQ(q0, q1, q2, q3);
  ::boost::math::quaternion<double>    quatcQ(q0, -q1, -q2, -q3);
  ::boost::math::quaternion<double>    quatx(0, 1, 0, 0);
  ::boost::math::quaternion<double>    quaty(0, 0, 1, 0);
  ::boost::math::quaternion<double>    quatz(0, 0, 0, 1);
  ::boost::math::quaternion<double>    quatBuff;
  /*See equation with label eq:newton_Mobjtoabs from the DevNote.pdf
   * Chapter gradient computation, case of NewtonEuler formulation
   * with quaternion
   */
  quatBuff = quatcQ * quatx * quatQ;
  mObjToAbs->setValue(0, 0, quatBuff.R_component_2());
  mObjToAbs->setValue(0, 1, quatBuff.R_component_3());
  mObjToAbs->setValue(0, 2, quatBuff.R_component_4());
  quatBuff = quatcQ * quaty * quatQ;
  mObjToAbs->setValue(1, 0, quatBuff.R_component_2());
  mObjToAbs->setValue(1, 1, quatBuff.R_component_3());
  mObjToAbs->setValue(1, 2, quatBuff.R_component_4());
  quatBuff = quatcQ * quatz * quatQ;
  mObjToAbs->setValue(2, 0, quatBuff.R_component_2());
  mObjToAbs->setValue(2, 1, quatBuff.R_component_3());
  mObjToAbs->setValue(2, 2, quatBuff.R_component_4());
  DEBUG_END("computeMObjToAbs(SP::SiconosVector q, SP::SimpleMatrix mObjToAbs)\n");

}

void computeT(SP::SiconosVector q, SP::SimpleMatrix T)
{
  DEBUG_BEGIN("computeT(SP::SiconosVector q, SP::SimpleMatrix T)\n")
  //  std::cout <<"\n NewtonEulerDS::computeT(SP::SiconosVector q)\n  " <<std::endl;
  double q0 = q->getValue(3) / 2.0;
  double q1 = q->getValue(4) / 2.0;
  double q2 = q->getValue(5) / 2.0;
  double q3 = q->getValue(6) / 2.0;
  T->setValue(3, 3, -q1);
  T->setValue(3, 4, -q2);
  T->setValue(3, 5, -q3);
  T->setValue(4, 3, q0);
  T->setValue(4, 4, -q3);
  T->setValue(4, 5, q2);
  T->setValue(5, 3, q3);
  T->setValue(5, 4, q0);
  T->setValue(5, 5, -q1);
  T->setValue(6, 3, -q2);
  T->setValue(6, 4, q1);
  T->setValue(6, 5, q0);
  DEBUG_END("computeT(SP::SiconosVector q, SP::SimpleMatrix T)\n")

}

// Private function to set linked with members of Dynamical top class
void NewtonEulerDS::connectToDS()
{
  // dim
  _n = 2 * 3;

}

// From a set of data; Mass filled-in directly from a siconosMatrix -
// This constructor leads to the minimum NewtonEuler System form: \f$ M\ddot q = p \f$
/*
Q0 : contains the center of mass coordinate, and the quaternion initial. (dim(Q0)=7)
Velocity0 : contains the initial velocity of center of mass and the omega initial. (dim(Velocity0)=6)
*/
NewtonEulerDS::NewtonEulerDS(): DynamicalSystem(6),
                                _computeJacobianFIntqByFD(false),
                                _computeJacobianFIntvByFD(false),
                                _computeJacobianMIntqByFD(false),
                                _computeJacobianMIntvByFD(false),
                                _epsilonFD(sqrt(std::numeric_limits< double >::epsilon()))
{
  _p.resize(3);
  _p[0].reset(new SiconosVector());
  _p[1].reset(new SiconosVector(_n)); // Needed in NewtonEulerR
  _p[2].reset(new SiconosVector());
  zeroPlugin();
  //assert(0);

  // --- NEWTONEULER INHERITED CLASS MEMBERS ---
  // -- Memory allocation for vector and matrix members --

  _qDim = 7;
  _n = 6;

  // Current state
  _q.reset(new SiconosVector(_qDim));
  // _deltaq.reset(new SiconosVector(_qDim));
  _v.reset(new SiconosVector(_n));

  _dotq.reset(new SiconosVector(_qDim));
  _workspace[freeresidu].reset(new SiconosVector(_n));
  _workspace[free].reset(new SiconosVector(dimension()));
  _massMatrix.reset(new SimpleMatrix(_n, _n));
  _luW.reset(new SimpleMatrix(_n, _n));
  _massMatrix->zero();
  _T.reset(new SimpleMatrix(_qDim, _n));

  _scalarMass = 0.;
}

void NewtonEulerDS::internalInit(SP::SiconosVector Q0, SP::SiconosVector Velocity0,
                                 double mass , SP::SiconosMatrix inertialMatrix)
{
  DEBUG_BEGIN("NewtonEulerDS::internalInit(SP::SiconosVector Q0, SP::SiconosVector Velocity0, double mass , SP::SiconosMatrix inertialMatrix)\n");
  _p.resize(3);
  _p[0].reset(new SiconosVector());
  _p[1].reset(new SiconosVector(_n)); // Needed in NewtonEulerR
  _p[2].reset(new SiconosVector());
  zeroPlugin();
  // --- NEWTONEULER INHERITED CLASS MEMBERS ---
  // -- Memory allocation for vector and matrix members --

  _scalarMass = mass;
  _qDim = 7;
  _n = 6;

  // Initial conditions
  _q0 = Q0;
  _v0 = Velocity0;

  _MObjToAbs.reset(new SimpleMatrix(3, 3));

  // Current state
  _q.reset(new SiconosVector(_qDim));
  // _deltaq.reset(new SiconosVector(_qDim));
  _v.reset(new SiconosVector(_n));
  (*_q) = (*_q0);
  _dotq.reset(new SiconosVector(_qDim));
  _massMatrix.reset(new SimpleMatrix(_n, _n));
  _jacobianFGyrv.reset(new SimpleMatrix(_n, _n));
  _luW.reset(new SimpleMatrix(_n, _n));
  _massMatrix->zero();
  _massMatrix->setValue(0, 0, _scalarMass);
  _massMatrix->setValue(1, 1, _scalarMass);
  _massMatrix->setValue(2, 2, _scalarMass);
  _I = inertialMatrix;
  Index dimIndex(2);
  dimIndex[0] = 3;
  dimIndex[1] = 3;
  Index startIndex(4);
  startIndex[0] = 0;
  startIndex[1] = 0;
  startIndex[2] = 3;
  startIndex[3] = 3;
  setBlock(_I, _massMatrix, dimIndex, startIndex);
  _workspace[freeresidu].reset(new SiconosVector(_n));
  _workspace[free].reset(new SiconosVector(dimension()));

  _T.reset(new SimpleMatrix(_qDim, _n));
  _T->zero();
  _T->setValue(0, 0, 1.0);
  _T->setValue(1, 1, 1.0);
  _T->setValue(2, 2, 1.0);
  computeT();
  computeMObjToAbs();
  initForces();
  DEBUG_END("NewtonEulerDS::internalInit(SP::SiconosVector Q0, SP::SiconosVector Velocity0, double mass , SP::SiconosMatrix inertialMatrix)\n");
}
NewtonEulerDS::NewtonEulerDS(SP::SiconosVector Q0, SP::SiconosVector Velocity0,
                             double  mass, SP::SiconosMatrix inertialMatrix):
  DynamicalSystem(6),
  _computeJacobianFIntqByFD(false),
  _computeJacobianFIntvByFD(false),
  _computeJacobianMIntqByFD(false),
  _computeJacobianMIntvByFD(false),
  _epsilonFD(sqrt(std::numeric_limits< double >::epsilon()))
{
  internalInit(Q0, Velocity0, mass, inertialMatrix);
}

void NewtonEulerDS::zeroPlugin()
{
  _pluginFExt.reset(new PluggedObject());
  _pluginMExt.reset(new PluggedObject());
  _pluginFInt.reset(new PluggedObject());
  _pluginMInt.reset(new PluggedObject());
  _pluginJacqFInt.reset(new PluggedObject());
  _pluginJacvFInt.reset(new PluggedObject());
  _pluginJacqMInt.reset(new PluggedObject());
  _pluginJacvMInt.reset(new PluggedObject());
}

// Destructor
NewtonEulerDS::~NewtonEulerDS()
{
}

bool NewtonEulerDS::checkDynamicalSystem()
{
  bool output = true;
  // ndof

  // q0 and velocity0
  if (! _q0 || ! _v0)
  {
    RuntimeException::selfThrow("NewtonEulerDS::checkDynamicalSystem - initial conditions are badly set.");
    output = false;
  }

  // fInt
  //   if( ( _fInt && computeFIntPtr) && ( ! _jacobianFIntq || ! _jacobianFIntv ) )
  //     // ie if fInt is defined and not constant => its Jacobian must be defined (but not necessarily plugged)
  //     {
  //       RuntimeException::selfThrow("NewtonEulerDS::checkDynamicalSystem - You defined fInt but not its Jacobian (according to q and velocity).");
  //       output = false;
  //     }

  if (!output) std::cout << "NewtonEulerDS Warning: your dynamical system seems to be uncomplete (check = false)" <<std::endl;
  return output;
}

void NewtonEulerDS::initializeNonSmoothInput(unsigned int level)
{
  DEBUG_PRINTF("NewtonEulerDS::initializeNonSmoothInput(unsigned int level) for level = %i\n",level);

  if (_p[level]->size() == 0)
  {
    if (level == 0)
    {
      _p[0]->resize(_qDim);
    }
    else
    {
      _p[level]->resize(_n);
    }
  }

#ifdef DEBUG_MESSAGES
  DEBUG_PRINT("display() after initialization");
  display();
#endif
}

void NewtonEulerDS::initForces()
{
  DEBUG_BEGIN("NewtonEulerDS::initForces()\n")
  _forces.reset(new SiconosVector(_n));
  _fGyr.reset(new SiconosVector(3,0.0));
  _jacobianFGyrv.reset(new SimpleMatrix(_n, _n));
  _jacobianvForces.reset(new SimpleMatrix(_n, _n));
  DEBUG_END("NewtonEulerDS::initForces()\n")
}

void NewtonEulerDS::initRhs(double time)
{
  //  _workMatrix.resize(sizeWorkMat);

  // Solve Mq[2]=fL+p.
  //*_q = *(_p[2]); // Warning: r/p update is done in Interactions/Relations

  if (_forces)
  {
    computeForces(time);
    //      *_q += *_forces;
  }

}

void NewtonEulerDS::initialize(double time, unsigned int sizeOfMemory)
{
  // set q and q[1] to q0 and velocity0, initialize acceleration.
  *_q = *_q0;
  *_v = *_v0;

  // If z has not been set, we initialize it with a null vector of size 1, since z is required in plug-in functions call.
  if (! _z)
    _z.reset(new SiconosVector(1));

  if (_pluginFExt->fPtr && !_fExt)
    _fExt.reset(new SiconosVector(3, 0));

  if (_pluginMExt->fPtr && !_mExt)
    _mExt.reset(new SiconosVector(3, 0));

  if (_pluginFInt->fPtr && !_fInt)
    _fInt.reset(new SiconosVector(3, 0));

  if ((_pluginJacqFInt->fPtr  || _computeJacobianFIntqByFD) && !_jacobianFIntq)
  {
    _jacobianFIntq.reset(new SimpleMatrix(3, _qDim));
    if (!_jacobianqForces)
      _jacobianqForces.reset(new SimpleMatrix(_n, _qDim));
  }

  if ((_pluginJacvFInt->fPtr || _computeJacobianFIntvByFD) && !_jacobianFIntv)
    _jacobianFIntv.reset(new SimpleMatrix(3, _n));

  if (_pluginMInt->fPtr && !_mInt)
    _mInt.reset(new SiconosVector(3, 0));

  if ((_pluginJacqMInt->fPtr || _computeJacobianMIntqByFD) && !_jacobianMIntq)
  {
    if (!_jacobianqForces)
      _jacobianqForces.reset(new SimpleMatrix(_n, _qDim));
    _jacobianMIntq.reset(new SimpleMatrix(3, _qDim));
  }
  if ((_pluginJacvMInt->fPtr || _computeJacobianMIntvByFD) && !_jacobianMIntv)
    _jacobianMIntv.reset(new SimpleMatrix(3, _n));

  // Set links to variables of top-class DynamicalSystem.
  // Warning: this results only in pointers links.
  // No more memory allocation for vectors or matrices.
  connectToDS(); // note that connection can not be done during constructor call, since user can complete the ds after (add plugin or anything else).
  checkDynamicalSystem();

  initRhs(time);

  if (_boundaryConditions)
  {
    _reactionToBoundaryConditions.reset(new SiconosVector(_boundaryConditions->velocityIndices()->size()));
  }

  // Initialize memory vectors
  initMemory(sizeOfMemory);

}

void NewtonEulerDS::computeFExt(double time)
{
  if (_pluginFExt->fPtr)
    ((FExt_NE)_pluginFExt->fPtr)(time, &(*_fExt)(0), _qDim, &(*_q0)(0) ); // parameter z are assumed to be equal to q0
}

void NewtonEulerDS::computeMExt(double time)
{
  if (_pluginMExt->fPtr)
    ((FExt_NE)_pluginFExt->fPtr)(time, &(*_mExt)(0), _qDim, &(*_q0)(0) ); // parameter z are assumed to be equal to q0
}

void NewtonEulerDS::computeFInt(double time)
{
  computeFInt(time, _q, _v);
}

void NewtonEulerDS::computeFInt(double time, SP::SiconosVector q, SP::SiconosVector v)
{
  computeFInt(time,  q,  v, _fInt);
}

void NewtonEulerDS::computeFInt(double time, SP::SiconosVector q, SP::SiconosVector v, SP::SiconosVector fInt)
{
  if (_pluginFInt->fPtr)
    ((FInt_NE)_pluginFInt->fPtr)(time, &(*q)(0), &(*v)(0), &(*fInt)(0), _qDim,  &(*_q0)(0));// parameter z are assumed to be equal to q0
}

void NewtonEulerDS::computeMInt(double time)
{
  computeMInt(time, _q, _v);
}

void NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v)
{
  computeMInt(time, q, v, _mInt);
}

void NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v, SP::SiconosVector mInt)
{
  if (_pluginMInt->fPtr)
    ((FInt_NE)_pluginMInt->fPtr)(time, &(*q)(0), &(*v)(0), &(*mInt)(0), _qDim,  &(*_q0)(0));// parameter z are assumed to be equal to q0
}

void NewtonEulerDS::computeJacobianFIntq(double time)
{
  computeJacobianFIntq(time, _q, _v);
}
void NewtonEulerDS::computeJacobianFIntv(double time)
{
  computeJacobianFIntv(time, _q, _v);
}

void NewtonEulerDS::computeJacobianFIntq(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  DEBUG_PRINT("NewtonEulerDS::computeJacobianFIntq(...) starts");
  if (_pluginJacqFInt->fPtr)
    ((FInt_NE)_pluginJacqFInt->fPtr)(time, &(*q)(0), &(*velocity)(0), &(*_jacobianFIntq)(0, 0), _qDim,  &(*_q0)(0));
  else if (_computeJacobianFIntqByFD)
    computeJacobianFIntqByFD(time, q, velocity);
  DEBUG_EXPR(_jacobianFIntq->display(););
  DEBUG_END("NewtonEulerDS::computeJacobianFIntq(...)");
}

void NewtonEulerDS::computeJacobianFIntqByFD(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobianFIntqByFD(...)\n");
  SP::SiconosVector fInt(new SiconosVector(3));
  computeFInt(time, q, velocity, fInt);

  double fInt0 = fInt->getValue(0);
  double fInt1 = fInt->getValue(1);
  double fInt2 = fInt->getValue(2);

  SP::SiconosVector qeps(new SiconosVector(*q));
  _jacobianFIntq->zero();
  (*qeps)(0) += _epsilonFD;
  for (int j =0; j < 7; j++)
  {
    computeFInt(time, qeps, velocity, fInt);
    _jacobianFIntq->setValue(0,j,  (fInt->getValue(0) - fInt0)/_epsilonFD );
    _jacobianFIntq->setValue(1,j,  (fInt->getValue(1) - fInt1)/_epsilonFD );
    _jacobianFIntq->setValue(2,j,  (fInt->getValue(2) - fInt2)/_epsilonFD );
    (*qeps)(j) -= _epsilonFD;
    if (j<6) (*qeps)(j+1) += _epsilonFD;
  }
  DEBUG_END("NewtonEulerDS::computeJacobianFIntqByFD(...)\n");

}

void NewtonEulerDS::computeJacobianFIntv(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  if (_pluginJacvFInt->fPtr)
    ((FInt_NE)_pluginJacvFInt->fPtr)(time, &(*q)(0), &(*velocity)(0), &(*_jacobianFIntv)(0, 0), _qDim,  &(*_q0)(0));
  else if (_computeJacobianFIntvByFD)
    computeJacobianFIntvByFD(time, q, velocity);
}

void NewtonEulerDS::computeJacobianFIntvByFD(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobianFIntvByFD(...)\n");
  SP::SiconosVector fInt(new SiconosVector(3));
  computeFInt(time, q, velocity, fInt);

  double fInt0 = fInt->getValue(0);
  double fInt1 = fInt->getValue(1);
  double fInt2 = fInt->getValue(2);

  SP::SiconosVector veps(new SiconosVector(*velocity));
  _jacobianFIntv->zero();

  (*veps)(0) += _epsilonFD;
  for (int j =0; j < 6; j++)
  {
    computeFInt(time, q, veps, fInt);
    _jacobianFIntv->setValue(0,j,  (fInt->getValue(0) - fInt0)/_epsilonFD );
    _jacobianFIntv->setValue(1,j,  (fInt->getValue(1) - fInt1)/_epsilonFD );
    _jacobianFIntv->setValue(2,j,  (fInt->getValue(2) - fInt2)/_epsilonFD );
    (*veps)(j) -= _epsilonFD;
    if (j<5) (*veps)(j+1) += _epsilonFD;
  }

  DEBUG_END("NewtonEulerDS::computeJacobianFIntvByFD(...)\n");

}
void NewtonEulerDS::computeJacobianFGyrvByFD(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobianFGyrvByFD(...)\n");
  SP::SiconosVector fGyr(new SiconosVector(3));
  computeFGyr(velocity, fGyr);

  double fGyr0 = fGyr->getValue(0);
  double fGyr1 = fGyr->getValue(1);
  double fGyr2 = fGyr->getValue(2);

  SP::SiconosVector veps(new SiconosVector(*velocity));
  _jacobianFGyrv->zero();

  (*veps)(0) += _epsilonFD;
  for (int j =0; j < 6; j++)
  {
    computeFGyr(veps, fGyr);
    _jacobianFGyrv->setValue(3,j,  (fGyr->getValue(0) - fGyr0)/_epsilonFD );
    _jacobianFGyrv->setValue(4,j,  (fGyr->getValue(1) - fGyr1)/_epsilonFD );
    _jacobianFGyrv->setValue(5,j,  (fGyr->getValue(2) - fGyr2)/_epsilonFD );
    (*veps)(j) -= _epsilonFD;
    if (j<5) (*veps)(j+1) += _epsilonFD;
  }
  DEBUG_EXPR(_jacobianFGyrv->display());
  DEBUG_END("NewtonEulerDS::computeJacobianFGyrvByFD(...)\n");

}
void NewtonEulerDS::computeJacobianMIntq(double time)
{
  computeJacobianMIntq(time, _q, _v);
}
void NewtonEulerDS::computeJacobianMIntv(double time)
{
  computeJacobianMIntv(time, _q, _v);
}

void NewtonEulerDS::computeJacobianMIntq(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntq(...) starts");
  if (_pluginJacqMInt->fPtr)
    ((FInt_NE)_pluginJacqMInt->fPtr)(time, &(*q)(0), &(*velocity)(0), &(*_jacobianMIntq)(0, 0), _qDim,  &(*_q0)(0));
  else if (_computeJacobianMIntqByFD)
    computeJacobianMIntqByFD(time, q, velocity);
  DEBUG_EXPR(_jacobianMIntq->display());
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntq(...) ends");

}

void NewtonEulerDS::computeJacobianMIntqByFD(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntqByFD(...) starts\n");

  SP::SiconosVector mInt(new SiconosVector(3));
  computeMInt(time, q, velocity, mInt);
  double mInt0 = mInt->getValue(0);
  double mInt1 = mInt->getValue(1);
  double mInt2 = mInt->getValue(2);

  SP::SiconosVector qeps(new SiconosVector(*q));

  (*qeps)(0) += _epsilonFD;
  for (int j =0; j < 7; j++)
  {
    computeMInt(time, qeps, velocity, mInt);
    _jacobianMIntq->setValue(0,j,  (mInt->getValue(0) - mInt0)/_epsilonFD );
    _jacobianMIntq->setValue(1,j,  (mInt->getValue(1) - mInt1)/_epsilonFD );
    _jacobianMIntq->setValue(2,j,  (mInt->getValue(2) - mInt2)/_epsilonFD );
    (*qeps)(j) -= _epsilonFD;
    if (j<6) (*qeps)(j+1) += _epsilonFD;
  }
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntqByFD(...) ends\n");
}

void NewtonEulerDS::computeJacobianMIntv(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  if (_pluginJacvMInt->fPtr)
    ((FInt_NE)_pluginJacvMInt->fPtr)(time, &(*q)(0), &(*velocity)(0), &(*_jacobianMIntv)(0, 0), _qDim,  &(*_q0)(0));
  else if (_computeJacobianMIntvByFD)
    computeJacobianMIntvByFD(time,  q, velocity);
}

void NewtonEulerDS::computeJacobianMIntvByFD(double time, SP::SiconosVector q, SP::SiconosVector velocity)
{
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntvByFD(...) starts\n");

  SP::SiconosVector mInt(new SiconosVector(3));
  computeMInt(time, q, velocity, mInt);
  double mInt0 = mInt->getValue(0);
  double mInt1 = mInt->getValue(1);
  double mInt2 = mInt->getValue(2);

  SP::SiconosVector veps(new SiconosVector(*velocity));

  (*veps)(0) += _epsilonFD;
  for (int j =0; j < 6; j++)
  {
    computeMInt(time, q, veps, mInt);
    _jacobianMIntv->setValue(0,j,  (mInt->getValue(0) - mInt0)/_epsilonFD );
    _jacobianMIntv->setValue(1,j,  (mInt->getValue(1) - mInt1)/_epsilonFD );
    _jacobianMIntv->setValue(2,j,  (mInt->getValue(2) - mInt2)/_epsilonFD );
    (*veps)(j) -= _epsilonFD;
    if (j<5) (*veps)(j+1) += _epsilonFD;
  }
  DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntvByFD(...) ends\n");
}

void NewtonEulerDS::computeRhs(double time, bool isDSup)
{
  // if isDSup == true, this means that there is no need to re-compute mass ...
  //  *_q = *(_p[2]); // Warning: r/p update is done in Interactions/Relations
  if (_forces)
  {
    computeForces(time);
    //*_q += *_forces;
  }
}

void NewtonEulerDS::computeJacobianRhsx(double time, bool isDSup)
{
  RuntimeException::selfThrow("NewtonEulerDS::computeJacobianRhsx - not yet implemented.");
}

void NewtonEulerDS::computeForces(double time)
{
  computeForces(time, _q, _v);
}

void NewtonEulerDS::computeFGyr(SP::SiconosVector v, SP::SiconosVector fGyr)
{
  /*computation of \Omega times I \Omega*/
  DEBUG_BEGIN("NewtonEulerDS::computeFGyr(SP::SiconosVector v, SP::SiconosVector fGyr)\n");
  if (_I)
  {
    DEBUG_EXPR( _I->display());
    DEBUG_EXPR( v->display());
    SiconosVector bufOmega(3);
    SiconosVector bufIOmega(3);
    bufOmega.setValue(0, v->getValue(3));
    bufOmega.setValue(1, v->getValue(4));
    bufOmega.setValue(2, v->getValue(5));
    prod(*_I, bufOmega, bufIOmega, true);
    cross_product(bufOmega, bufIOmega, *fGyr);
  }
  DEBUG_EXPR(fGyr->display());
  DEBUG_END("NewtonEulerDS::computeFGyr(SP::SiconosVector v, SP::SiconosVector fGyr)\n");

}
void NewtonEulerDS::computeFGyr(SP::SiconosVector v)
{
  /*computation of \Omega times I \Omega*/
  //DEBUG_BEGIN("NewtonEulerDS::computeFGyr(SP::SiconosVector v)\n");
  computeFGyr( v, _fGyr);
  //DEBUG_END("NewtonEulerDS::computeFGyr(SP::SiconosVector v)\n");

}

void NewtonEulerDS::computeForces(double time, SP::SiconosVector q, SP::SiconosVector v)
{
  // Warning: an operator (fInt ...) may be set (ie allocated and not NULL) but not plugged, that's why two steps are required here.
  if (_forces)
  {
    _forces->zero();
    // 1 - Computes the required functions
    if (_fExt)
    {
      computeFExt(time);
      _forces->setBlock(0, *_fExt);
    }
    if (_mExt)
    {
      computeMExt(time);
      SiconosVector aux(3);
      //computeMObjToAbs();
      prod( *_mExt, *_MObjToAbs, aux); // aux =  transpose(_MObjToAbs) * _mext
      *_mExt = aux;
      _forces->setBlock(3, *_mExt);
    }
    if (_fInt)
    {
      computeFInt(time, q, v);
      // std::cout << "_fInt : "<< std::endl;
      // _fInt->display();
      _forces->setValue(0, _forces->getValue(0) - _fInt->getValue(0));
      _forces->setValue(1, _forces->getValue(1) - _fInt->getValue(1));
      _forces->setValue(2, _forces->getValue(2) - _fInt->getValue(2));

    }
    if (_mInt)
    {
      computeMInt(time, q , v);
      SiconosVector aux(3);
      //computeMObjToAbs();
      prod(*_mInt, *_MObjToAbs, aux);// aux =  transpose(_MObjToAbs) * _mInt
      *_mInt = aux;
      // std::cout << "_MObjToAbs " <<std::endl;
      // _MObjToAbs->display();
      //std::cout << "NewtonEulerDS::computeForces: _mint: " <<std::endl;
      //_mInt->display();
      _forces->setValue(3, _forces->getValue(3) - _mInt->getValue(0));
      _forces->setValue(4, _forces->getValue(4) - _mInt->getValue(1));
      _forces->setValue(5, _forces->getValue(5) - _mInt->getValue(2));
    }

    computeFGyr(v);
    // std::cout << "_fGyr " <<std::endl;
    // _fGyr->display();
    _forces->setValue(3, _forces->getValue(3) - _fGyr->getValue(0));
    _forces->setValue(4, _forces->getValue(4) - _fGyr->getValue(1));
    _forces->setValue(5, _forces->getValue(5) - _fGyr->getValue(2));

    // std::cout << "_forces : "<< std::endl;
    // _forces->display();
  }
  // else nothing.
}

void NewtonEulerDS::computeJacobianqForces(double time)
{
  if (_jacobianqForces)
  {
    _jacobianqForces->zero();
    if (_jacobianFIntq)
    {
      computeJacobianFIntq(time);
      _jacobianqForces->setBlock(0,0,-1.0 * *_jacobianFIntq);
    }
    if (_jacobianMIntq)
    {
      computeJacobianMIntq(time);
      SP::SimpleMatrix aux (new SimpleMatrix(3,_qDim));
      SP::SimpleMatrix RT (new SimpleMatrix(3,3));
      //computeMObjToAbs();
      RT->trans(*_MObjToAbs);
      prod(*RT, *_jacobianMIntq, *aux);
      _jacobianqForces->setBlock(3,0, -1.0* *aux);
      //_jacobianqForces->setBlock(3,0,-1.0* *_jacobianMIntq);
    }
    // std::cout << "_jacobianqForces : "<< std::endl;
    // _jacobianqForces->display();
  }
  //else nothing.
}

void NewtonEulerDS::computeJacobianvForces(double time)
{
  if (_jacobianvForces)
  {
    _jacobianvForces->zero();
    if (_jacobianFIntv)
    {
      computeJacobianFIntv(time);
      _jacobianvForces->setBlock(0,0,-1.0 * *_jacobianFIntv);
    }
    if (_jacobianMIntv)
    {
      computeJacobianMIntv(time);
      _jacobianvForces->setBlock(3,0,-1.0 * *_jacobianMIntv);
    }
    if (_jacobianFGyrv)
    {
      //computeJacobianFGyrvByFD(time,_q,_v);
      computeJacobianFGyrv(time);
      *_jacobianvForces -= *_jacobianFGyrv;
    }
    // std::cout << "_jacobianvForces : "<< std::endl;
    // _jacobianvForces->display();
  }
  //else nothing.
}

void NewtonEulerDS::computeJacobianFGyrv(double time)
{
  DEBUG_BEGIN("NewtonEulerDS::computeJacobianFGyrv(double time) \n");
  if (_jacobianFGyrv)
  {
    //Omega /\ I \Omega:
    _jacobianFGyrv->zero();
    SiconosVector omega(3);
    omega.setValue(0, _v->getValue(3));
    omega.setValue(1, _v->getValue(4));
    omega.setValue(2, _v->getValue(5));
    SiconosVector Iomega(3);
    prod(*_I, omega, Iomega, true);
    SiconosVector ei(3);
    SiconosVector Iei(3);
    SiconosVector ei_Iomega(3);
    SiconosVector omega_Iei(3);

    /*See equation of DevNotes.pdf, equation with label eq:NE_nablaFL1*/
    for (int i = 0; i < 3; i++)
    {
      ei.zero();
      ei.setValue(i, 1.0);
      prod(*_I, ei, Iei, true);
      cross_product(omega, Iei, omega_Iei);
      cross_product(ei, Iomega, ei_Iomega);
      for (int j = 0; j < 3; j++)
        _jacobianFGyrv->setValue(3 + j, 3 + i, ei_Iomega.getValue(j) + omega_Iei.getValue(j));
    }
    // Check if Jacobian is valid. Warning to the transpose operation in
    // _jacobianFGyrv->setValue(3 + j, 3 + i, ei_Iomega.getValue(j) + omega_Iei.getValue(j));
  }
  //else nothing.
  DEBUG_EXPR(_jacobianFGyrv->display());
  DEBUG_END("NewtonEulerDS::computeJacobianFGyrv(double time) \n");
}

void NewtonEulerDS::display() const
{
  std::cout << "=====> NewtonEuler System display (number: " << _number << ")." <<std::endl;
  std::cout << "- _n : " << _n <<std::endl;
  std::cout << "- q " <<std::endl;
  if (_q) _q->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- q0 " <<std::endl;
  if (_q0) _q0->display();
  std::cout << "- v " <<std::endl;
  if (_v) _v->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- v0 " <<std::endl;
  if (_v0) _v0->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- dotq " <<std::endl;
  if (_dotq) _dotq->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- p[0] " <<std::endl;
  if (_p[0]) _p[0]->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- p[1] " <<std::endl;
  if (_p[1]) _p[1]->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "- p[2] " <<std::endl;
  if (_p[2]) _p[2]->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "mass :" <<  _scalarMass <<std::endl;
  std::cout << "Inertia :" <<std::endl;
  if (_I) _I->display();
  else std::cout << "-> NULL" <<std::endl;
  std::cout << "===================================== " <<std::endl;
}

// --- Functions for memory handling ---
void NewtonEulerDS::initMemory(unsigned int steps)
{
  DynamicalSystem::initMemory(steps);

  if (steps == 0)
    std::cout << "Warning : NewtonEulerDS::initMemory with size equal to zero" <<std::endl;
  else
  {
    _qMemory.reset(new SiconosMemory(steps, _qDim));
    _vMemory.reset(new SiconosMemory(steps, _n));
    _forcesMemory.reset(new SiconosMemory(steps, _n));
    _dotqMemory.reset(new SiconosMemory(steps, _qDim));
    swapInMemory();
  }
}

void NewtonEulerDS::swapInMemory()
{
  //  _xMemory->swap(_x[0]);
  _qMemory->swap(*_q);
  _vMemory->swap(*_v);
  _dotqMemory->swap(*_dotq);
  _forcesMemory->swap(*_forces);
}

void NewtonEulerDS::resetAllNonSmoothPart()
{
  if (_p[1])
    _p[1]->zero();
  else
    _p[1].reset(new SiconosVector(_n));
}
void NewtonEulerDS::resetNonSmoothPart(unsigned int level)
{
  if (_p[level]->size() > 0)
    _p[level]->zero();
}

void NewtonEulerDS::computeT()
{
  ::computeT(_q,_T);
}

void NewtonEulerDS::computeTdot()
{
  if (!_Tdot)
  {
    _Tdot.reset(new SimpleMatrix(_qDim, _n));
    _Tdot->zero();
  }

  ::computeT(_dotq,_Tdot);
}

void NewtonEulerDS::normalizeq()
{
  double normq = sqrt(_q->getValue(3) * _q->getValue(3) + _q->getValue(4) * _q->getValue(4) + _q->getValue(5) * _q->getValue(5) + _q->getValue(6) * _q->getValue(6));
  assert(normq > 0);
  normq = 1 / normq;
  _q->setValue(3, _q->getValue(3) * normq);
  _q->setValue(4, _q->getValue(4) * normq);
  _q->setValue(5, _q->getValue(5) * normq);
  _q->setValue(6, _q->getValue(6) * normq);
}
void NewtonEulerDS::computeMObjToAbs()
{
  ::computeMObjToAbs(_q, _MObjToAbs);
}

void NewtonEulerDS::setComputeJacobianFIntqFunction(const std::string&  pluginPath, const std::string&  functionName)
{
  //    Plugin::setFunction(&computeJacobianFIntqPtr, pluginPath,functionName);
  _pluginJacqFInt->setComputeFunction(pluginPath, functionName);
}
void NewtonEulerDS::setComputeJacobianFIntvFunction(const std::string&  pluginPath, const std::string&  functionName)
{
  //    Plugin::setFunction(&computeJacobianFIntvPtr, pluginPath,functionName);
  _pluginJacvFInt->setComputeFunction(pluginPath, functionName);
}
void NewtonEulerDS::setComputeJacobianFIntqFunction(FInt_NE fct)
{
  _pluginJacqFInt->setComputeFunction((void *)fct);
}
void NewtonEulerDS::setComputeJacobianFIntvFunction(FInt_NE fct)
{
  _pluginJacvFInt->setComputeFunction((void *)fct);
}

void NewtonEulerDS::setComputeJacobianMIntqFunction(const std::string&  pluginPath, const std::string&  functionName)
{
  //    Plugin::setFunction(&computeJacobianFIntqPtr, pluginPath,functionName);
  _pluginJacqMInt->setComputeFunction(pluginPath, functionName);
}
void NewtonEulerDS::setComputeJacobianMIntvFunction(const std::string&  pluginPath, const std::string&  functionName)
{
  //    Plugin::setFunction(&computeJacobianFIntvPtr, pluginPath,functionName);
  _pluginJacvMInt->setComputeFunction(pluginPath, functionName);
}
void NewtonEulerDS::setComputeJacobianMIntqFunction(FInt_NE fct)
{
  _pluginJacqMInt->setComputeFunction((void *)fct);
}
void NewtonEulerDS::setComputeJacobianMIntvFunction(FInt_NE fct)
{
  _pluginJacvMInt->setComputeFunction((void *)fct);
}

double NewtonEulerDS::computeKineticEnergy()
{
  DEBUG_BEGIN("NewtonEulerDS::computeKineticEnergy()\n");
  assert(_v);
  assert(_massMatrix);
  DEBUG_EXPR(_v->display());
  DEBUG_EXPR(_massMatrix->display());

  SiconosVector tmp(6);
  prod(*_massMatrix, *_v, tmp, true);
  double K =0.5*inner_prod(tmp,*_v);

  DEBUG_PRINTF("Kinetic Energy = %e\n", K);
  DEBUG_END("NewtonEulerDS::computeKineticEnergy()\n");
  return K;
}


// This nodelet is meant to subscribe to so3 commands and then convert them
// to geometry_msgs/Twist which is the type for cmd_vel, the crazyflie control
// topic. The format of this is as follows:
//  linear.y = roll     [-30 to 30 degrees]         (may be negative)
//  linear.x = pitch    [-30 to 30 degrees]         (may be negative)
//  linear.z = thrust   [0 to 60,000]               (motors stiction around 2000)
//  angular.z = yawrate [-200 to 200 degrees/second] (note this is not yaw!)

#include <geometry_msgs/Twist.h>
#include <kr_mav_msgs/SO3Command.h>
#include <nav_msgs/Odometry.h>
#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <Eigen/Geometry>

// TODO: Remove CLAMP as macro
#define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x)

class SO3CmdToCrazyflie : public nodelet::Nodelet
{
 public:
  void onInit(void);

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 private:
  void so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg);
  void odom_callback(const nav_msgs::Odometry::ConstPtr &odom);

  bool odom_set_, so3_cmd_set_;
  Eigen::Quaterniond q_odom_;

  ros::Publisher attitude_raw_pub_;
  ros::Publisher crazy_fast_cmd_vel_pub_, crazy_cmd_vel_pub_;

  ros::Subscriber so3_cmd_sub_;
  ros::Subscriber odom_sub_;

  double so3_cmd_timeout_;
  ros::Time last_so3_cmd_time_;
  kr_mav_msgs::SO3Command last_so3_cmd_;

  double c1_;
  double c2_;
  double c3_;

  // TODO get rid of this for the gains coming in
  double kp_yaw_rate_;

  int thrust_pwm_min_;  // Necessary to overcome stiction
  int thrust_pwm_max_;  // Mapped to PWM

  int motor_status_;
};

void SO3CmdToCrazyflie::odom_callback(const nav_msgs::Odometry::ConstPtr &odom)
{
  if(!odom_set_)
    odom_set_ = true;

  q_odom_ = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x,
                               odom->pose.pose.orientation.y, odom->pose.pose.orientation.z);

  if(so3_cmd_set_ && ((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_))
  {
    // ROS_WARN("so3_cmd timeout. %f seconds since last command",
    //       (ros::Time::now() - last_so3_cmd_time_).toSec());
    const auto last_so3_cmd_ptr = boost::make_shared<kr_mav_msgs::SO3Command>(last_so3_cmd_);

    so3_cmd_callback(last_so3_cmd_ptr);
  }
}

void SO3CmdToCrazyflie::so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg)
{
  if(!so3_cmd_set_)
    so3_cmd_set_ = true;

  // switch on motors
  if(msg->aux.enable_motors)
  {
    // If the crazyflie motors are timed out, we need to send a zero message in order to get them to start
    if(motor_status_ < 3)
    {
      geometry_msgs::Twist::Ptr motors_vel_cmd(new geometry_msgs::Twist);
      crazy_cmd_vel_pub_.publish(motors_vel_cmd);
      last_so3_cmd_ = *msg;
      last_so3_cmd_time_ = msg->header.stamp;
      motor_status_ += 1;
      return;
    }
    // After sending zero message send min thrust
    if(motor_status_ < 10)
    {
      geometry_msgs::Twist::Ptr motors_vel_cmd(new geometry_msgs::Twist);
      motors_vel_cmd->linear.z = thrust_pwm_min_;
      crazy_cmd_vel_pub_.publish(motors_vel_cmd);
    }
    motor_status_ += 1;
  }
  else if(!msg->aux.enable_motors)
  {
    motor_status_ = 0;
    geometry_msgs::Twist::Ptr motors_vel_cmd(new geometry_msgs::Twist);
    crazy_cmd_vel_pub_.publish(motors_vel_cmd);
    last_so3_cmd_ = *msg;
    last_so3_cmd_time_ = msg->header.stamp;
    return;
  }

  /*
  // If the crazyflie motors are timed out, we need to send a zero message in order to get them to start
  if((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_){
    crazy_cmd_vel_pub_.publish(crazy_vel_cmd);
    // Keep in mind that this publishes two messages in a row, this may cause bandwith problems
  }
  */

  // grab desired forces and rotation from so3
  const Eigen::Vector3d f_des(msg->force.x, msg->force.y, msg->force.z);

  const Eigen::Quaterniond q_des(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);

  // check psi for stability
  const Eigen::Matrix3d R_des(q_des);
  const Eigen::Matrix3d R_cur(q_odom_);

  const float yaw_cur = std::atan2(R_cur(1, 0), R_cur(0, 0));
  const float yaw_des = std::atan2(R_des(1, 0), R_des(0, 0));

  // Map these into the current body frame (based on yaw)
  Eigen::Matrix3d R_des_new = R_des * Eigen::AngleAxisd(yaw_cur - yaw_des, Eigen::Vector3d::UnitZ());
  float pitch_des = -std::asin(R_des_new(2, 0));
  float roll_des = std::atan2(R_des_new(2, 1), R_des_new(2, 2));

  roll_des = roll_des * 180 / M_PI;
  pitch_des = pitch_des * 180 / M_PI;

  double thrust_f = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2);  // Force in Newtons
  // ROS_INFO("thrust_f is %2.5f newtons", thrust_f);
  thrust_f = std::max(thrust_f, 0.0);

  thrust_f = thrust_f * 1000 / 9.81;  // Force in grams
  // ROS_INFO("thrust_f is %2.5f grams", thrust_f);

  // ROS_INFO("coeffs are %2.2f, %2.2f, %2.2f", c1_, c2_, c3_);
  float thrust_pwm = c1_ + c2_ * std::sqrt(c3_ + thrust_f);

  // ROS_INFO("thrust_pwm is %2.5f from 0-1", thrust_pwm);

  thrust_pwm = thrust_pwm * thrust_pwm_max_;  // thrust_pwm mapped from 0-60000
  // ROS_INFO("thrust_pwm is calculated to be %2.5f in pwm", thrust_pwm);

  geometry_msgs::Twist::Ptr crazy_vel_cmd(new geometry_msgs::Twist);

  float e_yaw = yaw_des - yaw_cur;
  if(e_yaw > M_PI)
    e_yaw -= 2 * M_PI;
  else if(e_yaw < -M_PI)
    e_yaw += 2 * M_PI;

  float yaw_rate_des = ((-msg->kR[2] * e_yaw) - msg->angular_velocity.z) * (180 / M_PI);

  // TODO change this check to be a param
  // throttle the publish rate
  // if ((ros::Time::now() - last_so3_cmd_time_).toSec() > 1.0/30.0){
  crazy_vel_cmd->linear.y = roll_des + msg->aux.angle_corrections[0];
  crazy_vel_cmd->linear.x = pitch_des + msg->aux.angle_corrections[1];
  crazy_vel_cmd->linear.z = CLAMP(thrust_pwm, thrust_pwm_min_, thrust_pwm_max_);

  // ROS_INFO("commanded thrust is %2.2f", CLAMP(thrust_pwm, thrust_pwm_min_, thrust_pwm_max_));

  crazy_vel_cmd->angular.z = yaw_rate_des;
  // ROS_INFO("commanded yaw rate is %2.2f", yaw_rate_des); //yaw_debug

  crazy_fast_cmd_vel_pub_.publish(crazy_vel_cmd);
  // save last so3_cmd
  last_so3_cmd_ = *msg;
  // last_so3_cmd_time_ = ros::Time::now();
  last_so3_cmd_time_ = msg->header.stamp;
  //}
  // else {
  //  ROS_INFO_STREAM("Commands too quick, time since is: " << (ros::Time::now() - last_so3_cmd_time_).toSec());
  //}
}

void SO3CmdToCrazyflie::onInit(void)
{
  ros::NodeHandle priv_nh(getPrivateNodeHandle());

  // get thrust scaling parameters
  // Note that this is ignoring a constant based on the number of props, which
  // is captured with the lin_cof_a variable later.
  //
  // TODO this is where we load thrust scaling stuff
  if(priv_nh.getParam("kp_yaw_rate", kp_yaw_rate_))
    ROS_INFO("kp yaw rate is %2.2f", kp_yaw_rate_);
  else
    ROS_FATAL("kp yaw rate not found");

  // get thrust scaling parameters
  if(priv_nh.getParam("c1", c1_) && priv_nh.getParam("c2", c2_) && priv_nh.getParam("c3", c3_))
    ROS_INFO("Using %2.2f, %2.2f, %2.2f for thrust mapping", c1_, c2_, c3_);
  else
    ROS_FATAL("Must set coefficients for thrust scaling");

  // get param for so3 command timeout duration
  priv_nh.param("so3_cmd_timeout", so3_cmd_timeout_, 0.1);

  priv_nh.param("thrust_pwm_max", thrust_pwm_max_, 60000);
  priv_nh.param("thrust_pwm_min", thrust_pwm_min_, 10000);

  odom_set_ = false;
  so3_cmd_set_ = false;
  motor_status_ = 0;

  // TODO make sure this is publishing to the right place
  crazy_fast_cmd_vel_pub_ = priv_nh.advertise<geometry_msgs::Twist>("cmd_vel_fast", 10);

  crazy_cmd_vel_pub_ = priv_nh.advertise<geometry_msgs::Twist>("cmd_vel", 10);

  so3_cmd_sub_ =
      priv_nh.subscribe("so3_cmd", 1, &SO3CmdToCrazyflie::so3_cmd_callback, this, ros::TransportHints().tcpNoDelay());

  odom_sub_ =
      priv_nh.subscribe("odom", 10, &SO3CmdToCrazyflie::odom_callback, this, ros::TransportHints().tcpNoDelay());
}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(SO3CmdToCrazyflie, nodelet::Nodelet);


//  Copyright (c) 2001-2010 Hartmut Kaiser
//  Copyright (c) 2009 Carl Barron
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/detail/lightweight_test.hpp>
#include <boost/spirit/include/lex_lexertl.hpp>
#include <iostream>
#include <algorithm>
#include "matlib.h"

void test_matrix(std::vector<std::vector<double> > const& x)
{
    BOOST_TEST(x.size() == 3);
    BOOST_TEST(x[0].size() == 2 && x[0][0] == 1 && x[0][1] == 2);
    BOOST_TEST(x[1].size() == 1 && x[1][0] == 3);
    BOOST_TEST(x[2].size() == 3 && x[2][0] == 4 && x[2][1] == 5 && x[2][2] == 6);
}

int main ()
{
    std::string input("[[1,2][3][4,5,6]]");

    std::vector<std::vector<double> > results;
    typedef std::string::iterator iter;
    typedef boost::spirit::lex::lexertl::actor_lexer<
        boost::spirit::lex::lexertl::token<iter>
    > lexer_type;

    typedef matlib_tokens<lexer_type> matlib_type;
    matlib_type  matrix(results);
    iter first = input.begin();

    try {
        BOOST_TEST(boost::spirit::lex::tokenize(first, input.end(), matrix));
        test_matrix(results);
    }
    catch (std::runtime_error const& e) {
        std::cerr << "caught exception: " << e.what() << std::endl;
        BOOST_TEST(false);
    }
    return boost::report_errors();
}


/*

Copyright (c) 2005-2021, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef _ALARCON2004OXYGENBASEDCELLCYCLEODESYSTEM_HPP_
#define _ALARCON2004OXYGENBASEDCELLCYCLEODESYSTEM_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include <cmath>

#include "AbstractOdeSystem.hpp"

/**
 * Represents the Alarcon et al. (2004) system of ODEs (see ticket #461).
 * [doi:10.1016/j.jtbi.2004.04.016]
 *
 * The variables are
 *
 *  0. x = Cdh1-APC complexes
 *  1. y = cyclin-CDK
 *  2. z = p27
 *  3. m = mass
 *  4. u = RBNP
 *  5. P = oxygen concentration
 */
class Alarcon2004OxygenBasedCellCycleOdeSystem : public AbstractOdeSystem
{
private:

    /**
     * Constants for the Alarcon et al. (2004) model
     */

    /** Dimensionless parameter a_1. */
    double ma1;
    /** Dimensionless parameter a_2. */
    double ma2;
    /** Dimensionless parameter a_3. */
    double ma3;
    /** Dimensionless parameter a_4. */
    double ma4;
    /** Dimensionless parameter b_3. */
    double mb3;
    /** Dimensionless parameter b_4. */
    double mb4;
    /** Dimensionless parameter c_1. */
    double mc1;
    /** Dimensionless parameter c_2. */
    double mc2;
    /** Dimensionless parameter d_1. */
    double md1;
    /** Dimensionless parameter d_2. */
    double md2;
    /** Dimensionless parameter J_3. */
    double mJ3;
    /** Dimensionless parameter J_4. */
    double mJ4;
    /** Dimensionless parameter eta. */
    double mEta;
    /** Dimensionless parameter m_star. */
    double mMstar;
    /** Dimensionless parameter B. */
    double mB;
    /** Dimensionless parameter x_THR. */
    double mxThreshold;
    /** Dimensionless parameter y_THR. */
    double myThreshold;

    /** The oxygen concentration (this affects the ODE system). */
    double mOxygenConcentration;

    /** Whether the cell associated with this cell cycle ODE system is labelled (this affects the ODE system). */
    bool mIsLabelled;

    friend class boost::serialization::access;
    /**
     * Serialize the object and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractOdeSystem>(*this);
    }

public:

    /**
     * Constructor.
     *
     * @param oxygenConcentration is a non-dimensional oxygen concentration value between 0 and 1
     * @param isLabelled whether the cell associated with this cell cycle ODE system is labelled (this affects the ODE system)
     * @param stateVariables optional initial conditions for state variables (only used in archiving)
     */
    Alarcon2004OxygenBasedCellCycleOdeSystem(double oxygenConcentration,
                                             bool isLabelled,
                                             std::vector<double> stateVariables=std::vector<double>());

    /**
     * Destructor.
     */
    ~Alarcon2004OxygenBasedCellCycleOdeSystem();

    /**
     * Initialise parameter values.
     */
    void Init();

    /**
     * Compute the RHS of the Alarcon et al. (2004) system of ODEs.
     *
     * Returns a vector representing the RHS of the ODEs at each time step, y' = [y1' ... yn'].
     * An ODE solver will call this function repeatedly to solve for y = [y1 ... yn].
     *
     * @param time used to evaluate the RHS.
     * @param rY value of the solution vector used to evaluate the RHS.
     * @param rDY filled in with the resulting derivatives (using Alarcons et al. (2004) system of equations).
     */
    void EvaluateYDerivatives(double time, const std::vector<double>& rY, std::vector<double>& rDY);

    /**
     * Calculate whether the conditions for the cell cycle to finish have been met.
     *
     * @param time at which to calculate whether the stopping event has occurred
     * @param rY value of the solution vector used to evaluate the RHS.
     *
     * @return whether or not stopping conditions have been met
     */
    bool CalculateStoppingEvent(double time, const std::vector<double>& rY);

    /**
     * Set #mIsLabelled.
     *
     * @param isLabelled whether the cell associated with this cell cycle ODE system is labelled (this affects the ODE system)
     */
    void SetIsLabelled(bool isLabelled);

    /**
     * @return #mIsLabelled.
     */
    bool IsLabelled() const;

    /**
     * @return #mOxygenConcentration.
     */
    double GetOxygenConcentration() const;
};

// Declare identifier for the serializer
#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(Alarcon2004OxygenBasedCellCycleOdeSystem)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct an Alarcon2004OxygenBasedCellCycleOdeSystem.
 */
template<class Archive>
inline void save_construct_data(
    Archive & ar, const Alarcon2004OxygenBasedCellCycleOdeSystem * t, const unsigned int file_version)
{
    // Save data required to construct instance
    const double oxygen_concentration = t->GetOxygenConcentration();
    ar & oxygen_concentration;

    const bool is_labelled = t->IsLabelled();
    ar & is_labelled;

    const std::vector<double>& state_variables = t->rGetConstStateVariables();
    ar & state_variables;
}

/**
 * De-serialize constructor parameters and initialise an Alarcon2004OxygenBasedCellCycleOdeSystem.
 */
template<class Archive>
inline void load_construct_data(
    Archive & ar, Alarcon2004OxygenBasedCellCycleOdeSystem * t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    double oxygen_concentration;
    ar & oxygen_concentration;

    bool is_labelled;
    ar & is_labelled;

    std::vector<double> state_variables;
    ar & state_variables;

    // Invoke inplace constructor to initialise instance
    ::new(t)Alarcon2004OxygenBasedCellCycleOdeSystem(oxygen_concentration, is_labelled, state_variables);
}
}
} // namespace ...

#endif /*_ALARCON2004OXYGENBASEDCELLCYCLEODESYSTEM_HPP_*/


/* Boost interval/compare/explicit.hpp template implementation file
 *
 * Copyright 2000 Jens Maurer
 * Copyright 2002 Hervé Brönnimann, Guillaume Melquiond, Sylvain Pion
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_INTERVAL_COMPARE_EXPLICIT_HPP
#define BOOST_NUMERIC_INTERVAL_COMPARE_EXPLICIT_HPP

#include <boost/numeric/interval/detail/interval_prototype.hpp>

namespace boost {
namespace numeric {
namespace interval_lib {

/*
 * Certainly... operations
 */

template<class T, class Policies1, class Policies2> inline
bool cerlt(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  return x.upper() < y.lower();
}

template<class T, class Policies> inline
bool cerlt(const interval<T, Policies>& x, const T& y)
{
  return x.upper() < y;
}

template<class T, class Policies> inline
bool cerlt(const T& x, const interval<T, Policies>& y)
{
  return x < y.lower();
}

template<class T, class Policies1, class Policies2> inline
bool cerle(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  return x.upper() <= y.lower();
}

template<class T, class Policies> inline
bool cerle(const interval<T, Policies>& x, const T& y)
{
  return x.upper() <= y;
}

template<class T, class Policies> inline
bool cerle(const T& x, const interval<T, Policies>& y)
{
  return x <= y.lower();
}

template<class T, class Policies1, class Policies2> inline
bool cergt(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  return x.lower() > y.upper();
}

template<class T, class Policies> inline
bool cergt(const interval<T, Policies>& x, const T& y)
{
  return x.lower() > y;
}

template<class T, class Policies> inline
bool cergt(const T& x, const interval<T, Policies>& y)
{
  return x > y.upper();
}

template<class T, class Policies1, class Policies2> inline
bool cerge(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  return x.lower() >= y.upper();
}

template<class T, class Policies> inline
bool cerge(const interval<T, Policies>& x, const T& y)
{
  return x.lower() >= y;
}

template<class T, class Policies> inline
bool cerge(const T& x, const interval<T, Policies>& y)
{
  return x >= y.upper();
}

template<class T, class Policies1, class Policies2> inline
bool cereq(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  return x.lower() == y.upper() && y.lower() == x.upper();
}

template<class T, class Policies> inline
bool cereq(const interval<T, Policies>& x, const T& y)
{
  return x.lower() == y && x.upper() == y;
}

template<class T, class Policies> inline
bool cereq(const T& x, const interval<T, Policies>& y)
{
  return x == y.lower() && x == y.upper();
}

template<class T, class Policies1, class Policies2> inline
bool cerne(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  return x.upper() < y.lower() || y.upper() < x.lower();
}

template<class T, class Policies> inline
bool cerne(const interval<T, Policies>& x, const T& y)
{
  return x.upper() < y || y < x.lower();
}

template<class T, class Policies> inline
bool cerne(const T& x, const interval<T, Policies>& y)
{
  return x < y.lower() || y.upper() < x;
}

/*
 * Possibly... comparisons
 */

template<class T, class Policies1, class Policies2> inline
bool poslt(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  return x.lower() < y.upper();
}

template<class T, class Policies> inline
bool poslt(const interval<T, Policies>& x, const T& y)
{
  return x.lower() < y;
}

template<class T, class Policies> inline
bool poslt(const T& x, const interval<T, Policies>& y)
{
  return x < y.upper();
}

template<class T, class Policies1, class Policies2> inline
bool posle(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  return x.lower() <= y.upper();
}

template<class T, class Policies> inline
bool posle(const interval<T, Policies>& x, const T& y)
{
  return x.lower() <= y;
}

template<class T, class Policies> inline
bool posle(const T& x, const interval<T, Policies>& y)
{
  return x <= y.upper();
}

template<class T, class Policies1, class Policies2> inline
bool posgt(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  return x.upper() > y.lower();
}

template<class T, class Policies> inline
bool posgt(const interval<T, Policies>& x, const T& y)
{
  return x.upper() > y;
}

template<class T, class Policies> inline
bool posgt(const T& x, const interval<T, Policies> & y)
{
  return x > y.lower();
}

template<class T, class Policies1, class Policies2> inline
bool posge(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  return x.upper() >= y.lower();
}

template<class T, class Policies> inline
bool posge(const interval<T, Policies>& x, const T& y)
{
  return x.upper() >= y;
}

template<class T, class Policies> inline
bool posge(const T& x, const interval<T, Policies>& y)
{
  return x >= y.lower();
}

template<class T, class Policies1, class Policies2> inline
bool poseq(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  return x.upper() >= y.lower() && y.upper() >= x.lower();
}

template<class T, class Policies> inline
bool poseq(const interval<T, Policies>& x, const T& y)
{
  return x.upper() >= y && y >= x.lower();
}

template<class T, class Policies> inline
bool poseq(const T& x, const interval<T, Policies>& y)
{
  return x >= y.lower() && y.upper() >= x;
}

template<class T, class Policies1, class Policies2> inline
bool posne(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  return x.upper() != y.lower() || y.upper() != x.lower();
}

template<class T, class Policies> inline
bool posne(const interval<T, Policies>& x, const T& y)
{
  return x.upper() != y || y != x.lower();
}

template<class T, class Policies> inline
bool posne(const T& x, const interval<T, Policies>& y)
{
  return x != y.lower() || y.upper() != x;
}

} // namespace interval_lib
} // namespace numeric
} //namespace boost

#endif // BOOST_NUMERIC_INTERVAL_COMPARE_EXPLICIT_HPP


/** Calibration for friction/gravity compensation.
  *
**/
#include "ros/ros.h"
#include "cc2.h"
#include <iostream>
#include <bosch_arm_control/ArmState.h>
#include <boost/lexical_cast.hpp>
#include <bosch_arm_control/PointCmd.h>

using namespace std;
ros::Publisher joint_cmd_pub;
bosch_arm_control::PointCmd cmd;
bosch_arm_control::ArmState js;

double rate=360;
ros::Rate* loop_rate;

void jointStateCallBack(const bosch_arm_control::ArmState& msg)
{
  js=msg;
  cout<<"receive joint states\n";
}
//TODO: look at pr2's trajectory generation and the trajectory controller.
//The trajectory generation can be quite general, given the coordinate, speed
//and acceleration of the start and end point, output a smooth trajectory
//connecting two points.
//the trajectory controller should take some sparse way points or motion constraints,
//and generate the best trajectory based on the dynamic property of the arm
//However, no matter how smooth it is, way points are still artificial.
//is there a controll mechanism that do not need any way points but
//uses the constraints directly?

//initialized joint state and cmd.
void initialize()
{
  //set the first command as the current positon.
  ros::spinOnce();

  for (int i=0;i<4;i++)
  {
    cmd.coordinates[i]=js.position[i];
    cmd.velocity[i]=0;
    cmd.acceleration[i]=0;
  }
  //for the connection set up delay.
  for (int i=0;i<3;i++)
  {
    cmd.header.stamp=ros::Time::now();
    joint_cmd_pub.publish(cmd);
    loop_rate->sleep();
  }
}

void move_joints_relative(const double * rel,double t, double *torque)
{
  double init_cmd_pos[4];
  for (int i=0;i<4;i++)
  {
    init_cmd_pos[i]=cmd.coordinates[i];
  }
  //a simple trajectory:accelerate, linear, decelerate, still
  int t_end=int(t*rate);
  int t_dec=t_end-5;
  int t_acc=t_dec/3;
  int t_lin=t_dec-t_acc;
  double acc[4];
  for (int i=0;i<4;i++)
  {
    acc[i]=rel[i]/(2*t_acc*t_acc);
  }

  for (int t=1;t<=t_end;t++)
  {
    if (!ros::ok())
    {
      cout<<"calibration interrupted."<<endl;
      break;
    }
    for (int i=0;i<4;i++)
    {
      if (t<=t_acc)
      {
        cmd.velocity[i]+=acc[i];
        cmd.coordinates[i]+=cmd.velocity[i];
        cmd.acceleration[i]=acc[i];
      }
      else if (t<=t_lin)
      {
        cmd.velocity[i]=t_acc*acc[i];
        cmd.coordinates[i]+=cmd.velocity[i];
        cmd.acceleration[i]=0;
      }
      else if (t<=t_end)
      {
        cmd.velocity[i]-=acc[i];
        cmd.coordinates[i]+=cmd.velocity[i];
        cmd.acceleration[i]=-acc[i];
      }
      else
      {
        cmd.coordinates[i]=init_cmd_pos[i]+rel[i];
        cmd.velocity[i]=0;
        cmd.acceleration[i]=0;
      }

    }
    cmd.header.stamp=ros::Time::now();
    joint_cmd_pub.publish(cmd);
    loop_rate->sleep();
  }
  cout<<js.header.stamp.sec<<','<<js.header.stamp.nsec<<endl;
  ros::spinOnce();
  cout<<js.header.stamp.sec<<','<<js.header.stamp.nsec<<endl;
  sleep(2);
  ros::spinOnce();
  //read the torques
  for (int i=0;i<4;i++)
  {
    torque[i]=js.effort[i];
    cout<<torque[i]<<',';
  }
  cout<<js.header.stamp.sec<<','<<js.header.stamp.nsec<<endl;
}

void joint_linear_motion(double *v, double t_acc)
{
  double init_cmd_pos[4];
  for (int i=0;i<4;i++)
  {
    init_cmd_pos[i]=cmd.coordinates[i];
  }
  double vstep[4];
  int n_acc=(int)(t_acc*rate);
  for(int i=0;i<4;i++)
    vstep[i]=(v[i]/rate)/n_acc;
  int n=0;
  while(ros::ok())
  {
    n++;
    for(int i=0;i<4;i++)
    {
      if(n<=n_acc)
      {
        cmd.acceleration[i]=vstep[i];
        cmd.velocity[i]+=cmd.acceleration[i];
        cmd.coordinates[i]+=cmd.velocity[i];
      }
      else
      {
        cmd.acceleration[i]=0;
        //cmd.velocity[i]+=cmd.acceleration[i];
        cmd.coordinates[i]+=cmd.velocity[i];
      }
    }
    cmd.header.stamp=ros::Time::now();
    joint_cmd_pub.publish(cmd);
    loop_rate->sleep();
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "calibration");
  ros::NodeHandle n;
  joint_cmd_pub =  n.advertise<bosch_arm_control::PointCmd> ("/joint_point_cmd",100);
  ros::Subscriber joint_state_sub = n.subscribe("/joint_states", 2, jointStateCallBack);
  loop_rate=new ros::Rate(rate);
  cmd.coordinates.resize(4);
  cmd.velocity.resize(4);
  cmd.acceleration.resize(4);
  cmd.header.frame_id="joint_space";
  cmd.header.stamp=ros::Time::now();
  //get current joint angles
  while (js.position.size() ==0)
    ros::spinOnce();
  initialize();
  //performe measurements for joint 4.
  //relative motion of pi/12

  //double rel[]={0,0,0,constants::pi};
  //double torque[4];
  //move_joints_relative(rel,60, torque);
  //double v[]={0,0,0,constants::pi/30};
  //joint_linear_motion(v,0.2);

  //rotate joint 4 by -2.3

  //rotate joint 3 by -pi/2
  //rotate joint 1 by -pi/2
  //rotate joint 4 by 5, record friction profile
  //rotate joint 4 by -5, record friction profile
  //rotate joint 1 by pi/2
  //rotate joint 3 by pi/2

  //rotate joint 4 by 5, record friction profile

  //rotate joint 4 by -5 record friction profile

  //rotate joint 4 by 2.3, set compensation profile

  return 0;
}


#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <Eigen/Eigen>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <math.h>
#include <dynamic_reconfigure/server.h>
#include <pva_tracker/PVA_TrackerConfig.h>
#include <nav_msgs/Odometry.h>
#include "pva_tracker/input.h"

#include "tiny_dnn/tiny_dnn.h"
#define GRAVITATIONAL_ACC 9.81

using namespace Eigen;

// Coefficients
Vector3d position_error_p;
Vector3d position_error_d;
Vector3d position_error_i;
Vector3d velocity_error_p;
Vector3d velocity_error_d;
Vector3d velocity_error_i;

double p_i_acc_error_limit;
double v_i_acc_error_limit;

// Global Variables
Vector3d planned_p;
Vector3d planned_v;
Vector3d planned_a;
double planned_yaw;
Vector3d current_p;
Vector3d current_v;
Vector3d current_a;
Vector3d mpc_k;
Quaterniond current_att;
ros::Publisher att_ctrl_pub, odom_sp_enu_pub;
double thrust_factor;
std::vector<std::vector<float>> input_queue;
tiny_dnn::network<tiny_dnn::sequential> nn;
ros::Time last_time;
Vector3d last_a;
Vector3d last_current_v;

ros::Publisher nn_pub;

pva_tracker::input  input_msg;

float mpc_kpxy,mpc_kvxy, mpc_kaxy,mpc_kpz, mpc_kvz, mpc_kaz;

using namespace std;

void positionCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{

    /// ENU frame
    current_p << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
    current_att.w() = msg->pose.orientation.w;
    current_att.x() = msg->pose.orientation.x;
    current_att.y() = msg->pose.orientation.y;
    current_att.z() = msg->pose.orientation.z;
}

void velocityCallback(const geometry_msgs::TwistStamped::ConstPtr &msg)
{
    /// ENU frame
    current_v << msg->twist.linear.x, msg->twist.linear.y, msg->twist.linear.z;

    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("Accelerate 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;
}

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));
    // I use ENU coordinate system , so I plus ' - '
    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;
}

Vector3d vectorElementMultiply(Vector3d v1, Vector3d v2)
{
    Vector3d result;
    result << v1(0)*v2(0), v1(1)*v2(1), v1(2)*v2(2);
    return result;
}

void vector3dLimit(Vector3d &v, double limit)  ///limit should be positive
{
    if(limit > 0){
        for(int i=0; i<3; i++){
            v(i) = fabs(v(i)) > limit ? (v(i) > 0 ? limit : -limit) : v(i);
        }
    }
}
template <typename T>
void store_data3d(T &store_data, Vector3d vector3d)
{
    for (int i=0;i<3;i++)
    {
        store_data.push_back(vector3d[i]);
    }
}

void accel2quater(Vector3d a_des,Quaterniond &att_des_q, double &thrust_des)
{
    Vector3d att_des_norm = a_des / a_des.norm();

    Quaterniond z_w_quat(0, 0, 0, 1.0);
    Quaterniond att_current_vector_quat = current_att * z_w_quat * current_att.inverse();
    Vector3d current_att_vector(att_current_vector_quat.x(), att_current_vector_quat.y(), att_current_vector_quat.z());
    Vector3d z_w_norm(0, 0, 1.0);
//    Quaterniond att_des_q = Quaterniond::FromTwoVectors(current_att_vector, att_des_norm);
    att_des_q = Quaterniond::FromTwoVectors(z_w_norm, att_des_norm);

    //add yaw
    Quaterniond yaw_quat(cos(planned_yaw/2.0), att_des_norm(0)*sin(planned_yaw/2.0),
                         att_des_norm(1)*sin(planned_yaw/2.0),att_des_norm(2)*sin(planned_yaw/2.0));
    att_des_q = yaw_quat * att_des_q;

    //Calculate thrust
    thrust_des = a_des.norm() * thrust_factor;  //a_des.dot(att_current_vector) * THRUST_FACTOR

}

void pvaCallback(const trajectory_msgs::JointTrajectoryPoint::ConstPtr& msg)
{
    mavros_msgs::AttitudeTarget att_setpoint;

/*    /// NWU frame to ENU frame
    planned_p << -msg->positions[1], msg->positions[0], msg->positions[2];
    planned_yaw = msg->positions[3] + M_PI/2.0;
    planned_v << -msg->velocities[1], msg->velocities[0], msg->velocities[2];
    planned_a << -msg->accelerations[1], msg->accelerations[0], msg->accelerations[2];*/

    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];

    /// Publish to record in rosbag
    nav_msgs::Odometry odom_sp_enu;
    odom_sp_enu.header.stamp = ros::Time::now();
    odom_sp_enu.pose.pose.position.x = planned_p(0);
    odom_sp_enu.pose.pose.position.y = planned_p(1);
    odom_sp_enu.pose.pose.position.z = planned_p(2);
    odom_sp_enu.twist.twist.linear.x = planned_v(0);
    odom_sp_enu.twist.twist.linear.y = planned_v(1);
    odom_sp_enu.twist.twist.linear.z = planned_v(2);
    odom_sp_enu_pub.publish(odom_sp_enu);

    /// Calculate desired thrust and attitude
    Vector3d p_error = planned_p - current_p;
    Vector3d v_error = planned_v - current_v;

    static Vector3d p_error_last;
    static Vector3d v_error_last;
    static Vector3d p_error_accumulate;
    static Vector3d v_error_accumulate;
    static bool if_init = true;

    if(if_init){
        if_init = false;
        p_error_last = p_error;
        v_error_last = v_error;
        p_error_accumulate = p_error;
        v_error_accumulate = v_error;
        return;
    }

    /**Core code**/
    Vector3d delt_p_error = p_error - p_error_last;
    Vector3d delt_v_error = v_error - v_error_last;

    p_error_accumulate += p_error;
    v_error_accumulate += v_error;
    vector3dLimit(p_error_accumulate, p_i_acc_error_limit);
    vector3dLimit(v_error_accumulate, v_i_acc_error_limit);

    Vector3d a_fb =   /// PID
            vectorElementMultiply(p_error, position_error_p) + vectorElementMultiply(v_error, velocity_error_p) +
            vectorElementMultiply(delt_p_error, position_error_d) + vectorElementMultiply(delt_v_error, velocity_error_d) +
            vectorElementMultiply(p_error_accumulate, position_error_i) + vectorElementMultiply(v_error_accumulate, velocity_error_i);

    p_error_last = p_error;
    v_error_last = v_error;

    Quaterniond att_des_q;
    double thrust_des;

    // Set a maximum acceleration feed forward value given by position and velocity error.
    for(int i=0; i<3; i++){
        if(fabs(a_fb(i)) > 5.0) a_fb(i) = 5.0 * a_fb(i) / fabs(a_fb(i));
    }
    Vector3d z_w_norm(0, 0, 1.0);
    Vector3d a_des = a_fb + planned_a + GRAVITATIONAL_ACC * z_w_norm;
    accel2quater(a_des,att_des_q,thrust_des);

    //ROS_INFO("first:  %f  %f  %f",a_des(0),a_des(1),a_des(2));
    Vector3d euler_angle=quaternion2euler_eigen(att_des_q.x(),att_des_q.y(),att_des_q.z(),att_des_q.w());

    tiny_dnn::vec_t nn_input;

    Matrix<float,10,1> max_min;
    max_min<<1.7747, 1.6724, 4.8957, 7.2604, 5.7810, 6.1120,3.79378,  2.71257 , 3.22697,  1.729003;

    //max_min<<0.681179,0.644664,0.220777,1.54497,1.47908,0.772466,8.58691,12.9761,5.17432,1.71691,1.68792,0.323826,0.274193;
    Matrix<float,10,1> min;
    min<<-0.924959 ,-0.911663 ,-0.593314 ,-3.45101  ,-2.77456 , -2.49199 , -2.1737, -1.21307 , -1.11182 ,  0.164217;
    //min<<-0.377693,-0.297301,-0.122026,-0.768945,-0.713422,-0.282714,-4.43739,-8.88778,-2.23553,-0.871013,-0.859454,-0.153833,0.577107;

    if(input_queue.size()>6)
    {
        for(int last=3;last>=1;last--)
        {
            for(int i=9;i<=14;i++)
            {
                nn_input.push_back(input_queue[input_queue.size()-last][i]-input_queue[input_queue.size()-last-1][i-9]);
            }
            for(int i=18;i<=21;i++)
            {
                nn_input.push_back(input_queue[input_queue.size()-last][i]);
            }
        }

        std::vector<float> current_pva;
        store_data3d(current_pva,current_p);
        store_data3d(current_pva,current_v);
        store_data3d(current_pva,current_a);
        for(int i=0;i<=5;i++)
        {
            nn_input.push_back(current_pva[i]-input_queue.back()[i]);
        }
        nn_input.push_back(euler_angle(0));
        nn_input.push_back(euler_angle(1));
        nn_input.push_back(euler_angle(2));
        nn_input.push_back(thrust_des);

        //归一化
        for(int i=0;i<nn_input.size();i++)
        {
            int temp =i%10;
            nn_input[i]=(nn_input[i]-min(temp))/max_min(temp); //归一化
        }
        for(int i=0;i<nn_input.size();i++)
        {
            input_msg.input[i] =nn_input[i];
        }

        nn_pub.publish(input_msg)

        ros::Time last_request = ros::Time::now();
        tiny_dnn::vec_t predict=nn.predict(nn_input);
        //ROS_INFO_THROTTLE(1,"ann spend: %f",ros::Time::now().toSec()-last_request.toSec());

        //输出反归一化
        for(int i=0;i<predict.size();i++)
        {
            predict[i]=predict[i]*max_min[i]+min[i];
        }

        ROS_INFO_THROTTLE(2,"%f  %f   %f  %f  %f  %f  %f  %f  %f",predict[0],predict[1],predict[2],predict[3],predict[4]
                    ,predict[5],predict[6],predict[7],predict[8]);

        a_des(0)=a_des(0)-(mpc_kpxy*predict[0]+mpc_kvxy*predict[3]+mpc_kaxy*predict[6]);
        a_des(1)=a_des(1)-(mpc_kpxy*predict[1]+mpc_kvxy*predict[4]+mpc_kaxy*predict[7]);
        a_des(2)=a_des(2)-(mpc_kpz*predict[2]+mpc_kvz*predict[5]+mpc_kaz*predict[8]);
        accel2quater(a_des,att_des_q,thrust_des);

        //ROS_INFO("second:  %f  %f  %f",a_des(0),a_des(1),a_des(2));

    }

    att_setpoint.header.stamp = ros::Time::now();
    att_setpoint.orientation.w = att_des_q.w();
    att_setpoint.orientation.x = att_des_q.x();
    att_setpoint.orientation.y = att_des_q.y();
    att_setpoint.orientation.z = att_des_q.z();
    att_setpoint.thrust = thrust_des;

    std::vector<float> store_data;
    store_data3d(store_data,planned_p);  // 0 1 2
    store_data3d(store_data,planned_v); // 3 4 5
    store_data3d(store_data,planned_a);  // 6 7 8
    store_data3d(store_data,current_p);  //9 10 11
    store_data3d(store_data,current_v);  // 12 13 14
    store_data3d(store_data,current_a);  //15 16 17

    euler_angle=quaternion2euler_eigen(att_des_q.x(),att_des_q.y(),att_des_q.z(),att_des_q.w());

    store_data.push_back(euler_angle(0));  //18
    store_data.push_back(euler_angle(1));  // 19
    store_data.push_back(euler_angle(2));  //20
    store_data.push_back(thrust_des);    //21

    input_queue.push_back(store_data);
    if(input_queue.size()>10)
    {
        input_queue.erase(input_queue.begin());
    }

/*    ROS_INFO_THROTTLE(2.0, "Attitude Quaternion Setpoint is w=%f, x=%f, y=%f, z=%f, thrust=%f", att_setpoint.orientation.w,
                      att_setpoint.orientation.x, att_setpoint.orientation.y, att_setpoint.orientation.z, att_setpoint.thrust);*/

    att_ctrl_pub.publish(att_setpoint);
}

void configureCallback(tracker::PVA_TrackerConfig &config, uint32_t level) {
    position_error_p << config.position_p_xy, config.position_p_xy, config.position_p_z;
    position_error_d << config.position_d_xy, config.position_d_xy, config.position_d_z;
    position_error_i << config.position_i_xy, config.position_i_xy, config.position_i_z;
    p_i_acc_error_limit = config.p_i_acc_error_limit;

    velocity_error_p << config.velocity_p_xy, config.velocity_p_xy, config.velocity_p_z;
    velocity_error_d << config.velocity_d_xy, config.velocity_d_xy, config.velocity_d_z;
    velocity_error_i << config.velocity_i_xy, config.velocity_i_xy, config.velocity_i_z;
    v_i_acc_error_limit = config.v_i_acc_error_limit;

    thrust_factor = config.hover_thrust_factor;

    mpc_kpxy=config.mpc_k_pxy;
    mpc_kvxy=config.mpc_k_vxy;
    mpc_kaxy=config.mpc_k_axy;
    mpc_kpz=config.mpc_k_pz;
    mpc_kvz=config.mpc_k_vz;
    mpc_kaz=config.mpc_k_az;

}

int main(int argc, char** argv) {
    ros::init(argc, argv, "mpc_tracker");

    dynamic_reconfigure::Server<tracker::PVA_TrackerConfig> server;
    dynamic_reconfigure::Server<tracker::PVA_TrackerConfig>::CallbackType f;
    f = boost::bind(&configureCallback, _1, _2);
    server.setCallback(f);

    ros::NodeHandle nh;

    nn.load("/home/pengpeng/catkin_ws/src/pva_tracker/cmake-build-debug/devel/lib/pva_tracker/LeNet_eluer_-model_v=4_last_5");

    ros::Subscriber position_sub = nh.subscribe("/mavros/local_position/pose", 1, positionCallback);
    ros::Subscriber velocity_sub = nh.subscribe("/mavros/local_position/velocity_local", 1, velocityCallback);
    ros::Subscriber pva_sub = nh.subscribe("/pva_setpoint", 1, pvaCallback);

    att_ctrl_pub = nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 1);
    odom_sp_enu_pub = nh.advertise<nav_msgs::Odometry>("/odom_sp_enu", 1);

    nn_pub=nh.advertise<pva_tracker::input>("/nn_compute",1)

    ros::spin();
    return 0;
}


/**
 * \ file MultipleUniversalFixedDelayLineFilter.cpp
 */

#include <ATK/Delay/MultipleUniversalFixedDelayLineFilter.h>

#include <array>
#include <fstream>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <ATK/Mock/SimpleSinusGeneratorFilter.h>
#include <ATK/Mock/TriangleCheckerFilter.h>

#include <ATK/Tools/SumFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_linel100_delay50_test )
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay(0, 50);
  filter.set_feedforward(0, 0, 1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(gsl::index i = 0; i < 50; ++i)
  {
    BOOST_REQUIRE_EQUAL(0, outdata[i]);
  }

  for(gsl::index i = 50; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i - 50], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE(StereoUniversalFixedDelayLineFilter_sinus_line100_delay50_progressive_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_frequency(480);

  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay(0, 50);
  filter.set_feedforward(0, 0, 1);
  filter.process(50);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &filter, 0);
  sum.set_input_port(1, &generator, 0);

  ATK::TriangleCheckerFilter<double> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &sum, 0);
  output.set_amplitude(0);

  for (std::size_t i = 1; i < 100; ++i)
  {
    output.process(i);
  }
}

BOOST_AUTO_TEST_CASE(StereoUniversalFixedDelayLineFilter_sinus_line1000_delay50_progressive_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_frequency(480);

  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(1000);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay(0, 50);
  filter.set_feedforward(0, 0, 1);
  filter.process(50);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &filter, 0);
  sum.set_input_port(1, &generator, 0);

  ATK::TriangleCheckerFilter<double> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &sum, 0);
  output.set_amplitude(0);

  for (std::size_t i = 50; i < 1000; ++i)
  {
    output.process(i);
  }
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_linel25_delay24_blend_1_feedforward_1_feedback_0_test )
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay(0, 24);
  filter.set_blend(0, 1);
  filter.set_feedback(0, 0, 0);
  filter.set_feedforward(0, 0, 1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(gsl::index i = 24; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_SMALL(outdata[i], 0.0001);
  }
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_linel25_delay24_blend_0_feedforward_0_feedback_1_test )
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay(0, 24);
  filter.set_blend(0, 0);
  filter.set_feedback(0, 0, .999999);
  filter.set_feedforward(0, 0, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(gsl::index i = 24; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_SMALL(outdata[i], 0.0001);
  }
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_liner100_delay50_test )
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay(1, 50);
  filter.set_feedforward(1, 1, 1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 1);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(gsl::index i = 0; i < 50; ++i)
  {
    BOOST_REQUIRE_EQUAL(0, outdata[i]);
  }

  for(gsl::index i = 50; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i - 50], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE(StereoUniversalFixedDelayLineFilter_sinus_liner100_delay50_progressive_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_frequency(480);

  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay(1, 50);
  filter.set_feedforward(1, 1, 1);
  filter.process(50);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &filter, 1);
  sum.set_input_port(1, &generator, 0);

  ATK::TriangleCheckerFilter<double> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &sum, 0);
  output.set_amplitude(0);

  for (std::size_t i = 1; i < 100; ++i)
  {
    output.process(i);
  }
}

BOOST_AUTO_TEST_CASE(StereoUniversalFixedDelayLineFilter_sinus_liner1000_delay50_progressive_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_frequency(480);

  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(1000);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay(1, 50);
  filter.set_feedforward(1, 1, 1);
  filter.process(50);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &filter, 1);
  sum.set_input_port(1, &generator, 0);

  ATK::TriangleCheckerFilter<double> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &sum, 0);
  output.set_amplitude(0);

  for (std::size_t i = 50; i < 1000; ++i)
  {
    output.process(i);
  }
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_liner25_delay24_blend_1_feedforward_1_feedback_0_test )
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay(1, 24);
  filter.set_blend(1, 1);
  filter.set_feedback(1, 1, 0);
  filter.set_feedforward(1, 1, 1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 1);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(gsl::index i = 24; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_SMALL(outdata[i], 0.0001);
  }
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_liner25_delay24_blend_0_feedforward_0_feedback_1_test )
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay(1, 24);
  filter.set_blend(1, 0);
  filter.set_feedback(1, 1, .999999);
  filter.set_feedforward(1, 1, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 1);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(gsl::index i = 24; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_SMALL(outdata[i], 0.0001);
  }
}

const int OTHERPROCESSSIZE = 9600;

BOOST_AUTO_TEST_CASE(StereoUniversalFixedDelayLineFilter_sinus_lr_complex_test)
{
  std::vector<double> datal(OTHERPROCESSSIZE);
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/input_lstereo.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(datal.data()), OTHERPROCESSSIZE * sizeof(double));
  }
  std::vector<double> datar(OTHERPROCESSSIZE);
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/input_rstereo.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(datar.data()), OTHERPROCESSSIZE * sizeof(double));
  }

  ATK::InPointerFilter<double> generatorl(datal.data(), 1, OTHERPROCESSSIZE, false);
  generatorl.set_output_sampling_rate(96000);
  ATK::InPointerFilter<double> generatorr(datar.data(), 1, OTHERPROCESSSIZE, false);
  generatorr.set_output_sampling_rate(96000);

  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(100000);
  filter.set_input_sampling_rate(96000);
  filter.set_input_port(0, &generatorl, 0);
  filter.set_input_port(1, &generatorr, 0);
  filter.set_delay(0, 4800);
  filter.set_delay(1, 3600);
  filter.set_blend(0, 1);
  filter.set_blend(1, 1);
  filter.set_feedforward(0, 0, -1);
  filter.set_feedforward(0, 1, .1);
  filter.set_feedforward(1, 0, .7);
  filter.set_feedforward(1, 1, -1);
  filter.set_feedback(0, 0, -.5);
  filter.set_feedback(0, 1, .1);
  filter.set_feedback(1, 0, .3);
  filter.set_feedback(1, 1, -.1);

  filter.process(OTHERPROCESSSIZE);

  std::vector<double> outdatal(OTHERPROCESSSIZE);
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/output_lstereo.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(outdatal.data()), OTHERPROCESSSIZE * sizeof(double));
  }
  std::vector<double> outdatar(OTHERPROCESSSIZE);
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/output_rstereo.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(outdatar.data()), OTHERPROCESSSIZE * sizeof(double));
  }

  for (unsigned int i = 0; i < OTHERPROCESSSIZE; ++i)
  {
    BOOST_CHECK_CLOSE(outdatal[i], filter.get_output_array(0)[i], 0.0001);
    BOOST_CHECK_CLOSE(outdatar[i], filter.get_output_array(1)[i], 0.0001);
  }
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_delay_test )
{
  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(128);
  filter.set_delay(0, 10);
  BOOST_CHECK_EQUAL(filter.get_delay(0), 10);
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_delay_range_test )
{
  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(128);
  BOOST_CHECK_THROW(filter.set_delay(0, 0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_delay_range2_test )
{
  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(128);
  BOOST_CHECK_THROW(filter.set_delay(0, 128), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_blend_test )
{
  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(128);
  filter.set_blend(0, 0.5);
  BOOST_CHECK_EQUAL(filter.get_blend(0), 0.5);
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_feedforward_test )
{
  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(128);
  filter.set_feedforward(0, 0, 0.5);
  BOOST_CHECK_EQUAL(filter.get_feedforward(0, 0), 0.5);
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_feedback_test )
{
  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(128);
  filter.set_feedback(0, 0, 0.5);
  BOOST_CHECK_EQUAL(filter.get_feedback(0, 0), 0.5);
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_feedback_range_test )
{
  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(128);
  BOOST_CHECK_THROW(filter.set_feedback(0, 0, 1), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_feedback_range2_test )
{
  ATK::MultipleUniversalFixedDelayLineFilter<double, 2> filter(128);
  BOOST_CHECK_THROW(filter.set_feedback(0, 0, -1), ATK::RuntimeError);
}


/////////////////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2021 Andreas Milton Maniotis.
//
// Email: andreas.maniotis@gmail.com
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
/////////////////////////////////////////////////////////////////////////////////////////////

#include "aml/dictionary.hpp"
#include "aml/exponent.hpp"
#include "aml/set.hpp"

#ifdef ACTIVATE_TEST_GRAPH

namespace aml::fsm::graph
{
    template<typename...>
    struct edge;

    template<typename Source, typename Target>
    struct edge
    {
        static_assert( is_same< Source, Target >::eval() == false, "Loops of length 0 are forbidden" );

        using source = Source;
        using target = Target;
    };

    template<typename Edge>
    using source  =  typename Edge::source;

    template<typename Edge>
    using target  =  typename Edge::target;

    template< typename... >
    struct arrows;

    template< typename Source
            , typename... Targets
            >
    struct arrows< Source, Targets... >
    {
        using source  = Source;
        using targets = set< Targets... >;
        using edges   = set< edge<source, Targets>... >;
    };

    template< typename... >
    struct graph;

    template<>
    struct graph<dictionary<> >
    {
    private:

        template<typename... Edges>
        struct add_edges_
        {
            static_assert( sizeof...(Edges) != 0, "" );

            using src  =  source< head< conslist<Edges...> > >;
            using trg  =  target< head< conslist<Edges...> > >;

            using type =  typename tail<Edges...>::
                          template apply<graph< dictionary< entry< src, arrows<src, trg> >,
                                                            entry< trg, arrows<trg> > > > >::template add_edges>;
        };

    public:
        using nodes = set<>;
        using edges = set<>;

        template<typename... Node>
        struct outgoing_from
        {
            using error = bool_< sizeof...(Node) != sizeof...(Node) >;
            static_assert(error , "An empty graph has no outgoing nodes" );
        };

        template< typename... Nodes >
        using add_nodes  =  graph< dictionary< entry< Nodes, arrows< Nodes > >... >;

        template<typename... Edges>
        struct add_edges  =  typename bool_< sizeof...(Edges) != 0 >::
                             template conditional< add_edges_<Edges...>, hull< graph<dictionary<>, Base> > >::type;

        template< template<typename...> class>
        using isomorphic_node_transform  =  graph< dictionary<> >;

        template<typename... Nodes>
        using identify_nodes
        {
            static_assert( sizeof...(Nodes) == 0, "Empty dictionary contains no nodes. "
                                                   "Hence the current list is not identifiable. " );
            template<typename... Node>
            using with = typename if_< bool_<sizeof...(Node) == 1> >::enable< graph<dictionary<> >;
        };
    };

    template<typename... Entries>
    struct graph< dictionary< Entries... > >
    {
    private:
        using all_nodes_ = join< typename term<Entries>::subterms... >::template apply<set>;

        static_assert( decltype( all_nodes_() == set< key<Entries>... >() ),
                       "Every node must be explicitly present as an entry");

    public:
        using nodes  =  set< key<Entries...> >;
        using edges  =  typename join< typename value< Entries >::list... >::template apply< set >;

        template<typename... Node>
        using outgoing_from = typename dictionary<Entries...>::template at< identity<Node...> >;

        template<typename... Nodes>
        using add_nodes  =  graph< dictionary<  Entries...,    entry<Nodes, arrows<Nodes> >...  >;

        template<typenane... Edges >
        using add_edges  =  typename
                            term
                            <
                                decltype
                                (
                                    typename
                                    join< conslist< source< Edges >, target< Edges > >... >::template apply<set>()
                                    -
                                    nodes()
                                )::list::template apply< add_node >
                            >::
                            subterms::
                            entries::
                            template apply< update_entries_with<Edges...>::template apply >::
                            template apply< dictionary >::
                            template apply< graph >;

        template< template<typename...> class F>
        using isomorphic_node_transform  =  graph< dictionary< node_transform_<F, Entries>... >;

        template<typename... Nodes>
        struct identify_nodes
        {
            template<typename... New_node>
            struct with;

        }

    };

    template<typename... Arrows>
    using make_graph  =  typename join
                         <
                             conslist< entry< typename Arrows::source, Arrows>... >,

                             typename
                             decltype(
                                        typename join<typename Arrows::targets::list...>::template apply< set > >()
                                        -
                                        set< typename Arrows::source... >()

                                      )::  // Here conslist<node...> with all nodes that do not appear in arrows
                                           // as a soure

                             template pointwise_apply< diagonal >::        // conslist< conslist<node, node>... >
                             emplate pointwise_apply< pointwise_action<identity, arrows> >::
                                                             // Here: conslist< conslist< node, arrows<node>... >
                             template pointwise_apply< entry >
                         >::
                         template apply<dictionary>::
                         template apply<graph>;

}

#endif

#include "aml/string.hpp"

using aml::operator""_;

#include <boost/core/demangle.hpp>
#include <iostream>

int main()
{

}