sensors.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 "sensors.h"
#include "configurationhelper.h"
#include "motorcontrollers.h"
#include "logger.h"
#include "graphicalwobject.h"
#include "arena.h"
#include <QStringList>
#include <QList>
#include <QtAlgorithms>
#include <limits>
#include <cmath>
#include <QLinkedList>

namespace farsa {

//ObjectPositionSensor : begin implementation
// it returns the absolute coordinate of an object into the world
ObjectPositionSensor::ObjectPositionSensor(ConfigurationParameters &params, QString prefix) :
    Sensor(params, prefix) {
    neuronsIteratorResource = ConfigurationHelper::getString(params, prefix + "neuronsIterator", "neuronsIterator");
    objectName = ConfigurationHelper::getString( params, prefix+"object", "object" );
    QVector<double> vec1 = ConfigurationHelper::getVector( params, prefix+"bbMin" );
    QVector<double> vec2 = ConfigurationHelper::getVector( params, prefix+"bbMax" );
    if ( vec1.size() == 3 && vec2.size() == 3 ) {
        linearize = true;
        bbMin = wVector( vec1[0], vec1[1], vec1[2] );
        bbMax = wVector( vec2[0], vec2[1], vec2[2] );
    } else {
        linearize = false;
        if ( ! (vec1.isEmpty() && vec2.isEmpty()) ) {
            Logger::warning( QString("ObjectPositionSensor %1 - bbMin and/or bbMax parameters are not well specified; they will be ignored").arg(name()) );
        }
    }

    // Declaring the resources that are needed here
    usableResources( QStringList() << objectName << neuronsIteratorResource );
}

ObjectPositionSensor::~ObjectPositionSensor() {
    // nothing to do
}

void ObjectPositionSensor::describe( QString type ) {
    Sensor::describe( type );
    Descriptor d = addTypeDescription( type, "Sensor for reading the three absolute coordinate (position into the worlf frame) of an object" );
    d.describeString("neuronsIterator").def("neuronsIterator").help("the name of the resource associated with the neural network iterator (default is \"neuronsIterator\")");
    d.describeString( "object" ).def( "object" ).props( IsMandatory ).help( "The name of the resource associated with the object to track with this sensor" );
    d.describeReal( "bbMin" ).props( IsList ).help( "The minimum 3D point used for linearize the object position into [0,1]" );
    d.describeReal( "bbMax" ).props( IsList ).help( "The maximum 3D point used for linearize the object position into [0,1]" );
}

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

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

    WObject* object = getResource<WObject>( objectName );
    wVector pos = object->matrix().w_pos;
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=0; i<3; i++, evonetIt->nextNeuron() ) {
        if ( linearize ) {
            // linearize into [0,1]
            evonetIt->setInput( linearMap( pos[i], bbMin[i], bbMax[i], 0, 1 ) );
        } else {
            evonetIt->setInput( pos[i] );
        }
    }
}

int ObjectPositionSensor::size() {
    return 3;
}

void ObjectPositionSensor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    if (changeType == Deleted) {
        resetNeededResourcesCheck();
        return;
    }

    if (resourceName == objectName) {
        // Nothing to do here, we get the object with getResource() in update()
    } else if (resourceName == neuronsIteratorResource) {
        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        for( int i=0; i<3; i++, evonetIt->nextNeuron() ) {
            evonetIt->setGraphicProperties( QString("obj")+QString::number(i), -10.0, 10.0, Qt::red );
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

void ObjectPositionSensor::save(ConfigurationParameters &params, QString prefix)
{
    Sensor::save( params, prefix );
    params.startObjectParameters( prefix, "ObjectPositionSensor", this );
    params.createParameter(prefix, "neuronsIterator", neuronsIteratorResource);
    params.createParameter( prefix, "object", objectName );
    if ( linearize ) {
        params.createParameter( prefix, "bbMin", QString("%1 %2 %3").arg(bbMin[0]).arg(bbMin[1]).arg(bbMin[2]) );
        params.createParameter( prefix, "bbMax", QString("%1 %2 %3").arg(bbMax[0]).arg(bbMax[1]).arg(bbMax[2]) );
    }
}
//ObjectPositionSensor : end implementation

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

    const float linearCameraCubeSide = 0.02f;

    const float linearCameraReceptorsLength = 0.1f;

    class LinearCameraGraphic : public GraphicalWObject
    {
    public:
        LinearCameraGraphic(WObject *object, const wMatrix& transformation, double minAngle, double maxAngle, unsigned int numReceptors, QString name = "unamed") :
            GraphicalWObject(object->world(), name),
            m_object(object),
            m_transformation(transformation),
            m_minAngle(minAngle),
            m_maxAngle(maxAngle),
            m_numReceptors(numReceptors),
            m_receptorRange((m_maxAngle - m_minAngle) / double(m_numReceptors)),
            m_receptors(m_numReceptors, Qt::black)
        {
            // Attaching to object (which also becomes our owner)
            attachToObject(m_object, true);

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

        ~LinearCameraGraphic()
        {
        }

        void setPerceivedColors(const QVector<QColor>& receptors)
        {
            m_receptorsMutex.lock();
                m_receptors = receptors;
            m_receptorsMutex.unlock();
        }

    protected:
        virtual void render(RenderWObject* renderer, QGLContext* gw)
        {
            // Bringing the frame of reference at the center of the camera
            wMatrix mtr = m_transformation * tm;
            glPushMatrix();
            renderer->container()->setupColorTexture(gw, renderer);
            GLMultMatrix(&mtr[0][0]);

            // First of all drawing the camera as a small white box. The face in the
            // direction of view (X axis) is painted half green: the green part is the
            // one in the direction of the upvector (Z axis)
            glBegin(GL_QUADS);
            const float hside = linearCameraCubeSide / 2.0;

            // front (top part)
            glColor3f(0.0, 1.0, 0.0);
            glNormal3f(1.0, 0.0, 0.0);
            glVertex3f( hside, -hside,  hside);
            glVertex3f( hside, -hside,    0.0);
            glVertex3f( hside,  hside,    0.0);
            glVertex3f( hside,  hside,  hside);

            // front (bottom part)
            glColor3f(1.0, 1.0, 1.0);
            glNormal3f(1.0, 0.0, 0.0);
            glVertex3f( hside, -hside,    0.0);
            glVertex3f( hside, -hside, -hside);
            glVertex3f( hside,  hside, -hside);
            glVertex3f( hside,  hside,    0.0);

            // back
            glNormal3f(-1.0, 0.0, 0.0);
            glVertex3f(-hside, -hside, -hside);
            glVertex3f(-hside, -hside,  hside);
            glVertex3f(-hside,  hside,  hside);
            glVertex3f(-hside,  hside, -hside);

            // top
            glNormal3f(0.0, 1.0, 0.0);
            glVertex3f(-hside,  hside,  hside);
            glVertex3f( hside,  hside,  hside);
            glVertex3f( hside,  hside, -hside);
            glVertex3f(-hside,  hside, -hside);

            // bottom
            glNormal3f(0.0, -1.0, 0.0);
            glVertex3f(-hside, -hside, -hside);
            glVertex3f( hside, -hside, -hside);
            glVertex3f( hside, -hside,  hside);
            glVertex3f(-hside, -hside,  hside);

            // right
            glNormal3f(0.0, 0.0, 1.0);
            glVertex3f( hside, -hside,  hside);
            glVertex3f(-hside, -hside,  hside);
            glVertex3f(-hside,  hside,  hside);
            glVertex3f( hside,  hside,  hside);

            // left
            glNormal3f(0.0, 0.0, -1.0);
            glVertex3f( hside, -hside, -hside);
            glVertex3f(-hside, -hside, -hside);
            glVertex3f(-hside,  hside, -hside);
            glVertex3f( hside,  hside, -hside);

            glEnd();

            // Now we draw white lines to separare the various sectors of the camera
            // Disabling lighting here (we want pure lines no matter from where we look at them)
            glPushAttrib(GL_LIGHTING_BIT);
            glDisable(GL_LIGHTING);
            glLineWidth(2.5);
            glColor3f(1.0, 1.0, 1.0);

            // Drawing the lines
            glBegin(GL_LINES);
            for (unsigned int i = 0; i <= m_numReceptors; i++) {
                const double curAngle = m_minAngle + double(i) * m_receptorRange;

                const wVector lineEnd = wVector(cos(curAngle), sin(curAngle), 0.0).scale(linearCameraReceptorsLength);

                glVertex3f(0.0, 0.0, 0.0);
                glVertex3f(lineEnd.x, lineEnd.y, lineEnd.z);
            }
            glEnd();

            // Now drawing the state of receptors. Here we also have to lock the semaphore for
            // the m_receptors vector
            m_receptorsMutex.lock();

            // Drawing the status
            glBegin(GL_QUADS);
            glNormal3f(0.0, 1.0, 0.0);
            const double colorPatchAngle = m_receptorRange / 3.0;
            const double colorPatchMinLength = linearCameraReceptorsLength / 3.0;
            const double colorPatchMaxLength = 2.0 * linearCameraReceptorsLength / 3.0;
            for (unsigned int i = 0; i < m_numReceptors; i++) {
                const double curAngle = m_minAngle + double(i) * m_receptorRange;

                for (unsigned int c = 0; c < 3; c++) {
                    const double startAngle = curAngle + double(c) * colorPatchAngle;
                    const double endAngle = curAngle + double(c + 1) * colorPatchAngle;

                    // Computing the four vertexes
                    const wVector v1 = wVector(cos(startAngle), sin(startAngle), 0.0).scale(colorPatchMinLength);
                    const wVector v2 = wVector(cos(startAngle), sin(startAngle), 0.0).scale(colorPatchMaxLength);
                    const wVector v3 = wVector(cos(endAngle), sin(endAngle), 0.0).scale(colorPatchMaxLength);
                    const wVector v4 = wVector(cos(endAngle), sin(endAngle), 0.0).scale(colorPatchMinLength);

                    // Setting the color
                    switch (c) {
                        case 0:
                            glColor3f(m_receptors[i].redF(), 0.0, 0.0);
                            break;
                        case 1:
                            glColor3f(0.0, m_receptors[i].greenF(), 0.0);
                            break;
                        case 2:
                            glColor3f(0.0, 0.0, m_receptors[i].blueF());
                            break;
                        default:
                            break;
                    }

                    // Drawing the patch
                    glVertex3f(v1.x, v1.y, v1.z);
                    glVertex3f(v2.x, v2.y, v2.z);
                    glVertex3f(v3.x, v3.y, v3.z);
                    glVertex3f(v4.x, v4.y, v4.z);
                }
            }
            glEnd();
            m_receptorsMutex.unlock();

            // Restoring lighting status
            glPopAttrib();

            glPopMatrix();
        }

        WObject* const m_object;

        const wMatrix m_transformation;

        const double m_minAngle;

        const double m_maxAngle;

        const unsigned int m_numReceptors;

        const double m_receptorRange;

        QVector<QColor> m_receptors;

        QMutex m_receptorsMutex;
    };
}

using namespace __LinearCamera_internal;

LinearCamera::LinearCamera(WObject* obj, wMatrix mtr, double aperture, unsigned int numReceptors, QColor backgroundColor) :
    ConcurrentResourcesUser(),
    m_receptors(numReceptors),
    m_object(obj),
    m_transformation(mtr),
    m_aperture((aperture > (2.0 * PI_GRECO)) ? (2.0 * PI_GRECO) : ((aperture < 0.0) ? 0.0 :  aperture)),
    m_numReceptors(numReceptors),
    m_backgroundColor(backgroundColor),
    m_apertureMin(-m_aperture / 2.0),
    m_apertureMax(m_aperture / 2.0),
    m_receptorRange(m_aperture / double(m_numReceptors)),
    m_arena(NULL),
    m_drawCamera(false),
    m_graphicalCamera(NULL)
    
{
    // Stating which resources we use here
    addUsableResource("arena");
}

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

namespace {
    // This namespace contains some structures used in the LinearCamera::update() function

    // The structure containing a color and the range of the camera field hit by this color.
    // It also contains the distance from the camera for ordering.
    struct ColorRangeAndDistance
    {
        ColorRangeAndDistance() :
            color(),
            minAngle(0.0),
            maxAngle(0.0),
            distance(0.0)
        {
        }

        ColorRangeAndDistance(QColor c, double min, double max, double d) :
            color(c),
            minAngle(min),
            maxAngle(max),
            distance(d)
        {
        }

        // This is to order objects of this type
        bool operator<(const ColorRangeAndDistance& other) const
        {
            return (distance < other.distance);
        }

        QColor color;
        double minAngle;
        double maxAngle;
        double distance;
    };

    // An helper class to ease computations with multiple intervals. This class starts with a single
    // interval and the allows to remove portions. When removing an interval, returns the portion
    // of the initial range that was actually removed
    class MultiInterval
    {
    private:
        struct SingleInterval
        {
            double start;
            double end;
        };

    public:
        MultiInterval() :
            m_originalSize(0.0),
            m_intervals()
        {
        }

        void initMultiInterval(double start, double end)
        {
            m_originalSize = end - start;

            SingleInterval i;
            i.start = start;
            i.end = end;
            m_intervals.append(i);
        }

        double removeInterval(double start, double end)
        {
            double removedSize = 0.0;

            // We exit from the cycle when both these variables are true: intervals are ordered so,
            // if we have found both the interval for start and the one for end we can exit
            bool foundStartInIntervals = false;
            bool foundEndInIntervals = false;
            QLinkedList<SingleInterval>::iterator it = m_intervals.begin();
            while ((it != m_intervals.end()) && (!foundStartInIntervals || !foundEndInIntervals)) {
                if ((start <= it->start) && (end >= it->end)) {
                    // Removing the whole interval and continuing
                    removedSize += it->end - it->start;
                    it = m_intervals.erase(it);
                } else if ((start >= it->start) && (start < it->end) && (end > it->start) && (end <= it->end)) {
                    // Here we have to split the interval in two. We put the two new intervals in place
                    // of the old one
                    removedSize += end - start;
                    SingleInterval i1, i2;
                    i1.start = it->start;
                    i1.end = start;
                    i2.start = end;
                    i2.end = it->end;
                    it = m_intervals.erase(it);
                    // Going one step back to insert the two new items
                    --it;
                    it = m_intervals.insert(it, i1);
                    it = m_intervals.insert(it, i2);

                    // This interval was completely inside another interval, so no other interval will
                    // be intersected and we can exit from the cycle
                    foundStartInIntervals = true;
                    foundEndInIntervals = true;
                } else if ((start > it->start) && (start < it->end)) {
                    // Here we have to reduce the interval by setting the new end
                    removedSize += it->end - start;
                    it->end = start;
                    foundStartInIntervals = true;
                    ++it;
                } else if ((end > it->start) && (end < it->end)) {
                    // Here we have to reduce the interval, by setting the new start
                    removedSize += end - it->start;
                    it->start = end;
                    foundEndInIntervals = true;
                    ++it;
                } else {
                    // Simply incrementing the iterator
                    ++it;
                }
            }

            return removedSize / m_originalSize;
        }

    private:
        double m_originalSize;
        // Intervals will always be ordered from the one with the lowest start to the one with the highest start.
        // Moreover two intervals will never intersect
        QLinkedList<SingleInterval> m_intervals;
    };

    // An helper structure memorizing information about colors in a single receptor. minAngle and maxAngle
    // are used to store the current portion of the receptor for which we already know the color, while
    // colorsAndFractions is the list of colors and the portion of the receptor occupied by that color
    struct ColorsInReceptor
    {
        MultiInterval curInterval;

        struct ColorAndFraction {
            ColorAndFraction() :
                color(),
                fraction(0.0)
            {
            }

            ColorAndFraction(QColor c, double f) :
                color(c),
                fraction(f)
            {
            }

            QColor color;
            double fraction;
        };
        QList<ColorAndFraction> colorsAndFractions;
    };
}

void LinearCamera::update()
{
#ifdef __GNUC__
    #warning APPENA I ROBOT SONO NELLA LISTA DEGLI OGGETTI, BISOGNA RICORDARSI DI ESCLUDERE L OGGETTO CUI LA CAMERA È ATTACCATA QUANDO SI CALCOLA L ATTIVAZIONE
#endif
    // Getting the list of objects from the arena (if we have the pointer to the arena)
    if (m_arena == NULL) {
        m_receptors.fill(m_backgroundColor);

        return;
    }
    const QVector<PhyObject2DWrapper*>& objectsList = m_arena->getObjects();

    // If no object is present, we can fill the receptors list with background colors and return
    if (objectsList.size() == 0) {
        m_receptors.fill(m_backgroundColor);
    
        return;
    }

    // Updating the matrix with the current camera position
    wMatrix currentMtr = m_transformation * m_object->matrix();

    // First of all we need to compute which color hits each receptor

    // Now filling the list with colors, ranges and distances. If an object is perceived at the
    // extremities of the aperture, it is split in two different ColorRangeAndDistance objects
    QList<ColorRangeAndDistance> colorsRangesAndDistances;

    // For the moment we use the distance to order objects (see ColorRangeAndDistance::operator<), however
    // this is not correct (occlusion doesn't work well) and so should be changed
    for (int i = 0; i < objectsList.size(); i++) {
        const QColor color = objectsList[i]->color();
        double minAngle;
        double maxAngle;
        double distance;
        objectsList[i]->computeLinearViewFieldOccupiedRange(currentMtr, minAngle, maxAngle, distance);

        // computeLinearViewFieldOccupiedRange returns a negative distance if the object is outside the view field
        if (distance < 0.0) {
            continue;
        }

        // If the minAngle is greater than the maxAngle, splitting in two, so that we do not have to
        // make special cases in the subsequent part of the function. Here we also check if the object
        // is completely outside the view field or not (in the first case we don't add it to the list)
        // We just check if the object is at least partially visible, we don't set the limits to be
        // within the view field
        if (minAngle > maxAngle) {
            if ((minAngle > m_apertureMin) && (minAngle < m_apertureMax)) {
                colorsRangesAndDistances.append(ColorRangeAndDistance(color, minAngle, m_apertureMax, distance));
            }
            if ((maxAngle > m_apertureMin) && (maxAngle < m_apertureMax)) {
                colorsRangesAndDistances.append(ColorRangeAndDistance(color, m_apertureMin, maxAngle, distance));
            }
        } else {
            if (((minAngle > m_apertureMin) && (minAngle < m_apertureMax)) || ((maxAngle > m_apertureMin) && (maxAngle < m_apertureMax))) {
                colorsRangesAndDistances.append(ColorRangeAndDistance(color, minAngle, maxAngle, distance));
            }
        }
    }

    // Ordering colors by distance from the camera
    qSort(colorsRangesAndDistances);

    // Now we can add the background color at the end of the list. It covers all receptors to be sure to fill
    // the whole field with a valid color
    colorsRangesAndDistances.append(ColorRangeAndDistance(m_backgroundColor, m_apertureMin, m_apertureMax, std::numeric_limits<double>::infinity()));

    // The next step is to calculate the percentage of each color in the colorsRangesAndDistances list
    // in each receptor
    QVector<ColorsInReceptor> colorsInReceptors(m_numReceptors);
    for (QList<ColorRangeAndDistance>::const_iterator it = colorsRangesAndDistances.begin(); it != colorsRangesAndDistances.end(); ++it) {
        // Computing the index of receptors which are interested by this color
        const int minIndex = max(0, floor((it->minAngle - m_apertureMin) / m_receptorRange));
        const int maxIndex = min(m_numReceptors - 1, floor((it->maxAngle - m_apertureMin) / m_receptorRange));

        // Now cycling over the computed receptors in the colorsInReceptors list to fill it
        for (int i = minIndex; i <= maxIndex; i++) {
            if (colorsInReceptors[i].colorsAndFractions.size() == 0) {
                // This is the first color in the receptor, we have to initialize the interval
                const double receptorMin = m_apertureMin + m_receptorRange * double(i);
                const double receptorMax = m_apertureMin + m_receptorRange * double(i + 1);
                colorsInReceptors[i].curInterval.initMultiInterval(receptorMin, receptorMax);
            }

            const double fraction = min(1.0, colorsInReceptors[i].curInterval.removeInterval(it->minAngle, it->maxAngle));
            colorsInReceptors[i].colorsAndFractions.append(ColorsInReceptor::ColorAndFraction(it->color, fraction));
        }
    }

    // The final step is to compute the resulting color for each receptor. See class description for a comment
    // on this procedure
    for (int i = 0; i < m_numReceptors; i++) {
        double red = 0.0;
        double green = 0.0;
        double blue = 0.0;
        for (QList<ColorsInReceptor::ColorAndFraction>::const_iterator it = colorsInReceptors[i].colorsAndFractions.begin(); it != colorsInReceptors[i].colorsAndFractions.end(); ++it) {
            red += it->color.redF() * it->fraction;
            green += it->color.greenF() * it->fraction;
            blue += it->color.blueF() * it->fraction;
        }
        m_receptors[i] = QColor::fromRgbF(min(1.0, red), min(1.0, green), min(1.0, blue));
    }

    // Updating graphics if we have to
    if (m_drawCamera) {
        m_graphicalCamera->setPerceivedColors(m_receptors);
    }
}

void LinearCamera::drawCamera(bool d)
{
    if (m_drawCamera == d) {
        return;
    }

    m_drawCamera = d;
    if (m_drawCamera) {
        m_graphicalCamera = new LinearCameraGraphic(m_object, m_transformation, m_apertureMin, m_apertureMax, m_numReceptors, "linearCamera");
    } else {
        delete m_graphicalCamera;
    }
}

void LinearCamera::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    if (resourceName == "arena") {
        switch (changeType) {
            case Created:
            case Modified:
                m_arena = getResource<Arena>();
                break;
            case Deleted:
                m_arena = NULL;
                break;
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " (in LinearCamera)");
    }
}

} // end namespace farsa