/* 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_SPOutDecoder_h
#define INCLUDED_SPOutDecoder_h

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

static const int SPOutPacketHeaderLength=1+2+5; // dataType + length + timestamp

/*------------------------------------------------------------------
CLASS
  SPOutDecoder

DESCRIPTION
  Encodes messages being sent out to SPOut.
------------------------------------------------------------------*/
class SPOutDecoder {
public:
  SPOutDecoder();

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

  void setCallback(void *user_data,PacketCallback callback);
  void setRobotId(int id);
  
  void reset();

  void processInput(serial *port);

  void printText(uchar *start, int count);

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

  void sendData(const void *data, int size);

  // converts 8 bytes of 7 bit data to 7 bytes of 8 bit data
  static void decode7bits(uchar *data_8bit,uchar *data_7bit) {
    ulong leftover;
    int leftover_bits;
    int idx_8bit;

    leftover = 0UL;
    leftover_bits = 0;
    idx_8bit=0;
    for(int i=0; i<8; i++) {
      // copy in new data
      leftover |= (data_7bit[i] & 0x7F) << leftover_bits;
      leftover_bits += 7;

      if(leftover_bits >= 8) {
        data_8bit[idx_8bit++] = (uchar)(leftover & 0xFF);
        leftover = leftover >> 8;
        leftover_bits -= 8;
      }
    }
  }

  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
  int numChars;
  int rawPacketStart;
  uchar buf_8bit[8];
  uchar buf_7bit[8];
  RobotDataPacket packet;
  bool inPacket;
  bool readHeader;
  bool binary;
  int sourceLengthToRead;
  uchar *destData;
  int readPos;

  PacketCallback packetCallback;
  void *userData;
  int robotId;
};

#endif