/******************************************************************************
 *                 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.                                           *
 *                                                                             *
 *******************************************************************************/
#include "nAxesVelocityController.hpp"
#include <ocl/Component.hpp>
#include <rtt/Logger.hpp>

namespace robot_simulators
{
    using namespace RTT;
    using namespace std;

    nAxesVelocityController::nAxesVelocityController(string name)
        : TaskContext(name,PreOperational),
        P_initialPositions("initialPositions","initial positions (rad) for the axes"),
    	P_naxes("naxes","number of axes")
    {
    	this->addOperation("startAllAxes", &nAxesVelocityController::startAllAxes, this,
        	    OwnThread).doc("start the simulation axes");
        this->addOperation( "stopAllAxes", &nAxesVelocityController::stopAllAxes, this,
        	    OwnThread ).doc("stop the simulation axes");
        this->addOperation( "lockAllAxes", &nAxesVelocityController::lockAllAxes, this,
        	    OwnThread ).doc("lock the simulation axes");
        this->addOperation( "unlockAllAxes", &nAxesVelocityController::unlockAllAxes, this,
        	    OwnThread ).doc("unlock the simulation axes");

        this->ports()->addPort( "nAxesOutputVelocity",D_driveValues );
        this->ports()->addPort( "nAxesSensorPosition",D_positionValues );

        this->properties()->addProperty(P_initialPositions);
        this->properties()->addProperty(P_naxes);

    }

    bool nAxesVelocityController::configureHook()
    {
        naxes=P_naxes.value();
        if(P_initialPositions.value().size()!=naxes){
            Logger::In in(this->getName().data());
            log(Error)<<"Size of "<<P_initialPositions.getName()
                      <<" does not match "<<P_naxes.getName()
                      <<endlog();
            return false;
        }

        driveValues.resize(naxes);
        positionValues=P_initialPositions.value();
        simulation_axes.resize(naxes);

        for (unsigned int i = 0; i<naxes; i++){
            simulation_axes[i] = new SimulationAxis(positionValues[i]);
            log(Debug)<<"InitialPositionValues= "<< P_initialPositions.value()[i]<<endlog();
            log(Debug)<<"positionValues= "<<positionValues[i] <<endlog();
        }

        D_positionValues.write( positionValues );

        return true;
    }

    bool nAxesVelocityController::startHook()
    {
        //check connection and sizes of input-ports
        if(!D_driveValues.connected()){
            Logger::In in(this->getName().data());
            log(Error)<<D_driveValues.getName()<<" not ready"<<endlog();
            return false;
        }

        //TODO: is there a good rtt2.2 alternative for this code? see temporary solution below
        //if(D_driveValues.Get().size()!=naxes){
        //    Logger::In in(this->getName().data());
        //    log(Error)<<"Size of "<<D_driveValues.getName()<<": "<<D_driveValues.Get().size()<<" != " << naxes<<endlog();
        //    return false;
        //}

        D_driveValues.read(driveValuesCheck);
        if(driveValuesCheck.size()!=naxes){
			Logger::In in(this->getName().data());
			log(Error)<<"Size of "<<D_driveValues.getName()<<": "<<driveValuesCheck.size()<<" != " << naxes<<endlog();
			return false;
        }


        return true;
    }

    void nAxesVelocityController::updateHook()
    {
        D_driveValues.read( driveValues );
        for (unsigned int i=0;i<naxes;i++){
            positionValues[i]=simulation_axes[i]->getSensor("Position")->readSensor();
            if(simulation_axes[i]->isDriven())
                simulation_axes[i]->drive(driveValues[i]);
        }
        D_positionValues.write( positionValues );
    }

    void nAxesVelocityController::cleanupHook()
    {
        simulation_axes.clear();
    }

    void nAxesVelocityController::stopHook()
    {
    }

    bool nAxesVelocityController::startAllAxes()
    {
        bool retval=true;
        for(vector<SimulationAxis*>::iterator axis=simulation_axes.begin();
            axis!=simulation_axes.end();axis++)
            retval&=(*axis)->drive(0.0);
        return retval;
    }

    bool nAxesVelocityController::stopAllAxes()
    {
            bool retval=true;
        for(vector<SimulationAxis*>::iterator axis=simulation_axes.begin();
            axis!=simulation_axes.end();axis++)
            retval&=(*axis)->stop();
        return retval;
    }

    bool nAxesVelocityController::lockAllAxes()
    {
    bool retval=true;
        for(vector<SimulationAxis*>::iterator axis=simulation_axes.begin();
            axis!=simulation_axes.end();axis++)
            retval&=(*axis)->lock();
        return retval;

    }

    bool nAxesVelocityController::unlockAllAxes()
    {
    bool retval=true;
        for(vector<SimulationAxis*>::iterator axis=simulation_axes.begin();
            axis!=simulation_axes.end();axis++)
            retval&=(*axis)->unlock();
        return retval;
    }
}
ORO_CREATE_COMPONENT(robot_simulators::nAxesVelocityController)