#ifndef SENSORPACKAGEUNIT_H
#define SENSORPACKAGEUNIT_H

#include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/units/UnitInterface.h>
#include <RobotAPI/interface/units/OrientedTactileSensorUnit.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <netinet/in.h>
#include <fstream>
#include <stdio.h>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <Eigen/Dense>

namespace armarx
{
    class OrientedTactileSensorUnitPropertyDefinitions:
        public ComponentPropertyDefinitions
    {
    public:
        OrientedTactileSensorUnitPropertyDefinitions(std::string prefix):
            ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>(
                "SerialInterfaceDevice",
                "/dev/ttyACM0",
                "The serial device the arduino is connected to.");

            defineOptionalProperty<std::string>(
                "TopicName",
                "OrientedTactileSensorValues",
                "Name of the topic on which the sensor values are provided");

            defineOptionalProperty<std::string>(
                "CalibrationData",
                "65524 3 12 65534 65534 1 1208 119 58726 1000 943 ",
                "Sensor Register Data to calibrate the sensor");

            defineOptionalProperty<std::string>(
                "SamplesRotation",
                "20",
                "number of orientation values to differentiate");

            defineOptionalProperty<std::string>(
                "SamplesPressure",
                "10",
                "number of pressure values to differentiate");

            defineOptionalProperty<std::string>(
                "SamplesAcceleration",
                "20",
                "number of pressure values to differentiate");

            defineOptionalProperty<bool>(
                "calibrateSensor",
                "false"
                "Set true to calibrate the sensor and get calibration data and false to use existent calibration data");
        }

    };

    /**
     * @class OrientedTactileSensorUnit
     * @brief ArmarX wrapper for an arduino due with one BNO055 IMU and one BMP280 pressure sensor
     *
     */
    class OrientedTactileSensorUnit:
        virtual public armarx::Component
        //TODO: needs interface to send calibration data
    {
    public:
        OrientedTactileSensorUnit();

        virtual std::string getDefaultName() const
        {
            return "OrientedTactileSensorUnit";
        }

        struct SensorData
        {
            int id;
            float pressure;
            float qw, qx, qy, qz;
            float accelx, accely, accelz;
        };

        struct CalibrationData
        {
            int accel_offset_x, accel_offset_y, accel_offset_z, gyro_offset_x, gyro_offset_y, gyro_offset_z, mag_offset_x, mag_offset_y, mag_offset_z, accel_radius, mag_radius;
        };

        struct PressureRate
        {
            IceUtil::Time timestamp;
            float pressure;
        };

        struct RotationRate
        {
            IceUtil::Time timestamp;
            Eigen::Quaternionf orientation;
        };

        struct AccelerationRate
        {
            IceUtil::Time timestamp;
            float rotationRate;
        };
        struct LinAccRate
        {
            IceUtil::Time timestamp;
            float accx, accy, accz;
        };

    protected:
        virtual void onInitComponent();
        virtual void onConnectComponent();

        virtual PropertyDefinitionsPtr createPropertyDefinitions();

    private:
        std::fstream arduino;
        RunningTask<OrientedTactileSensorUnit>::pointer_type readTask;
        OrientedTactileSensorUnitListenerPrx topicPrx;
        OrientedTactileSensorUnitInterfacePrx interfacePrx;

        void run();
        SensorData getValues(std::string line);
        bool getCalibrationValues(std::string line);
        bool loadCalibration();
        int fd;
        CalibrationData calibration;

        std::vector<RotationRate> samplesRotation;
        std::vector<PressureRate> samplesPressure;
        std::vector<AccelerationRate> samplesAcceleration;
        int maxSamplesRotation;
        int sampleIndexRotation;
        int maxSamplesPressure;
        int sampleIndexPressure;
        int maxSamplesAcceleration;
        int sampleIndexAcceleration;
    };
}
#endif // SENSORPACKAGEUNIT_H