/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX 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, see <http://www.gnu.org/licenses/>.
 *
 * @package    ArmarXCore::units
 * @author     Peter Kaiser <peter dot kaiser at kit dot edu>
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */

#ifndef _ARMARX_ROBOTAPI_HAPTIC_OBSERVER_H
#define _ARMARX_ROBOTAPI_HAPTIC_OBSERVER_H

#include <RobotAPI/interface/units/HapticUnit.h>
#include <ArmarXCore/observers/Observer.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
#include <ArmarXCore/util/variants/eigen3/Eigen3VariantObjectFactories.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>


namespace armarx
{
    /**
     * \class HapticObserverPropertyDefinitions
     * \brief
     */
    class HapticObserverPropertyDefinitions:
        public ObserverPropertyDefinitions
    {
    public:
        HapticObserverPropertyDefinitions(std::string prefix):
            ObserverPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("HapticTopicName", "HapticValues", "Name of the HapticUnit Topic");
        }
    };

    /**
     * \class HapticSampleStatistics
     * \brief
     */
    class HapticSampleStatistics
    {
    public:
        HapticSampleStatistics(unsigned int count, long timestamp)
        {
            this->lastTimestamp = timestamp;
            this->count = count;
            this->pos = 0;
        }

        void add(long timestamp)
        {
            long delta = timestamp - lastTimestamp;

            if (deltas.size() < count)
            {
                deltas.push_back(delta);
            }
            else
            {
                deltas.at(pos) = delta;
                pos = (pos + 1) % count;
            }

            lastTimestamp = timestamp;
        }

        /*long average(long timestamp)
        {
            long sum = timestamp - lastTimestamp;
            for(std::vector<long>::iterator it = deltas.begin(); it != deltas.end(); ++it)
            {
                sum += *it;
            }
            return sum / (deltas.size() + 1);
        }*/

        long average()
        {
            if (deltas.size() == 0)
            {
                return 0;
            }

            long sum = 0;

            for (std::vector<long>::iterator it = deltas.begin(); it != deltas.end(); ++it)
            {
                sum += *it;
            }

            return sum / deltas.size();
        }

    private:
        long lastTimestamp;
        unsigned int count;
        int pos;
        std::vector<long> deltas;
    };

    /**
     * \class HapticObserver
     * \ingroup RobotAPI-SensorActorUnits-observers
     * \brief Observer monitoring haptic sensor values
     *
     * The HapticObserver monitors haptic sensor values published by HapticUnit-implementations and offers condition checks on these values.
     * Available condition checks are: *updated*, *larger*, *equals* and *smaller*.
     */
    class HapticObserver :
        virtual public Observer,
        virtual public HapticUnitObserverInterface
    {
    public:
        HapticObserver();

        void setTopicName(std::string topicName);

        // framework hooks
        virtual std::string getDefaultName() const
        {
            return "HapticUnitObserver";
        }
        void onInitObserver();
        void onConnectObserver();
        void onExitObserver();

        void reportSensorValues(const ::std::string& device, const ::std::string& name, const ::armarx::MatrixFloatBasePtr& values, const ::armarx::TimestampBasePtr& timestamp, const ::Ice::Current& = ::Ice::Current());

        /**
         * \see PropertyUser::createPropertyDefinitions()
         */
        virtual PropertyDefinitionsPtr createPropertyDefinitions();
    private:
        armarx::Mutex dataMutex;
        std::string topicName;
        PeriodicTask<HapticObserver>::pointer_type statisticsTask;

        void updateStatistics();

        std::map<std::string, HapticSampleStatistics> statistics;

    };
}

#endif