/* 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_MotionFile_h
#define INCLUDED_MotionFile_h

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

#ifdef PLATFORM_APERIOS
#include <MCOOP.h>
#endif

#include "Motion.h"

namespace Motion{

struct MotionFileHeader{
  int magic;    // "MOTI"
  int version;  // 1...
  int motions;  // number of types of motions
  int frames;   // total number of position frames
};

struct MotionFileMotion{
  short id;
  short frame_offset;
  short num;
  short reserved;
};

class MotionFile{
  int num;
  MotionFileMotion *motion;
  BodyStateMotion *frame;

  BodyState start;
  int time,num_frames;
public:
  MotionFile();
  bool load(char *filename);
  bool play(int id);

  int getAngles();
  int areBusy();
};

}; // namespace Motion

#if 0

class MotionRecording {
  int jointMask;
  double *angles;

public:
  static const int NumJoints=18;

  class Iterator {
    int frameIdx;
    MotionRecording *recording;
  public:
    Iterator() {
      frameIdx=0;
      recording=NULL;
    }

    Iterator(int frame_idx, MotionRecording *recording_param) :
      frameIdx(frame_idx),
      recording(recording_param) {
    }

    bool operator==(const Iterator &x) const {
      return (frameIdx==x.frameIdx &&
              recording==x.recording);
    }

    bool operator!=(const Iterator &x) const {return !(*this==x);}

    Iterator &operator ++() {
      frameIdx++;
      return *this;
    }

    Iterator &operator ++(int) {
      Iterator *old=this;
      ++(*this);
      return *old;
    }

    const double *operator *() {
      return &recording->angles[frameIdx*NumJoints];
    }
  };

  friend class Iterator;

  typedef Iterator iterator;

  int numFrames;

  MotionRecording() {
    numFrames=jointMask=0; angles=NULL;
  }

  ~MotionRecording() {
#ifdef PLATFORM_APERIOS
    DeleteRegion(angles);
#else
    delete[] angles;
#endif
  }

  bool initialize(int joint_mask,int num_frames);

  bool load(char *filename);

  Iterator begin() {
    return Iterator(0,this);
  }

  Iterator end() {
    return Iterator(numFrames,this);
  }
};

class MotionPlayer {
  MotionRecording *recording;
  MotionRecording::iterator pos;
  bool atEnd;
public:
  MotionPlayer() :
    recording(NULL) {
  }

  ~MotionPlayer() {
    delete recording;
  }

  void reset() {
    pos=recording->begin();
    atEnd=false;
  }

  void init(MotionRecording *recording_param) {
    recording=recording_param;
    reset();
  }

  void getAngles(double *angles) {
    const double *record_angles;

    record_angles = *pos;

    for(int joint_idx=0; joint_idx<MotionRecording::NumJoints; joint_idx++) {
      angles[joint_idx] = record_angles[joint_idx];
    }

    MotionRecording::iterator new_pos=pos;
    ++new_pos;
    if(new_pos!=recording->end())
      pos=new_pos;
    else
      atEnd=true;
  }

  int areBusy() {
    if(atEnd)
      return 0;
    else
      return LEGS_BUSY | HEAD_BUSY; // FIXME: only set ones for joints driven
  }
};

#endif

#endif