/* 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_WLDecoder_h
#define INCLUDED_WLDecoder_h

#include "serial.h"
#include "../log_processing/shared/PacketDecoder.h"
#include "../../agent/headers/CircBufPacket.h"
#include "../../agent/headers/WaveLAN.h"

static const int WLPacketHeaderLength=1+4+4; // senderId + dataType + length + timestamp

static const int MaxRobotId = 8;

#ifndef MAX
#define MAX(x,y) ((x > y) ? x : y)
#endif

/*------------------------------------------------------------------
CLASS
  WLDecoder

DESCRIPTION
  Deccodes messages being recieved from WL from one robot.
------------------------------------------------------------------*/
class WLDecoder {
public:
  static const int MaxPacketLength = 100*1024;
  static const int BufSize=MAX(WLPacketHeaderLength,16);

  WLDecoder();

  typedef void (*PacketCallback)(int robot_id,void *user_data,RobotDataPacket *packet);

  void setCallback(void *user_data,PacketCallback callback);

  void setRobotId(int robot_id);
  
  void reset();

  bool processInput(int port);

  void printText(uchar *start, int count);

  static void dumpBinary(uchar *data, int length);

  static void grabFromEncoding(uchar **buf,uchar *dest,int length) {
    memcpy(dest,*buf,length);
    (*buf)+=length;
  }

  template<class T>
  static T grab(uchar **buf) {
    T tmp;
    grabFromEncoding(buf,(uchar *)&tmp,sizeof(T));
    return tmp;
  }

  bool isPrintable(char c);

protected:
private: // methods
private: // data
  RobotDataPacket packet;
  int rawPacketStart;
  bool inPacket;
  bool readHeader;
  bool binary;
  int sourceLengthToRead;
  uchar *destData;
  int readPos;
  int robotId;
  uchar readBuf[BufSize];

  PacketCallback packetCallback;
  void *userData;
};

/*------------------------------------------------------------------
CLASS
  WLDemuxer

DESCRIPTION
  Demuxes the stream sent over from the PetProxy into one stream per
robot.
------------------------------------------------------------------*/
class WLDemuxer {
public:
  WLDemuxer();

  ~WLDemuxer();

  typedef void (*PacketCallback)(int robot_id,void *user_data,RobotDataPacket *packet);

  void setCallback(void *user_data,PacketCallback callback);

  bool processInput(int port);

private:
  void createDecoder(int robot_idx);

  bool activeDecoders[MaxRobotId+1];
  WLDecoder *decoders[MaxRobotId+1];
  int readFDs [MaxRobotId+1];
  int writeFDs[MaxRobotId+1];

  uchar buf[4096];
  int bufIdx;
  bool readingHeader;
  int curRobotId;
  int dataToRead;

  PacketCallback packetCallback;
  void *userData;
};

#endif