/* 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 "../headers/CircBufPacket.h"
#include "../headers/PacketMux.h"

PacketMuxRoundRobin::PacketMuxRoundRobin() {
  streams.reserve(5);
  nextPackets.reserve(5);
  nextStream=0;
}

void
PacketMuxRoundRobin::addPSC(PacketStreamCollection *psc) {
  int num_streams=0;

  num_streams = psc->getNumStreams();
  for(int stream_id=0; stream_id<num_streams; stream_id++) {
    PacketStream *stream;
    stream = psc->getStream(stream_id);
    streams.push_back(stream);
    nextPackets.push_back(0);
  }
}

bool
PacketMuxRoundRobin::getNextPacket(RobotDataPacket *packet,int data_length) {
  if(streams.size() == 0U)
    return false;

  PacketStream *stream;
  int packet_id;
  uint start_stream;
  bool success;

  success=false;
  start_stream = nextStream;
  do {
    stream = streams[nextStream];
    packet_id = nextPackets[nextStream];
    if(stream->makePacketIdValidCatchup(&packet_id)) {
      success = stream->read(packet_id,packet,data_length);
      nextPackets[nextStream] = packet_id+1;
    }
    //else
    //  cout << "V" << endl;
    //if(success)
    //  cout << "fpos" << nextStream << "oo" << streams.size() << endl;
    nextStream = (nextStream + 1) % streams.size();
  } while(!success && nextStream != start_stream);

  //if(!success) {
  //  cout << "N" << endl;
  //  start_stream = nextStream;
  //  do {
  //    stream = streams[nextStream];
  //    packet_id = nextPackets[nextStream];
  //    cout << "s" << nextStream << "pi" << packet_id 
  //         << "fi" << stream->firstValidPacket << "lp" << stream->lastPacket << endl;
  //    nextStream = (nextStream + 1) % streams.size();
  //  } while(nextStream != start_stream);
  //}

  return success;
}

PacketMuxFIFO::PacketMuxFIFO() {
  streams.reserve(5);
  nextPackets.reserve(5);
}

void
PacketMuxFIFO::addPSC(PacketStreamCollection *psc) {
  int num_streams=0;

  num_streams = psc->getNumStreams();
  for(int stream_id=0; stream_id<num_streams; stream_id++) {
    PacketStream *stream;
    stream = psc->getStream(stream_id);
    streams.push_back(stream);
    nextPackets.push_back(0);
  }
}

bool
PacketMuxFIFO::getNextPacket(RobotDataPacket *packet,int data_length) {
  if(streams.size() == 0U)
    return false;

  PacketStream *stream;
  int packet_id;
  bool success;
  ulong min_time;
  ulong min_stream_idx;
  static const ulong MinStreamNone=~0UL;

  success=false;
  do {
    // find stream with lowest timestamp
    min_time=~0UL;
    min_stream_idx=MinStreamNone;
    for(uint stream_id=0; stream_id<streams.size(); stream_id++) {
      stream = streams[stream_id];
      packet_id = nextPackets[stream_id];
      if(stream->makePacketIdValidFirst(&packet_id)) {
        if(packet_id != nextPackets[stream_id])
          nextPackets[stream_id] = packet_id;

        bool read_ok;
        read_ok = stream->readHeader(packet_id,packet);
        if(read_ok) {
          if(packet->timestamp < min_time) {
            min_time = packet->timestamp;
            min_stream_idx = stream_id;
          }
        }
      }
    }

    if(min_stream_idx!=MinStreamNone) {
      // try to read from stream
      stream = streams[min_stream_idx];
      packet_id = nextPackets[min_stream_idx];
      success = stream->read(packet_id,packet,data_length);
      nextPackets[min_stream_idx] = packet_id+1;
    }
  } while(!success && min_stream_idx!=MinStreamNone);

  return success;
}