vCodec.h

/*
 *   Copyright (C) 2017 Event-driven Perception for Robotics
 *   Author: arren.glover@iit.it
 *
 *   This program is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU Lesser General Public License as published by
 *   the Free Software Foundation, either version 3 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 Lesser General Public License
 *   along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */

#ifndef __VCODEC__
#define __VCODEC__

#include "event-driven/vtsHelper.h"
#include <memory>
#include <deque>
#include <math.h>
#include <vector>
#include <iostream>

namespace ev {

#define IS_SKIN(x)      x&0x01000000
#define IS_SAMPLE(x)    x&0x00804000
#define IS_SSA(x)       x&0x00004000
#define IS_SSV(x)       x&0x00800000
#define IS_IMUSAMPLE(x) x&0x02000000
#define IS_AUDIO(x)     x&0x04000000

//macros
class vEvent;

template<typename V = vEvent> using event = std::shared_ptr<V>;
template<typename V1, typename V2> inline event<V1> as_event(event<V2> orig_event) {
    return std::dynamic_pointer_cast<V1>(orig_event);
}
template<typename V1, typename V2> inline event<V1> is_event(event<V2> orig_event) {
    return std::static_pointer_cast<V1>(orig_event);
}
template<typename V> event<V> inline make_event(void) {
    return std::make_shared<V>();
}
template<typename V> inline V* read_as(const event<> &orig_event) {
    return (V *)orig_event.get();
}
template<typename V1, typename V2> event<V1> make_event(event<V2> orig_event) {
    return std::make_shared<V1>(*(orig_event.get()));
}
using vQueue = std::deque< event<vEvent> >;

void qsort(vQueue &q, bool respectWraps = false);

event<> createEvent(const std::string &type);

unsigned int packetSize(const std::string &type);

enum { VLEFT = 0, VRIGHT = 1 } ;

//event declarations
class vEvent
{
public:
    static const std::string tag;
    unsigned int stamp:31;

    vEvent();
    virtual ~vEvent();

    virtual event<> clone();
    virtual void encode(yarp::os::Bottle &b) const;
    virtual void encode(std::vector<int32_t> &b, unsigned int &pos) const;
    virtual bool decode(const yarp::os::Bottle &packet, size_t &pos);
    virtual void decode(const int32_t *&data);
    virtual yarp::os::Property getContent() const;
    virtual std::string getType() const;
    virtual int getChannel() const;
    virtual void setChannel();
};

class AddressEvent : public vEvent
{
public:
    static const std::string tag;

    union
    {
        uint32_t _coded_data;
        struct {
            unsigned int polarity:1;
            unsigned int x:9;
            unsigned int _xfill:2;
            unsigned int y:8;
            unsigned int _yfill:2;
            unsigned int channel:1;
            unsigned int type:1;
            unsigned int skin:1;
            unsigned int _fill:7;
        };
    };

    AddressEvent();
    AddressEvent(const vEvent &v);
    AddressEvent(const AddressEvent &v);

    virtual event<> clone();
    virtual void encode(yarp::os::Bottle &b) const;
    virtual void encode(std::vector<int32_t> &b, unsigned int &pos) const;
    virtual bool decode(const yarp::os::Bottle &packet, size_t &pos);
    virtual void decode(const int32_t *&data);
    virtual yarp::os::Property getContent() const;
    virtual std::string getType() const;
    virtual int getChannel() const;
    virtual void setChannel(const int channel);
};
using AE = AddressEvent;

class SkinEvent : public vEvent
{
public:
    static const std::string tag;

    union
    {
        uint32_t _skei;
        struct {
            unsigned int polarity:1;
            unsigned int taxel:10;
            unsigned int _reserved1:2;
            unsigned int cross_base:1;
            unsigned int _sample:1;
            unsigned int _error:1;
            unsigned int body_part:3;
            unsigned int _reserved2:3;
            unsigned int side:1;
            unsigned int type:1;
            unsigned int skin:1;
        };
    };

    SkinEvent();
    SkinEvent(const vEvent &v);
    SkinEvent(const SkinEvent &v);

    virtual event<> clone();
    virtual void encode(yarp::os::Bottle &b) const;
    virtual void encode(std::vector<int32_t> &b, unsigned int &pos) const;
    virtual bool decode(const yarp::os::Bottle &packet, size_t &pos);
    virtual void decode(const int32_t *&data);
    virtual yarp::os::Property getContent() const;
    virtual std::string getType() const;

};

class SkinSample : public SkinEvent
{
public:
    static const std::string tag;

    unsigned int _ts:31;
    unsigned int value:16;

    SkinSample();
    SkinSample(const vEvent &v);
    SkinSample(const SkinSample &v);

    virtual event<> clone();
    virtual void encode(yarp::os::Bottle &b) const;
    virtual void encode(std::vector<int32_t> &b, unsigned int &pos) const;
    virtual bool decode(const yarp::os::Bottle &packet, size_t &pos);
    virtual void decode(const int32_t *&data);
    virtual yarp::os::Property getContent() const;
    virtual std::string getType() const;

};

class FlowEvent : public AddressEvent
{
public:
    static const std::string tag;
    union {
        uint32_t _fei[2];
        struct {
            float vx;
            float vy;
        };
    };


    FlowEvent();
    FlowEvent(const vEvent &v);
    FlowEvent(const FlowEvent &v);

    virtual event<> clone();
    virtual void encode(yarp::os::Bottle &b) const;
    virtual void encode(std::vector<int32_t> &b, unsigned int &pos) const;
    virtual bool decode(const yarp::os::Bottle &packet, size_t &pos);
    virtual void decode(const int32_t *&data);
    virtual yarp::os::Property getContent() const;
    virtual std::string getType() const;

    int getDeath() const;
};

class LabelledAE : public AddressEvent
{
public:
    static const std::string tag;
    int ID;

    LabelledAE();
    LabelledAE(const vEvent &v);
    LabelledAE(const LabelledAE &v);

    virtual event<> clone();
    virtual void encode(yarp::os::Bottle &b) const;
    virtual void encode(std::vector<int32_t> &b, unsigned int &pos) const;
    virtual bool decode(const yarp::os::Bottle &packet, size_t &pos);
    virtual void decode(const int32_t *&data);
    virtual yarp::os::Property getContent() const;
    virtual std::string getType() const;
};

class GaussianAE : public LabelledAE
{
public:
    static const std::string tag;
    union {
        uint32_t _gaei[3];
        struct {
            float sigx;
            float sigy;
            float sigxy;
        };
    };

    GaussianAE();
    GaussianAE(const vEvent &v);
    GaussianAE(const GaussianAE &v);

    virtual event<> clone();
    virtual void encode(yarp::os::Bottle &b) const;
    virtual void encode(std::vector<int32_t> &b, unsigned int &pos) const;
    virtual bool decode(const yarp::os::Bottle &packet, size_t &pos);
    virtual void decode(const int32_t *&data);
    virtual yarp::os::Property getContent() const;
    virtual std::string getType() const;
};

class IMUevent : public vEvent
{
public:
    static const std::string tag;

    union
    {
        uint32_t _coded_data;
        struct {
            int value:16;
            unsigned int sensor:4;
            unsigned int _r1:2;
            unsigned int channel:1;
            unsigned int type:1;
            unsigned int _r2:8;
        };
    };

    const static unsigned int _max_value = 32768;

    IMUevent();
    IMUevent(const vEvent &v);
    IMUevent(const IMUevent &v);

    virtual event<> clone();
    virtual void encode(yarp::os::Bottle &b) const;
    virtual void encode(std::vector<int32_t> &b, unsigned int &pos) const;
    virtual bool decode(const yarp::os::Bottle &packet, size_t &pos);
    virtual void decode(const int32_t *&data);
    virtual yarp::os::Property getContent() const;
    virtual std::string getType() const;
    virtual int getChannel() const { return channel;}
    virtual void setChannel(const int channel) { this->channel = channel;}
};

template <class T> size_t countEvents(const T &q) { return q.size(); }
//template <> size_t countEvents<vQueue>(const T &q) { return q.size(); }

template <typename T> inline int countTime(const T &q, int &p_time)
{
    if(!p_time) p_time = q.front().stamp;
    int dt = q.back().stamp - q.front().stamp;
    p_time = q.back().stamp;
    if(dt < 0) dt += vtsHelper::max_stamp;
    return dt;
}

template <> inline int countTime<vQueue> (const vQueue &q, int &p_time)
{
    if(!p_time) p_time = q.front()->stamp;
    int dt = q.back()->stamp - q.front()->stamp;
    p_time = q.back()->stamp;
    if(dt < 0) dt += vtsHelper::max_stamp;
    return dt;
}

template <> inline int countTime< std::vector<int32_t> > (const std::vector<int32_t> &q, int &p_time)
{
    if(!p_time) p_time = q[0] & vtsHelper::max_stamp;
    int dt = (q[q.size()-2] & vtsHelper::max_stamp) - p_time;
    p_time = q[q.size()-2] & vtsHelper::max_stamp;
    if(dt < 0) dt += vtsHelper::max_stamp;
    return dt;
}



}

#endif