/* 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 OPENR_STUBGEN
#define OPENR_STUBGEN
#endif

#include <iostream>
#include <math.h>

#include <OPENR/OPENR.h>
#include <OPENR/OPENRMessages.h>
#include <OPENR/core_macro.h>

#include <OPENR/OUnits.h>

#include "../headers/AperiosMessageStructures.h"
#include "../headers/CircBufPacket.h"
#include "../headers/Config.h"
#include "../headers/DogTypes.h"
#include "../headers/Reporting.h"
#include "../headers/Sensors.h"
#include "../headers/SharedMem.h"
#include "../headers/SPOutEncoder.h"
#include "../headers/Utility.h"
#include "../headers/Util.h"
#include "../Vision/VisionInterface.h"

#include "def.h"

#include "MotionInterface.h"
#include "WalkParam.h"
#include "Motion.h"
#include "SPOutMotionEncoder.h"
#include "globals.h"

#include "MotionObject.h"

Motion::MotionLocalizationUpdateQueue *LocUpdateQueueBuffer = NULL;
PacketStream *TextOutputStream = NULL;

static const int MotionOutputDataSize = 64*1024; // 64KB
static const int TotalTextOutput = 32*1024; // 32KB
static const int MaxTextMessages = 256; // maximum number of in flight text messages

static const int MotionUpdateSize = 2*2+8*4+1;
static const int MaxMotionUpdateMessages = 20;
static const int TotalMotionUpdate       = MotionUpdateSize * MaxMotionUpdateMessages;

static const bool DriveEars=false; // currently not double buffering correctly

static const unsigned int MaxLocQueueSize=10U;
static const unsigned int FrameTime=8U; // (ms)

static const unsigned int SpeakerSamplingRate = 8000;

//#define LIMPDOG
//#define ZERODOG

extern MotionObject Self;

OPrimitiveID *primitiveID;

char* model="ERS210";

Motion::Motion motion;
SensorData sensor;

const char* const PrimitiveName_ERS210 [NumOutputs_ERS210] = {
  "PRM:/r2/c1-Joint2:j1",       // the left front leg   the joint 
  "PRM:/r2/c1/c2-Joint2:j2",    // the left front leg   the shoulder 
  "PRM:/r2/c1/c2/c3-Joint2:j3", // the left front leg   the knee 
  "PRM:/r4/c1-Joint2:j1",       // the right front leg   the joint 
  "PRM:/r4/c1/c2-Joint2:j2",    // the right front leg    the shoulder 
  "PRM:/r4/c1/c2/c3-Joint2:j3", // the right front leg   the knee 

  "PRM:/r3/c1-Joint2:j1",       // the left hind leg   the joint 
  "PRM:/r3/c1/c2-Joint2:j2",    // the left hind leg   the shoulder 
  "PRM:/r3/c1/c2/c3-Joint2:j3", // the left hind leg   the knee
  "PRM:/r5/c1-Joint2:j1",       // the right hind leg   the joint 
  "PRM:/r5/c1/c2-Joint2:j2",    // the right hind leg   the shoulder 
  "PRM:/r5/c1/c2/c3-Joint2:j3", // the right hind leg   the knee 

  "PRM:/r1/c1-Joint2:j1",       // the neck  tilt (12)
  "PRM:/r1/c1/c2-Joint2:j2",    // the neck   pan 
  "PRM:/r1/c1/c2/c3-Joint2:j3", // the neck   roll 

  "PRM:/r6/c1-Joint2:j1",       // the tail pan (15)
  "PRM:/r6/c2-Joint2:j2",       // the tail tilt

  "PRM:/r1/c1/c2/c3/c4-Joint2:j4", // the mouth (17)

  "PRM:/r1/c1/c2/c3/l1-LED2:l1", // lower  left  LED (18)
  "PRM:/r1/c1/c2/c3/l4-LED2:l4", // lower  right LED
  "PRM:/r1/c1/c2/c3/l2-LED2:l2", // middle left  LED
  "PRM:/r1/c1/c2/c3/l5-LED2:l5", // middle right LED
  "PRM:/r1/c1/c2/c3/l3-LED2:l3", // upper  left  LED
  "PRM:/r1/c1/c2/c3/l6-LED2:l6", // upper  right LED
  "PRM:/r1/c1/c2/c3/l7-LED2:l7", // top          LED

  "PRM:/r6/l2-LED2:l2", // tail red  LED
  "PRM:/r6/l1-LED2:l1", // tail blue LED

  "PRM:/r1/c1/c2/c3/e1-Joint3:j5", // left ear (27)
  "PRM:/r1/c1/c2/c3/e2-Joint3:j6", // right ear
  "PRM:/r1/c1/c2/c3/s1-Speaker:S1" // speaker
};


const char* const PrimitiveName_ERS7 [NumOutputs_ERS7] = {
  "PRM:/r2/c1-Joint2:21",       // the left front leg   the joint 
  "PRM:/r2/c1/c2-Joint2:22",    // the left front leg   the shoulder 
  "PRM:/r2/c1/c2/c3-Joint2:23", // the left front leg   the knee 
  "PRM:/r4/c1-Joint2:41",       // the right front leg   the joint 
  "PRM:/r4/c1/c2-Joint2:42",    // the right front leg    the shoulder 
  "PRM:/r4/c1/c2/c3-Joint2:43", // the right front leg   the knee 

  "PRM:/r3/c1-Joint2:31",       // the left hind leg   the joint 
  "PRM:/r3/c1/c2-Joint2:32",    // the left hind leg   the shoulder 
  "PRM:/r3/c1/c2/c3-Joint2:33", // the left hind leg   the knee
  "PRM:/r5/c1-Joint2:51",       // the right hind leg   the joint 
  "PRM:/r5/c1/c2-Joint2:52",    // the right hind leg   the shoulder 
  "PRM:/r5/c1/c2/c3-Joint2:53", // the right hind leg   the knee 

  "PRM:/r1/c1-Joint2:11",       // the neck  tilt (12)
  "PRM:/r1/c1/c2-Joint2:12",    // the neck   pan
  "PRM:/r1/c1/c2/c3-Joint2:13", // the neck   tilt2

  "PRM:/r6/c2-Joint2:62",       // the tail pan (15)
  "PRM:/r6/c1-Joint2:61",       // the tail tilt

  "PRM:/r1/c1/c2/c3/c4-Joint2:14", // the mouth (17)

  "PRM:/r1/c1/c2/c3/l1-LED2:l1",   // Head light(color) - orange half circle
  "PRM:/r1/c1/c2/c3/l2-LED2:l2",   // Head light(white) - white half circle  
  "PRM:/r1/c1/c2/c3/l3-LED2:l3",   // Mode Indicator(red) - ear light orange           
  "PRM:/r1/c1/c2/c3/l4-LED2:l4",   // Mode Indicator(green) - ear light green
  "PRM:/r1/c1/c2/c3/l5-LED2:l5",   // Mode Indicator(blue) - ear light blue
  "PRM:/r1/c1/c2/c3/l6-LED2:l6",   // Wireless light - small rectangular light, blue               
  "PRM:/r1/c1/c2/c3/la-LED3:la",   // Face light1                     
  "PRM:/r1/c1/c2/c3/lb-LED3:lb",   // Face light2                     
  "PRM:/r1/c1/c2/c3/lc-LED3:lc",   // Face light3                     
  "PRM:/r1/c1/c2/c3/ld-LED3:ld",   // Face light4                     
  "PRM:/r1/c1/c2/c3/le-LED3:le",   // Face light5                     
  "PRM:/r1/c1/c2/c3/lf-LED3:lf",   // Face light6                     
  "PRM:/r1/c1/c2/c3/lg-LED3:lg",   // Face light7                     
  "PRM:/r1/c1/c2/c3/lh-LED3:lh",   // Face light8                     
  "PRM:/r1/c1/c2/c3/li-LED3:li",   // Face light9                     
  "PRM:/r1/c1/c2/c3/lj-LED3:lj",   // Face light10                    
  "PRM:/r1/c1/c2/c3/lk-LED3:lk",   // Face light11                    
  "PRM:/r1/c1/c2/c3/ll-LED3:ll",   // Face light12                    
  "PRM:/r1/c1/c2/c3/lm-LED3:lm",   // Face light13        
  "PRM:/r1/c1/c2/c3/ln-LED3:ln",   // Face light14        
   
  "PRM:/lu-LED3:lu",               // Back light(front, color)        
  "PRM:/lv-LED3:lv",               // Back light(front, white)        
  "PRM:/lw-LED3:lw",               // Back light(middle, color)       
  "PRM:/lx-LED3:lx",               // Back light(middle, white)       
  "PRM:/ly-LED3:ly",               // Back light(rear, color)         
  "PRM:/lz-LED3:lz",               // Back light(rear, white)  

  "PRM:/r1/c1/c2/c3/e5-Joint4:15", // left ear (27)
  "PRM:/r1/c1/c2/c3/e6-Joint4:16", // right ear
  "PRM:/s1-Speaker:S1"             // speaker
};

char const * const *PrimitiveName;

const unsigned LEDOffset = NumPIDJoints;
unsigned EarOffset;
unsigned SpeakerOffset;

/*
> Pid: pgain, igain, dgain, pshift, ishift, dshift
> 
> PGAIN: Proportional gain (16bit depth)
> IGAIN: Integral gain (16bit depth)
> DGAIN: Differential gain (16bit depth)
>  
> PSHIFT: Proportional gain shift amount  (4bit)
> ISHIFT: Proportional gain shift amount  (4bit)
> DSHIFT: Proportional gain shift amount  (4bit)
>  
> Operation :
>  
> P, I, D, value, respectively
> Multiple the gain G to the error between the desired value Vd and current
> value Vc,
>      F = (Vd - Vc ) * G
> and shift (0x10 - SHIFT) bit right side.
>      F >> SHIFT
> Namely, SHIFT=0x0E  (0x10 - 0x0E) = 2 bit shift
> Assume Vd = 50, Vc= 20, Gain = 0x16=22, SHIFT=0xE,
> The operation will be
>      (50 - 20) * 22 = 660 =1010010100
>      1010010100 >> 2 = 10100101 =  165
*/

struct PidGain {
  word pgain,igain,dgain;
  word pshift,ishift,dshift;
};

const PidGain Pid_ERS210[NumPIDJoints] =
  {
    // Legs
    { 0x16, 0x04, 0x08, 0x0E, 0x02, 0x0F },
    { 0x14, 0x04, 0x06, 0x0E, 0x02, 0x0F },
    { 0x23, 0x04, 0x05, 0x0E, 0x02, 0x0F },
    
    { 0x16, 0x04, 0x08, 0x0E, 0x02, 0x0F },
    { 0x14, 0x04, 0x06, 0x0E, 0x02, 0x0F },
    { 0x23, 0x04, 0x05, 0x0E, 0x02, 0x0F },
    
    { 0x16, 0x04, 0x08, 0x0E, 0x02, 0x0F },
    { 0x14, 0x04, 0x06, 0x0E, 0x02, 0x0F },
    { 0x23, 0x04, 0x05, 0x0E, 0x02, 0x0F },
    
    { 0x16, 0x04, 0x08, 0x0E, 0x02, 0x0F },
    { 0x14, 0x04, 0x06, 0x0E, 0x02, 0x0F },
    { 0x23, 0x04, 0x05, 0x0E, 0x02, 0x0F },
    
    // Head
    { 0x0A, 0x08, 0x0C, 0x0E, 0x02, 0x0F },
    { 0x0D, 0x08, 0x0B, 0x0E, 0x02, 0x0F },
    { 0x0C, 0x08, 0x0C, 0x0E, 0x02, 0x0F },
    
    // Tail
    { 0x0A, 0x00, 0x18, 0x0E, 0x02, 0x0F },
    { 0x07, 0x00, 0x11, 0x0E, 0x02, 0x0F },
    
    // Mouth
    { 0x0E, 0x08, 0x10, 0x0E, 0x02, 0x0F },
    
    //  { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
    //  { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
};

const PidGain Pid_ERS7[NumPIDJoints] =
  {
    // Legs
    { 0x1C, 0x08, 0x01, 0x0E, 0x02, 0x0F }, // Left fore legJ1     PRM:/r2/c1-Joint2:21         
    { 0x14, 0x04, 0x01, 0x0E, 0x02, 0x0F }, // Left fore legJ2     PRM:/r2/c1/c2-Joint2:22      
    { 0x1C, 0x08, 0x01, 0x0E, 0x02, 0x0F }, // Left fore legJ3     PRM:/r2/c1/c2/c3-Joint2:23   
    { 0x1C, 0x08, 0x01, 0x0E, 0x02, 0x0F }, // Right fore legJ1    PRM:/r4/c1-Joint2:41         
    { 0x14, 0x04, 0x01, 0x0E, 0x02, 0x0F }, // Right fore legJ2    PRM:/r4/c1/c2-Joint2:42      
    { 0x1C, 0x08, 0x01, 0x0E, 0x02, 0x0F }, // Right fore legJ3    PRM:/r4/c1/c2/c3-Joint2:43   
    { 0x1C, 0x08, 0x01, 0x0E, 0x02, 0x0F }, // Left hind legJ1     PRM:/r3/c1-Joint2:31         
    { 0x14, 0x04, 0x01, 0x0E, 0x02, 0x0F }, // Left hind legJ2     PRM:/r3/c1/c2-Joint2:32      
    { 0x1C, 0x08, 0x01, 0x0E, 0x02, 0x0F }, // Left hind legJ3     PRM:/r3/c1/c2/c3-Joint2:33   
    { 0x1C, 0x08, 0x01, 0x0E, 0x02, 0x0F }, // Right hind legJ1    PRM:/r5/c1-Joint2:51         
    { 0x14, 0x04, 0x01, 0x0E, 0x02, 0x0F }, // Right hind legJ2    PRM:/r5/c1/c2-Joint2:52      
    { 0x1C, 0x08, 0x01, 0x0E, 0x02, 0x0F }, // Right hind legJ3    PRM:/r5/c1/c2/c3-Joint2:53   

    // Head
    { 0x0A, 0x04, 0x02, 0x0E, 0x02, 0x0F }, // Neck tilt1          PRM:/r1/c1-Joint2:11         
    { 0x0C, 0x02, 0x04, 0x0E, 0x02, 0x0F }, // Neck pan            PRM:/r1/c1/c2-Joint2:12      
    { 0x0F, 0x04, 0x02, 0x0E, 0x02, 0x0F }, // Neck tilt2          PRM:/r1/c1/c2/c3-Joint2:13   

    // Tail
    { 0x0A, 0x04, 0x04, 0x0E, 0x02, 0x0F }, // Tail tilt           PRM:/r6/c1-Joint2:61         
    { 0x0A, 0x04, 0x04, 0x0E, 0x02, 0x0F }, // Tail pan            PRM:/r6/c2-Joint2:62         

    // Mouth
    { 0x08, 0x00, 0x04, 0x0E, 0x02, 0x0F }, // Mouth               PRM:/r1/c1/c2/c3/c4-Joint2:14
  };

const PidGain *Pid=NULL;

/*
const int LocStoppedReportPeriod = 250; // (ms)
MotionLocalizationUpdate NoMoveDelta = {
  0.0, 0.0, 0.0,
  false, false
};
*/

static const int NumFrames=4;
// ocommandMAX_FRAMES;


MotionObject::MotionObject() :
  triggerVR(false)
{
  NewComData;
  requestedRegion = false;
  needToSendOutBuffer=false;
  needToSendLocBuffer=false;

  motionUpdateStream = NULL;
  encoder = new SPOutMotionEncoder;

   char design[256];
  OPENR::GetRobotDesign(design);
  if(strcmp(design,"ERS-210")==0){

    PrimitiveName = PrimitiveName_ERS210;
    Pid = Pid_ERS210;
    Motion::kin = Kinematics(Kinematics::ERS210);
    Motion::camera_horz_fov = VisionInterface::ers210_HorzFOV;
    Motion::camera_vert_fov = VisionInterface::ers210_VertFOV;
    model = "ERS210";

    NumOutputs = NumOutputs_ERS210;
    NumFastOutputs = NumFastOutputs_ERS210;
    NumLEDs = NumLEDs_ERS210;
    NumBinLEDs = NumLEDs_ERS210;
    NumTriLEDs = 0; 
      
  }else{

    PrimitiveName = PrimitiveName_ERS7;
    Pid = Pid_ERS7;
    Motion::kin = Kinematics(Kinematics::ERS7);
    Motion::camera_horz_fov = VisionInterface::ers7_HorzFOV;
    Motion::camera_vert_fov = VisionInterface::ers7_VertFOV;
    model = "ERS7";

    NumOutputs = NumOutputs_ERS7;
    NumFastOutputs = NumFastOutputs_ERS7;
    NumLEDs = NumLEDs_ERS7;
    NumBinLEDs = NumBinLEDs_ERS7;
    NumTriLEDs = NumTriLEDs_ERS7;

  }
  
  primitiveID = new OPrimitiveID[NumOutputs];

  EarOffset = NumPIDJoints + NumLEDs;
  SpeakerOffset = NumPIDJoints + NumLEDs + NumEars;

  motion.init();
  motion.setSpeakerSamplingRate(SpeakerSamplingRate);

  memset(&sensor,0,sizeof(sensor));
}

OStatus MotionObject::DoInit(const OSystemEvent& info)
{
  // cout << "MotionObject::DoInit() is invoked." << endl;

  config.init();
  config.config("/MS/config/config.cfg");

  SensorData::InitializeSensors();

  SetComData;

  SetupSound();

  // make sure the library doesn't drop data "for" us
  //   on this reliable communication channel
  observer[obsSharedMemRegionInfo]->SetBufCtrlParam(0,1,16);

  motion.init(NULL);

  return oSUCCESS;
}


MotionObject &
MotionObject::GetObject() {
  return Self;
}

OStatus MotionObject::DoStart(const OSystemEvent& info)
{
  // cout << "MotionObject::DoStart() is invoked." << endl;

  // our setup
  void *base_addr;

  // allocate the localiation update queue memory and allow attachement
  base_addr=NULL;
  motionLocUpdateMemRgn.init(sizeof(Motion::MotionLocalizationUpdateQueue));
  base_addr = motionLocUpdateMemRgn.getData();

  LocUpdateQueueBuffer=(Motion::MotionLocalizationUpdateQueue *)base_addr;
  for(int i=0; i<Motion::MotionLocalizationUpdateQueueBufferSize; i++) {
    LocUpdateQueueBuffer->buffer[i].timestamp=0UL;
  }

  needToSendLocBuffer=true;
  sendLocUpdateQueueBuffer();

  // allocate the memory for spout style output
  base_addr=NULL;
  outMemRgn.init(sizeof(PacketStreamCollection)+MotionOutputDataSize);
  base_addr = outMemRgn.getData();

  PacketStreamCollection *out_psc;
  uchar *start_address;

  out_psc=(PacketStreamCollection *)base_addr;
  start_address = ((uchar *)base_addr) + sizeof(PacketStreamCollection); 
  out_psc->init(start_address, start_address + MotionOutputDataSize);

  int text_output_stream_id;
  text_output_stream_id = out_psc->allocateStream(TotalTextOutput,MaxTextMessages);
  TextOutputStream = out_psc->getStream(text_output_stream_id);
  TextOutputStream->type   = SPOutEncoder::TextLead;
  TextOutputStream->binary = false;

  int motion_update_stream_id;
  motion_update_stream_id = out_psc->allocateStream(TotalMotionUpdate,MaxMotionUpdateMessages);
  motionUpdateStream = out_psc->getStream(motion_update_stream_id);
  motionUpdateStream->type   = SPOutEncoder::MotionUpdateLead;
  motionUpdateStream->binary = true;

  needToSendOutBuffer=true;

  sendOutputBuffer();

  // Aperios Setup
  current = 0;
  step    = NumFrames;
  SetUpBuffer();

  OPENR::SetMotorPower(opowerON);

  triggerVR = true;

  SetJointGain(0.0);
  OPENR::SetDefaultJointGain(oprimitiveID_UNDEF);
  OPENR::EnableJointGain(oprimitiveID_UNDEF);
  SetJointGain(0.0);
  SendJointData();

  for(int i=0; i<numOfObserver; ++i){
    if(observer[i]->AssertReady() != oSUCCESS){
      cout << "\tMOARF{" << i << "}" << endl; // motion object assert ready failed
    }
  }

  RequestRegions();

  return(oSUCCESS);
}

OStatus MotionObject::DoStop(const OSystemEvent& info)
{
  // cout << "MotionObject::DoStop() is invoked." << endl;

  triggerVR = false;

  for (int i=0; i<numOfObserver; ++i){
    observer[i]->DeassertReady();
  }

  return(oSUCCESS);
}

OStatus MotionObject::DoDestroy(const OSystemEvent& info)
{
  // cout << "MotionObject::DoDestroy() is invoked." << endl;

  DeleteComData;
  //delete primitiveID;

  return(oSUCCESS);
}

void MotionObject::SetUpBuffer()
{
  int buf_id;
  uint prim_id;
  OCommandInfo *info;

  // Initialize a joint pass for data to be sent to OVRComm
  // For the definition of Joint,  refer to MotionObject.h.    
  for(uint i=0; i<NumOutputs; i++) {
    OPENR::OpenPrimitive((char*)PrimitiveName[i],&primitiveID[i]);
  }

  // Set that each data is used for a Joint control
  for(buf_id=0; buf_id<NumBuffers; buf_id++) {
    OPENR::NewCommandVectorData(NumFastOutputs,&cmdVecFastID[buf_id],&cmdVecFast[buf_id]);
    cmdVecFast[buf_id]->SetNumData(NumFastOutputs);
    cmdVecFastRgn[buf_id]=new RCRegion(cmdVecFast[buf_id]->vectorInfo.memRegionID,
                                       cmdVecFast[buf_id]->vectorInfo.offset,
                                       (void *)cmdVecFast[buf_id],
                                       cmdVecFast[buf_id]->vectorInfo.totalSize);

    OPENR::NewCommandVectorData(NumBinJoints,&cmdVecSlowID[buf_id],&cmdVecSlow[buf_id]);
    cmdVecSlow[buf_id]->SetNumData(NumBinJoints);
    cmdVecSlowRgn[buf_id]=new RCRegion(cmdVecSlow[buf_id]->vectorInfo.memRegionID,
                                       cmdVecSlow[buf_id]->vectorInfo.offset,
                                       (void *)cmdVecSlow[buf_id],
                                       cmdVecSlow[buf_id]->vectorInfo.totalSize);

    /*
    OPENR::NewCommandVectorData(NumOutputs,&cmdVecSlowID[buf_id],&cmdVecSlow[buf_id]);
    cmdVecSlow[buf_id]->SetNumData(NumOutputs);
    cmdVecSlowRgn[buf_id]=new RCRegion(reinterpret_cast<SharedMemoryHeader *>(cmdVecSlow[buf_id]));
    */

    for(prim_id=0; prim_id<NumPIDJoints; prim_id++) {
      info = cmdVecFast[buf_id]->GetInfo(prim_id);
      info->Set(odataJOINT_COMMAND2,primitiveID[prim_id],NumFrames);
      /*
      info = cmdVecSlow[buf_id]->GetInfo(prim_id);
      info->Set(odataJOINT_COMMAND2,primitiveID[prim_id],NumFrames);
      */
    }

    for(prim_id=LEDOffset; prim_id<LEDOffset+NumBinLEDs; prim_id++) {
      info = cmdVecFast[buf_id]->GetInfo(prim_id);
      info->Set(odataLED_COMMAND2,primitiveID[prim_id],NumFrames);
    }
    for(prim_id=LEDOffset+NumBinLEDs; prim_id<LEDOffset+NumTriLEDs+NumBinLEDs; prim_id++) {
      info = cmdVecFast[buf_id]->GetInfo(prim_id);
      info->Set(odataLED_COMMAND3,primitiveID[prim_id],NumFrames);
    }

    for(prim_id=EarOffset; prim_id<EarOffset+NumBinJoints; prim_id++) {
      info = cmdVecSlow[buf_id]->GetInfo(prim_id);
      info->Set(odataJOINT_COMMAND3,primitiveID[prim_id],NumFrames);
    }
  }
}


void MotionObject::ReadyVelParam(const OReadyEvent &event) {
}

void MotionObject::ReadyMoveJoint(const OReadyEvent& event)
{
  // MotionLocalizationUpdate step;

  /* MOVE PRINTOUTS */
  //static EventRateReporter rmj_event("ReadyMoveJoint",1000000UL,25,&TextOutputStream);
  //rmj_event.addToCount(true);

  static EventTimeReporter reporter("MotionObject::ReadyMoveJoint",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  //cout << "j" << endl;

  /*
  // FIXME: Remove this part
  MotionLocomotionCommand command;
  command.walk_type = MOTION_WALK;
  command.dx=120.0;
  command.dy=  0.0;
  command.da=  0.0;
  command.style=0;
  motion.update(command,current*8);
  // FIXME: end bogus stuff
  */

  static unsigned int cnt=0;
  static unsigned int cnt_trigger=0;

  if(triggerVR==true){
    // send joint gains every half second for the first 5 seconds
    if(cnt<4000 && cnt>=cnt_trigger){
      cnt_trigger += 300;

#ifndef LIMPDOG
      SetJointGain((double)cnt / 3000.0);
#endif
#ifdef LIMPDOG
      SetJointGain(0.0);
#endif

    }


#ifdef LIMPDOG
    SetJointGain(0.0);
#endif
    SendJointData();

    cnt+=FrameTime*NumFrames;
  }

  // Localization report
  //if(motion.getDelta(step)){
  //  addMotionComplete(step);
  //  last_update = current;
  //}else if(motion.isStopped() &&
  //	     (current - last_update)*8 > LocStoppedReportPeriod){
  //  step = NoMoveDelta;
  //  addMotionComplete(step);
  //  last_update = current;
  //}
}

void MotionObject::SetJointGain(double v)
{
  v = bound(v,0.0,1.0);

  for(uint i=0; i<NumPIDJoints; i++){
    OPENR::SetJointGain(primitiveID[i],
                        (word)(v*Pid[i].pgain),
                        (word)(v*Pid[i].igain),
                        (word)(v*Pid[i].dgain),
                        Pid[i].pshift, Pid[i].ishift, Pid[i].dshift);
  }
}

void MotionObject::SendJointData()
{
  static EventTimeReporter reporter("MotionObject::SendJointData",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);


  double angles[NumJoints];
  Motion::LEDState *LED_state;

  static int num_times_not_ready=0;

  int time;
  int frame,buf_id;

  // Check if VRComm is ready or not.
  bool isready = subject[sbjMoveJoint]->IsReady();
  if(!isready){
    // cout << "VRComm not ready " << num_times_not_ready << endl;
    num_times_not_ready++;
    return;
  }

  //static unsigned int debug_cnt=0;
  //debug_cnt++;
  //bool debug_prints=true;//((debug_cnt % 13)==0);

  bool fast_used[NumBuffers];
  bool slow_used[NumBuffers];
  int num_used;

  for(buf_id=0; buf_id<NumBuffers; buf_id++) {
    fast_used[buf_id] = (cmdVecFastRgn[buf_id]->NumberOfReference() != 1);
    slow_used[buf_id] = (cmdVecSlowRgn[buf_id]->NumberOfReference() != 1);
  }

  num_used=0;
  for(buf_id=0; buf_id<NumBuffers; buf_id++) {
    num_used += (fast_used[buf_id] ? 1 : 0);
    num_used += (slow_used[buf_id] ? 1 : 0);
  }

  //if(debug_prints) pprintf(TextOutputStream,"nu %d sa %d sb %d fa %d fb %d\n",num_used,slow_used[0],slow_used[1],fast_used[0],fast_used[1]);

  static const int target_used=2;
  static void *output_cmd[NumOutputs_ERS7];

  // Create data for 16 frames and notify it to OVirtualRobotComm in a double buffer.   
  // Notify Return Status after sending the frame.  
  while(num_used < target_used) {
    static int cnt=0;
    bool slow;
    
    cnt = (cnt + 1) % 32;
    if(DriveEars)
      slow = (cnt==0);
    else
      slow = false;

    RCRegion *cmd_vec_rgn;
    OCommandVectorData *cmd_vec_p;
    if(slow) {
      int buf_to_use=0;
      while(slow_used[buf_to_use] && buf_to_use<NumBuffers)
        buf_to_use++;
      if(buf_to_use==NumBuffers) {
        pprintf(TextOutputStream,"Ran out of slow buffers\n");
        buf_to_use=NumBuffers-1;
      }

      slow_used[buf_to_use]=true;

      cmd_vec_rgn = cmdVecSlowRgn[buf_to_use];
      cmd_vec_p = reinterpret_cast<OCommandVectorData*>(cmd_vec_rgn->Base());
    }
    else {
      int buf_to_use=0;
      while(fast_used[buf_to_use] && buf_to_use<NumBuffers)
        buf_to_use++;
      if(buf_to_use==NumBuffers) {
        pprintf(TextOutputStream,"Ran out of fast buffers\n");
        buf_to_use=NumBuffers-1;
      }

      fast_used[buf_to_use]=true;

      cmd_vec_rgn = cmdVecFastRgn[buf_to_use];
      cmd_vec_p = reinterpret_cast<OCommandVectorData*>(cmd_vec_rgn->Base());
    }

    num_used++;

    //if(debug_prints) pprintf(TextOutputStream,"nu %d sa %d sb %d fa %d fb %d\n",num_used,slow_used[0],slow_used[1],fast_used[0],fast_used[1]);

    // In case the Buffer is available, set the address in commandValue.
    for(uint joint_idx=0; joint_idx<NumPIDJoints; joint_idx++) {
      OCommandData *data = cmd_vec_p->GetData(joint_idx);
      output_cmd[joint_idx] = data->value;
      angles[joint_idx] = 0.0;

      // hack testing sync between motions and sensor readings.
      // As each frame is executed OVRComm sets the frameNumber
      // field in the OCommandInfo struct to the appropriate
      // value. It's up to us to get around to testing it, but
      // we should be able to test for it going non-zero (after
      // we zero it out ourselves) to see when the frame is
      // actually executed (if we care and aren't just after
      // an after the fact matching)
      //      if(joint_idx==0) {
      //	OCommandInfo *info = cmd_vec_p->GetInfo(0);
      //	pprintf(TextOutputStream, "motion frame number: %ld\n",
      //		info->frameNumber);
      //	info->frameNumber = 0;
      //}
    }
    
    for(uint joint_idx=LEDOffset; joint_idx<LEDOffset+NumLEDs; joint_idx++) {
      OCommandData *data = cmd_vec_p->GetData(joint_idx);
      output_cmd[joint_idx] = data->value;
    }

    if(slow) {
      for(uint joint_idx=EarOffset; joint_idx<EarOffset+NumBinJoints; joint_idx++) {
        OCommandData *data = cmd_vec_p->GetData(joint_idx);
        output_cmd[joint_idx] = data->value;
      }
    }

    for(frame=0; frame<NumFrames; frame++){
      time = (current + frame)*FrameTime;

#ifndef ZERODOG
      motion.getAngles(angles);
#endif

      if(frame==NumFrames-1){
        static int buffer_num=0;
      
        if(LocUpdateQueueBuffer!=NULL){
          motion.getMotionUpdate(LocUpdateQueueBuffer->buffer[buffer_num],GetTime()+(NumFrames+NumFrames/2)*FrameTime*1000);
      
          static int dump_count=0;
          if(config.spoutConfig.dumpMoveUpdate) {
            if(++dump_count >= config.spoutConfig.dumpMoveUpdate) {
              if(encoder!=NULL && motionUpdateStream!=NULL) {
                static uchar buf[MotionUpdateSize];
                int out_size=0;
                out_size = encoder->encodeMotionLocalizationUpdate
                  (buf,&LocUpdateQueueBuffer->buffer[buffer_num]);
                motionUpdateStream->writeBinary(buf,out_size);
              } else {
                pprintf(TextOutputStream,"MNENUS");
              }
              dump_count=0;
            }
          }
      
          buffer_num = (buffer_num + 1) % Motion::MotionLocalizationUpdateQueueBufferSize;
        }else{
          pprintf(TextOutputStream,"no fbk queue\n");
        }
      }

      for(uint i=0; i<NumPIDJoints; i++){
        ((OJointCommandValue2*)(output_cmd[i]))[frame].value = micro(angles[i]);
      }

      LED_state = motion.getLEDState();

      for(uint led_idx=0; led_idx<NumBinLEDs; led_idx++) {
	if((LED_state->cmd >> led_idx) & 1){
	  ((OLEDCommandValue2*)(output_cmd[LEDOffset + led_idx]))[frame].led = oledON;
	  ((OLEDCommandValue2*)(output_cmd[LEDOffset + led_idx]))[frame].period = 1;
	}else{
	  ((OLEDCommandValue2*)(output_cmd[LEDOffset + led_idx]))[frame].led = oledOFF;
	  ((OLEDCommandValue2*)(output_cmd[LEDOffset + led_idx]))[frame].period = 1;
	}
      }

      for(uint led_idx=NumBinLEDs; led_idx<NumTriLEDs+NumBinLEDs; led_idx++) {
	if((LED_state->cmd >> led_idx) & 1){
	  ((OLEDCommandValue3*)(output_cmd[LEDOffset + led_idx]))[frame].intensity = (int)LED_state->intensity[led_idx];
	  ((OLEDCommandValue3*)(output_cmd[LEDOffset + led_idx]))[frame].period = 1;
	  ((OLEDCommandValue3*)(output_cmd[LEDOffset + led_idx]))[frame].mode = LED_state->mode ? oled3_MODE_B:oled3_MODE_A;
	}else{
	  ((OLEDCommandValue3*)(output_cmd[LEDOffset + led_idx]))[frame].intensity = 0;
	  ((OLEDCommandValue3*)(output_cmd[LEDOffset + led_idx]))[frame].period = 1;
	  ((OLEDCommandValue3*)(output_cmd[LEDOffset + led_idx]))[frame].mode = LED_state->mode ? oled3_MODE_B:oled3_MODE_A;
	}
      }

    }

    if(slow) {
      ((OJointCommandValue3*)(output_cmd[EarOffset + 0]))[0].value=((time+750)/1500)%2;
      ((OJointCommandValue3*)(output_cmd[EarOffset + 1]))[0].value=( time     /1500)%2;
    }

    current = (current + step);

    // Passing the data over to Handler
    subject[sbjMoveJoint]->SetData(cmd_vec_rgn);
  }

  // Send data to Handler
  subject[sbjMoveJoint]->NotifyObservers();
}

void
MotionObject::ReadyRegisterRegion(const OReadyEvent &event) {
  //if(needToSendLocBuffer) {
  //  sendLocUpdateQueueBufferIfReady(event.SenderID());
  //} else if(needToSendOutBuffer) {
  //  sendOutputBufferIfReady(event.SenderID());
  //}
}

void
MotionObject::sendOutputBuffer() {
  //cout << "SharedMem::SendMemBlockID() invoked: memID " << regionId << "." << endl;
  OSubject* thisSubj = subject[sbjRegisterRegion];

  if(outMemRgn.getData()!=NULL && needToSendOutBuffer) {
    needToSendOutBuffer=false;
    outMemRgn.setId(MemoryId::MotionOutId);
    thisSubj->SetData(outMemRgn.getMsgForm());
    thisSubj->NotifyObservers();
    //cout << "SharedMemID sent" << endl;
  }
}

void MotionObject::sendLocUpdateQueueBuffer() {
  OSubject* thisSubj = subject[sbjRegisterRegion];

  if(motionLocUpdateMemRgn.validData() && needToSendLocBuffer) {
    needToSendLocBuffer=false;
    motionLocUpdateMemRgn.setId(MemoryId::MotionLocUpdateId);
    thisSubj->SetData(motionLocUpdateMemRgn.getMsgForm());
    thisSubj->NotifyObservers();
  }
}

void MotionObject::sendVelParam(Motion::VelParam &vp){
  subject[sbjVelParam]->SetData(&vp,sizeof(Motion::VelParam));
  subject[sbjVelParam]->NotifyObservers();
}

void MotionObject::RequestRegions() {
}

void MotionObject::ReadyRequestRegion(const OReadyEvent &event)
{
}

void MotionObject::ResultMemRegion(const ONotifyEvent &event)
{
//  int event_num_data = event.NumOfData();
//  for(int event_data_id=0; event_data_id<event_num_data; event_data_id++) {
//    SMMSharedMemRegion region;
//    int region_id;
//    
//    region.init((SMMSharedMemRegion::MsgForm)event.RCData(event_data_id));
//    region_id = region.getId();
//    {
//      cout << "got buffer for region id " << region_id << "?" << endl;
//    }
//  }
//    
//  observer[obsSharedMemRegionInfo]->AssertReady();
}

void MotionObject::ReadyControlBC(const OReadyEvent& event)
{
  static EventTimeReporter reporter("MotionObject::ReadyControlBC",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  if(triggerBC) SendPowerOnParam();
}

void MotionObject::SendPowerOnParam()
{
  OPENR::SetMotorPower(opowerON);
  triggerBC = false;
}

void MotionObject::ReadyMotionComplete(const OReadyEvent &event)
{
}

void MotionObject::GotSensorFrame(const ONotifyEvent &event)
{
  static EventTimeReporter reporter("MotionObject::GotSensorFrame",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
 
  for(int i=0; i<event.NumOfData(); i++) {
    // Receiving Sensor Data from OVRComm 
    OSensorFrameVectorData *sd = reinterpret_cast<OSensorFrameVectorData *>(event.RCData(i)->Base());
    // double angles[NumPIDJoints];
    // static int pressed = 0;

    ulong s_frame = sd->info[1].frameNumber;

    sensor.read(sd, GetTime(), s_frame);

    /*
      bool chin_switch,back_switch;
      double head_front_press,head_back_press;
      // Get chin switch
      chin_switch = ((OSwitchValue)sd[0].GetData( 7)->frame[0].value==oswitchON);
      // Get back switch
      back_switch = ((OSwitchValue)sd[0].GetData(27)->frame[0].value==oswitchON);
      // Get head front pressure sensor (usually 0 if not pressed, .6-1.0 when pressed)
      head_front_press = ((double)sd[0].GetData(4)->frame[0].value)/1.0e6;
      // Get head back  pressure sensor (usually 0 if not pressed, .6-1.0 when pressed)
      // pressing bakc button produces 0-0.7 on front button
      head_back_press  = ((double)sd[0].GetData(3)->frame[0].value)/1.0e6;
    */
    

    // Testing only
    if(sensor.button & HeadFrontButton){
      motion.setLEDState(Motion::LED_TAIL_BLUE,Motion::LED_TAIL_BLUE);
    }
    if(sensor.button & HeadBackButton){
      motion.setLEDState(Motion::LED_TAIL_RED,Motion::LED_TAIL_RED);
    }
    if(sensor.button & ChinButton){
      motion.setLEDState(Motion::LED_TAIL_BLUE,Motion::LED_TAIL_BLUE);
    }
    if(sensor.button & BackButton){
      motion.setLEDState(Motion::LED_TAIL_RED,Motion::LED_TAIL_RED);
    }
    
  }

  observer[obsSensorFrame]->AssertReady();
}

void MotionObject::UpdateControl(const ONotifyEvent &event)
{
#ifndef ZERODOG
  Motion::MotionCommand cmd;
  int time;

  // if(TRACE && OutputBuf) OutputBuf->write((uchar *)"E\xFF");

  time = (current)*FrameTime;
  memcpy(&cmd,event.Data(0),sizeof(cmd));
  motion.setCommand(cmd);
#endif
  
  observer[obsControl]->AssertReady();

  /**********************/
  
  //check if we parameters have changed and we need to update VelParam for behaviors
  if(motion.newParam()){
    sendVelParam(motion.getParams()->max_vel);
    motion.sentVelParam();
  }


}

// Process aperios messages from either MainObj or
// OpenRLinux setting new walk parameters from
// wp_beh (walk_beh.prm contains initial value
// on memstick)
void MotionObject::UpdateParams(const ONotifyEvent &event)
{
  static EventTimeReporter reporter("MotionObject::UpdateParams",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  Motion::WalkParam params;

  memcpy(&params,event.Data(0),sizeof(params));
  motion.setParams(params);

  observer[obsParams]->AssertReady();
}

void MotionObject::SetupSound()
{
  char *pt;
  int k;

  pprintf(TextOutputStream,"SetupSound");
  OPENR::OpenPrimitive((char*)PrimitiveName[SpeakerOffset], &primitiveID[SpeakerOffset]);

  if (OPENR::ControlPrimitive(primitiveID[SpeakerOffset], oprmreqSPEAKER_MUTE_OFF, 0, 0, 0, 0) != oSUCCESS){
    pprintf(TextOutputStream,"MUTE_OFF ERROR");
    //cout << "MUTE_OFF Error" << endl;
  }
 
  OPrimitiveControl_SpeakerVolume volume(ospkvol10dB);
  //OPrimitiveControl_SpeakerVolume volume(ospkvolinfdB);
  if(!OPENR::ControlPrimitive(primitiveID[SpeakerOffset],
                              oprmreqSPEAKER_SET_VOLUME,
                              &volume, sizeof(volume), 0, 0)){
    pprintf(TextOutputStream,"Set speaker volume failed.\n");
  }

  for(int i=0; i<2; i++){
    OPENR::NewSoundVectorData(1, SoundTableSize, &soundMemID[i], &soundVecData[i]);
    soundVecData[i]->SetNumData(1);
    soundVecRgn[i]=new RCRegion(soundVecData[i]->vectorInfo.memRegionID,
                                soundVecData[i]->vectorInfo.offset,
                                (void*)(soundVecData[i]),
                                soundVecData[i]->vectorInfo.totalSize);
    
    OSoundInfo *info = soundVecData[i]->GetInfo(0);
    info->Set(odataSOUND_VECTOR, primitiveID[SpeakerOffset], SoundTableSize);
    info->format        = osoundformatPCM;
    info->channel       = 1;
    info->samplingRate  = SpeakerSamplingRate;
    info->bitsPerSample = 8;
    info->frameSize     = info->samplingRate / 1000 * 32;
    
    pt = (char*)soundVecData[i]->GetData(0);
    for(k=0; k<SoundTableSize; k++) pt[k] = rand()%256 - 128;
  }
  soundCur = 0;

  //OPENR::ControlPrimitive(primitiveID[29], oprmreqSPEAKER_MUTE_ON, 0, 0, 0, 0);
}

OStatus MotionObject::ReadySpeaker(const OReadyEvent &info)
{
  static EventTimeReporter reporter("MotionObject::ReadySpeaker",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  char *pt;
  int i;

  for(i=0; i<2; i++){
    if(soundVecRgn[i]->NumberOfReference() == 1) {
      pt = (char*)soundVecData[i]->GetData(0);
      motion.getSoundData(pt,SoundTableSize);

      subject[sbjSpeaker]->SetData(soundVecRgn[i]);
    }
  }

  subject[sbjSpeaker]->NotifyObservers();

  return oSUCCESS;
}