marxbotsensors.cpp

/********************************************************************************
 *  FARSA Experiments Library                                                   *
 *  Copyright (C) 2007-2012                                                     *
 *  Gianluca Massera <emmegian@yahoo.it>                                        *
 *  Stefano Nolfi <stefano.nolfi@istc.cnr.it>                                   *
 *  Tomassino Ferrauto <tomassino.ferrauto@istc.cnr.it>                         *
 *  Onofrio Gigliotta <onofrio.gigliotta@istc.cnr.it>                           *
 *                                                                              *
 *  This program is free software; you can redistribute it and/or modify        *
 *  it under the terms of the GNU General Public License as published by        *
 *  the Free Software Foundation; either version 2 of the License, or           *
 *  (at your option) any later version.                                         *
 *                                                                              *
 *  This program is distributed in the hope that it will be useful,             *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of              *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               *
 *  GNU General Public License for more details.                                *
 *                                                                              *
 *  You should have received a copy of the GNU General Public License           *
 *  along with this program; if not, write to the Free Software                 *
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA  *
 ********************************************************************************/

#include "marxbotsensors.h"
#include "configurationhelper.h"
#include "logger.h"
#include "graphicalwobject.h"
#include "arena.h"
#include <QMutex>

namespace farsa {

MarXbotSensor::MarXbotSensor(ConfigurationParameters& params, QString prefix) :
    Sensor(params, prefix),
    m_marxbotResource("robot"),
    m_neuronsIteratorResource("neuronsIterator")
{
    // Reading parameters
    m_marxbotResource = actualResourceNameForMultirobot(ConfigurationHelper::getString(params, prefix + "marxbot", m_marxbotResource));
    m_neuronsIteratorResource = actualResourceNameForMultirobot(ConfigurationHelper::getString(params, prefix + "neuronsIterator", m_neuronsIteratorResource));

    // Declaring the resources that are needed here
    usableResources(QStringList() << m_marxbotResource << m_neuronsIteratorResource);
}

MarXbotSensor::~MarXbotSensor()
{
    // Nothing to do here
}

void MarXbotSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    Sensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotSensor", this);
    params.createParameter(prefix, "marxbot", m_marxbotResource);
    params.createParameter(prefix, "neuronsIterator", m_neuronsIteratorResource);
}

void MarXbotSensor::describe(QString type)
{
    // Calling parent function
    Sensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The base class for MarXbot sensors");
    d.describeString("marxbot").def("robot").help("The name of the resource associated with the MarXbot robot to use (default is \"robot\")");
    d.describeString("neuronsIterator").def("neuronsIterator").help("The name of the resource associated with the neural network iterator (default is \"neuronsIterator\")");
}

void MarXbotSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    Sensor::resourceChanged(resourceName, changeType);

    // Here we only check whether the resource has been deleted and reset the check flag, the
    // actual work is done in subclasses
    if (changeType == Deleted) {
        resetNeededResourcesCheck();
        return;
    }
}

MarXbotProximityIRSensor::MarXbotProximityIRSensor(ConfigurationParameters& params, QString prefix) :
    MarXbotSensor(params, prefix),
    m_robot(NULL),
    m_neuronsIterator(NULL),
    m_activeSensors(ConfigurationHelper::getBoolVector(params, prefix + "activeSensors", "111111111111111111111111")),
    m_numActiveSensors(m_activeSensors.count(true))
{
    if (m_activeSensors.size() != 24) {
        ConfigurationHelper::throwUserConfigError(prefix + "activeSensors", params.getValue(prefix + "activeSensors"), "The parameter must be a list of exactly 24 elements either equal to 1 or to 0 (do not use any space to separate elements, just put them directly one after the other)");
    }
}

MarXbotProximityIRSensor::~MarXbotProximityIRSensor()
{
    // Nothing to do here
}

void MarXbotProximityIRSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    MarXbotSensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotProximityIRSensor", this);
    QString activeSensorsString;
    for (int i = 0; i < m_activeSensors.size(); i++) {
        activeSensorsString += (m_activeSensors[i] ? "1" : "0");
    }
    params.createParameter(prefix, "activeSensors", activeSensorsString);
}

void MarXbotProximityIRSensor::describe(QString type)
{
    // Calling parent function
    MarXbotSensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The infrared proximity sensors of the MarXbot robot", "The infrared proximity sensors of the MarXbot robot. These are the very short range IR sensors all around the base");
    d.describeString("activeSensors").def("111111111111111111111111").help("Which IR sensors of the robot are actually enabled", "This is a string of exactly 24 elements. Each element can be either \"0\" or \"1\" to respectively disable/enable the corresponding proximity IR sensor. The first sensor is the one on the left side of the robot and the others follow counterclockwise (i.e. left, back, right, front)");
}

void MarXbotProximityIRSensor::update()
{
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker( this );

    m_neuronsIterator->setCurrentBlock(name());
    for (int i = 0; i < m_activeSensors.size(); i++) {
        if (m_activeSensors[i]) {
            m_neuronsIterator->setInput(m_robot->proximityIRSensorController()->activation(i));
            m_neuronsIterator->nextNeuron();
        }
    }
}

int MarXbotProximityIRSensor::size()
{
    return m_numActiveSensors;
}

void MarXbotProximityIRSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    MarXbotSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == m_marxbotResource) {
        m_robot = getResource<PhyMarXbot>();

        // Eanbling sensors
        m_robot->proximityIRSensorController()->setEnabled(true);

        // Now enabling/disabling individual sensors depending on the m_activeSensors vector
        for (int i = 0; i < m_activeSensors.size(); i++) {
            m_robot->proximityIRSensorController()->setSensorActive(i, m_activeSensors[i]);
        }
    } else if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());

        // We use the actual sensor ID in the neuron name
        for (int i = 0; i < m_activeSensors.size(); i++) {
            if (m_activeSensors[i]) {
                m_neuronsIterator->setGraphicProperties("ir" + QString::number(i), 0.0, 1.0, Qt::red);
                m_neuronsIterator->nextNeuron();
            }
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

MarXbotGroundBottomIRSensor::MarXbotGroundBottomIRSensor(ConfigurationParameters& params, QString prefix) :
    MarXbotSensor(params, prefix),
    m_robot(NULL),
    m_neuronsIterator(NULL)
{
}

MarXbotGroundBottomIRSensor::~MarXbotGroundBottomIRSensor()
{
    // Nothing to do here
}

void MarXbotGroundBottomIRSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    MarXbotSensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotGroundBottomIRSensor", this);
}

void MarXbotGroundBottomIRSensor::describe(QString type)
{
    // Calling parent function
    MarXbotSensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The infrared ground bottom sensors of the MarXbot robot", "The infrared ground bottom sensors of the MarXbot robot. These are the four ground sensors below the robot battery pack.");
}

void MarXbotGroundBottomIRSensor::update()
{
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker( this );

    m_neuronsIterator->setCurrentBlock(name());
    for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
        m_neuronsIterator->setInput(m_robot->groundBottomIRSensorController()->activation(i));
    }
}

int MarXbotGroundBottomIRSensor::size()
{
    return 4;
}

void MarXbotGroundBottomIRSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    MarXbotSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == m_marxbotResource) {
        m_robot = getResource<PhyMarXbot>();

        // Eanbling sensors
        m_robot->groundBottomIRSensorController()->setEnabled(true);
    } else if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());
        for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setGraphicProperties("gb" + QString::number(i), 0.0, 1.0, Qt::red);
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

MarXbotGroundAroundIRSensor::MarXbotGroundAroundIRSensor(ConfigurationParameters& params, QString prefix) :
    MarXbotSensor(params, prefix),
    m_robot(NULL),
    m_neuronsIterator(NULL)
{
}

MarXbotGroundAroundIRSensor::~MarXbotGroundAroundIRSensor()
{
    // Nothing to do here
}

void MarXbotGroundAroundIRSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    MarXbotSensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotGroundAroundIRSensor", this);
}

void MarXbotGroundAroundIRSensor::describe(QString type)
{
    // Calling parent function
    MarXbotSensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The infrared ground around sensors of the MarXbot robot", "The infrared ground around sensors of the MarXbot robot. These are the eight ground sensors below the base of the robot (just above the wheels).");
}

void MarXbotGroundAroundIRSensor::update()
{
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker( this );

    m_neuronsIterator->setCurrentBlock(name());
    for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
        m_neuronsIterator->setInput(m_robot->groundAroundIRSensorController()->activation(i));
    }
}

int MarXbotGroundAroundIRSensor::size()
{
    return 8;
}

void MarXbotGroundAroundIRSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    MarXbotSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == m_marxbotResource) {
        m_robot = getResource<PhyMarXbot>();

        // Eanbling sensors
        m_robot->groundAroundIRSensorController()->setEnabled(true);
    } else if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());
        for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setGraphicProperties("ga" + QString::number(i), 0.0, 1.0, Qt::red);
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

MarXbotLinearCameraSensor::MarXbotLinearCameraSensor(ConfigurationParameters& params, QString prefix) :
    MarXbotSensor(params, prefix),
    m_robot(NULL),
    m_neuronsIterator(NULL),
    m_numReceptors(ConfigurationHelper::getInt(params, prefix + "numReceptors", 8)),
    m_useRedChannel(ConfigurationHelper::getBool(params, prefix + "useRedChannel", true)),
    m_useGreenChannel(ConfigurationHelper::getBool(params, prefix + "useGreenChannel", true)),
    m_useBlueChannel(ConfigurationHelper::getBool(params, prefix + "useBlueChannel", true)),
    m_usedColorChannels((m_useRedChannel ? 1 : 0) + (m_useGreenChannel ? 1 : 0) + (m_useBlueChannel ? 1 : 0)),
    m_aperture(ConfigurationHelper::getDouble(params, prefix + "aperture", 360.0f)),
    m_camera(NULL),
    m_drawCamera(ConfigurationHelper::getBool(params, prefix + "drawCamera", true))
{
    // Few sanity checks
    if (m_numReceptors <= 0) {
        ConfigurationHelper::throwUserConfigError(prefix + "numReceptors", params.getValue(prefix + "numReceptors"), "The parameter must be an integer greater or equal to 1");
    }
    if ((m_aperture < 0.0f) || (m_aperture > 360.0f)) {
        ConfigurationHelper::throwUserConfigError(prefix + "aperture", params.getValue(prefix + "aperture"), "The parameter must be a real number between 0 and 360");
    }
}

MarXbotLinearCameraSensor::~MarXbotLinearCameraSensor()
{
    // Deleting the camera
    delete m_camera;
}

void MarXbotLinearCameraSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    MarXbotSensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotLinearCameraSensor", this);
    params.createParameter(prefix, "numReceptors", QString::number(m_numReceptors));
    params.createParameter(prefix, "useRedChannel", (m_useRedChannel ? "true" : "false"));
    params.createParameter(prefix, "useGreenChannel", (m_useGreenChannel ? "true" : "false"));
    params.createParameter(prefix, "useBlueChannel", (m_useBlueChannel ? "true" : "false"));
    params.createParameter(prefix, "aperture", QString::number(m_aperture));
    params.createParameter(prefix, "drawCamera", (m_drawCamera ? "true" : "false"));
}

void MarXbotLinearCameraSensor::describe(QString type)
{
    // Calling parent function
    MarXbotSensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The linear camera sensor of the MarXbot robot", "This is a 360° linear camera");
    d.describeInt("numReceptors").def(8).limits(1, MaxInteger).help("The number of receptors of the sensor", "Each receptor returns three values, one for each of the three colors (red, green, blue). This means that the size returned by this sensor is 3 * numReceptors (default is 8)");
    d.describeBool("useRedChannel").def(true).help("Whether the red component of the perceived objects should be used or not (default true)");
    d.describeBool("useGreenChannel").def(true).help("Whether the green component of the perceived objects should be used or not (default true)");
    d.describeBool("useBlueChannel").def(true).help("Whether the blue component of the perceived objects should be used or not (default true)");
    d.describeReal("aperture").def(360.0f).limits(0.0f, 360.0f).help("The aperture of the camera in degrees", "The real MarXbot has an omnidirectional camera, so you can use here any value up to 360 degrees (default is 360)");
    d.describeBool("drawCamera").def(true).help("Whether to draw the camera or not");
}

void MarXbotLinearCameraSensor::update()
{
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker(this);

    // Updating the camera
    m_camera->update();

    // Reading activations: first the red one, then the green one and finally the blue one
    m_neuronsIterator->setCurrentBlock(name());
    if (m_useRedChannel) {
        for (int i = 0; i < m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setInput(m_camera->colorForReceptor(i).redF());
        }
    }
    if (m_useGreenChannel) {
        for (int i = 0; i < m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setInput(m_camera->colorForReceptor(i).greenF());
        }
    }
    if (m_useBlueChannel) {
        for (int i = 0; i < m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setInput(m_camera->colorForReceptor(i).blueF());
        }
    }
}

int MarXbotLinearCameraSensor::size()
{
    return m_numReceptors * m_usedColorChannels;
}

void MarXbotLinearCameraSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    MarXbotSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        // Deleting the camera if the robot was deleted
        if (resourceName == m_marxbotResource) {
            delete m_camera;
            m_camera = NULL;
        }

        return;
    }

    if (resourceName == m_marxbotResource) {
        m_robot = getResource<PhyMarXbot>();

        // Now we can also create the camera
        wMatrix mtr = wMatrix::roll(-PI_GRECO / 2.0);
#ifdef __GNUC__
    #warning QUI INVECE DI UNA COSTANTE, CALCOLARSI UNA POSIZIONE DALLE DIMENSIONI DEL ROBOT
#endif
        mtr.w_pos.z = 0.13f;
        m_camera = new LinearCamera(m_robot, mtr, toRad(m_aperture), m_numReceptors, Qt::black);

        // Sharing resources with the camera
        m_camera->shareResourcesWith(this);
        m_camera->drawCamera(m_drawCamera);
    } else if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());
        if (m_useRedChannel) {
            for (int i = 0; i < m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
                m_neuronsIterator->setGraphicProperties("lr" + QString::number(i), 0.0, 1.0, Qt::red);
            }
        }
        if (m_useGreenChannel) {
            for (int i = 0; i < m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
                m_neuronsIterator->setGraphicProperties("lg" + QString::number(i), 0.0, 1.0, Qt::red);
            }
        }
        if (m_useBlueChannel) {
            for (int i = 0; i < m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
                m_neuronsIterator->setGraphicProperties("lb" + QString::number(i), 0.0, 1.0, Qt::red);
            }
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

namespace __MarXbotTractionSensor_internal {
    #ifndef GLMultMatrix
    #define GLMultMatrix glMultMatrixf
    // for double glMultMatrixd
    #endif

    class TractionSensorGraphic : public GraphicalWObject
    {
    public:
        TractionSensorGraphic(WObject *object, const wVector& offset, real radius, real scale = 1.0, real maxLength = 1.0, QString name = "unamed") :
            GraphicalWObject(object->world(), name),
            m_object(object),
            m_radius(radius),
            m_scale(scale),
            m_maxLength(maxLength),
            m_vector(0.0, 0.0, 0.0),
            m_vectorMutex()
        {
            // Attaching to object (which also becomes our owner). We also build the displacement matrix
            wMatrix displacement = wMatrix::identity();
            displacement.w_pos = offset;
            attachToObject(m_object, true, displacement);

            // We also use our own color and texture
            setUseColorTextureOfOwner(false);
            setTexture("");
        }

        ~TractionSensorGraphic()
        {
        }

        void setVector(const wVector vector)
        {
            m_vectorMutex.lock();
                m_vector = vector;
            m_vectorMutex.unlock();
        }

    protected:
        virtual void render(RenderWObject* renderer, QGLContext* gw)
        {
            // Copying the m_activations vector to a local vector to avoid concurrent accesses.
            m_vectorMutex.lock();
                const wVector vector = m_vector;
            m_vectorMutex.unlock();

            // Setting the matrix to draw the vector.
            glPushMatrix();
            GLMultMatrix(&tm[0][0]);

            // Before drawing computing the length of the body and of the tip of the arrow. The
            // tip is always m_radius long, unless the arrow is so small that no body is present.
            // The bodyLength will be negative if the vector is very short: in this cas we won't
            // draw the arrow body. The radius of the tip is 3/2 of the cylinder radius unless the
            // arrow is very small (and has no body): in this case the radius is scaled accordingly.
            // We also have to take into account the maximum allowed length of the vector and
            // change color if it is too long
            const real originalVectorLength = vector.norm();
            const wVector axis = vector.scale(1.0 / originalVectorLength);
            const QColor vectorColor = (originalVectorLength > m_maxLength) ? Qt::red : Qt::cyan;
            const real vectorLength = min(originalVectorLength, m_maxLength) * m_scale;
            const real tipLength = min(vectorLength, m_radius);
            const real tipRadius = ((m_radius * 3.0) / 2.0) * (tipLength / m_radius);
            const real bodyLength = vectorLength - m_radius;

            // Setting the color of the vector
            setColor(vectorColor);
            renderer->container()->setupColorTexture(gw, renderer);

            // First drawing the body if its length is positive
            if (bodyLength > 0.0) {
                RenderWObjectContainer::drawCylinder(axis, axis.scale(0.5 * bodyLength), bodyLength, m_radius, color());
            }

            // Now drawing the tip as a cone:
            {
                GLUquadricObj *pObj;

                // opengl cylinder are aligned alogn the z axis, we want it along our axis,
                // we create a rotation matrix to do the alignment. Moreover the cone must
                // start at the end of the cylinder we draw before
                const wVector tipDisplacement = (bodyLength < 0.0) ? wVector(0.0, 0.0, 0.0) : axis.scale(bodyLength);
                glPushMatrix();
                wMatrix matrix = wMatrix::grammSchmidt(axis);
                matrix.w_pos = tipDisplacement;
                GLMultMatrix(&matrix[0][0]);

                // Get a new Quadric off the stack
                pObj = gluNewQuadric();
                gluQuadricTexture(pObj, true);
                gluCylinder(pObj, tipRadius, 0, tipLength, 20, 2);

                // render the caps
                gluQuadricOrientation(pObj, GLU_INSIDE);
                gluDisk(pObj, 0.0f, tipRadius, 20, 1);

                gluDeleteQuadric(pObj);
                glPopMatrix();
            }

            // Popping twice because we pushed both our matrix and the offset matrix
            glPopMatrix();
            glPopMatrix();
        }

        WObject *const m_object;

        const real m_radius;

        const real m_scale;

        const real m_maxLength;

        wVector m_vector;

        QMutex m_vectorMutex;
    };
}

using namespace __MarXbotTractionSensor_internal;

MarXbotTractionSensor::MarXbotTractionSensor(ConfigurationParameters& params, QString prefix) :
    MarXbotSensor(params, prefix),
    m_robot(NULL),
    m_neuronsIterator(NULL),
    m_maxForce(ConfigurationHelper::getDouble(params, prefix + "maxForce", 1.0f)),
    m_drawSensor(ConfigurationHelper::getBool(params, prefix + "drawSensor", true)),
    m_graphics(NULL)
{
    if (m_maxForce < 0.0) {
        ConfigurationHelper::throwUserConfigError(prefix + "maxForce", params.getValue(prefix + "maxForce"), "The parameter must be a positive real number");
    }
}

MarXbotTractionSensor::~MarXbotTractionSensor()
{
    // Nothing to do here
}

void MarXbotTractionSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    MarXbotSensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotTractionSensor", this);
    params.createParameter(prefix, "maxForce", QString::number(m_maxForce));
    params.createParameter(prefix, "drawSensor", m_drawSensor ? QString("true") : QString("false"));
}

void MarXbotTractionSensor::describe(QString type)
{
    // Calling parent function
    MarXbotSensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The traction sensors of the MarXbot robot", "The traction sensors of the MarXbot robot. It is placed between the base and the turret. Note that this sensor only works when the robot is in dynamic mode");
    d.describeReal("maxForce").def(1.0f).limits(0.0f, +Infinity).help("The maximum possible value of the force", "This is the value of the force on one axis over which the corresponding neuron is activated with 1");
    d.describeBool("drawSensor").def(true).help("Whether to draw the sensor", "If true the sensor is graphically represented by an arrow in the direction of the current traction");
}

void MarXbotTractionSensor::update()
{
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker( this );

    m_neuronsIterator->setCurrentBlock(name());
    const wVector& t = m_robot->tractionSensorController()->traction();

    // The first two neurons correspond to the x axis, the second two to the y axis
    if (t.x > 0.0) {
        const real f = min(t.x, m_maxForce);
        m_neuronsIterator->setInput(f / m_maxForce);
        m_neuronsIterator->nextNeuron();
        m_neuronsIterator->setInput(0.0);
        m_neuronsIterator->nextNeuron();
    } else {
        const real f = max(t.x, -m_maxForce);
        m_neuronsIterator->setInput(0.0);
        m_neuronsIterator->nextNeuron();
        m_neuronsIterator->setInput(-f / m_maxForce);
        m_neuronsIterator->nextNeuron();
    }

    if (t.y > 0.0) {
        const real f = min(t.y, m_maxForce);
        m_neuronsIterator->setInput(f / m_maxForce);
        m_neuronsIterator->nextNeuron();
        m_neuronsIterator->setInput(0.0);
        m_neuronsIterator->nextNeuron();
    } else {
        const real f = max(t.y, -m_maxForce);
        m_neuronsIterator->setInput(0.0);
        m_neuronsIterator->nextNeuron();
        m_neuronsIterator->setInput(-f / m_maxForce);
        m_neuronsIterator->nextNeuron();
    }

    // Updating graphical representation if we have to
    if (m_drawSensor) {
        // We only draw the x and y components of the vector
        m_graphics->setVector(wVector(t.x, t.y, 0.0));
    }
}

int MarXbotTractionSensor::size()
{
    return 4;
}

void MarXbotTractionSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    MarXbotSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == m_marxbotResource) {
        m_robot = getResource<PhyMarXbot>();

        // Eanbling sensors
        m_robot->proximityIRSensorController()->setEnabled(true);

        // If graphics is enabled, creating the graphical object
        if (m_drawSensor) {
            const wVector offset(0.0, 0.0, PhyMarXbot::basez + PhyMarXbot::bodyh + 0.1);

            m_graphics = new TractionSensorGraphic(m_robot, offset, 0.005, 0.1 / m_maxForce, m_maxForce, "Traction sensor");
        }
    } else if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());
        m_neuronsIterator->setGraphicProperties("t+x", 0.0, 1.0, Qt::red);
        m_neuronsIterator->nextNeuron();
        m_neuronsIterator->setGraphicProperties("t-x", 0.0, 1.0, Qt::red);
        m_neuronsIterator->nextNeuron();
        m_neuronsIterator->setGraphicProperties("t+y", 0.0, 1.0, Qt::red);
        m_neuronsIterator->nextNeuron();
        m_neuronsIterator->setGraphicProperties("t-y", 0.0, 1.0, Qt::red);
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

MarXbotSampledProximityIRSensor::MarXbotSampledProximityIRSensor(ConfigurationParameters& params, QString prefix) :
    MarXbotSensor(params, prefix),
    m_robot(NULL),
    m_arena(NULL),
    m_neuronsIterator(NULL),
    m_activeSensors(ConfigurationHelper::getBoolVector(params, prefix + "activeSensors", "111111111111111111111111")),
    m_numActiveSensors(m_activeSensors.count(true)),
    m_roundSamples(ConfigurationHelper::getString(params, prefix + "roundSamples", "round.sam")),
    m_smallSamples(ConfigurationHelper::getString(params, prefix + "smallSamples", "small.sam")),
    m_wallSamples(ConfigurationHelper::getString(params, prefix + "wallSamples", "wall.sam"))
{
    if (m_activeSensors.size() != 24) {
        ConfigurationHelper::throwUserConfigError(prefix + "activeSensors", params.getValue(prefix + "activeSensors"), "The parameter must be a list of exactly 24 elements either equal to 1 or to 0 (do not use any space to separate elements, just put them directly one after the other)");
    }

    // Checking that the sampled files have the right number of IR sensors
    if (m_roundSamples.numIR() != 24) {
        ConfigurationHelper::throwUserConfigError(prefix + "roundSamples", m_roundSamples.filename(), "The file has samples for the wrong number of sensors, expected 24, got " + QString::number(m_roundSamples.numIR()));
    }
    if (m_smallSamples.numIR() != 24) {
        ConfigurationHelper::throwUserConfigError(prefix + "smallSamples", m_smallSamples.filename(), "The file has samples for the wrong number of sensors, expected 24, got " + QString::number(m_smallSamples.numIR()));
    }
    if (m_wallSamples.numIR() != 24) {
        ConfigurationHelper::throwUserConfigError(prefix + "wallSamples", m_wallSamples.filename(), "The file has samples for the wrong number of sensors, expected 8, got " + QString::number(m_wallSamples.numIR()));
    }

    // Here we also need the arena to work
    addUsableResource("arena");
}

MarXbotSampledProximityIRSensor::~MarXbotSampledProximityIRSensor()
{
    // Nothing to do here
}

void MarXbotSampledProximityIRSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    MarXbotSensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotSampledProximityIRSensor", this);
    QString activeSensorsString;
    for (int i = 0; i < m_activeSensors.size(); i++) {
        activeSensorsString += (m_activeSensors[i] ? "1" : "0");
    }
    params.createParameter(prefix, "activeSensors", activeSensorsString);
    params.createParameter(prefix, "roundSamples", m_roundSamples.filename());
    params.createParameter(prefix, "smallSamples", m_smallSamples.filename());
    params.createParameter(prefix, "wallSamples", m_wallSamples.filename());
}

void MarXbotSampledProximityIRSensor::describe(QString type)
{
    // Calling parent function
    MarXbotSensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The sampled proximity infrared sensors of the MarXbot", "This is the sampled version of the proximity infrared sensors of the MarXbot. This sensor only works with objects created using the Arena");
    d.describeString("activeSensors").def("111111111111111111111111").help("Which IR sensors of the robot are actually enabled", "This is a string of exactly 24 elements. Each element can be either \"0\" or \"1\" to respectively disable/enable the corresponding proximity IR sensor. The first sensor is the one on the left side of the robot and the others follow counterclockwise (i.e. left, back, right, front)");
    d.describeString("roundSamples").def("round.sam").help("The name of the file with samples for big round objects");
    d.describeString("smallSamples").def("small.sam").help("The name of the file with samples for small round objects");
    d.describeString("wallSamples").def("wall.sam").help("The name of the file with samples for walls");
}

void MarXbotSampledProximityIRSensor::update()
{
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker(this);

    // Getting the list of objects in the arena
    const QVector<PhyObject2DWrapper*>& objectsList = m_arena->getObjects();

    // Preparing the vector with activations and setting all values to 0
    QVector<real> activations(m_activeSensors.size(), 0.0);

    // Cycling through the list of objects. We first need to get the current position and orientation of the robot
    const wVector robotPos = m_robot->position();
    const real robotAng = m_robot->orientation(m_arena->getPlane());
    foreach(const PhyObject2DWrapper* obj, objectsList) {
        // Computing angle and distance. We don't need to remove the robot to which this sensor belongs because
        // the calculatations will give a negative distance
        double distance;
        double angle;

        // If computeDistanceAndOrientationFromRobot returns false, we have to discard this object
        if (!obj->computeDistanceAndOrientationFromRobot(*(m_arena->getRobotWrapper(m_marxbotResource)), distance, angle)) {
            continue;
        }

        // Getting the activation. The switch is to understand which samples to use
        QVector<unsigned int>::const_iterator actIt = QVector<unsigned int>::const_iterator();
        switch (obj->type()) {
            case PhyObject2DWrapper::Wall:
                actIt = m_wallSamples.getActivation(distance, angle);
                break;
            case PhyObject2DWrapper::SmallCylinder:
                actIt = m_smallSamples.getActivation(distance, angle);
                break;
            case PhyObject2DWrapper::BigCylinder:
            case PhyObject2DWrapper::WheeledRobot:
                actIt = m_roundSamples.getActivation(distance, angle);
                break;
            default:
                Logger::warning("The sampled infrared sensor only works with Small Cylinders, Big Cylinders, Walls and other Robots");
                continue;
        }

        // Adding activations in the activations vector
        for (int i = 0; i < activations.size(); ++i, ++actIt) {
            activations[i] = min(1.0, activations[i] + (real(*actIt) / 1024.0));
        }
    }

    // Finally activating neurons
    m_neuronsIterator->setCurrentBlock(name());
    for (int i = 0; i < m_activeSensors.size(); i++) {
        if (m_activeSensors[i]) {
            m_neuronsIterator->setInput(activations[i]);
            m_neuronsIterator->nextNeuron();
        }
    }
}

int MarXbotSampledProximityIRSensor::size()
{
    return m_numActiveSensors;
}

void MarXbotSampledProximityIRSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    MarXbotSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == m_marxbotResource) {
        m_robot = getResource<MarXbot>();

        // Disabling proximity IR sensors, they are not used here
        m_robot->proximityIRSensorController()->setEnabled(false);
    } else if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());

        // We use the actual sensor ID in the neuron name
        for (int i = 0; i < m_activeSensors.size(); i++) {
            if (m_activeSensors[i]) {
                m_neuronsIterator->setGraphicProperties("ir" + QString::number(i), 0.0, 1.0, Qt::red);
                m_neuronsIterator->nextNeuron();
            }
        }
    } else if (resourceName == "arena") {
        // Storing the pointer to the arena
        m_arena = getResource<Arena>();
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

MarXbotAttachmentDeviceSensor::MarXbotAttachmentDeviceSensor(ConfigurationParameters& params, QString prefix) :
    MarXbotSensor(params, prefix),
    m_robot(NULL),
    m_neuronsIterator(NULL),
    m_enablePosition(ConfigurationHelper::getBool(params, prefix + "enablePosition", true)),
    m_enableStatus(ConfigurationHelper::getBool(params, prefix + "enableStatus", true)),
    m_enableAttached(ConfigurationHelper::getBool(params, prefix + "enableAttached", true)),
    m_enableOtherAttached(ConfigurationHelper::getBool(params, prefix + "enableOtherAttached", true)),
    m_numInputs((m_enablePosition ? 2 : 0) + (m_enableStatus ? 1 : 0) + (m_enableAttached ? 1 : 0) + (m_enableOtherAttached ? 1 : 0))
{
}

MarXbotAttachmentDeviceSensor::~MarXbotAttachmentDeviceSensor()
{
    // Nothing to do here
}

void MarXbotAttachmentDeviceSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    MarXbotSensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotAttachmentDeviceSensor", this);
    params.createParameter(prefix, "enablePosition", (m_enablePosition ? "true" : "false"));
    params.createParameter(prefix, "enableStatus", (m_enableStatus ? "true" : "false"));
    params.createParameter(prefix, "enableAttached", (m_enableAttached ? "true" : "false"));
    params.createParameter(prefix, "enableOtherAttached", (m_enableOtherAttached ? "true" : "false"));
}

void MarXbotAttachmentDeviceSensor::describe(QString type)
{
    // Calling parent function
    MarXbotSensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The sensor providing the attachment device proprioception", "This sensor provides information about the current status of the attachment device. It provides several inputs (which can be disabled using parameters). The position of the gripper is provided using two neurons (the sin() and the cos() of the angle) to alleviate the problem of 0° and 360° being the same angle with opposite activations.");
    d.describeBool("enablePosition").def(true).help("Whether inputs with the position of the attachment device should be enabled or not (default: true)");
    d.describeBool("enableStatus").def(true).help("Whether the input with the status of the attachment device should be enabled or not (default: true)");
    d.describeBool("enableAttached").def(true).help("Whether the input telling if this robot is attached to another robot should be enabled or not (default: true)");
    d.describeBool("enableOtherAttached").def(true).help("Whether the input telling if another robot is attached to this robot should be enabled or not (default: true)");
}

void MarXbotAttachmentDeviceSensor::update()
{
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker(this);

    // Getting the motor controller for the attachment device
    const MarXbotAttachmentDeviceMotorController *const ctrl = m_robot->attachmentDeviceController();

    // Now reading values
    m_neuronsIterator->setCurrentBlock(name());
    if (m_enablePosition) {
        const real pos = ctrl->getPosition();
        m_neuronsIterator->setInput(sin(pos) * 0.5 + 0.5);
        m_neuronsIterator->nextNeuron();
        m_neuronsIterator->setInput(cos(pos) * 0.5 + 0.5);
        m_neuronsIterator->nextNeuron();
    }
    if (m_enableStatus) {
        switch (ctrl->getStatus()) {
            case MarXbotAttachmentDeviceMotorController::Open:
                m_neuronsIterator->setInput(0.0);
                break;
            case MarXbotAttachmentDeviceMotorController::HalfClosed:
                m_neuronsIterator->setInput(0.5);
                break;
            case MarXbotAttachmentDeviceMotorController::Closed:
                m_neuronsIterator->setInput(1.0);
                break;
        }
        m_neuronsIterator->nextNeuron();
    }
    if (m_enableAttached) {
        if (ctrl->attachedToRobot()) {
            m_neuronsIterator->setInput(1.0);
        } else {
            m_neuronsIterator->setInput(0.0);
        }
        m_neuronsIterator->nextNeuron();
    }
    if (m_enableOtherAttached) {
        if (ctrl->otherAttachedToUs()) {
            m_neuronsIterator->setInput(1.0);
        } else {
            m_neuronsIterator->setInput(0.0);
        }
        m_neuronsIterator->nextNeuron();
    }
}

int MarXbotAttachmentDeviceSensor::size()
{
    return m_numInputs;
}

void MarXbotAttachmentDeviceSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    MarXbotSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == m_marxbotResource) {
        m_robot = getResource<MarXbot>();

        // Enabling the attachment device and its motor controller
        m_robot->enableAttachmentDevice(true);
        m_robot->attachmentDeviceController()->setEnabled(true);
    } else if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());

        // Creating labels depending on which inputs are activated
        if (m_enablePosition) {
            m_neuronsIterator->setGraphicProperties("aps", 0.0, 1.0, Qt::red);
            m_neuronsIterator->nextNeuron();
            m_neuronsIterator->setGraphicProperties("apc", 0.0, 1.0, Qt::red);
            m_neuronsIterator->nextNeuron();
        }
        if (m_enableStatus) {
            m_neuronsIterator->setGraphicProperties("as", 0.0, 1.0, Qt::red);
            m_neuronsIterator->nextNeuron();
        }
        if (m_enableAttached) {
            m_neuronsIterator->setGraphicProperties("aa", 0.0, 1.0, Qt::red);
            m_neuronsIterator->nextNeuron();
        }
        if (m_enableOtherAttached) {
            m_neuronsIterator->setGraphicProperties("ao", 0.0, 1.0, Qt::red);
            m_neuronsIterator->nextNeuron();
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

}