/* 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.
  ========================================================================= */

#include <iostream>
#include <stdarg.h>
#include <stdio.h>

using namespace std;

#include "../headers/system_config.h"

#include "../headers/CircBufPacket.h"
#include "../headers/SystemUtility.h"
#include "../headers/Util.h"

namespace PacketUtil {
  void *
  GetPtr(uchar *base, PacketOffset offset) {
    return reinterpret_cast<void *>(base + offset);
  }
  
  template<class T>
  PacketOffset
  GetOffset(uchar *base, T *ptr) {
    return (PacketOffset)((uchar *)ptr - base);
  }
}

void
PacketStream::init(uchar *data_start, uchar *data_end, int max_packets) {
  type = 0;
  binary = false;
  lastPacket = 0;
  firstValidPacket = 1;

  maxPackets = max_packets;
  
  dataStartOffset = getOffset(data_start);
  dataEndOffset   = getOffset(data_end);

  for(int packet_idx=0; packet_idx<maxPackets; packet_idx++) {
    packets[packet_idx].offset = dataStartOffset;
    packets[packet_idx].length = 0;
  }
}

bool
PacketStream::writeBinary(const uchar *data, int length, ulong timestamp) {
  //cout << "wb" << (void *)data << "l" << length << endl;

  // data packet too big for buffer
  if(length > dataEndOffset - dataStartOffset)
    return false;

  PacketOffset offset_write_loc;
  PacketOffset offset_write_end;

  offset_write_loc = 
    packets[packetNumToIdx(lastPacket)].offset + 
    packets[packetNumToIdx(lastPacket)].length;
  offset_write_end = offset_write_loc + length;
  if(offset_write_end > dataEndOffset) {
    //cout << "write past end case" << endl;
    // write would run past end of buffer so start over at beginning
    offset_write_loc = dataStartOffset;
    offset_write_end = offset_write_loc + length;
    // invalidate packets that we are about to overwrite data of
    // while packet valid and data will be clobbered, invalidate
    while(firstValidPacket <= lastPacket && 
          offset_write_end > packets[packetNumToIdx(firstValidPacket)].offset)
      firstValidPacket++;
  }
  else {
    //cout << "nc" << endl;
    while(firstValidPacket <= lastPacket && 
          (offset_write_loc < (int)(packets[packetNumToIdx(firstValidPacket)].offset + 
                                    packets[packetNumToIdx(firstValidPacket)].length) &&
           offset_write_end > packets[packetNumToIdx(firstValidPacket)].offset) )
      firstValidPacket++;
  }

  // invalidate packets that we are about to overwrite metadata of
  if(packetNumToIdx(firstValidPacket) == packetNumToIdx(lastPacket+1) &&
     firstValidPacket != lastPacket+1)
    firstValidPacket++;
  // copy data
  memcpy(getPtr(offset_write_loc),data,length);
  // publish data
  BareRobotDataPacket *packet;
  packet = &packets[packetNumToIdx(lastPacket+1)];
  packet->offset = offset_write_loc;
  packet->length = length;
  packet->timestamp = timestamp;
  lastPacket++;

  //cout << "fvp" << firstValidPacket << "lp" << lastPacket << endl;

  return true;
}

bool
PacketStream::writeBinary(const uchar *data, int length) {
  return writeBinary(data,length,GetTime());
}

bool
PacketStream::writeText(const char *msg, ulong timestamp) {
  return writeBinary((const uchar *)msg,strlen(msg),timestamp);
}

bool
PacketStream::writeText(const char *msg) {
  return writeBinary((const uchar *)msg,strlen(msg),GetTime());
}

int
pprintf(PacketStream *ps,const char *fmt, ...) {
  static uchar buf[2048];
  int ret;

  va_list al;
  
  va_start(al,fmt);
  ret = vsnprintf((char *)buf, 2048, fmt, al);
  va_end(al);

  if(ps!=NULL) {
    ps->writeText((char *)buf);
  }
  else {
    //printf("%s",(char *)buf);
    cout << (char *)buf;
  }

  return ret;
}

bool
PacketStream::readHeader(int packet_id, RobotDataPacket *packet) {
  if(packet_id < firstValidPacket ||
     packet_id > lastPacket) {
    //cout << "I" << endl;
    return false;
  }

  BareRobotDataPacket *bare_packet;

  bare_packet = &packets[packetNumToIdx(packet_id)];
  // check to make sure we have enough room
  int length;
  length = bare_packet->length; // put in var to make atomic
  packet->length    = length;
  packet->timestamp = bare_packet->timestamp;
  packet->dataType = type;
  packet->contentType = (binary ? RobotDataPacket::BINARY : RobotDataPacket::TEXT);

  bool ret_val;
  ret_val = (packet_id >= firstValidPacket);
  //if(!ret_val)
  //  cout << "O" << endl;
  return ret_val;
}

bool
PacketStream::read(int packet_id, RobotDataPacket *packet, int data_length) {
  if(packet_id < firstValidPacket ||
     packet_id > lastPacket) {
    //cout << "I" << endl;
    return false;
  }

  BareRobotDataPacket *bare_packet;

  bare_packet = &packets[packetNumToIdx(packet_id)];
  // check to make sure we have enough room
  int length;
  length = bare_packet->length; // put in var to make atomic
  if(data_length < length) {
    //cout << "L" << data_length << "oo" << length << endl;
    return false;
  }
  packet->length    = length;
  packet->timestamp = bare_packet->timestamp;
  memcpy(packet->data,getPtr(bare_packet->offset),length);
  packet->dataType = type;
  packet->contentType = (binary ? RobotDataPacket::BINARY : RobotDataPacket::TEXT);

  bool ret_val;
  ret_val = (packet_id >= firstValidPacket);
  //if(!ret_val)
  //  cout << "O" << endl;
  return ret_val;
}

bool
PacketStream::makePacketIdValidCatchup(int *packet_id) {
  if(*packet_id < firstValidPacket) {
    *packet_id = max(firstValidPacket,lastPacket);
  }
  
  return *packet_id <= lastPacket;
}

bool
PacketStream::makePacketIdValidFirst(int *packet_id) {
  if(*packet_id < firstValidPacket) {
    *packet_id = firstValidPacket;
  }
  
  return *packet_id <= lastPacket;
}

void
PacketStreamCollection::init(uchar *data_start,uchar *data_end) {
  numStreams = 0;

  dataStartOffset = getOffset(data_start);
  dataEndOffset   = getOffset(data_end  );

  freeStartOffset = dataStartOffset;
}

int
PacketStreamCollection::allocateStream(ulong size, int max_packets) {
  if(numStreams >= MAX_STREAMS) {
    cout << "OOS: Out of streams" << endl;
    return -1;
  }

  PacketStream *stream;
  PacketOffset stream_start_offset,stream_end_offset;
  int stream_id;

  stream_id = numStreams;

  stream = &streams[stream_id];
  stream_start_offset = freeStartOffset;
  stream_end_offset   = freeStartOffset+size;
  if(stream_end_offset > dataEndOffset) {
    stream_end_offset = dataEndOffset;
    cout << "RUNNING OUT OF ROOM FOR PACKET STREAMS" << endl;
  }
  stream->init(getPtr(stream_start_offset),getPtr(stream_end_offset),max_packets);
  
  freeStartOffset = stream_end_offset;
  numStreams++;
  
  return stream_id;
}

PacketStream *
PacketStreamCollection::getStream(int id) {
  if(0 <= id && id < numStreams) {
    return &streams[id];
  }
  else
    return NULL;
}

int
PacketStreamCollection::getNumStreams() {
  return numStreams;
}