#ifndef STRUCTURE_LIDAR_H
#define STRUCTURE_LIDAR_H

#include <Pacpus/PacpusTools/pacpusStruct.h>
#include <QVariant>
#include <QVector>
#include <QMatrix4x4>

namespace pacpus {

typedef enum {
    NONE = 0,
    ALASCA_XT = 10,
    SICK = 20,
    HOKUYO_ = 30,
    HOKUYO_30LX = 40,
    VELODYNE_64HEL = 50
} ScannerType;

typedef struct {
    float x,y,z;
    quint16 d;        // distance in cm
    quint8 echo;
    quint8 intensity;
} LidarPoint; // Plusieurs

//template <class T>
struct LidarLayer{
    quint16 layerNumber;
    quint16 nbPoint;
    float verticalAngle;
    //QVector<T> points;
    QVector<LidarPoint> points;
};

//template <class T>
struct LidarScan : public PacpusTimeStampedData{

    quint8 scannerType;
    quint8 scannerId;
    quint8 nbLayer;
    quint32 nbPoint;
    QMatrix4x4 calib;
    //int reserved;
    //QVector< LidarLayer<T> > layers;
    QVector<LidarLayer> layers;
    LidarScan(int layerCount = 1, int pointsPerLayer = 2100){
        layers.resize(layerCount);
        /*for (typename QVector< LidarLayer<T> >::iterator it = layers.begin(), itend = layers.end(); it != itend; ++it) {
            it->points.reserve(pointsPerLayer);
        }*/
        nbLayer = layerCount;
        for (typename QVector<LidarLayer>::iterator it = layers.begin(), itend = layers.end(); it != itend; ++it)
                    it->points.reserve(pointsPerLayer);

    }
};

// A définir
typedef struct {


} LidarConfig;

} // end namespace pacpus

Q_DECLARE_METATYPE(pacpus::LidarPoint)
inline QDataStream & operator << (QDataStream & out, const pacpus::LidarPoint & Valeur){
    out << Valeur.x << Valeur.y << Valeur.z << Valeur.intensity << Valeur.d << Valeur.echo;
    return out;}

inline QDataStream & operator >> (QDataStream & in, pacpus::LidarPoint & Valeur){
    in >> Valeur.x >> Valeur.y >> Valeur.z >> Valeur.intensity >> Valeur.d >> Valeur.echo;
    return in;}

Q_DECLARE_METATYPE(pacpus::LidarLayer)
inline QDataStream & operator << (QDataStream & out, const pacpus::LidarLayer & Valeur){
    out << Valeur.nbPoint << Valeur.layerNumber << Valeur.verticalAngle;
    for(int i = 0; i < Valeur.nbPoint ; ++i)
        out << Valeur.points[i];
    return out;}

inline QDataStream & operator >> (QDataStream & in, pacpus::LidarLayer & Valeur){
    in >> Valeur.nbPoint >> Valeur.layerNumber >> Valeur.verticalAngle;
    for(int i = 0; i < Valeur.nbPoint; ++i)
        in >> Valeur.points[i];
    return in;}

Q_DECLARE_METATYPE(pacpus::LidarScan)
inline QDataStream & operator << (QDataStream & out, const pacpus::LidarScan & Valeur){
    out << (quint64)Valeur.time << Valeur.timerange << Valeur.sequenceNumber <<
           Valeur.scannerId << Valeur.scannerType << Valeur.nbPoint << Valeur.nbLayer;
    for(int i=0; i < Valeur.nbLayer ; ++i)
        out << Valeur.layers[i];
    return out;
}

inline QDataStream & operator >> (QDataStream & in, pacpus::LidarScan & Valeur){
    in >> (quint64&)Valeur.time >> Valeur.timerange >> Valeur.sequenceNumber >>
           Valeur.scannerId >> Valeur.scannerType >> Valeur.nbPoint >> Valeur.nbLayer;
    for(int i=0; i < Valeur.nbLayer ; ++i)
        in >> Valeur.layers[i];
    return in;
}

#endif // STRUCTURE_LIDAR_H