/* 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 <stdio.h>
#include <string.h>

#include "BehaviorPacketDecoder.h"

BehNames::BehNames()
{
  memset(beh_names,0,sizeof(beh_names));
}

void BehNames::dump()
{
  for(int beh_id=0; beh_id<MaxBehaviors; beh_id++){
    if(beh_names[beh_id].name[0] == '\00')
      continue;

    printf("beh %02d %s\n",beh_id,beh_names[beh_id].name);
    for(int state_id=0; state_id<beh_names[beh_id].num_states; state_id++){
      StateName *state_name;
      state_name = &beh_names[beh_id].state_names[state_id];
      if(state_name->name[0] == '\00')
        continue;
      printf("  state %02d %s\n",state_id,state_name->name);
      for(int trans_id=0; trans_id<MaxTranses; trans_id++){
        TransName *trans_name;
        trans_name = &state_name->trans_names[trans_id];
        if(trans_name->name[0] == '\00')
          continue;
        printf("    trans %02d %s\n",trans_id,trans_name->name);
      }
    }
  }
}

static inline bool TryUseData(int *data_left, int len_to_use)
{
  if(len_to_use > *data_left)
    return false;
  *data_left -= len_to_use;
  return true;
}

#define TRY_USE_DATA(_data_left,_len_to_use) \
  {if(!TryUseData(_data_left,_len_to_use)) \
    return false;}

bool BehaviorNamesPacketDecoder::decode(uchar **bufp,int *data_left,BehNames::TransName *trans_names)
{
  TRY_USE_DATA(data_left,2*sizeof(uchar));

  int trans_id;
  trans_id = grab<uchar>(bufp);
  if(trans_id < 0 || trans_id > BehNames::MaxTranses)
    return false;
  
  int trans_name_len;
  trans_name_len = grab<uchar>(bufp);
  if(trans_name_len < 0 || trans_name_len > BehNames::MaxNameLen)
    return false;

  TRY_USE_DATA(data_left,trans_name_len);
  grabFromEncoding(bufp,(uchar *)trans_names[trans_id].name,trans_name_len);
  trans_names[trans_id].name[trans_name_len] = '\00';

  return true;
}

bool BehaviorNamesPacketDecoder::decode(uchar **bufp,int *data_left,BehNames::StateName *state_name)
{
  TRY_USE_DATA(data_left,2*sizeof(uchar));

  int state_name_len;
  state_name_len = grab<uchar>(bufp);
  if(state_name_len < 0 || state_name_len > BehNames::MaxNameLen)
    return false;
  if(state_name_len > 0){
    TRY_USE_DATA(data_left,state_name_len);

    grabFromEncoding(bufp,(uchar *)state_name->name,state_name_len);
    state_name->name[state_name_len] = '\00';
  }

  int num_transes;
  num_transes = grab<uchar>(bufp);
  if(num_transes < 0 || num_transes > BehNames::MaxTranses)
    return false;
  
  bool success = true;
  for(int trans_idx=0; trans_idx<num_transes; trans_idx++){
    success = decode(bufp,data_left,state_name->trans_names);
    if(!success)
      return false;
  }

  return true;
}

bool BehaviorNamesPacketDecoder::decode(uchar **bufp,int *data_left,BehNames *beh_names)
{
  TRY_USE_DATA(data_left,sizeof(uchar)+sizeof(uchar));

  int beh_id;
  beh_id = grab<uchar>(bufp);
  if(beh_id < 0 || beh_id >= BehNames::MaxBehaviors)
    return false;

  int beh_name_len;
  beh_name_len = grab<uchar>(bufp);
  if(beh_name_len < 0 || beh_name_len > BehNames::MaxNameLen)
    return false;
  if(beh_name_len > 0){
    TRY_USE_DATA(data_left,beh_name_len);

    grabFromEncoding(bufp,(uchar *)beh_names->beh_names[beh_id].name,beh_name_len);
    beh_names->beh_names[beh_id].name[beh_name_len] = '\00';
  }

  TRY_USE_DATA(data_left,sizeof(uchar));

  int num_states;
  num_states = grab<uchar>(bufp);
  if(num_states < 0 || num_states > BehNames::MaxStates)
    return false;
  if(beh_names->beh_names[beh_id].num_states < num_states)
    beh_names->beh_names[beh_id].num_states = num_states;

  bool success=true;
  for(int state_id=0; state_id<num_states; state_id++){
    success = decode(bufp,data_left,&beh_names->beh_names[beh_id].state_names[state_id]);
    if(!success)
      return false;
  }

  TRY_USE_DATA(data_left,sizeof(uchar));
  int num_sub_behs;
  num_sub_behs = grab<uchar>(bufp);
  if(num_sub_behs < 0 || num_sub_behs > 20)
    return false;

  for(int sub_beh_idx=0; sub_beh_idx<num_sub_behs; sub_beh_idx++){
    success = decode(bufp,data_left,beh_names);
    if(!success)
      return false;
  }

  return true;
}

bool BehaviorNamesPacketDecoder::decode(BehNames *beh_names,RobotDataPacket *packet)
{
  int packet_data_left;
  uchar *old_buf;
  uchar *buf;

  packet_data_left = packet->length;
  buf = packet->data;
  old_buf = buf;

  //printf("got packet\n");
  //dumpBinary(buf,packet_data_left);

  return decode(&buf,&packet_data_left,beh_names);
}

//#define SHOW_ALL

bool BehaviorTracePacketDecoder::decode(uchar **bufp,int *data_left,int indent,const BehNames *beh_names)
{
  TRY_USE_DATA(data_left,sizeof(uchar));
  int beh_id;
  beh_id = grab<uchar>(bufp);
  if(beh_id < 0 || beh_id >= BehNames::MaxBehaviors)
    return false;

  TRY_USE_DATA(data_left,sizeof(uchar));
  bool asleep;
  asleep = grab<uchar>(bufp) > 0;

  const char *beh_name_str;
  const struct BehNames::BehName *beh_name;
  beh_name = &beh_names->beh_names[beh_id];
#ifndef SHOW_ALL
  if(!asleep)
#endif
    {
      for(int i=0; i<indent; i++) printf(" ");
    }
#ifdef SHOW_ALL
  if(!asleep)
    printf("*");
#endif
#ifndef SHOW_ALL
  if(!asleep)
#endif
    {
      if(beh_name->name[0] == '\00')
        printf("beh%02d\n",beh_id);
      else
        printf("%s\n",beh_name->name);
    }

  TRY_USE_DATA(data_left,sizeof(uchar));
  int num_events;
  num_events = grab<uchar>(bufp);
  if(num_events < 0 || num_events > BehNames::MaxTranses)
    return false;

  TRY_USE_DATA(data_left,sizeof(uchar)*(2*num_events+1));
#ifndef SHOW_ALL
  if(!asleep)
#endif
    {
      for(int i=0; i<indent; i++) printf(" ");
    }
  for(int event_idx=0; event_idx<num_events; event_idx++){
    int state_id;
    int trans_id;

    state_id = grab<uchar>(bufp);
    if(state_id < 0 || state_id >= BehNames::MaxStates)
      return false;
    const struct BehNames::StateName *state_name;
    state_name = &beh_name->state_names[state_id];
    trans_id = grab<uchar>(bufp);
    if(trans_id < 0 || trans_id >= BehNames::MaxTranses)
      return false;
    const struct BehNames::TransName *trans_name;
    trans_name = &state_name->trans_names[trans_id];
#ifndef SHOW_ALL
    if(!asleep)
#endif
      {
        if(state_name->name[0]=='\00')
          printf("state%02d",state_id);
        else
          printf("%s",state_name->name);
        printf("::");
        if(trans_name->name[0]=='\00')
          printf("trans%02d",trans_id);
        else
          printf("%s",trans_name->name);
        printf(" -> ");
      }
  }
  int state_id;
  state_id = grab<uchar>(bufp);
  if(state_id < 0 || state_id >= BehNames::MaxStates)
    return false;
  const struct BehNames::StateName *state_name;
  state_name = &beh_name->state_names[state_id];
#ifndef SHOW_ALL
    if(!asleep)
#endif
      {
        if(state_name->name[0]=='\00')
          printf("state%02d",state_id);
        else
          printf("%s",state_name->name);
        printf("\n");
      }

  TRY_USE_DATA(data_left,sizeof(uchar));
  int num_sub_machines;
  num_sub_machines = grab<uchar>(bufp);
  for(int sub_idx=0; sub_idx<num_sub_machines; sub_idx++){
    if(!decode(bufp,data_left,indent+2,beh_names))
      return false;
  }

  return true;
}

bool BehaviorTracePacketDecoder::decode(RobotDataPacket *packet,const BehNames *beh_names)
{
  uchar *buf;
  int data_left;

  buf = packet->data;
  data_left = packet->length;

  //printf("behavior trace packet\n");
  //dumpBinary(buf,data_left);

  return decode(&buf,&data_left,0,beh_names);
}