/******************************************************************************
 *                 This file is part of the OROCOS project
 *
 *                           (C) 2007 Ruben Smits                              *
 *                        (C) 2010 Dominick Vanthienen
 *                    dominick.vanthienen@mech.kuleuven.be,
 *                    Department of Mechanical Engineering,                    *
 *                   Katholieke Universiteit Leuven, Belgium.                  *
 *                                                                             *
 *       You may redistribute this software and/or modify it under either the  *
 *       terms of the GNU Lesser General Public License version 2.1 (LGPLv2.1  *
 *       <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>) or (at your *
 *       discretion) of the Modified BSD License:                              *
 *       Redistribution and use in source and binary forms, with or without    *
 *       modification, are permitted provided that the following conditions    *
 *       are met:                                                              *
 *       1. Redistributions of source code must retain the above copyright     *
 *       notice, this list of conditions and the following disclaimer.         *
 *       2. 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.  *
 *       3. The name of the author may not be used to endorse or promote       *
 *       products derived from this software without specific prior written    *
 *       permission.                                                           *
 *       THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 SIMULATION_NAXES_VELOCITY_CONTROLLER_HPP
#define SIMULATION_NAXES_VELOCITY_CONTROLLER_HPP

#include <rtt/RTT.hpp>

#include <rtt/TaskContext.hpp>
#include <rtt/Port.hpp>
#include <rtt/Operation.hpp>
#include <rtt/OperationCaller.hpp>
#include <rtt/extras/Properties.hpp>

#include "SimulationAxis.hpp"
#include <rtt/extras/dev/AxisInterface.hpp>

#include <ocl/OCL.hpp>

namespace robot_simulators
{
    /**
     * This class implements a Taskcontext that simulates a robot with
     * a configurable number of axes.
     *
     */

    class nAxesVelocityController: public RTT::TaskContext
    {
    public:
        nAxesVelocityController(std::string name);
        virtual ~nAxesVelocityController(){};

    private:
        bool startAllAxes();
        bool stopAllAxes();
        bool lockAllAxes();
        bool unlockAllAxes();

        virtual bool configureHook();
        virtual bool startHook();
        virtual void updateHook();
        virtual void stopHook();
        virtual void cleanupHook();

        unsigned int naxes;

        std::vector<double> driveValues;
        std::vector<double> positionValues;
        std::vector<double> driveValuesCheck;

        std::vector<SimulationAxis*> simulation_axes;

    protected:

        RTT::InputPort<std::vector<double> > D_driveValues;
        RTT::OutputPort<std::vector<double> > D_positionValues;

        RTT::Property<std::vector<double> > P_initialPositions;
        RTT::Property<unsigned int> P_naxes;

    };
}
#endif