/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 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.
*/

#ifndef MechanicsIO_hpp
#define MechanicsIO_hpp

#include <MechanicsFwd.hpp>
#ifdef HAVE_SICONOS_MECHANISMS
#include <MechanismsFwd.hpp>
#endif
#include <SiconosPointers.hpp>
#include <SiconosFwd.hpp>

class MechanicsIO
{
protected:

  template<typename T, typename G>
  SP::SimpleMatrix visitAllVerticesForVector(const G& graph) const;

  template<typename T, typename G>
  SP::SiconosVector visitAllVerticesForDouble(const G& graph) const;

public:
  /** default constructor
   */
  MechanicsIO() {};

  /** get all positions: translation (x,y,z) + orientation quaternion
   * (qw, qx, qy, qz)
   * \param nsds current nonsmooth dynamical system
   * \return a SP::SimpleMatrix where the columns are
   *          id, x, y, z, qw, qx, qy, qz
   *   id is the DynamicalSystem number + 1
   */
  SP::SimpleMatrix positions(const NonSmoothDynamicalSystem& nsds) const;

  /** get all velocities: translation (xdot, ydot, zdot) + orientation velocities
   * ox, oy, oz
   * \param nsds current nonsmooth dynamical system
   *   \return a matrix where the columns are id, xdot, ydot, zdot,
   *   ox, oy, oz
   * id is the DynamicalSystem number + 1
   */
  SP::SimpleMatrix velocities(const NonSmoothDynamicalSystem& nsds) const;

  /** get the coordinates of all contact points, normals, reactions and velocities
   * \param nsds current nonsmooth dynamical system
   * \param index_set the index set number.
   \return a matrix where the columns are mu x y z, nx, ny, nz, rx, ry, rz, vx, vy, vz, ox, oy, oz, id
  */
  SP::SimpleMatrix contactPoints(const NonSmoothDynamicalSystem& nsds, unsigned int index_set=1) const;

  /** get the domain of each contact point
   * \param nsds current nonsmooth dynamical system
   * \return a matrix where the columns are domain, id
  */
  SP::SimpleMatrix domains(const NonSmoothDynamicalSystem& nsds) const;
};


#endif
