/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2006 Radu Bogdan Rusu (rusu@cs.tum.edu)
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/*
 * $Id: sr3000.cc 7297 2009-01-24 23:14:21Z thjc $
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_sr3000 sr3000
 * @brief SR3000

The sr3000 driver controls the Swiss Ranger SR3000 camera. A broad range of
camera option parameters are supported, via the libusbSR library. The driver
provides a @ref interface_pointcloud3d interface and two @ref interface_camera
interfaces for both distance and intensity images, *or* a @ref interface_stereo
interface.

This version of the driver works with libusbSR v1.0.10+

@par Compile-time dependencies

- none

@par Provides

- @ref interface_pointcloud3d : the 3d point cloud generated by the SR3000
- @ref interface_camera : snapshot images (both distance and intensity) taken by
                          the SR3000
- @ref interface_stereo : intensity and distance images as left and right
                          channels, and the 3d point cloud generated by the SR3000

@par Requires

- none

@par Supported configuration requests

  - none

@par Properties provided

  - auto_illumination (integer): Set to 1 to turn auto illumination on.
  - integration_time (integer): Integration time.
  - modulation_freq (integer): This device employs the following values:
                                40MHz  -> 3.75 m,
                                30MHz  -> 5.0  m,
                                21MHz  -> 7.1  m,
                                20MHz  -> 7.5  m,
                                19MHz  -> 7.9  m,
                                10MHz  -> 15.0 m,
                                6.6MHz -> 22.5 m,
                                5MHz   -> 30.0 m
  - amp_threshold (integer): Amplification threshold.

@par Configuration file options

  - none

@par Example

@verbatim
driver
(
  name "sr3000"

  provides ["pointcloud3d:0" "distance:::camera:0" "intensity:::camera:1"]

  # OR ...

  provides ["stereo:0"]
)
@endverbatim

@author Radu Bogdan Rusu
 */
/** @} */

#include <stdlib.h>
#include <libplayercore/playercore.h>
#include <libusbSR.h>

// Older library: #define MODE (AM_COR_FIX_PTRN | AM_COR_LED_NON_LIN | AM_MEDIAN)
//#define MODE (AM_MEDIAN | AM_COR_FIX_PTRN | AM_CONV_GRAY | AM_SW_ANF | AM_SR3K_2TAP_PROC | AM_MEDIANCROSS)
#define MODE (AM_MEDIAN | AM_COR_FIX_PTRN | AM_SW_ANF | AM_SR3K_2TAP_PROC | AM_MEDIANCROSS)
#define CAM_ROWS 144
#define CAM_COLS 176

class SR3000:public ThreadedDriver
{
  public:
	// constructor
    SR3000 (ConfigFile* cf, int section);
    ~SR3000 ();

    int MainSetup ();
    void MainQuit ();

    // MessageHandler
    virtual int ProcessMessage (QueuePointer &resp_queue,
                                player_msghdr * hdr,
                                void * data);

  private:

    // Camera MessageHandler
    int ProcessMessageCamera (QueuePointer &resp_queue,
                              player_msghdr * hdr,
                              void * data,
                              player_devaddr_t);
    virtual void Main ();
    void RefreshData  ();

    // device identifier
    CMesaDevice* srCam;

    // SR3000 specific values
    unsigned int rows, cols, inr;

    ImgEntry* imgEntryArray;
    float *buffer, *xp, *yp, *zp;

    // device bookkeeping
    player_devaddr_t stereo_addr, pcloud_addr, d_cam_addr, i_cam_addr;

    player_pointcloud3d_data_t pcloud_data;
    player_camera_data_t       d_cam_data, i_cam_data;
    player_stereo_data_t       stereo_data;

  protected:
    // Properties
    IntProperty auto_illumination, integration_time, modulation_freq, amp_threshold;

    bool providePCloud, provideDCam, provideICam, provideStereo;
};

////////////////////////////////////////////////////////////////////////////////
//Factory creation function. This functions is given as an argument when
// the driver is added to the driver table
Driver*
    SR3000_Init (ConfigFile* cf, int section)
{
  return ((Driver*)(new SR3000 (cf, section)));
}

////////////////////////////////////////////////////////////////////////////////
//Registers the driver in the driver table. Called from the
// player_driver_init function that the loader looks for
void
    sr3000_Register (DriverTable* table)
{
  table->AddDriver ("sr3000", SR3000_Init);
}

////////////////////////////////////////////////////////////////////////////////
// Constructor.  Retrieve options from the configuration file and do any
// pre-Setup() setup.
SR3000::SR3000 (ConfigFile* cf, int section)
	: ThreadedDriver (cf, section),
	auto_illumination ("auto_illumination", 0, 0),
	integration_time ("integration_time", 0, 0),
	modulation_freq ("modulation_freq", 0, 0),
	amp_threshold ("amp_threshold", 0, 0)
{
  memset (&this->stereo_addr, 0, sizeof (player_devaddr_t));
  memset (&this->pcloud_addr, 0, sizeof (player_devaddr_t));
  memset (&this->d_cam_addr,  0, sizeof (player_devaddr_t));
  memset (&this->i_cam_addr,  0, sizeof (player_devaddr_t));

  this->RegisterProperty ("auto_illumination", &this->auto_illumination, cf, section);
  this->RegisterProperty ("integration_time", &this->integration_time, cf, section);
  this->RegisterProperty ("modulation_freq", &this->modulation_freq, cf, section);
  this->RegisterProperty ("amp_threshold", &this->amp_threshold, cf, section);

  providePCloud = FALSE; provideDCam = FALSE; provideICam = FALSE; provideStereo = FALSE;

  if (cf->ReadDeviceAddr (&(this->stereo_addr), section, "provides",
      PLAYER_STEREO_CODE, -1, NULL) == 0)
  {
    if (this->AddInterface (this->stereo_addr) != 0)
    {
      this->SetError (-1);
      return;
    }
    provideStereo = TRUE;
  }
  else
  {
    // Outgoing pointcloud interface
    if (cf->ReadDeviceAddr (&(this->pcloud_addr), section, "provides",
        PLAYER_POINTCLOUD3D_CODE, -1, NULL) == 0)
    {
      if (this->AddInterface (this->pcloud_addr) != 0)
      {
        this->SetError (-1);
        return;
      }
      providePCloud = TRUE;
    }

    // Outgoing distance::camera:0 interface
    if (cf->ReadDeviceAddr (&(this->d_cam_addr), section, "provides",
        PLAYER_CAMERA_CODE, -1, "distance") == 0)
    {
      if (this->AddInterface (this->d_cam_addr) != 0)
      {
        this->SetError (-1);
        return;
      }
      provideDCam = TRUE;
    }

    // Outgoing intensity::camera:1 interface
    if (cf->ReadDeviceAddr (&(this->i_cam_addr), section, "provides",
        PLAYER_CAMERA_CODE, -1, "intensity") == 0)
    {
      if (this->AddInterface (this->i_cam_addr) != 0)
      {
        this->SetError (-1);
        return;
      }
      provideICam = TRUE;
    }

    provideStereo = FALSE;
  }
}

////////////////////////////////////////////////////////////////////////////////
// Destructor.
SR3000::~SR3000 ()
{
}

////////////////////////////////////////////////////////////////////////////////
// Set up the device.  Return 0 if things go well, and -1 otherwise.
int
    SR3000::MainSetup ()
{
  int res;
  // ---[ Open the camera ]---
  res = SR_OpenUSB (&srCam, 0);          //returns the device ID used in other calls

  PLAYER_MSG0 (1, "> Connecting to SR3000... [done]");

  // ---[ Get the number of rows, cols, ... ]---
  rows = SR_GetRows (srCam);
  cols = SR_GetCols (srCam);
  inr  = SR_GetImageList (srCam, &imgEntryArray);
  modulation_freq  = SR_GetModulationFrequency (srCam);
  integration_time = SR_GetIntegrationTime (srCam);
  PLAYER_MSG3 (2, ">> Expecting %dx%dx%d", cols, rows, inr);

  if ( (cols != CAM_COLS) || (rows != CAM_ROWS) || (inr < 1) || (imgEntryArray == 0) )
  {
    PLAYER_ERROR ("> Error while connecting to camera!");
    SR_Close (srCam);
    return (-1);
  }

  // ---[ Set the acquisition mode ]---
  SR_SetMode (srCam, MODE);

  // Points array
  size_t buffer_size = rows * cols * 3 * sizeof (float);
  buffer = (float*)malloc (buffer_size);
  memset (buffer, 0xaf, buffer_size);

  xp = buffer;
  yp = &xp[rows*cols];
  zp = &yp[rows*cols];

  return (0);
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
void
    SR3000::MainQuit ()
{
  StopThread ();

  // ---[ Close the camera ]---
  int res = SR_Close (srCam);

  PLAYER_MSG1 (1, "> SR3000 driver shutting down... %d [done]", res);

  // ---[ Free the allocated memory buffer ]---
  free (buffer);

  return (0);
}

////////////////////////////////////////////////////////////////////////////////
// Process messages from/for a camera interface
int
    SR3000::ProcessMessageCamera (QueuePointer &resp_queue,
                                  player_msghdr * hdr,
                                  void * data,
                                  player_devaddr_t stereo_addr)
{
  int res;

  // Check for properties
  if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_SET_INTPROP_REQ, stereo_addr))
  {
    player_intprop_req_t req = *reinterpret_cast<player_intprop_req_t*> (data);
    if (auto_illumination.KeyIsEqual (req.key))
    {
      // ---[ Set Autoillumination
      if (req.value == 1)
        res = SR_SetAutoIllumination (srCam, 5, 255, 10, 45);
      else
        res = SR_SetAutoIllumination (srCam, 255, 0, 0, 0);

      // Check the error code
      if (res == 0)
      {
        auto_illumination = req.value;
        Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
      }
      else
      {
        Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
      }
      return (0);
    }
    else if (integration_time.KeyIsEqual (req.key))
    {
      // ---[ Set integration time
      res = SR_SetIntegrationTime (srCam, req.value);

      // Check the error code
      if (res == 0)
      {
        integration_time = req.value;
        Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
      }
      else
      {
        Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
      }
      return (0);
    }
    else if (modulation_freq.KeyIsEqual (req.key))
    {
      // ---[ Set modulation frequency
      res = SR_SetModulationFrequency (srCam, (ModulationFrq)req.value);

      // Check the error code
      if (res == 0)
      {
        modulation_freq = req.value;
        Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
      }
      else
      {
        Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
      }
      return (0);
    }
    else if (amp_threshold.KeyIsEqual (req.key))
    {
      // ---[ Set amplitude threshold
      res = SR_SetAmplitudeThreshold (srCam, req.value);

      // Check the error code
      if (res == 0)
      {
        amp_threshold = req.value;
        Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
      }
      else
      {
        Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
      }
      return (0);
    }
    return (-1);    // Let the default property handling handle it
  }
  else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_GET_INTPROP_REQ, device_addr))
  {
    player_intprop_req_t req = *reinterpret_cast<player_intprop_req_t*> (data);
    if (modulation_freq.KeyIsEqual (req.key))
    {
      // ---[ Get modulation frequency
      modulation_freq.SetValue (SR_GetModulationFrequency (srCam));
      modulation_freq.GetValueToMessage (reinterpret_cast<void*> (&req));
      Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_GET_INTPROP_REQ, reinterpret_cast<void*> (&req), sizeof(player_intprop_req_t), NULL);
      return (0);
    }
    else if (integration_time.KeyIsEqual (req.key))
    {
      // ---[ Get integration time
      integration_time.SetValue (SR_GetIntegrationTime (srCam));
      integration_time.GetValueToMessage (reinterpret_cast<void*> (&req));
      Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_GET_INTPROP_REQ, reinterpret_cast<void*> (&req), sizeof(player_intprop_req_t), NULL);
      return (0);
    }
    return (-1);    // Let the default property handling handle it
  }

  return (0);
}

////////////////////////////////////////////////////////////////////////////////
// ProcessMessage
int SR3000::ProcessMessage (QueuePointer &resp_queue,
                            player_msghdr * hdr,
                            void * data)
{
  assert (hdr);

  if (provideStereo)
    ProcessMessageCamera (resp_queue, hdr, data, stereo_addr);
  else
  {
    ProcessMessageCamera (resp_queue, hdr, data, d_cam_addr);
    ProcessMessageCamera (resp_queue, hdr, data, i_cam_addr);
  }

  return (0);
}



////////////////////////////////////////////////////////////////////////////////
void SR3000::Main ()
{
  timespec sleepTime = {0, 10000};

  memset (&stereo_data, 0, sizeof (stereo_data));
  memset (&pcloud_data, 0, sizeof (pcloud_data));
  memset (&d_cam_data,  0, sizeof (d_cam_data ));
  memset (&i_cam_data,  0, sizeof (i_cam_data ));

  for (;;)
  {
    // handle commands and requests/replies --------------------------------
    pthread_testcancel ();
    ProcessMessages ();

    // get data ------------------------------------------------------------
    if ((rows != 1) && (cols != 1))
      RefreshData ();
    nanosleep (&sleepTime, NULL);
  }
}

////////////////////////////////////////////////////////////////////////////////
// RefreshData function
void SR3000::RefreshData ()
{
  int res;
  unsigned int i;

  res = SR_Acquire (srCam);

  uint8_t *distance_image  = (unsigned char*)imgEntryArray->data;
  uint8_t *intensity_image = (unsigned char*)imgEntryArray->data + (imgEntryArray->width * imgEntryArray->height * 2);
//  buffer_size/2;

  // Points array
  res = SR_CoordTrfFlt (srCam, xp, yp, zp, sizeof (float), sizeof (float), sizeof (float));

  if (provideStereo)
  {
    stereo_data.points_count = rows * cols;
    stereo_data.points = new player_pointcloud3d_stereo_element_t[stereo_data.points_count];
    for (i = 0; i < rows*cols; i++)
    {
      player_pointcloud3d_stereo_element_t element;
      element.px = xp[i];
      element.py = yp[i];
      element.pz = zp[i];

      element.red   = intensity_image[i*2 + 1];
      element.green = intensity_image[i*2 + 1];
      element.blue  = intensity_image[i*2 + 1];
      stereo_data.points[i] = element;
    }

    // Prepare distance camera data
    d_cam_data.width       = cols;
    d_cam_data.height      = rows;
    d_cam_data.bpp         = 16;
    d_cam_data.format      = PLAYER_CAMERA_FORMAT_MONO16;
    d_cam_data.fdiv        = 1;
    d_cam_data.compression = PLAYER_CAMERA_COMPRESS_RAW;
    d_cam_data.image_count = rows*cols*2;
    d_cam_data.image       = distance_image;

    stereo_data.left_channel = d_cam_data;

    // Prepare intensity camera data
    i_cam_data.width       = cols;
    i_cam_data.height      = rows;
    i_cam_data.bpp         = 16;
    i_cam_data.format      = PLAYER_CAMERA_FORMAT_MONO16;
    i_cam_data.fdiv        = 1;
    i_cam_data.compression = PLAYER_CAMERA_COMPRESS_RAW;
    i_cam_data.image_count = rows*cols*2;
    i_cam_data.image       = intensity_image;

    stereo_data.right_channel = i_cam_data;

    // Publish the stereo data
    //  int total_size = 28+(i_cam_data.image_count) +
    //                   28+(d_cam_data.image_count) +
    //                   4 + pcloud_data.points_count*sizeof (player_pointcloud3d_element_t);

    Publish (stereo_addr, PLAYER_MSGTYPE_DATA, PLAYER_STEREO_DATA_STATE, &stereo_data);  //, total_size, NULL);

    delete [] stereo_data.points;
  }
  else
  {
    // Publish pointcloud3d data if subscribed
    if (providePCloud)
    {
      pcloud_data.points_count = rows * cols;
      pcloud_data.points = new player_pointcloud3d_element_t[pcloud_data.points_count];
      for (i = 0; i < rows*cols; i++)
      {
        player_pointcloud3d_element_t element;
        element.point.px = xp[i];
        element.point.py = yp[i];
        element.point.pz = zp[i];

        element.color.alpha = 255;
        element.color.red   = intensity_image[i*2 + 1];
        element.color.green = intensity_image[i*2 + 1];
        element.color.blue  = intensity_image[i*2 + 1];
        pcloud_data.points[i] = element;
      }

      // Write the Pointcloud3d data
      Publish (pcloud_addr, PLAYER_MSGTYPE_DATA, PLAYER_POINTCLOUD3D_DATA_STATE,
               &pcloud_data); //, 4 + pcloud_data.points_count*sizeof (player_pointcloud3d_element_t), NULL);

      delete [] pcloud_data.points;
    }

    // Publish distance camera data if subscribed
    if (provideDCam)
    {
      d_cam_data.width       = cols;
      d_cam_data.height      = rows;
      d_cam_data.bpp         = 16;
      d_cam_data.format      = PLAYER_CAMERA_FORMAT_MONO16;
      d_cam_data.fdiv        = 1;
      d_cam_data.compression = PLAYER_CAMERA_COMPRESS_RAW;
      d_cam_data.image_count = rows*cols*2;
      d_cam_data.image       = distance_image;

      // Write the distance camera data
      Publish (d_cam_addr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE,
               &d_cam_data); //, 28+(d_cam_data.image_count), NULL);
    }

    // Publish intensity camera data if subscribed
    if (provideICam)
    {
      i_cam_data.width       = cols;
      i_cam_data.height      = rows;
      i_cam_data.bpp         = 16;
      i_cam_data.format      = PLAYER_CAMERA_FORMAT_MONO16;
      i_cam_data.fdiv        = 1;
      i_cam_data.compression = PLAYER_CAMERA_COMPRESS_RAW;
      i_cam_data.image_count = rows*cols*2;
      i_cam_data.image       = intensity_image;

      // Write the intensity camera data
      Publish (i_cam_addr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE,
               &i_cam_data); //, 28+(i_cam_data.image_count), NULL);
    }
  } // if (Stereo) ... else

  return;
}
