/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#ifndef INCLUDED_FieldExtractor_h
#define INCLUDED_FieldExtractor_h

#include <stdio.h>

#include "../../../agent/headers/CircBufPacket.h"

class RobotDataPacket;
class VisionRLEPacketDecoder;
class VisionRawPacketDecoder;

/*------------------------------------------------------------------
CLASS
  FieldExtractor

DESCRIPTION
  Extracts a field from a data packet.
------------------------------------------------------------------*/
class FieldExtractor {
public:
  virtual bool operator()(char *string_rep,RobotDataPacket *packet)=0;
};

/*------------------------------------------------------------------
CLASS
  FieldExtractorHeader

DESCRIPTION
  Extracts a field from the header of a data packet.
------------------------------------------------------------------*/
class FieldExtractorHeader : public FieldExtractor {
public:
  FieldExtractorHeader();

  enum HeaderField {P_TYPE=0,P_LENGTH=1,P_TIMESTAMP=2};
  static char const * const PTypeStr;
  static char const * const PLengthStr;
  static char const * const PTimestampStr;
  static char const * const HeaderFieldString[3];

  void setFieldHeader(enum HeaderField field_param);
  void setFieldHeader(const char *field_param);

  void setPacketTypeAll();
  void setPacketType(uchar type);

  virtual bool operator()(char *string_rep,RobotDataPacket *packet);

private:
  HeaderField field;
  
  bool all;
  uchar packetType;
};

/*------------------------------------------------------------------
CLASS
  FieldExtractorBodyGeneric

DESCRIPTION
  Extracts a field from the body of a data packet.
------------------------------------------------------------------*/
class FieldExtractorBodyGeneric : public FieldExtractor {
public:
  FieldExtractorBodyGeneric();

  void setFormat(const char *format_param);

  void setFieldBody(int offset_param);

  void setPacketType(uchar type);

  virtual bool operator()(char *string_rep,RobotDataPacket *packet) = 0;

protected:
  int offset;
  uchar packetType;

  const char *format;
};

#define FIELD_EXTRACTOR_TEMPLATE_DECLS template<class T>
#define FIELD_EXTRACTOR_TEMPLATE_FUNC T

/*------------------------------------------------------------------
CLASS
  FieldExtractorBody

DESCRIPTION
  Extracts a field from the body of a data packet.
------------------------------------------------------------------*/
FIELD_EXTRACTOR_TEMPLATE_DECLS
class FieldExtractorBody : public FieldExtractorBodyGeneric {
public:
  FieldExtractorBody();

  virtual bool operator()(char *string_rep,RobotDataPacket *packet);

private:
};

FIELD_EXTRACTOR_TEMPLATE_DECLS
FieldExtractorBody<FIELD_EXTRACTOR_TEMPLATE_FUNC>::FieldExtractorBody() {
}

FIELD_EXTRACTOR_TEMPLATE_DECLS
bool
FieldExtractorBody<FIELD_EXTRACTOR_TEMPLATE_FUNC>::operator()(char *string_rep,RobotDataPacket *packet) {
  if(packet->dataType != packetType)
    return false;

  if(packet->length < offset + sizeof(T)) {
    fprintf(stderr,
            "packet too short (%lu long) to extract field of type T (size=%d) at offset %d\n",
            packet->length,sizeof(T),offset);
    return false;
  }

  T tmp;
  memcpy(&tmp,&packet->data[offset],sizeof(T));
  sprintf(string_rep,format,tmp);
  return true;
}

/*------------------------------------------------------------------
CLASS
  FieldExtractorBodyBitfieldOffsetGeneric

DESCRIPTION
  Extracts a field from the body of a data packet that is based on
items being there or not based on a bitfield.
------------------------------------------------------------------*/
class FieldExtractorBodyBitfieldGeneric : public FieldExtractorBodyGeneric {
public:
  FieldExtractorBodyBitfieldGeneric();

  void setBitfieldSize(uint size);
  void setObjectSize  (uint size);

  void setObjectIdx(uint idx);

  virtual bool operator()(char *string_rep,RobotDataPacket *packet)=0;

protected:
  uint objectSize;   // in bytes
  uint bitfieldSize; // in bytes
  uint objectIdx;
};

#define FIELD_EXTRACTOR_BITFIELD_TEMPLATE_DECLS template<class T>
#define FIELD_EXTRACTOR_BITFIELD_TEMPLATE_FUNC T

/*------------------------------------------------------------------
CLASS
  FieldExtractorBodyBitfield

DESCRIPTION
  Extracts a field from the body of a data packet that is based on
items being there or not based on a bitfield.
------------------------------------------------------------------*/
FIELD_EXTRACTOR_BITFIELD_TEMPLATE_DECLS
class FieldExtractorBodyBitfield : public FieldExtractorBodyBitfieldGeneric {
public:
  FieldExtractorBodyBitfield();

  virtual bool operator()(char *string_rep,RobotDataPacket *packet);
};

FIELD_EXTRACTOR_BITFIELD_TEMPLATE_DECLS
FieldExtractorBodyBitfield<FIELD_EXTRACTOR_BITFIELD_TEMPLATE_FUNC>::FieldExtractorBodyBitfield() {
}

FIELD_EXTRACTOR_BITFIELD_TEMPLATE_DECLS
bool
FieldExtractorBodyBitfield<FIELD_EXTRACTOR_BITFIELD_TEMPLATE_FUNC>::operator()
  (char *string_rep,RobotDataPacket *packet) {

  if(packet->dataType != packetType)
    return false;

  if(objectIdx >= bitfieldSize*8) {
    fprintf(stderr,
            "index of %d is invalid for a bit field of size=%d\n",
            objectIdx,bitfieldSize);
    return false;
  }

  if(packet->length < bitfieldSize) {
    fprintf(stderr,
            "packet too short (%lu long) to extract bit field of size=%d\n",
            packet->length,bitfieldSize);
    return false;
  }

  bool object_present=false;
  uint objects_to_skip=0;
  for(uint i=0; i<=objectIdx; i++) {
    uint byte_offset;
    uint bit_offset;

    byte_offset = i/8; // little endian
    bit_offset = i % 8;

    bool obj_here;
    obj_here = ( (packet->data[byte_offset] & (1 << bit_offset)) != 0 );
    if(obj_here) {
      if(i==objectIdx)
        object_present=true;
      else
        objects_to_skip++;
    }
  }

  if(object_present) {
    uint data_offset;
    data_offset = bitfieldSize + objects_to_skip*objectSize + offset;

    if(packet->length < offset + sizeof(T)) {
      fprintf(stderr,
              "packet too short (%lu long) to extract field of type T (size=%d) at offset %d\n",
              packet->length,sizeof(T),offset);
      return false;
    }

    T tmp;
    memcpy(&tmp,&packet->data[data_offset],sizeof(T));
    sprintf(string_rep,format,tmp);

    //printf("packet length %lu seen x%x x%x objects_to_skip %u offset %u\n",
    //       packet->length,packet->data[0],packet->data[1],
    //       objects_to_skip,data_offset);
  }

  return object_present;
}

/*------------------------------------------------------------------
CLASS
  FieldExtractorBodyString

DESCRIPTION
  Extracts a string from a data packet.
------------------------------------------------------------------*/
class FieldExtractorBodyString : public FieldExtractor {
public:
  FieldExtractorBodyString();

  void setPacketType(uchar type);

  virtual bool operator()(char *string_rep,RobotDataPacket *packet);

private:
  uchar packetType;
};

/*------------------------------------------------------------------
CLASS
  SyncCounter

DESCRIPTION
  A counter that only increments on seeing different timestamps.
Sort of like a virtual clock.
------------------------------------------------------------------*/
class SyncCounter {
public:
  SyncCounter();

  int getValue();

  int updateValue(ulong timestamp);

private:
  int value;
  ulong lastTimestamp;
};

/*------------------------------------------------------------------
CLASS
  FieldExtractorBodyCamAngles

DESCRIPTION
  Extracts data about body angles from data packet.
------------------------------------------------------------------*/
class FieldExtractorBodyCamAngles : public FieldExtractor {
public:
  FieldExtractorBodyCamAngles(SyncCounter *counter_param);

  void setPacketType(uchar type);

  virtual bool operator()(char *string_rep,RobotDataPacket *packet);

private:
  SyncCounter *counter;
  double angles[5];
  VisionRLEPacketDecoder *decoder;

  uchar packetType;
};

/*------------------------------------------------------------------
CLASS
  FieldExtractorBodyRLEImage

DESCRIPTION
  Extracts an RLEImage from a data packet.
------------------------------------------------------------------*/
class FieldExtractorBodyRLEImage : public FieldExtractor {
public:
  FieldExtractorBodyRLEImage(SyncCounter *counter_param,int image_x_size,int image_y_size);

  void setPacketType(uchar type);

  virtual bool operator()(char *string_rep,RobotDataPacket *packet);

private:
  SyncCounter *counter;
  int imageXSize,imageYSize;
  int size;
  uchar *colors;
  uchar *img;

  VisionRLEPacketDecoder *decoder;

  uchar packetType;
};

/*------------------------------------------------------------------
CLASS
  FieldExtractorBodyRLE2Image

DESCRIPTION
  Extracts an RLEImage version 2 from a data packet.
------------------------------------------------------------------*/
class FieldExtractorBodyRLE2Image : public FieldExtractor {
public:
  FieldExtractorBodyRLE2Image(SyncCounter *counter_param,int max_image_x_size,int max_image_y_size);

  void setPacketType(uchar type);

  virtual bool operator()(char *string_rep,RobotDataPacket *packet);

private:
  SyncCounter *counter;
  int maxImageXSize,maxImageYSize;
  int size;
  uchar *colors;
  uchar *img;

  VisionRLEPacketDecoder *decoder;

  uchar packetType;
};

/*------------------------------------------------------------------
CLASS
  FieldExtractorBodyRawImage

DESCRIPTION
  Extracts an raw image from a data packet.
------------------------------------------------------------------*/
class FieldExtractorBodyRawImage : public FieldExtractor {
public:
  FieldExtractorBodyRawImage(SyncCounter *counter_param,int image_x_size,int image_y_size);

  void setPacketType(uchar type);

  virtual bool operator()(char *string_rep,RobotDataPacket *packet);

private:
  SyncCounter *counter;
  int imageXSize,imageYSize;
  int size;
  uchar *img;

  uchar packetType;
};


/*------------------------------------------------------------------
CLASS
  FieldExtractorBodyRawImage

DESCRIPTION
  Extracts an raw image from a data packet.
------------------------------------------------------------------*/
class FieldExtractorBodyRaw2Image : public FieldExtractor {
public:
  FieldExtractorBodyRaw2Image(SyncCounter *counter_param,
			      int max_image_x_size,
			      int max_image_y_size);

  void setPacketType(uchar type);

  virtual bool operator()(char *string_rep,RobotDataPacket *packet);

private:
  SyncCounter *counter;
  int maxImageXSize, maxImageYSize;
  int max_size;
  uchar *img;
  VisionRawPacketDecoder *decoder;

  uchar packetType;
};

#endif