/*
 *  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 extracting line/corner features from a laser scan.
// Author: Andrew Howard
// Date: 29 Aug 2002
// CVS: $Id: laserfeature.cc 4126 2007-08-20 06:37:30Z thjc $
//
// Theory of operation - Uses an EKF to segment the scan into line
// segments, the does a best-fit for each segment.  The EKF is based
// on the approach by Stergios Romelioutis.
//
// Requires - Laser device.
//
///////////////////////////////////////////////////////////////////////////

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_laserfeature laserfeature
 * @brief Extract line/corner features from a laser scan.
 
@todo This driver is currently disabled because it needs to be updated to
the Player 2.0 API.

@todo Document this driver.

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

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

#include <libplayercore/playercore.h>

// Driver for detecting features in laser scan.
class LaserFeature : public Driver
{
  // Constructor
  public: LaserFeature( ConfigFile* cf, int section);

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

  // Client interface (this device has no thread).
  public: virtual size_t GetData(void* client,unsigned char *dest, 
                                 size_t maxsize, uint32_t* timestamp_sec, 
                                 uint32_t* timestamp_usec);

  // Client interface (this device has no thread).
  public: virtual int PutConfig(player_device_id_t* device, void *client, 
                                void *data, size_t len);

  // Process laser data.  Returns non-zero if the laser data has been
  // updated.
  private: int UpdateLaser();

  // Write laser data to a file (testing).
  private: int WriteLaser();

  // Read laser data from a file (testing).
  private: int ReadLaser();

  // Segment the scan into straight-line segments.
  private: void SegmentLaser();

  // Update the line filter.  Returns an error signal.
  private: double UpdateFilter(double x[2], double P[2][2], double Q[2][2],
                               double R, double z, double res);

  // Fit lines to the segments.
  private: void FitSegments();

  // Merge overlapping segments with similar properties.
  private: void MergeSegments();

  // Update the device data (the data going back to the client).
  private: void UpdateData();

  // Process requests.  Returns 1 if the configuration has changed.
  private: int HandleRequests();

  // Handle geometry requests.
  private: void HandleGetGeom(void *client, void *req, int reqlen);

  // Device pose relative to robot.
  private: double pose[3];
  
  // Laser stuff.
  private: int laser_index;
  private: Driver *laser_device;
  private: player_laser_data_t laser_data;
  private: uint32_t laser_timesec, laser_timeusec;

  // Line filter settings.
  private: double model_range_noise;
  private: double model_angle_noise;
  private: double sensor_range_noise;

  // Threshold for segmentation
  private: double segment_range;

  // Thresholds for merging.
  private: double merge_angle;

  // Thresholds for discarding.
  private: double discard_length;

  // Segment mask.
  private: int mask[PLAYER_LASER_MAX_SAMPLES];

  // Description for each extracted line segment.
  private: struct segment_t
  {
    int first, last, count;
    double pose[3];
    double length;
  };

  // List of extracted line segments.
  private: int segment_count;
  private: segment_t segments[PLAYER_LASER_MAX_SAMPLES];

  // TESTING
  private: FILE *laser_file;

  // Fiducila stuff (the data we generate).
  private: player_fiducial_data_t data;
  private: uint32_t timesec, timeusec;
};


// Initialization function
Driver* LaserFeature_Init( ConfigFile* cf, int section)
{
  if (strcmp( PLAYER_FIDUCIAL_STRING) != 0)
  {
    PLAYER_ERROR1("driver \"laserfeature\" does not support interface \"%s\"\n",
                  interface);
    return (NULL);
  }
  return ((Driver*) (new LaserFeature( cf, section)));
}


// a driver registration function
void LaserFeature_Register(DriverTable* table)
{
  table->AddDriver("laserfeature", LaserFeature_Init);
}


////////////////////////////////////////////////////////////////////////////////
// Constructor
LaserFeature::LaserFeature( ConfigFile* cf, int section)
    : Driver(cf, section, 0, 0, 0, 1)
{
  // Device pose relative to robot.
  this->pose[0] = 0;
  this->pose[1] = 0;
  this->pose[2] = 0;

  // If laser_index is not overridden by an argument here, then we'll
  // use the device's own index, which we can get in Setup() below.
  this->laser_index = cf->ReadInt(section, "laser", -1);
  this->laser_driver = NULL;
  this->laser_timesec = 0;
  this->laser_timeusec = 0;

  // Line filter settings.
  this->model_range_noise = cf->ReadLength(section, "model_range_noise", 0.02);
  this->model_angle_noise = cf->ReadAngle(section, "model_angle_noise", 10 * M_PI / 180);
  this->sensor_range_noise = cf->ReadLength(section, "sensor_range_noise", 0.05);

  // Segmentation settings
  this->segment_range = cf->ReadLength(section, "segment_range", 0.05);

  // Segment merging settings.
  this->merge_angle = cf->ReadAngle(section, "merge_angle", 10 * M_PI / 180);

  // Post-processing
  this->discard_length = cf->ReadLength(section, "discard_length", 1.00);

  // Initialise segment list.
  this->segment_count = 0;
  
  // Fiducial data.
  this->timesec = 0;
  this->timeusec = 0;
  memset(&this->data, 0, sizeof(this->data));

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the device (called by server thread).
int LaserFeature::Setup()
{
  /*
  player_device_id_t id;

  // Subscribe to the laser.
  id.code = PLAYER_LASER_CODE;
  id.index = (this->laser_index >= 0 ? this->laser_index : this->device_id.index);
  id.port = this->device_id.port;
  this->laser_driver = deviceTable->GetDriver(id);
  if (!this->laser_device)
  {
    PLAYER_ERROR("unable to locate suitable laser device");
    return(-1);
  }
  if (this->laser_device->Subscribe(this) != 0)
  {
    PLAYER_ERROR("unable to subscribe to laser device");
    return(-1);
  }
  */

  // Get the laser geometry.
  // TODO: no support for this at the moment.
  //this->pose[0] = 0.10;
  //this->pose[1] = 0;
  //this->pose[2] = 0;

  // TESTING
  this->laser_file = fopen("laser02.log", "r");
  assert(this->laser_file);
  
  while (this->ReadLaser() == 0)
    this->SegmentLaser();

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device (called by server thread).
int LaserFeature::Shutdown()
{
  // Unsubscribe from devices.
  this->laser_device->Unsubscribe(this);

  // TESTING
  fclose(this->laser_file);
  
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Get data from buffer (called by client thread)
size_t LaserFeature::GetData(void* client,unsigned char *dest, size_t maxsize,
                             uint32_t* timesec, uint32_t* timeusec)
{
  // Get the current laser data.
  this->laser_device->GetData((uint8_t*) &this->laser_data, sizeof(this->laser_data),
                              &this->laser_timesec, &this->laser_timeusec);
  
  // If there is new laser data, update our data.  Otherwise, we will
  // just reuse the existing data.
  if (this->laser_timesec != this->timesec || this->laser_timeusec != this->timeusec)
  {
    this->UpdateLaser();
    this->UpdateData();
  }
  
  // Copy results
  assert(maxsize >= sizeof(this->data));
  memcpy(dest, &this->data, sizeof(this->data));

  // Copy the laser timestamp
  this->timesec = this->laser_timesec;
  this->timeusec = this->laser_timeusec;
  *timesec = this->timesec;
  *timeusec = this->timeusec;

  return (sizeof(this->data));
}


////////////////////////////////////////////////////////////////////////////////
// Process laser data.
int LaserFeature::UpdateLaser()
{
  int i;
  
  // Do some byte swapping on the laser data.
  this->laser_data.resolution = ntohs(this->laser_data.resolution);
  this->laser_data.min_angle = ntohs(this->laser_data.min_angle);
  this->laser_data.max_angle = ntohs(this->laser_data.max_angle);
  this->laser_data.range_count = ntohs(this->laser_data.range_count);
  for (i = 0; i < this->laser_data.range_count; i++)
    this->laser_data.ranges[i] = ntohs(this->laser_data.ranges[i]);

  // TESTING
  //this->WriteLaser();
    
  // Segment the scan into straight-line segments.
  this->SegmentLaser();

  // Fit lines to the segments.
  this->FitSegments();

  // Merge similar segments.
  this->MergeSegments();

  // Re-do the fit for the merged segments.
  this->FitSegments();

  return 1;
}


////////////////////////////////////////////////////////////////////////////////
// Write laser data to a file (testing).
int LaserFeature::WriteLaser()
{
  int i;
  
  fprintf(this->laser_file, "%d %d %d %d ",
          this->laser_data.resolution,
          this->laser_data.min_angle,
          this->laser_data.max_angle,
          this->laser_data.range_count);
    
  for (i = 0; i < this->laser_data.range_count; i++)
    fprintf(this->laser_file, "%d ", this->laser_data.ranges[i]);

  fprintf(this->laser_file, "\n");
  
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Read laser data from a file (testing).
int LaserFeature::ReadLaser()
{
  int i, n;

  n = fscanf(this->laser_file, "%hd %hd %hd %hd ",
         &this->laser_data.resolution,
         &this->laser_data.min_angle,
         &this->laser_data.max_angle,
         &this->laser_data.range_count);
  if (n < 0 || n == EOF)
    return -1;
    
  for (i = 0; i < this->laser_data.range_count; i++)
    fscanf(this->laser_file, "%hd ", &this->laser_data.ranges[i]);

  fscanf(this->laser_file, "\n");

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Segment the scan into straight-line segments.
void LaserFeature::SegmentLaser()
{
  int i;
  double r, b;
  double res;
  double x[2], P[2][2];
  double Q[2][2], R;
  double err;
  int mask;
  
  // Angle between successive laser readings.
  res = (double) (this->laser_data.resolution) / 100.0 * M_PI / 180;

  // System noise.
  Q[0][0] = this->model_range_noise * this->model_range_noise;
  Q[0][1] = 0;
  Q[1][0] = 0;
  Q[1][1] = this->model_angle_noise * this->model_angle_noise;

  // Sensor noise.
  R = this->sensor_range_noise * this->sensor_range_noise;

  // Initial estimate and covariance.
  x[0] = 1.0;
  x[1] = M_PI / 2;
  P[0][0] = 100;
  P[0][1] = 0.0;
  P[1][0] = 0.0;
  P[1][1] = 100;

  // Initialise the segments.
  this->segment_count = 0;
  
  // Apply filter anti-clockwise.
  mask = 0;
  for (i = 0; i < this->laser_data.range_count; i++)
  {
    r = (double) (this->laser_data.ranges[i]) / 1000;
    b = (double) (this->laser_data.min_angle) / 100.0 * M_PI / 180 + i * res;
    
    err = this->UpdateFilter(x, P, Q, R, r, res);

    if (err < this->segment_range)
    {
      if (mask == 0)
        this->segments[this->segment_count++].first = i;        
      this->segments[this->segment_count - 1].last = i;
      mask = 1;
    }
    else
      mask = 0;
      
    //fprintf(stderr, "%f %f ", r, b);
    //fprintf(stderr, "%f %f %f ", x[0], x[1], err);

    /*
    double psi = M_PI / 2 + b + x[1];
    
    double px = x[0] * cos(b);
    double py = x[0] * sin(b);
    
    double qx = px + 0.2 * cos(psi);
    double qy = py + 0.2 * sin(psi);

    fprintf(stderr, "%f %f\n%f %f\n\n", px, py, qx, qy);
    */

    /*
    // TESTING
    double psi, px, py;
    px = r * cos(b);
    py = r * sin(b);
    psi = M_PI / 2 + b + x[1];
    px += 0.5 * cos(psi);
    py += 0.5 * sin(psi);
    fprintf(stderr, "%f %f\n", sqrt(px * px + py * py), atan2(py, px));
    */
  }
  //fprintf(stderr, "\n\n");
  
  // Apply filter clockwise.
  mask = 0;
  for (i = this->laser_data.range_count - 1; i >= 0; i--)
  {
    r = (double) (this->laser_data.ranges[i]) / 1000;
    b = (double) (this->laser_data.min_angle) / 100.0 * M_PI / 180 + i * res;
        
    err = this->UpdateFilter(x, P, Q, R, r, -res);

    if (err < this->segment_range)
    {
      if (mask == 0)
        this->segments[this->segment_count++].last = i;        
      this->segments[this->segment_count - 1].first = i;
      mask = 1;
    }
    else
      mask = 0;

    //fprintf(stderr, "%f %f ", r, b);
    //fprintf(stderr, "%f %f %f\n", x[0], x[1], err);
  }
  //fprintf(stderr, "\n\n");

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Update the line filter.  Returns an error signal.
double LaserFeature::UpdateFilter(double x[2], double P[2][2], double Q[2][2],
                                  double R, double z, double res)
{
  int i, j, k, l;
  double x_[2], P_[2][2];
  double F[2][2];
  double r, S;
  double K[2];

  // A priori state estimate.
  x_[0] = sin(x[1]) / sin(x[1] - res) * x[0];
  x_[1] = x[1] - res;

  // Jacobian for the system function.
  F[0][0] = sin(x[1]) / sin(x[1] - res);
  F[0][1] = -sin(res) / (sin(x[1] - res) * sin(x[1] - res)) * x[0];
  F[1][0] = 0;
  F[1][1] = 1;
  
  // Covariance of a priori state estimate.
  for (i = 0; i < 2; i++)
  {
    for (j = 0; j < 2; j++)
    {
      P_[i][j] = 0.0;
      for (k = 0; k < 2; k++)
        for (l = 0; l < 2; l++)
          P_[i][j] += F[i][k] * P[k][l] * F[j][l];
      P_[i][j] += Q[i][j];
    }
  }

  // Residual (difference between prediction and measurement).
  r = z - x_[0];

  // Covariance of the residual.
  S = P_[0][0] + R;

  // Kalman gain.
  K[0] = P_[0][0] / S;
  K[1] = P_[1][0] / S;

  // Posterior state estimate.
  x[0] = x_[0] + K[0] * r;
  x[1] = x_[1] + K[1] * r;    

  // Posterior state covariance.
  for (i = 0; i < 2; i++)
    for (j = 0; j < 2; j++)
      P[i][j] = P_[i][j] - K[i] * S * K[j];

  return fabs(r); //r * r / S;
}


////////////////////////////////////////////////////////////////////////////////
// Fit lines to the extracted segments.
void LaserFeature::FitSegments()
{
  int i, j;
  segment_t *segment;
  double r, b, x, y;
  double n, sx, sy, sxx, sxy, syy;
  double px, py, pa;
  double ax, ay, bx, by, cx, cy;
  
  for (j = 0; j < this->segment_count; j++)
  {
    segment = this->segments + j;

    n = sx = sy = sxx = syy = sxy = 0.0;
    ax = ay = bx = by = 0.0;
    
    for (i = segment->first; i <= segment->last; i++)
    {
      r = (double) (this->laser_data.ranges[i]) / 1000;
      b = (double) (this->laser_data.min_angle + i * this->laser_data.resolution)
        / 100.0 * M_PI / 180;
      x = r * cos(b);
      y = r * sin(b);

      if (i == segment->first)
      {
        ax = x;
        ay = y;
      }
      else if (i == segment->last)
      {
        bx = x;
        by = y;
      }
      
      n += 1;
      sx += x;
      sy += y;
      sxx += x * x;
      syy += y * y;
      sxy += x * y;
    }

    px = sx / n;
    py = sy / n;
    pa = atan2((n * sxy  - sy * sx), (n * sxx - sx * sx));

    // Make sure the orientation is normal to surface.
    pa += M_PI / 2;
    if (fabs(NORMALIZE(pa - atan2(py, px))) < M_PI / 2)
      pa += M_PI;

    segment->count = (int) n;
    segment->pose[0] = px;
    segment->pose[1] = py;
    segment->pose[2] = pa;

    cx = bx - ax;
    cy = by - ay;
    segment->length = sqrt(cx * cx + cy * cy);
  }
                 
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Merge overlapping segments with similar properties.
void LaserFeature::MergeSegments()
{
  int i, j;
  segment_t *sa, *sb;
  double da;
  
  for (i = 0; i < this->segment_count; i++)
  {
    for (j = i + 1; j < this->segment_count; j++)
    {
      sa = this->segments + i;
      sb = this->segments + j;

      // See if segments overlap...
      if (sb->first <= sa->last && sa->first <= sb->last)
      {
        // See if the segments have the same orientation...
        da = NORMALIZE(sb->pose[2] - sa->pose[2]);
        if (fabs(da) < this->merge_angle)
        {
          sa->first = min(sa->first, sb->first);
          sa->last = max(sa->last, sb->last);
          sb->first = 0;
          sb->last = -1;
        }
      }
    }
  }
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Update the device data (the data going back to the client).
void LaserFeature::UpdateData()
{
  int i;
  double px, py, pa;
  double r, b, o;
  segment_t *segment;
  
  this->data.count = 0;

  for (i = 0; i < this->segment_count; i++)
  {
    segment = this->segments + i;

    if (segment->count >= 4 && segment->length >= this->discard_length)
    {
      px = segment->pose[0];
      py = segment->pose[1];
      pa = segment->pose[2];
      
      r = sqrt(px * px + py * py);
      b = atan2(py, px);
      o = pa;

      this->data.fiducials[this->data.count].id = 0;
      this->data.fiducials[this->data.count].pose[0] = htons(((int16_t) (1000 * r)));
      this->data.fiducials[this->data.count].pose[1] = htons(((int16_t) (180 * b / M_PI)));
      this->data.fiducials[this->data.count].pose[2] = htons(((int16_t) (180 * o / M_PI)));
      this->data.count++;
    }
  }
  
  this->data.count = htons(this->data.count);
  
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Put configuration in buffer (called by client thread)
int LaserFeature::PutConfig(player_device_id_t* device, void *client, void *data, size_t len) 
{
  uint8_t subtype;

  subtype = ((uint8_t*) data)[0];
  
  switch (subtype)
  {
    case PLAYER_FIDUCIAL_REQ_GET_GEOM:
      HandleGetGeom(client, data, len);
      break;
    default:
      if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
        PLAYER_ERROR("PutReply() failed");
      break;
  }
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Handle geometry requests.
void LaserFeature::HandleGetGeom(void *client, void *request, int len)
{
  player_fiducial_geom_t geom;

  if (len != 1)
  {
    PLAYER_ERROR2("geometry request len is invalid (%d != %d)", len, 1);
    if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
      PLAYER_ERROR("PutReply() failed");
    return;
  }

  geom.pose[0] = htons((short) (this->pose[0] * 1000));
  geom.pose[1] = htons((short) (this->pose[1] * 1000));
  geom.pose[2] = htons((short) (this->pose[2] * 180/M_PI));

  if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &geom, sizeof(geom)) != 0)
    PLAYER_ERROR("PutReply() failed");

  return;
}


