/* 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/FileSystem.h"

#include "MotionFile.h"

#if 0

bool
MotionRecording::initialize(int joint_mask, int num_frames)
{
  jointMask=joint_mask;
  numFrames=num_frames;
  if(angles)
#ifdef PLATFORM_APERIOS
    DeleteRegion(angles);
#else
    delete[] angles;
#endif

#ifdef PLATFORM_APERIOS
  int size=sizeof(double)*numFrames*NumJoints; sError result = NewRegion(size, reinterpret_cast<void**>(&angles)); 
  if (result != sSUCCESS) {angles=NULL; pprintf(TextOutputStream,"Unable to allocate motion frame buffer of size %d\n",size);}
#else
  angles = new double[numFrames * NumJoints];
#endif
  if(!angles) {
    jointMask=0;
    numFrames=0;
    return(false);
  }

  return(true);
}

bool
MotionRecording::load(char *filename)
{
  MotionFileHeader mfh;
  HFS::FILE *in;
  float channel_angles[NumJoints];

  //pprintf(TextOutputStream,"Trying to open file '%s'\n",filename);
  in = HFS::fopen(filename,"rb");
  if(!in) return(false);

  HFS::fread(&mfh,sizeof(mfh),1,in);
  jointMask = 0;
  for(int joint_idx=0; joint_idx<NumJoints; joint_idx++){
    jointMask |= (mfh.channel[joint_idx]!=MotionFileHeader::NullChannel) << joint_idx;
  }

  if(!initialize(jointMask,mfh.numFrames)){
    pprintf(TextOutputStream,"test motion recording initialization failed\n");
    HFS::fclose(in);
    return(false);
  }

  // read angles
  for(int frame_idx=0; frame_idx<numFrames; frame_idx++){
    HFS::fread(channel_angles,sizeof(float),mfh.numChannels,in);
    for(int joint_idx=0; joint_idx<NumJoints; joint_idx++){
      int channel;

      channel = mfh.channel[joint_idx];
      angles[frame_idx*NumJoints + joint_idx] = (channel != MotionFileHeader::NullChannel) ? channel_angles[channel] : 0.0;
    }
  }

  HFS::fclose(in);
  return(true);
}

#endif