/* 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_MotionObject_h
#define INCLUDED_MotionObject_h

#include <OPENR/OPENR.h>
#include <OPENR/OPENRAPI.h>
#include <OPENR/OObject.h>
#include <OPENR/OSubject.h>
#include <OPENR/OObserverVector.h>

class PacketStream;
class SPOutMotionEncoder;

#include "def.h"
//#define LIMPDOG

#include "../headers/AperiosSharedMem.h"
#include "WalkParam.h"
// #include "Motion.h"

// Robot Information
const unsigned NumPIDJoints  = 18;
const unsigned NumBinJoints  =  2;
const unsigned NumEars       =  2;
const unsigned NumSpeakers   =  1;
const unsigned NumWlanSensors=  1;

const unsigned NumLEDs_ERS210 =  9;
const unsigned NumLEDs_ERS7   =  26;
const unsigned NumBinLEDs_ERS7=  6;
const unsigned NumTriLEDs_ERS7=  20;

const unsigned NumFastOutputs_ERS210 = NumPIDJoints + NumLEDs_ERS210;
const unsigned NumFastOutputs_ERS7   = NumPIDJoints + NumLEDs_ERS7;
const unsigned NumJoints             = NumPIDJoints + NumBinJoints;
const unsigned NumOutputs_ERS210     = NumPIDJoints + NumBinJoints + NumLEDs_ERS210 + NumSpeakers;
const unsigned NumOutputs_ERS7       = NumPIDJoints + NumBinJoints + NumLEDs_ERS7 + NumSpeakers;


class MotionObject : public OObject {
public:
  OSubject        *subject[numOfSubject];
  OObserverVector *observer[numOfObserver];

  // Must Be Public
  MotionObject();
  ~MotionObject() {}

  static MotionObject &GetObject();

  // OPEN-R Method 
  virtual OStatus DoInit   ( const OSystemEvent& );
  virtual OStatus DoStart  ( const OSystemEvent& );
  virtual OStatus DoStop   ( const OSystemEvent& );
  virtual OStatus DoDestroy( const OSystemEvent& );

  // Inter Object Communication Method 
  void ReadyMoveJoint     (const OReadyEvent &event);
  void ReadyControlBC     (const OReadyEvent &event);
  void ReadyRequestRegion (const OReadyEvent &event);
  void ReadyMotionComplete(const OReadyEvent &event);
  void ReadyRegisterRegion(const OReadyEvent &event);
  OStatus ReadySpeaker    (const OReadyEvent &event);
  void ReadyVelParam      (const OReadyEvent &event);

  void UpdateControl      (const ONotifyEvent &event);
  void ResultMemRegion    (const ONotifyEvent &event);
  void GotSensorFrame     (const ONotifyEvent &event);
  void UpdateParams       (const ONotifyEvent &event);

  void ControlSpeaker     (const ONotifyEvent &info);

  void sendLocUpdateQueueBuffer();
  void sendOutputBuffer();
  void sendVelParam(Motion::VelParam &vp);

  void RequestRegions();

private:
  bool triggerVR, triggerBC;
  int  current; // what 8ms we are on
  int  last_update; // last 8ms frame localization motion report
  int  step;

  unsigned int NumOutputs;
  unsigned int NumFastOutputs;
  unsigned int NumLEDs;
  unsigned int NumBinLEDs;
  unsigned int NumTriLEDs;

  // int walksteps; // how many 8ms frames per full cycle
  // Motion motion;

  // Aperios Output buffers
  static const int NumBuffers = 2;
  //OPrimitiveID          primitiveID[NumOutputs];
  RCRegion             *cmdVecFastRgn[NumBuffers];
  RCRegion             *cmdVecSlowRgn[NumBuffers];
  MemoryRegionID        cmdVecFastID[NumBuffers];
  MemoryRegionID        cmdVecSlowID[NumBuffers];
  OCommandVectorData   *cmdVecFast[NumBuffers];
  OCommandVectorData   *cmdVecSlow[NumBuffers];
  RCRegion             *soundVecRgn[NumBuffers];
  OSoundVectorData     *soundVecData[NumBuffers];
  MemoryRegionID       soundMemID[NumBuffers];
  //OLEDCommandValue2    *led_cmd[NumLEDs];

  // Sound Stuff
  static const int SoundTableSize = 512;
  int soundCur;

  void SetUpBuffer();
  void SetJointGain(double v);
  void SetJointGain() {SetJointGain(1.0);}
  void SendJointData();
  void SendPowerOnParam();
  void SetupSound();

  // output data
  bool requestedRegion;
  SMMSharedMemRegion outputMemRgn;
  SMMSharedMemRegion motionLocUpdateMemRgn;

  // we create
  bool needToSendOutBuffer;
  bool needToSendLocBuffer;
  SMMSharedMemRegion outMemRgn;

  PacketStream *motionUpdateStream;
  SPOutMotionEncoder *encoder;
};

#endif // INCLUDED_MotionObject_h