/* 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_Sensors_h
#define INCLUDED_Sensors_h

#include <string>

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

#ifdef PLATFORM_APERIOS
#include <OPENR/ObjcommTypes.h>
#include <OPENR/OFbkImage.h>
#include <OPENR/OPENR.h>
#include <OPENR/OPENRAPI.h>
#include <OPENR/OPENRMessages.h>
#endif

#include "../headers/Geometry.h"
#include "../Main/Events.h"

// tail constants
static const int TailPanReading  = 0;
static const int TailTiltReading = 1;

// Which buttons are pressed
static const int HeadFrontButtonIdx = 0;
static const int HeadBackButtonIdx  = 1;
static const int ChinButtonIdx      = 2;
static const int BackButtonIdx      = 3;
static const int BackFrontButtonIdx = 4;
static const int BackRearButtonIdx  = 5;

static const int HeadFrontButton = 1<<HeadFrontButtonIdx;
static const int HeadBackButton  = 1<<HeadBackButtonIdx;
static const int ChinButton      = 1<<ChinButtonIdx;
static const int BackButton      = 1<<BackButtonIdx;
static const int BackFrontButton = 1<<BackFrontButtonIdx;
static const int BackRearButton  = 1<<BackRearButtonIdx;

// virtual buttons
static const int LogButtonIdx            = 6;
static const int VisionObjPlusButtonIdx  = 7;
static const int VisionObjMinusButtonIdx = 8;
static const int ResetModelsButtonIdx    = 9;
static const int SelNextButtonIdx        = 10;
static const int SelPrevButtonIdx        = 11;

static const int LogButton            = 1<<LogButtonIdx;
static const int VisionObjPlusButton  = 1<<VisionObjPlusButtonIdx;
static const int VisionObjMinusButton = 1<<VisionObjMinusButtonIdx;
static const int ResetModelsButton    = 1<<ResetModelsButtonIdx;
static const int SelNextButton        = 1<<SelNextButtonIdx;
static const int SelPrevButton        = 1<<SelPrevButtonIdx;

static const double IROORDist = 900.0; // If IR returns this, we're out of range

static const int NumSavedSensorFrames = 64; // 512 ms of data

struct SensorDataFrame{

  // This is the time at which MainObject recieved the sensor frame.
  // 4 sensor frames at a time arrive in the same message and all
  // frames are labeled with the same timestamp.
  ulong timestamp;
  ulong sensor_frame;

  double legAngles[3*4]; // rotator, shoulder, knee for LF,RF,LH,RH (radians)
  double headAngles[3];  // tilt, pan, roll (radians) (must follow leg array!)
  double tailAngles[2];  // tail tilt,pan
  double IRDistance;     // Distance sensor: mm: between 100.0 and 900.0

  // The 210 only has a single distance sensor (above). We will set
  // IRDistance to IRDistHeadNear and we'll also create a second
  // variable so it's clear which reading comes from which sensor on
  // the ERS-7.
  double IRDistChest;
  double IRDistHeadNear;
  double IRDistHeadFar;

  short  legDutyCycles[3*4];  //PWM duty cycle value for each joint
  bool paw[4];           // Foot switches
  vector3d accel;        // In G's [-2.0,2.0]
  unsigned int button;   // bitwise OR of above button constants
  bool wlan;
};

class SensorData : public EventProcessor {
public:
  SensorData();
  virtual ~SensorData() {}

  // get a frame of sensor data
  //  0 = most recent frame
  // -1 = one frame ago
  // etc.
  const SensorDataFrame *getFrame(int frame_rel_time);
  
  // Returns the frame closest to frame_number in the buffer.
  // frame_number is the robot sensor frame the data is from. each
  // frame is 8 ms long and they arrive 4 at a time.
  const SensorDataFrame *getByFrameNumber(ulong frame_number);

  unsigned int button;   // bitwise OR of above button constants for latest frame

  static std::string name;
  static EventProcessor *create() {return new SensorData;}
  virtual bool update(ulong time,const EventList *events);
  virtual SensorData *get(ulong time);

#ifdef PLATFORM_APERIOS
  void read(OSensorFrameVectorData *sensor, ulong time, ulong frame_num);
 private:
  void read(SensorDataFrame *frame,int src_frame_idx,
	    OSensorFrameVectorData *sensor, ulong time, ulong frame_num);
 public:
#endif
  // must be called before button translations are allowed to occur
  static void InitializeSensors();
  static void ResetButtonTranslations();
  static void AddButtonTranslation(int old_button_idx, int new_button_idx);
  static int ButtonStringToNumber(const char *button_str);
private:
  static const int NumButtonTranslations=sizeof(int)*8;
  static unsigned int ButtonTranslations[NumButtonTranslations];

  SensorDataFrame sensorFrames[NumSavedSensorFrames];

  // This is miss-named - it refers to the most recent location in
  // the circular buffer that has had data added to it. NOT THE
  // FRAME NUMBER in the Aperios sense.
  int mostRecentFrame;
};

#endif