/*
 * Copyright (C) 2015 Fondazione Istituto Italiano di Tecnologia
 *
 * Licensed under either the GNU Lesser General Public License v3.0 :
 * https://www.gnu.org/licenses/lgpl-3.0.html
 * or the GNU Lesser General Public License v2.1 :
 * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 * at your option.
 */

#ifndef IDYNTREE_AXIS_H
#define IDYNTREE_AXIS_H

#include <string>

#include <iDynTree/Core/Direction.h>
#include <iDynTree/Core/Position.h>
#include <iDynTree/Core/Utils.h>

namespace iDynTree
{
    class Transform;
    class TransformDerivative;
    class Twist;
    class SpatialAcc;
    /**
     * Class representing an axis (a directed line) in space.
     *
     * The axis is represented as a origin plus a direction.
     *
     * \ingroup iDynTreeCore
     *
     *
     */
    class Axis
    {
    private:
        Direction direction;
        Position  origin;

        /**
         * Set the object to the default axis: direction : 1, 0, 0 , point: 0, 0, 0
         */
        void setToDefault();

    public:
        /**
         * Default constructor.
         * The data is not reset to the default for perfomance reason.
         * Please initialize the data in the class before any use.
         */
        inline Axis() {}

        /**
         * Constructor from a Direction and an origin, represented by a Position object.
         */
        Axis(const Direction & _direction, const Position & _origin);

        /**
         * Copy constructor: create a Axis from another Axis
         */
        Axis(const Axis & other);

         /**
         * Get the direction of the axis
         */
        const Direction & getDirection() const;

        /**
         * Get the origin of the axis
         */
        const Position & getOrigin() const;

        /**
         * Set the direction of the axis
         */
        void setDirection(const Direction & _direction);

        /**
         * Set the origin of the axis
         */
        void setOrigin(const Position & _position);

        /**
         * Get the transform induced by a rotation of an angle theta
         * around this axis. The returned transform is the nonRotated_T_rotated,
         * such that if we have a quantity expressed in the frame obtained
         * by the rotation v_rotated, we can transform it back in the
         * non-rotated frame using the returned transform:
         * v_nonRotated = nonRotated_T_rotated*v_rotated
         */
        Transform getRotationTransform(const double theta) const;

        /**
         * Get the derivative of the getRotationTransform function with respect
         * to the theta argument.
         */
        TransformDerivative getRotationTransformDerivative(const double theta) const;

        Twist getRotationTwist(const double dtheta) const;

        SpatialAcc getRotationSpatialAcc(const double d2theta) const;

        /**
         * Get the transform induced by a translation of a distance dist
         * along this axis. The returned transform is the nonTranslated_T_translated,
         * such that if we have a quantity expressed in the frame obtained
         * by the translation v_translated, we can transform it back in the
         * non-translated frame using the returned transform:
         * v_nonTranslated = nonTranslated_T_translated*v_translated
         */
        Transform getTranslationTransform(const double dist) const;

        /**
         * Get the derivative of the getTranslationTransform function with respect
         * to the dist argument.
         */
        TransformDerivative getTranslationTransformDerivative(const double /*dist*/) const;

        Twist getTranslationTwist(const double ddist) const;

        SpatialAcc getTranslationSpatialAcc(const double d2dist) const;

        /**
         * Check if two axes are parallel (i.e. their direction are parallel).
         *
         * @param otherAxis the axes to check for parallelism.
         * @param tolerance tolerance to use in the parallelism check.
         */
        bool isParallel(const Axis & otherAxis, const double tolerance) const;

        /**
         * Return the axis with the same origin, but reversed direction.
         */
        Axis reverse() const;

        /**
         * @name Output helpers.
         *  Output helpers.
         */
        ///@{
        std::string toString() const;

        std::string reservedToString() const;
        ///@}
    };
}

#endif
