/* 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_SPOutEncoder_h
#define INCLUDED_SPOutEncoder_h

#include <string.h>

struct RobotPositionInfo;

#include "DogTypes.h"

/*------------------------------------------------------------------
CLASS
  SPOutEncoder

DESCRIPTION
  Encodes messages being sent out to SPOut.
------------------------------------------------------------------*/
class SPOutEncoder {
public:
  //range 82 -> BF
  static const uchar VisionRLELead      =(uchar)'\xBF';
  static const uchar TextLead           =(uchar)'\xBE';
  static const uchar VisionObjLead      =(uchar)'\xBD';
  static const uchar LocalizationLead   =(uchar)'\xBC';
  static const uchar ModelObjLead       =(uchar)'\xBB';
  static const uchar MotionUpdateLead   =(uchar)'\xBA';
  static const uchar GSensorLead        =(uchar)'\xB9';
  static const uchar VisionRawLead      =(uchar)'\xB8';
  static const uchar VisionAvgColorLead =(uchar)'\xB7';
  static const uchar VisionColorAreaLead=(uchar)'\xB6';
  static const uchar VisionRadialMapLead=(uchar)'\xB5';
  static const uchar ModelRadialMapLead =(uchar)'\xB4';
  static const uchar TeamMsgMgrObjLead  =(uchar)'\xB3';
  static const uchar FootSensorLead     =(uchar)'\xB2';
  static const uchar BehaviorNamesLead  =(uchar)'\xB1';
  static const uchar BehaviorTraceLead  =(uchar)'\xB0';
  static const uchar DutyCycleLead      =(uchar)'\xAF';
  static const uchar VisionRLE2Lead     =(uchar)'\xAE';
  static const uchar VisionRaw2Lead     =(uchar)'\xAD';
  static const uchar TrackerLead        =(uchar)'\xAC';
  static const uchar WMDebugLead        =(uchar)'\xAB';

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

    leftover = 0UL;
    leftover_bits = 0;
    idx_8bit=0;
    for(int i=0; i<8; i++) {
      if(leftover_bits < 7) {
        leftover |= ((ulong)data_8bit[idx_8bit++] << leftover_bits);
        leftover_bits += 8;
      }
      // shift over needed data and mask out data we've shouldn't use yet
      data_7bit[i] = leftover & 0x7F;
      leftover = leftover >> 7;
      leftover_bits -= 7;
    }
  }

  static void addToEncoding(uchar **buf,const uchar *source,int length) {
    memcpy(*buf,source,length);
    (*buf) += length;
  }

  static void encodeAsFloat(uchar **buf,double value) {
    float tmp;
    tmp = value;
    addToEncoding(buf,(uchar *)&tmp,sizeof(float));
  }

  template<class T>
  static void encodeAs(uchar **buf,T value) {
    T tmp;
    tmp = value;
    addToEncoding(buf,(uchar *)&tmp,sizeof(T));
  }

  template<class LengthT>
  static void encodeString(uchar **buf,const char *str) {
    int length = strlen(str);
    encodeAs<LengthT>(buf,length);
    addToEncoding(buf,(const uchar *)str,length);
  }

  static uchar armorData(uchar data) {
    if(data=='\x0A')
      return (uchar)'\x81';
    else
      return data;
  }

protected:
  static uchar *encodeRaw  (uchar *buf, uchar val) {
    if(val==(uchar)'\xFF') {
      *(buf++)=val;
      *(buf++)='\x01';
      return buf;
    }
    else if(val=='\x0A') {
      *(buf++)=(uchar)'\xFF';
      *(buf++)=(uchar)'\x8F';
      return buf;
    }
    else {
      *buf=val;
      return ++buf;
    }
  }

  static uchar *encodeInt  (uchar *buf, int   val);

  static uchar *encodeShort(uchar *buf, short val) {
    static union {
      uchar bytes[sizeof(short)];
      short val;
    } transfer;

    transfer.val=val;
    for(int i=0; i<(int)(sizeof(short)); i++)
      buf=encodeRaw(buf,transfer.bytes[i]);
    return buf;
  }

  static uchar *encodeFloat(uchar *buf, float val) {
    static union {
      uchar bytes[sizeof(float)];
      float val;
    } transfer;

    transfer.val=val;
    for(int i=0; i<(int)(sizeof(float)); i++)
      buf=encodeRaw(buf,transfer.bytes[i]);
    return buf;
  }

  static uchar *encodeStop(uchar *buf) {
    *(buf++)=(uchar)'\xFF';
    *(buf++)='\x00';
    return buf;
  }
};

#endif