/*

ARIA header files for use with ARNL 1.7.1

Copyright(C) 2004, 2005 ActivMedia Robotics, LLC. 
Copyright(C) 2006, 2007, 2008, 2009 MobileRobots Inc.
All rights reserved.

This copy of Aria was relicensed for use with Arnl and the Arnl
license by MobileRobots Inc.  If you wish to download a seperate
distribution of Aria licensed under the GPL or a commercial license go to
http://www.mobilerobots.com/SOFTWARE/aria.html or contact MobileRobots
Inc, at robots@mobilerobots.com or MobileRobots Inc,
10 Columbia Drive, Amherst, NH 03031; 800-639-9481

MobileRobots Inc hereby grants to other individuals or
organizations permission to use this software with Arnl and in
compliance with the Arnl license.  This software may not be
distributed to others except by MobileRobots Inc.

MobileRobots Inc does not make any representations about the
suitability of this software for any purpose.  It is provided "as is"
without express or implied warranty.

*/
#ifndef ARPIXELDEVICE_H
#define ARPIXELDEVICE_H

#include <stdio.h>
#include "Aria.h"
/*!
  @class ArPixelDevice.
  @brief Holds data from a sensor that provides data arranged in a 2d array.

  Subclasses are used for specific sensor implementations, such
  as the ArFocusPixelDevice for the Focus Robotics nDepth 
  stereocamera.  

  The data are arranged in an x,y grid, with the origin being
  in the upper left corner, from the perspective of the robot.
  It's in a row-major format.

  The Field of View (FOV) is assumed to be centered with zero
  being the center of the area, such that the permitted x angles
  are between ((-x_fov/2) to (+x_fov/2)) and 
  ((-y_fov/2) to (+y_fov/2)).  Negative angles are to the lower
  left of the grid from the perspective of the robot.

  @param x_size dimension of data grid in x direction

  @param y_size dimension of data grid in y direction

  @param x_fov Field of View of sensor in X direction (angle in degrees)

  @param y_fov Field of View of sensor in Y direction (angle in degrees)

  @param name the name of this device
*/
template<class DataObject>
class ArPixelDevice
{
 public:
  /// Base Constructor
  AREXPORT ArPixelDevice(int x_size, int y_size, double x_fov, double y_fov,
		      const char *name)
  {
    myDeviceMutex.setLogName("ArPixelDevice::myDeviceMutex");
    myXSize = x_size;
    myYSize = y_size;
    myXFOV = x_fov;
    myYFOV = y_fov;
    myName = name;

    mySensorData = NULL;
    
    if (!allocateSensorDataMemory()) 
    {
      ArLog::log(ArLog::Terse, "Failed to allocate memory for ArPixelDevice %s", getName());
    }
    else
    {
      ArLog::log(ArLog::Verbose, "Allocated memory for ArPixelDevice %s", getName());
    }

    if (!allocateSensorXYZMemory()) 
    {
      ArLog::log(ArLog::Terse, "Failed to allocate XYZ memory for ArPixelDevice %s", getName());
    }
    else
    {
      ArLog::log(ArLog::Verbose, "Allocated XYZ memory for ArPixelDevice %s", getName());
    }
  }
  /// Base destructor
  AREXPORT virtual ~ArPixelDevice()
  {
    if (mySensorData != NULL)
    {
      for (int i=0; i < myXSize; i++)
      {
	for (int j=0; j < myYSize; j++)
	{
	  delete mySensorData[i][j];
	}
	delete [] (mySensorData[i]);
      }
      delete [] (mySensorData);
    }
    if (mySensorXYZ != NULL)
    {
      for (int i=0; i < myXSize; i++)
      {
	for (int j=0; j < myYSize; j++)
	{
	  delete [] mySensorXYZ[i][j];
	}
	delete [] (mySensorXYZ[i]);
      }
      delete [] (mySensorXYZ);
    }

  }
  /// Get the value of the sensor at the (x,y) coords
  DataObject *getSensorData(int x, int y)
  {
    if ((x >= 0) && (x < myXSize) && (y >= 0) && (y < myYSize)) 
    {
      return mySensorData[x][y];
    }
    else 
    {
      return NULL;
    }
  }
  /// Get the xyz array of the sensor at the (x,y) pizels.
  DataObject* getSensorXYZ(int x, int y)
  {
    if ((x >= 0) && (x < myXSize) && (y >= 0) && (y < myYSize)) 
    {
      return mySensorXYZ[x][y];
    }
    else 
    {
      return NULL;
    }
  }
  /// Get the dimension of the grid in the x direction
  int getXDimension(void) { return myXSize; }
  /// Get the dimension of the grid in the y direction
  int getYDimension(void) { return myYSize; }
  /// Get the X direction Field of View, in degrees
  double getXFOV(void) { return myXFOV; }
  /// Get the Y direction Field of View, in degrees
  double getYFOV(void) { return myYFOV; }
  /// Get the name of the device
  const char *getName(void) { return myName.c_str(); }
  /// Gets the raw sensor data
  DataObject ***getRawSensorData(void) { return mySensorData; }
  /// Gets the raw XYZ data
  DataObject ***getRawSensorXYZ(void) { return mySensorXYZ; }
  /// Lock this device
  AREXPORT virtual int lockDevice() { return(myDeviceMutex.lock()); }
  /// Try to lock this device
  AREXPORT virtual int tryLockDevice() { return(myDeviceMutex.tryLock()); }
  /// Unlock this device
  AREXPORT virtual int unlockDevice() { return(myDeviceMutex.unlock()); }
protected:
  std::string myName;
  int myXSize;
  int myYSize;
  double myXFOV;
  double myYFOV;
  DataObject ***mySensorData;
  DataObject ***mySensorXYZ;
  ArMutex myDeviceMutex;

  bool allocateSensorDataMemory()
  {    
    if ((myXSize < 1) || (myYSize < 1)) 
    {
      ArLog::log(ArLog::Normal, "Bad array size for ArPixelDevice %s", getName());
      return false;
    }
    mySensorData = new DataObject**[myXSize];
    if (mySensorData == NULL)
    {
      ArLog::log(ArLog::Normal, "Cannot allocate memory for ArPixelDevice %s", getName());
      return false;
    }
    for (int i = 0; i < myXSize; i++)
    {
      if ((mySensorData[i] = new DataObject*[myYSize]) == NULL)
      {
	ArLog::log(ArLog::Normal, "Cannot allocate memory for ArPixelDevice %s", getName());
	return false;
      }
      for (int j = 0; j < myYSize; j++) 
      {
	mySensorData[i][j] = new DataObject;
      }      
    }
    return true;
  }

  bool allocateSensorXYZMemory()
  {    
    if ((myXSize < 1) || (myYSize < 1)) 
    {
      ArLog::log(ArLog::Normal, "Bad array size for ArPixelDevice %s", getName());
      return false;
    }
    mySensorXYZ = new DataObject**[myXSize];
    if (mySensorXYZ == NULL)
    {
      ArLog::log(ArLog::Normal, "Cannot allocate memory for ArPixelDevice %s", getName());
      return false;
    }
    for (int i = 0; i < myXSize; i++)
    {
      if ((mySensorXYZ[i] = new DataObject*[myYSize]) == NULL)
      {
	ArLog::log(ArLog::Normal, "Cannot allocate memory for ArPixelDevice %s", getName());
	return false;
      }
      for (int j = 0; j < myYSize; j++) 
      {
	mySensorXYZ[i][j] = new DataObject[3];
      }      
    }
    return true;
  }

};

#endif // ARPIXELDEVICE_H