/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
 *                      gerkey@usc.edu    kaspers@robotics.usc.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
 *
 */
///////////////////////////////////////////////////////////////////////////
//
// Desc: Driver for detecting laser barcodes.
// Author: Andrew Howard
// Date: 29 Jan 2001
// CVS: $Id: laserbarcode.cc 7305 2009-01-27 01:18:55Z gbiggs $
//
// Theory of operation:
//  Will detect binary coded beacons (i.e. bar-codes) in laser data.
//  Reflectors represent '1' bits, non-reflectors represent '0' bits.
//  The first and last bits of the beacon must be '1'.
//
// Requires: laser
// Provides: fiducial
//
///////////////////////////////////////////////////////////////////////////

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_laserbarcode laserbarcode
 * @brief Laser barcode detector

@todo This driver has not been tested with the player 2.0 API

The laser barcode detector searches for specially constructed barcodes in
the laser range finder data.  An example laser barcode is shown below.
The barcode is constructed using strips of retro-reflective paper.
Each retro-reflective strip represents a `1' bit; each non-reflective
strip represents a `0' bit.  By default, the laserbarcode driver
searches for barcodes containing 8 bits, each of which is exactly 50mm
wide (the total barcode width is thus 400mm).  The first and last bits
are used as start and end markers, and the remaining bits are used to
determine the identity of the barcode; with an 8-bit barcode there are
64 unique IDs.  The number of bits and the width of each bit can be set
in the configuration file.

The range at which barcodes can be detected identified is dependent on the
bit width and the angular resolution of the laser.  With 50mm bits and an
angular resolution of 0.5 deg, barcodes can be detected and identified
at a range of about 2.5m.  With the laser resolution set to  0.25 deg,
this distance is roughly doubled to about 5m.

See also the @ref driver_laserbar and
@ref driver_laservisualbarcode drivers.

@image html beacon.jpg "A sample laser barcode.  This barcode has 8 bits, each of which is 50mm wide."

@par Compile-time dependencies

- none

@par Provides

- This driver provides detected target information through a @ref
  interface_fiducial device.

@par Requires

- This driver finds targets in scans from a @ref interface_laser
  device.

@par Configuration requests

- PLAYER_FIDUCIAL_REQ_GET_GEOM

@par Configuration file options

- bit_count (integer)
  - Default: 8
  - Number of bits in each barcode.
- bit_width (length)
  - Default: 0.05 m
  - Width of each bit.
- max_depth (length)
  - Default: 0.05 m
  - Maximum variance in the flatness of the beacon.
- accept_thresh (float)
  - Default: 1.0
  - Acceptance threshold
- zero_thresh (float)
  - Default: 0.6
  - Zero threshold
- one_thresh (float)
  - Default: 0.6
  - One threshold

@par Example

@verbatim
driver
(
  name "laserbarcode"
  requires ["laser:0"]
  provides ["fiducial:0"]
  bit_count 5
  bit_width 0.1
)
@endverbatim

@author Andrew Howard
*/
/** @} */

#define PLAYER_ENABLE_TRACE 0
#define PLAYER_ENABLE_MSG 0

#include <errno.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>  // for atoi(3)

#include <libplayercore/playercore.h>

// The laser barcode detector.
class LaserBarcode : public Driver
{
  // Constructor
  public: LaserBarcode( ConfigFile* cf, int section);

  // Setup/shutdown routines
  //
  public: virtual int Setup();
  public: virtual int Shutdown();

  // Process incoming messages from clients
  int ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void * data);

  // Main function for device thread
  //private: virtual void Main(void);

  // Get the laser data
  //private: int ReadLaser();

  // Analyze the laser data and return beacon data
  private: void FindBeacons(const player_laser_data_t *laser_data,
                            player_fiducial_data_t *beacon_data);

  // Analyze the candidate beacon and return its id (0 == none)
  private: int IdentBeacon(int a, int b, double ox, double oy, double oth,
                           const player_laser_data_t *laser_data);

  // Write fidicual data
  private: void WriteFiducial();

  // Pointer to laser to get data from
  private: player_devaddr_t laser_id;
  private: Device *laser;

  // Magic numbers
  private: int bit_count;
  private: double bit_width;
  private: double max_depth;
  private: double accept_thresh, zero_thresh, one_thresh;

  // Current laser data
  private: player_laser_data_t laser_data;
  private: struct timeval laser_timestamp;

  // Current fiducial data
  private: player_fiducial_data_t data;
  unsigned int fdata_allocated;
};


// Initialization function
Driver* LaserBarcode_Init( ConfigFile* cf, int section)
{
  return((Driver*)(new LaserBarcode( cf, section)));
}

// a driver registration function
void laserbarcode_Register(DriverTable* table)
{
  table->AddDriver("laserbarcode", LaserBarcode_Init);
}


////////////////////////////////////////////////////////////////////////////////
// Constructor
LaserBarcode::LaserBarcode( ConfigFile* cf, int section)
  : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_FIDUCIAL_CODE)
{
  // Must have an input laser
  if (cf->ReadDeviceAddr(&this->laser_id, section, "requires",
                       PLAYER_LASER_CODE, -1, NULL) != 0)
  {
    this->SetError(-1);
    return;
  }

  // Get beacon settings.
  this->bit_count = cf->ReadInt(section, "bit_count", 8);
  this->bit_width = cf->ReadLength(section, "bit_width", 0.05);

  // Maximum variance in the flatness of the beacon
  this->max_depth = cf->ReadLength(section, "max_depth", 0.05);

  // Default thresholds
  this->accept_thresh = cf->ReadFloat(section, "accept_thresh", 1.0);
  this->zero_thresh = cf->ReadFloat(section, "zero_thresh", 0.60);
  this->one_thresh = cf->ReadFloat(section, "one_thresh", 0.60);

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the device
int LaserBarcode::Setup()
{
  fdata_allocated = 0;
  data.fiducials = NULL;

  // Subscribe to the laser.
  if (Device::MatchDeviceAddress (laser_id, device_addr))
  {
    PLAYER_ERROR ("attempt to subscribe to self");
    return -1;
  }
  if (!(laser = deviceTable->GetDevice (laser_id)))
  {
    PLAYER_ERROR ("unable to locate suitable camera device");
    return -1;
  }
  if (laser->Subscribe (InQueue) != 0)
  {
    PLAYER_ERROR ("unable to subscribe to camera device");
    return -1;
  }


  PLAYER_MSG2(2, "laserbarcode device: bitcount [%d] bitwidth [%fm]",
              this->bit_count, this->bit_width);
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
int LaserBarcode::Shutdown()
{

  // Unsubscribe from devices.
  laser->Unsubscribe(InQueue);

  free(data.fiducials);

  PLAYER_MSG0(2, "laserbarcode device: shutdown");
  return 0;
}



////////////////////////////////////////////////////////////////////////////////
// Process an incoming message
int LaserBarcode::ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void * data)
{
  assert(hdr);

  if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA, PLAYER_LASER_DATA_SCAN, laser_id))
  {
    laser_data = *reinterpret_cast<player_laser_data_t * > (data);

    // Analyse the laser data
    this->FindBeacons(&this->laser_data, &this->data);

    // Write out the fiducials
    this->WriteFiducial();

    return 0;
  }

  return -1;
}

////////////////////////////////////////////////////////////////////////////////
// Analyze the laser data and return beacon data
void LaserBarcode::FindBeacons(const player_laser_data_t *laser_data,
                               player_fiducial_data_t *data)
{
  data->fiducials_count = 0;

  int ai = -1;
  int bi = -1;
  double ax, ay;
  double bx, by;

  // Expected width of beacon
  //
  double min_width = (this->bit_count - 1) * this->bit_width;
  double max_width = (this->bit_count + 1) * this->bit_width;

  ax = ay = 0;
  bx = by = 0;

  // Find the beacons in this scan
  for (unsigned int i = 0; i < laser_data->ranges_count; i++)
  {
    double range = (laser_data->ranges[i]);
    double bearing = (laser_data->min_angle + i * laser_data->resolution);
    int intensity = (laser_data->intensity[i]);

    double px = range * cos(bearing);
    double py = range * sin(bearing);

    if (intensity > 0)
    {
      if (ai < 0)
      {
        ai = i;
        ax = px;
        ay = py;
      }
      bi = i;
      bx = px;
      by = py;
    }
    if (ai < 0)
      continue;

    double width = sqrt((px - ax) * (px - ax) + (py - ay) * (py - ay));
    if (width < max_width)
      continue;

    width = sqrt((bx - ax) * (bx - ax) + (by - ay) * (by - ay));
    if (width < min_width)
      continue;
    if (width > max_width)
    {
      ai = -1;
      continue;
    }

    // Assign an id to the beacon
    double orient = atan2(by - ay, bx - ax);
    int id = IdentBeacon(ai, bi, ax, ay, orient, laser_data);

    // Reset counters so we can find new beacons
    //
    ai = -1;
    bi = -1;

    // Ignore invalid ids
    //
    if (id < 0)
      continue;

    if (this->data.fiducials_count+1 > this->fdata_allocated)
    {
      this->fdata_allocated = this->data.fiducials_count+1;
      this->data.fiducials = (player_fiducial_item_t*)realloc(this->data.fiducials, sizeof(this->data.fiducials[0])*this->fdata_allocated);
    }

    double ox = (bx + ax) / 2;
    double oy = (by + ay) / 2;
    range = sqrt(ox * ox + oy * oy);
    bearing = atan2(oy, ox);

    // Create an entry for this beacon.
    // Note that we return the surface normal for the beacon orientation.
    data->fiducials[data->fiducials_count].id = (id > 0 ? id : -1);
    data->fiducials[data->fiducials_count].pose.px = range * cos(bearing);
    data->fiducials[data->fiducials_count].pose.py = range * sin(bearing);
    data->fiducials[data->fiducials_count].pose.pyaw = NORMALIZE(orient + M_PI/2);
    data->fiducials_count++;
  }
}


////////////////////////////////////////////////////////////////////////////////
// Analyze the candidate beacon and return its id.
// Will return -1 if this is not a beacon.
// Will return  0 if this is a beacon, but it cannot be identified.
// Will return beacon id otherwise.
int LaserBarcode::IdentBeacon(int a, int b, double ox, double oy, double oth,
                              const player_laser_data_t *laser_data)
{
  // Compute pose of laser relative to beacon
  double lx = -ox * cos(-oth) + oy * sin(-oth);
  double ly = -ox * sin(-oth) - oy * cos(-oth);
  double la = -oth;

  // Initialise our probability distribution.
  // We determine the probability that each bit is set using Bayes law.
  double prob[8][2];
  for (int bit = 0; bit < ARRAYSIZE(prob); bit++)
    prob[bit][0] = prob[bit][1] = 0;

  // Scan through the readings that make up the candidate.
  for (int i = a; i <= b; i++)
  {
    double range = laser_data->ranges[i];
    double bearing = laser_data->min_angle + i * laser_data->resolution;
    int intensity = (int) (laser_data->intensity[i]);
    double res = laser_data->resolution;

    // Compute point relative to beacon
    double py = ly + range * sin(la + bearing);

    // Discard candidate points are not close to x-axis (ie candidate
    // is not flat).
    if (fabs(py) > this->max_depth)
      return -1;

    // Compute intercept with beacon
    //double cx = lx + ly * tan(la + bearing + M_PI/2);
    double ax = lx + ly * tan(la + bearing - res/2 + M_PI/2);
    double bx = lx + ly * tan(la + bearing + res/2 + M_PI/2);

    // Update our probability distribution
    // Use Bayes law
    for (int bit = 0; bit < this->bit_count; bit++)
    {
      // Use a rectangular distribution
      double a = (bit + 0.0) * this->bit_width;
      double b = (bit + 1.0) * this->bit_width;

      double p = 0;
      if (ax < a && bx > b)
        p = 1;
      else if (ax > a && bx < b)
        p = 1;
      else if (ax > b || bx < a)
        p = 0;
      else if (ax < a && bx < b)
        p = 1 - (a - ax) / (bx - ax);
      else if (ax > a && bx > b)
        p = 1 - (bx - b) / (bx - ax);
      else
        assert(0);

      //printf("prob : %f %f %f %f %f\n", ax, bx, a, b, p);

      if (intensity == 0)
      {
        assert(bit >= 0 && bit < ARRAYSIZE(prob));
        prob[bit][0] += p;
        prob[bit][1] += 0;
      }
      else
      {
        assert(bit >= 0 && bit < ARRAYSIZE(prob));
        prob[bit][0] += 0;
        prob[bit][1] += p;
      }
    }
  }

  // Now assign the id
  int id = 0;
  for (int bit = 0; bit < this->bit_count; bit++)
  {
    double pn = prob[bit][0] + prob[bit][1];
    double p0 = prob[bit][0] / pn;
    double p1 = prob[bit][1] / pn;

    if (pn < this->accept_thresh)
      id = -1;
    else if (p0 > this->zero_thresh)
      id |= (0 << bit);
    else if (p1 > this->one_thresh)
      id |= (1 << bit);
    else
      id = -1;

    //printf("%d %f %f : %f %f %f\n", bit, prob[bit][0], prob[bit][1], p0, p1, pn);
  }
  //printf("\n");

  if (id < 0)
    id = 0;

  return id;
}


////////////////////////////////////////////////////////////////////////////////
// Write fidicual data
void LaserBarcode::WriteFiducial()
{
  // Write the data with the laser timestamp
  this->Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_FIDUCIAL_DATA_SCAN, &this->data);

  return;
}

