/* 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 <OPENR/ObjcommTypes.h>
#include <OPENR/OFbkImage.h>
#include <OPENR/OPENR.h>
#include <OPENR/OPENRAPI.h>
#include <OPENR/OPENRMessages.h>

#include "../headers/CircBufPacket.h"
#include "../headers/Dictionary.h"
#include "../headers/Geometry.h"
#include "../headers/Reporting.h"
#include "../headers/Sensors.h"
#include "../headers/Util.h"
#include "../headers/Utility.h"
#include "../Motion/MotionObject.h"

static const int OffJointLFJoint            =  0; // Left front
static const int OffJointLFShoulder         =  1;
static const int OffJointLFKnee             =  2;
static const int OffJointRFJoint            =  3; // Right front
static const int OffJointRFShoulder         =  4;
static const int OffJointRFKnee             =  5;
static const int OffJointLHJoint            =  6; // Left hind
static const int OffJointLHShoulder         =  7;
static const int OffJointLHKnee             =  8;
static const int OffJointRHJoint            =  9; // Right hind
static const int OffJointRHShoulder         = 10;
static const int OffJointRHKnee             = 11;
static const int OffJointNeckTilt           = 12; // Head
static const int OffJointNeckPan            = 13;
static const int OffJointNeckRoll           = 14;
static const int OffJointTailPan            = 15; // Tail
static const int OffJointTailTilt           = 16;
static const int OffJointMouth              = 17;
static const int OffSensorHeadFrontPressure = 18;
static const int OffSensorHeadBackPressure  = 19;
static const int OffSensorBackSwitch        = 20;
static const int OffSensorChinSwitch        = 21;
static const int OffSensorLFPaw             = 22;
static const int OffSensorRFPaw             = 24;
static const int OffSensorLHPaw             = 23;
static const int OffSensorRHPaw             = 25;
static const int OffSensorPSD               = 26;
static const int OffSensorAccelFB           = 27; // Front-back (front positive)
static const int OffSensorAccelLR           = 28; // Left-right (right positive)
static const int OffSensorAccelUD           = 29; // Up-down (up positive)
//static const int OffSensorThermoSensor      = 30;
static const int OffSensorWlan              = 30;
static const int OffSensorChestPSD          = 31;
static const int OffSensorBackRearSwitch    = 32;
static const int OffSensorHeadFarPSD        = 33;

// ERS-7 reuses some of the offsets for different things than the 210
static const int OffSensorBackFrontSwitch   = 21; // frontmost back switch on ERS-7
static const int OffSensorHeadNearPSD       = 26;


const int CPCTranslate_ERS210[] =
  {
    8 , // 00 left  front rotate   PRM:/r2/c1-Joint2:j1,
    9 , // 01 left  front shoulder PRM:/r2/c1/c2-Joint2:j2,
    10, // 02 left  front knee     PRM:/r2/c1/c2/c3-Joint2:j3,
    16, // 03 right front rotate   PRM:/r4/c1-Joint2:j1,
    17, // 04 right front shoulder PRM:/r4/c1/c2-Joint2:j2,
    18, // 05 right front knee     PRM:/r4/c1/c2/c3-Joint2:j3,
    12, // 06 left  hind  rotate   PRM:/r3/c1-Joint2:j1,
    13, // 07 left  hind  shoulder PRM:/r3/c1/c2-Joint2:j2,
    14, // 08 left  hind  knee     PRM:/r3/c1/c2/c3-Joint2:j3,
    20, // 09 right hind  rotate   PRM:/r5/c1-Joint2:j1,
    21, // 10 right hind  shoulder PRM:/r5/c1/c2-Joint2:j2,
    22, // 11 right hind  knee     PRM:/r5/c1/c2/c3-Joint2:j3,
    0 , // 12 neck tilt            PRM:/r1/c1-Joint2:j1,
    1 , // 13 neck pan             PRM:/r1/c1/c2-Joint2:j2,
    2 , // 14 neck roll            PRM:/r1/c1/c2/c3-Joint2:j3,
    24, // 15 tail pan             PRM:/r6/c1-Joint2:j1,
    25, // 16 tail tilt            PRM:/r6/c2-Joint2:j2,
    6 , // 17 mouth                PRM:/r1/c1/c2/c3/c4-Joint2:j4,
    4 , // 18 head button front    PRM:/r1/c1/c2/c3/f1-Sensor:f1,
    3 , // 19 head button back     PRM:/r1/c1/c2/c3/f2-Sensor:f2,
    27, // 20 back button          PRM:/r6/s1-Sensor:s1,
    7 , // 21 chin/front back button       PRM:/r1/c1/c2/c3/c4/s5-Sensor:s5,
    11, // 22 left  front paw sw   PRM:/r2/c1/c2/c3/c4-Sensor:s4,
    19, // 24 right front paw sw   PRM:/r4/c1/c2/c3/c4-Sensor:s4,
    15, // 23 left  hind  paw sw   PRM:/r3/c1/c2/c3/c4-Sensor:s4,
    23, // 25 right hind  paw sw   PRM:/r5/c1/c2/c3/c4-Sensor:s4,
    5 , // 26 PSD                  PRM:/r1/c1/c2/c3/p1-Sensor:p1,
    28, // 27 x accel              PRM:/a1-Sensor:a1,
    29, // 28 -y accel             PRM:/a2-Sensor:a2,
    30, // 29 z accel              PRM:/a3-Sensor:a3,
    //26, // thermo sensor        PRM:/r6/t1-Sensor:t1,
  };

const int CPCTranslate_ERS7[] =
  {
    11, // 00 left  front rotate   PRM:/r1/c1-Joint2:11,
    10, // 01 left  front shoulder PRM:/r1/c1/c2-Joint2:12,
    9 , // 02 left  front knee     PRM:/r1/c1/c2/c3-Joint2:13,
    19, // 03 right front rotate   PRM:/r1/c1/c2/c3/c4-Joint2:14,
    18, // 04 right front shoulder PRM:/r2/c1-Joint2:21,
    17, // 05 right front knee     PRM:/r2/c1/c2-Joint2:22,
    15, // 06 left  hind  rotate   PRM:/r2/c1/c2/c3-Joint2:23,
    14, // 07 left  hind  shoulder PRM:/r4/c1-Joint2:41,
    13, // 08 left  hind  knee     PRM:/r4/c1/c2-Joint2:42,
    23, // 09 right hind  rotate   PRM:/r4/c1/c2/c3-Joint2:43,
    22, // 10 right hind  shoulder PRM:/r3/c1-Joint2:31,
    21, // 11 right hind  knee     PRM:/r3/c1/c2-Joint2:32,
    7 , // 12 neck tilt            PRM:/r3/c1/c2/c3-Joint2:33,
    6 , // 13 neck pan             PRM:/r5/c1-Joint2:51,
    2 , // 14 neck roll            PRM:/r5/c1/c2-Joint2:52,
    25, // 15 tail pan             PRM:/r5/c1/c2/c3-Joint2:53,
    24, // 16 tail tilt            PRM:/
    0 , // 17 mouth                PRM:/
    3 , // 18 head button front    PRM:/  (head button)
    1 , // 19 head button back     PRM:/  (chin button)
    32, // 20 back button          PRM:/  (back 2)
    33, // 21 chin button          PRM:/  (back 3)
    8 , // 22 left  front paw sw   PRM:/
    16, // 24 right front paw sw   PRM:/
    12, // 23 left  hind  paw sw   PRM:/
    20, // 25 right hind  paw sw   PRM:/
    4 , // 26 PSD                  PRM:/   (head psd near)
    26, // 27 x accel              PRM:/
    27, // 28 -y accel             PRM:/
    28, // 29 z accel              PRM:/
    30, // 30 wavelan switch       PRM:/b1-Sensor:b1 
    29, // 31 (chest distance sensor)
    31, // 32 (back 1)
    5,  // 33 (head psd far)
  };

const int *CPCTranslate=NULL;

//static const char * PrimLocs[] = {
//    "PRM:/r1/c1-Joint2:11"                 ,   // Neck tilt1                      
//    "PRM:/r1/c1/c2-Joint2:12"              ,   // Neck pan                        
//    "PRM:/r1/c1/c2/c3-Joint2:13"           ,   // Neck tilt2                      
//    "PRM:/r1/c1/c2/c3/c4-Joint2:14"        ,   // Mouth                           
//    "PRM:/r1/c1/c2/c3/e5-Joint4:15"        ,   // Left ear                        
//    "PRM:/r1/c1/c2/c3/e6-Joint4:16"        ,   // Right ear                       
//    "PRM:/r1/c1/c2/c3/t1-Sensor:t1"        ,   // Head sensor                     
//    "PRM:/r1/c1/c2/c3/p1-Sensor:p1"        ,   // Head distance sensor(near)      
//    "PRM:/r1/c1/c2/c3/p2-Sensor:p2"        ,   // Head distance sensor(far)       
//    "PRM:/r1/c1/c2/c3/c4/s5-Sensor:s5"     ,   // Chin sensor                     
//    "PRM:/r1/c1/c2/c3/l1-LED2:l1"          ,   // Head light(color)               
//    "PRM:/r1/c1/c2/c3/l2-LED2:l2"          ,   // Head light(white)               
//    "PRM:/r1/c1/c2/c3/l3-LED2:l3"          ,   // Mode Indicator(red)             
//    "PRM:/r1/c1/c2/c3/l4-LED2:l4"          ,   // Mode Indicator(green)           
//    "PRM:/r1/c1/c2/c3/l5-LED2:l5"          ,   // Mode Indicator(blue)            
//    "PRM:/r1/c1/c2/c3/l6-LED2:l6"          ,   // Wireless light                  
//    "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:/r1/c1/c2/c3/i1-FbkImageSensor:F1",   // Color camera                    
//    "PRM:/r2/c1-Joint2:21"                 ,   // Left fore legJ1                 
//    "PRM:/r2/c1/c2-Joint2:22"              ,   // Left fore legJ2                 
//    "PRM:/r2/c1/c2/c3-Joint2:23"           ,   // Left fore legJ3                 
//    "PRM:/r2/c1/c2/c3/c4-Sensor:24"        ,   // Left fore leg, paw sensor       
//    "PRM:/r3/c1-Joint2:31"                 ,   // Left hind legJ1                 
//    "PRM:/r3/c1/c2-Joint2:32"              ,   // Left hind legJ2                 
//    "PRM:/r3/c1/c2/c3-Joint2:33"           ,   // Left hind legJ3                 
//    "PRM:/r3/c1/c2/c3/c4-Sensor:34"        ,   // Left hind leg, paw sensor       
//    "PRM:/r4/c1-Joint2:41"                 ,   // Right fore legJ1                
//    "PRM:/r4/c1/c2-Joint2:42"              ,   // Right fore legJ2                
//    "PRM:/r4/c1/c2/c3-Joint2:43"           ,   // Right fore legJ3                
//    "PRM:/r4/c1/c2/c3/c4-Sensor:44"        ,   // Right fore leg, paw sensor      
//    "PRM:/r5/c1-Joint2:51"                 ,   // Right hind legJ1                
//    "PRM:/r5/c1/c2-Joint2:52"              ,   // Right hind legJ2                
//    "PRM:/r5/c1/c2/c3-Joint2:53"           ,   // Right hind legJ3                
//    "PRM:/r5/c1/c2/c3/c4-Sensor:54"        ,   // Right hind leg, paw sensor      
//    "PRM:/r6/c1-Joint2:61"                 ,   // Tail tilt                       
//    "PRM:/r6/c2-Joint2:62"                 ,   // Tail pan                        
//    "PRM:/r1/c1/c2/c3/m1-Mic:M1"           ,   // Stereo microphones              
//    "PRM:/s1-Speaker:S1"                   ,   // Speaker                         
//    "PRM:/a1-Sensor:a1"                    ,   // Acceleration sensor(front-back) 
//    "PRM:/a2-Sensor:a2"                    ,   // Acceleration sensor(right-left) 
//    "PRM:/a3-Sensor:a3"                    ,   // Acceleration sensor(up-down)    
//    "PRM:/p1-Sensor:p1"                    ,   // Chest distance sensor           
//    "PRM:/b1-Sensor:b1"                    ,   // Wireless LAN switch             
//    "PRM:/t2-Sensor:t2"                    ,   // Back sensor(front)              
//    "PRM:/t3-Sensor:t3"                    ,   // Back sensor(middle)             
//    "PRM:/t4-Sensor:t4"                    ,   // Back sensor(rear)               
//    "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)         
//};
//
//const uint NumPrimLocs = 65;//sizeof(PrimLocs)/sizeof(PrimLocs[0]);
//
//ulong PrimIds[NumPrimLocs];

//#define OGETD(idx,cpc) (((double)sensor[0].GetData(cpc)->frame[idx].value) / 1.0E6)
//#define OGETB(idx,cpc) ((bool)sensor[0].GetData(cpc)->frame[idx].value)
//#define OGETPWM(idx,cpc) (((OJointValue &)sensor[0].GetData(cpc)->frame[idx]).pwmDuty)

#define GET_VALUE(idx,cpc) (sensor[0].GetData(CPCTranslate[cpc])->frame[idx].value)
// get milli double
#define GET_MDBL(idx,cpc) (((double)sensor[0].GetData(CPCTranslate[cpc])->frame[idx].value) / 1.0E3)
// get micro double
#define GET_UDBL(idx,cpc) (((double)sensor[0].GetData(CPCTranslate[cpc])->frame[idx].value) / 1.0E6)
// get bool
#define GET_BOOL(idx,cpc) ((bool)sensor[0].GetData(CPCTranslate[cpc])->frame[idx].value)
// get integer as bool
#define GET_IBOOL(idx,cpc) (sensor[0].GetData(CPCTranslate[cpc])->frame[idx].value!=0)
// get switch bool
#define GET_SBOOL(idx,cpc) ((OSwitchValue)sensor[0].GetData(CPCTranslate[cpc])->frame[src_idx].value == oswitchON);
// get pwm duty
#define GET_PWM(idx,cpc) (((OJointValue &)sensor[0].GetData(CPCTranslate[cpc])->frame[idx]).pwmDuty)

std::string SensorData::name("SensorData");
unsigned int SensorData::ButtonTranslations[SensorData::NumButtonTranslations];

SensorData::SensorData() :
  mostRecentFrame(0)
{
  mzero(sensorFrames);

  InitializeSensors();

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

  //for(uint i=0; i<NumPrimLocs; i++) {
  //  OPENR::OpenPrimitive((char*)PrimLocs[i],&PrimIds[i]);
  //}
}

bool SensorData::update(ulong time,const EventList *events)
{
  return true;
}

SensorData *SensorData::get(ulong time)
{
  return this;
}

void SensorData::read(SensorDataFrame *frame,int src_idx, OSensorFrameVectorData *sensor,
		      ulong time, ulong frame_num)
{
  const double g = 9.80665;
  bool head_f,head_b;
  bool chin,back;
  bool back_f=false,back_b=false;

  frame->legAngles[0*3 + 0] = GET_UDBL(src_idx,OffJointLFJoint);
  frame->legAngles[0*3 + 1] = GET_UDBL(src_idx,OffJointLFShoulder);
  frame->legAngles[0*3 + 2] = GET_UDBL(src_idx,OffJointLFKnee);

  frame->legAngles[1*3 + 0] = GET_UDBL(src_idx,OffJointRFJoint);
  frame->legAngles[1*3 + 1] = GET_UDBL(src_idx,OffJointRFShoulder);
  frame->legAngles[1*3 + 2] = GET_UDBL(src_idx,OffJointRFKnee);

  frame->legAngles[2*3 + 0] = GET_UDBL(src_idx,OffJointLHJoint);
  frame->legAngles[2*3 + 1] = GET_UDBL(src_idx,OffJointLHShoulder);
  frame->legAngles[2*3 + 2] = GET_UDBL(src_idx,OffJointLHKnee);

  frame->legAngles[3*3 + 0] = GET_UDBL(src_idx,OffJointRHJoint);
  frame->legAngles[3*3 + 1] = GET_UDBL(src_idx,OffJointRHShoulder);
  frame->legAngles[3*3 + 2] = GET_UDBL(src_idx,OffJointRHKnee);

   
  //get joint duty cycles
  frame->legDutyCycles[0*3 + 0] = GET_PWM(src_idx,OffJointLFJoint);
  frame->legDutyCycles[0*3 + 1] = GET_PWM(src_idx,OffJointLFShoulder);
  frame->legDutyCycles[0*3 + 2] = GET_PWM(src_idx,OffJointLFKnee);
  
  frame->legDutyCycles[1*3 + 0] = GET_PWM(src_idx,OffJointRFJoint);
  frame->legDutyCycles[1*3 + 1] = GET_PWM(src_idx,OffJointRFShoulder);
  frame->legDutyCycles[1*3 + 2] = GET_PWM(src_idx,OffJointRFKnee);
  
  frame->legDutyCycles[2*3 + 0] = GET_PWM(src_idx,OffJointLHJoint);
  frame->legDutyCycles[2*3 + 1] = GET_PWM(src_idx,OffJointLHShoulder);
  frame->legDutyCycles[2*3 + 2] = GET_PWM(src_idx,OffJointLHKnee);
  
  frame->legDutyCycles[3*3 + 0] = GET_PWM(src_idx,OffJointRHJoint);
  frame->legDutyCycles[3*3 + 1] = GET_PWM(src_idx,OffJointRHShoulder);
  frame->legDutyCycles[3*3 + 2] = GET_PWM(src_idx,OffJointRHKnee);

  // Get head tilt,pan,roll joint anglea
  frame->headAngles[0] = GET_UDBL(src_idx,OffJointNeckTilt);
  frame->headAngles[1] = GET_UDBL(src_idx,OffJointNeckPan);
  frame->headAngles[2] = GET_UDBL(src_idx,OffJointNeckRoll);

  // Get tail angles
  frame->tailAngles[0] = GET_UDBL(src_idx,OffJointTailPan );
  frame->tailAngles[1] = GET_UDBL(src_idx,OffJointTailTilt);

  // Get IR distance sensor
  frame->IRDistance = GET_MDBL(src_idx,OffSensorPSD);
  frame->IRDistChest = -1.0; // we'll set this only for ERS-7
  frame->IRDistHeadNear = frame->IRDistance; 
  frame->IRDistHeadFar = frame->IRDistance; // set again below on ERS-7.

  // Get foot switches
  frame->paw[0] = GET_BOOL(src_idx,OffSensorLFPaw);
  frame->paw[1] = GET_BOOL(src_idx,OffSensorRFPaw);
  frame->paw[2] = GET_BOOL(src_idx,OffSensorLHPaw);
  frame->paw[3] = GET_BOOL(src_idx,OffSensorRHPaw);

  // Get acceleration sensors
  frame->accel.x =  GET_UDBL(src_idx,OffSensorAccelFB) / g;
  frame->accel.y = -GET_UDBL(src_idx,OffSensorAccelLR) / g;
  frame->accel.z =  GET_UDBL(src_idx,OffSensorAccelUD) / g;

  frame->timestamp = time;
  frame->sensor_frame = frame_num;

  char design[256];
  OPENR::GetRobotDesign(design);
  if(strcmp(design,"ERS-210")==0){
    chin   = GET_SBOOL(src_idx,OffSensorChinSwitch);
    back   = GET_SBOOL(src_idx,OffSensorBackSwitch);
    head_f = (GET_UDBL(src_idx,OffSensorHeadFrontPressure) > 0.50);
    head_b = (GET_UDBL(src_idx,OffSensorHeadBackPressure ) > 0.75);

  }else{
    chin   = GET_IBOOL(src_idx,OffSensorChinSwitch);
    back   = GET_IBOOL(src_idx,OffSensorBackSwitch);
    head_f = GET_IBOOL(src_idx,OffSensorHeadFrontPressure); 
    head_b = GET_IBOOL(src_idx,OffSensorHeadBackPressure);
    back_f = GET_IBOOL(src_idx,OffSensorBackFrontSwitch);
    back_b = GET_IBOOL(src_idx,OffSensorBackRearSwitch);

    // Get wlan sensor value
    frame->wlan = GET_BOOL(src_idx,OffSensorWlan);

    // we have a chest sensor
    frame->IRDistChest = GET_MDBL(src_idx,OffSensorChestPSD);

    // near distance is already set correctly; just update far.
    frame->IRDistHeadFar = GET_MDBL(src_idx,OffSensorHeadFarPSD);
  }


  frame->button = 0;
  if(chin)          frame->button |= ChinButton      | ButtonTranslations[ChinButtonIdx     ];
  if(back)          frame->button |= BackButton      | ButtonTranslations[BackButtonIdx     ];
  if(head_f)        frame->button |= HeadFrontButton | ButtonTranslations[HeadFrontButtonIdx];
  if(head_b)        frame->button |= HeadBackButton  | ButtonTranslations[HeadBackButtonIdx ];
  if(back_f)        frame->button |= BackFrontButton | ButtonTranslations[BackFrontButtonIdx];
  if(back_b)        frame->button |= BackRearButton  | ButtonTranslations[BackRearButtonIdx ];
}

void SensorData::read(OSensorFrameVectorData *sensor, ulong time, ulong frame_num)
{
  for(int src_idx=0; src_idx<4; src_idx++) {
    mostRecentFrame = (mostRecentFrame + 1) % NumSavedSensorFrames;
    read(&sensorFrames[mostRecentFrame],src_idx,sensor, time, frame_num+src_idx);
  }

  button = sensorFrames[mostRecentFrame].button;
}

const SensorDataFrame *SensorData::getFrame(int frame_rel_time) {
  int frame_time;

  frame_rel_time = bound(frame_rel_time,-(NumSavedSensorFrames-1),0);
  
  frame_time = mostRecentFrame + frame_rel_time;
  frame_time += NumSavedSensorFrames;
  frame_time = frame_time % NumSavedSensorFrames;
  
  return &sensorFrames[frame_time];
}

const SensorDataFrame *SensorData::getByFrameNumber(ulong frame_number) {
  
  int delta = frame_number - sensorFrames[mostRecentFrame].sensor_frame;

  return getFrame(delta); // this does clipping for us
}

int
SensorData::ButtonStringToNumber(const char *button_str) {
  if(strcmp(button_str,"HeadFrontButton")==0)
    return HeadFrontButtonIdx;
  else if(strcmp(button_str,"HeadBackButton")==0)
    return HeadBackButtonIdx;
  else if(strcmp(button_str,"ChinButton")==0)
    return ChinButtonIdx;
  else if(strcmp(button_str,"BackButton")==0)
    return BackButtonIdx;
  else if(strcmp(button_str,"BackFrontButton")==0)
    return BackFrontButtonIdx;
  else if(strcmp(button_str,"BackRearButton")==0)
    return BackRearButtonIdx;
  else
    return -1;
}

void
SensorData::InitializeSensors() {
  ResetButtonTranslations();
  Dictionary button_dict;  

  button_dict.read("/MS/config/button.cfg");

  const char *physical_button_str="";
  int physical_button_num;
  if(button_dict.getValueString("LogButton",physical_button_str) &&
     (physical_button_num=ButtonStringToNumber(physical_button_str))!=-1) {
    AddButtonTranslation(physical_button_num,LogButtonIdx);
  }
  if(button_dict.getValueString("VisionObjPlusButton",physical_button_str) &&
     (physical_button_num=ButtonStringToNumber(physical_button_str))!=-1) {
    AddButtonTranslation(physical_button_num,VisionObjPlusButtonIdx);
  }
  if(button_dict.getValueString("VisionObjMinusButton",physical_button_str) &&
     (physical_button_num=ButtonStringToNumber(physical_button_str))!=-1) {
    AddButtonTranslation(physical_button_num,VisionObjMinusButtonIdx);
  }
  if(button_dict.getValueString("ResetModelsButton",physical_button_str) &&
     (physical_button_num=ButtonStringToNumber(physical_button_str))!=-1) {
    AddButtonTranslation(physical_button_num,ResetModelsButtonIdx);
  }
  if(button_dict.getValueString("SelNextButton",physical_button_str) &&
     (physical_button_num=ButtonStringToNumber(physical_button_str))!=-1) {
    AddButtonTranslation(physical_button_num,SelNextButtonIdx);
  }
  if(button_dict.getValueString("SelPrevButton",physical_button_str) &&
     (physical_button_num=ButtonStringToNumber(physical_button_str))!=-1) {
    AddButtonTranslation(physical_button_num,SelPrevButtonIdx);
  }
}

void
SensorData::ResetButtonTranslations() {
  for(int but_idx=0; but_idx<NumButtonTranslations; but_idx++) {
    ButtonTranslations[but_idx] = 1UL << but_idx;
  }
}

void
SensorData::AddButtonTranslation(int old_button_idx, int new_button_idx) {
  ButtonTranslations[old_button_idx] = 1UL << new_button_idx;
}

REGISTER_EVENT_PROCESSOR(SensorData,SensorData::name,SensorData::create);
