/*!
  \file
  \brief 特徴点、線分の抽出

  \author Satofumi KAMIMURA

  $Id: detectFeaturePoints.cpp 1475 2009-11-01 05:49:45Z satofumi $
*/

#include "detectFeaturePoints.h"
#include "RangeFinder.h"
#include "leastSquare.h"
#include "PointUtils.h"
#include "Rotate.h"
#include <cstdlib>

using namespace qrk;
using namespace std;


namespace
{
  Point<long> calculateXY(const RangeFinder& lrf,
                          size_t index, long length)
  {
    double radian = lrf.index2rad(index);
    long x = static_cast<long>(length * cos(radian));
    long y = static_cast<long>(length * sin(radian));

    return Point<long>(x, y);
  }


  void convert2d(vector<Point<long> >& points,
                 const RangeFinder& lrf, const distance_t& distance)
  {
    size_t n = distance.data.size();
    for (size_t i = 0; i < n; ++i) {
      points.push_back(calculateXY(lrf,
                                   distance.range.first + i,
                                   distance.data[i]));
    }
  }


  double arcLength(const vector<Point<long> >& points)
  {
    // 端と端の点の長さから、点列の長さを計算する
    const Point<long>& p0 = points.front();
    const Point<long>& p1 = points.back();

    double x_diff = p0.x - p1.x;
    double y_diff = p0.y - p1.y;
    return sqrt((x_diff * x_diff) + (y_diff * y_diff));
  }


  // 点列を n 分割してそれぞれに直線をあてはめ、
  // 角度が適切に変化しているかで円弧判定を行う
  bool similarCircle(const vector<Point<long> >& points)
  {
    enum { N = 4 };

    size_t points_size = points.size();
    if (points_size < (2 * N)) {
      // 点の数が 2*n 点に満たない場合は、戻る
      return false;
    }

    // !!! スキャンの向きに依存しない実装にすべき
    double a[N];
    double b[N];
    size_t each_points = points_size / N;
    leastSquare(a[0], b[0], &points[0], each_points);
    for (size_t i = 1; i < N; ++i) {
      leastSquare(a[i], b[i], &points[each_points * i], each_points);

      if (a[i] >= a[i - 1]) {
        return false;
      }
    }
    return true;
  }


  Point<long> centerPoint(const vector<Point<long> >& points)
  {
    Point<long> center;

    for (vector<Point<long> >::const_iterator it = points.begin();
         it != points.end(); ++it) {
      center += *it;
    }

    long n = points.size();
    return Point<long>(center.x / n, center.y / n);
  }


  Point<long> moveBackOffset(const RangeFinder& lrf,
                             size_t index, long offset)
  {
    return calculateXY(lrf, index, offset);
  }


  double eachRadian(const RangeFinder& lrf)
  {
    int index_p45 = lrf.deg2index(+45);
    int index_m45 = lrf.deg2index(-45);

    return  (M_PI / 2.0) / fabs(index_p45 - index_m45);
  }


  void calculatePoint(Position<long>& line,
                      long distance, const RangeFinder& lrf,
                      size_t index)
  {
    double radian = lrf.index2rad(index);
    line.x = static_cast<long>(distance * cos(radian));
    line.y = static_cast<long>(distance * sin(radian));

    line.angle = rad(radian + (M_PI / 2.0));
  }


  void createLine(double& a, double& b,
                  const long data[], size_t first_index, size_t size,
                  const RangeFinder& lrf)
  {
    vector<Point<long> > points;

    for (size_t i = 0; i < size; ++i) {
      long distance = data[i];
      double radian = lrf.index2rad(first_index + i);
      long x = static_cast<long>(distance * cos(radian));
      long y = static_cast<long>(distance * sin(radian));
      points.push_back(Point<long>(x, y));
    }

    leastSquare(a, b, &points[0], points.size());
  }


  bool createSegment(segment_t& segment, const RangeFinder& lrf,
                     const distance_t& distance, size_t data_offset,
                     size_t begin_index, size_t end_index)
  {
    int width = end_index - begin_index - 1;
    if (width <= 0) {
      return false;
    }

    Position<long> line_begin;
    calculatePoint(line_begin,
                   distance.data[data_offset], lrf, begin_index);
    Position<long> line_end;
    calculatePoint(line_end,
                   distance.data[data_offset + width], lrf, end_index);

    double direction = atan2(line_end.y - line_begin.y,
                             line_end.x - line_begin.x);
    segment.begin = Point<long>(line_begin.x, line_begin.y);
    segment.direction = rad(direction);
    segment.length = static_cast<long>(sqrt(pow(line_end.x - line_begin.x, 2) +
                                            pow(line_end.y - line_begin.y, 2)));
    return true;
  }


  double angleDifference(double difference)
  {
    if (difference < (M_PI / 2.0)) {
      difference = M_PI - difference;
    } else if (difference > M_PI) {
      difference -= M_PI;
    }
    if (difference > (2.0 * M_PI)) {
      difference -= 2.0 * M_PI;
    }
    return difference;
  };


  bool estimateCircle(featurePoint_t& feature_point,
                      const vector<segment_t>& segments,
                      int segment_first)
  {
    int n = segments.size() - segment_first;
    if (n < 2) {
      return false;
    }

    // 線分の角度差を計算し、小さすぎる場合は半径の計算を放棄する
    double previous_theta = segments[segment_first].direction.to_rad();
    double theta_diff = 0.0;
    Point<long> previous_point = segments[segment_first].begin;
    long line_length = 0;
    for (vector<segment_t>::const_iterator it =
           segments.begin() + segment_first + 1; it != segments.end(); ++it) {
      double theta = it->direction.to_rad();
      double diff = angleDifference(fabs(theta - previous_theta));
      theta_diff += diff;
      previous_theta = theta;

      line_length += length(it->begin, previous_point);
      previous_point = it->begin;
    }
    theta_diff /= n - 1;
    line_length /= n - 1;

    // 線分の角度差と線分の長さから、半径の長さを推定する
    const double adjust_ratio = 0.8; // 中心の位置をずらすための補正値
    double radius = line_length / 2.0 / cos(theta_diff / 2.0) * adjust_ratio;
    const double HugeRadius = 1000.0; // [mm]
    if (radius > HugeRadius) {
      // 中心位置が表面からの遠い物体は円として登録しない
      return false;
    }
    feature_point.radius = static_cast<long>(radius);

    // 線分毎に中心を計算し、その重心位置を円の中心として登録する
    Point<long> center;
    for (vector<segment_t>::const_iterator it =
           segments.begin() + segment_first; it != segments.end(); ++it) {
      double radian = it->direction.to_rad() - (theta_diff / 2.0);
      long x_offset = static_cast<long>(radius * cos(radian));
      long y_offset = static_cast<long>(radius * sin(radian));

      center.x += it->begin.x + x_offset;
      center.y += it->begin.y + y_offset;
    }
    feature_point.point.x = center.x / n;
    feature_point.point.y = center.y / n;
    feature_point.type = Poll;

    return true;
  }


  void registerEedge(vector<featurePoint_t>& feature_points,
                     const Point<long>& point,
                     FeaturePointType own_type,
                     FeaturePointType previous_type)
  {
    if (previous_type == own_type) {
      // 直前も同じ種類のエッジならば、直前の位置を修正する
      featurePoint_t& fp = feature_points.back();
      fp.point.x = (fp.point.x + point.x) / 2;
      fp.point.y = (fp.point.y + point.y) / 2;
    } else {
      featurePoint_t fp;
      fp.type = own_type;
      fp.point.x = point.x;
      fp.point.y = point.y;
      fp.radius = 0;
      feature_points.push_back(fp);
    }
  }


  void detectEdgePoint(vector<featurePoint_t>& feature_points,
                       const distance_t& distance,
                       const RangeFinder& lrf,
                       const vector<long>& data, const range_t& range, int step)
  {
    long min_distance = lrf.minDistance();

    int current_index = (step < 0) ? range.first : range.last - 1;
    int compare_index = current_index + step;
    if ((compare_index < 0) ||
        (compare_index >= static_cast<int>(lrf.maxRange()))) {
      return;
    }

    long compare_length = data[compare_index];
    if (compare_length <= min_distance) {
      return;
    }

    long current_length = data[current_index];
    featurePoint_t fp;
    const long CompareOffset = 100; // [mm]
    if (current_length < (compare_length + CompareOffset)) {
      // 凸エッジ
      fp.type = RightAngle;

      // グループ中の距離を用いてエッジの位置を計算する
      int actual_index =
        (step < 0) ? distance.range.first : distance.range.last - 1;

      Point<long> point =
        calculateXY(lrf, actual_index,
                    distance.data[actual_index - distance.range.first]);
      fp.point.x = point.x;
      fp.point.y = point.y;
      fp.radius = 0;
      //fprintf(stderr, "(%d, %d)_[%d,%d], ", fp.x, fp.y, current_length, compare_length);
      feature_points.push_back(fp);
    }
  }


  void detectEdges(vector<featurePoint_t>& feature_points,
                   const vector<segment_t>& segments, size_t segment_first,
                   const distance_t& distance,
                   const RangeFinder& lrf,
                   const vector<long>& data, const range_t& range)
  {
    const long AngleThreshold = 60;

    int n = segments.size() - segment_first;
    if (n < 2) {
      return;
    }

    detectEdgePoint(feature_points, distance, lrf, data, range, -1);

    // 線分の角度差が大きい位置をエッジとして登録する
    FeaturePointType previous_type = Unknown;
    double previous_theta = segments[segment_first].direction.to_rad();
    for (vector<segment_t>::const_iterator it =
           segments.begin() + segment_first + 1; it != segments.end(); ++it) {
      double theta = it->direction.to_rad();

      double difference = angleDifference(theta - previous_theta);
      long diff_deg =
        static_cast<long>(180.0 * difference / M_PI) - 180;
      if (abs(diff_deg) > AngleThreshold) {
        if (diff_deg > 0) {
          // 凸エッジ
          registerEedge(feature_points, it->begin, RightAngle, previous_type);
          previous_type = RightAngle;

        } else {
          // 凹エッジ
          registerEedge(feature_points, it->begin, ReflexAngle, previous_type);
          previous_type = ReflexAngle;
        }
      } else {
        previous_type = Unknown;
      }
      previous_theta = theta;
    }

    detectEdgePoint(feature_points, distance, lrf, data, range, +1);
  }
}


void qrk::detectFeaturePoints(std::vector<featurePoint_t>& feature_points,
                              std::vector<segment_t>& segments,
                              const Position<long>& position,
                              const RangeFinder& lrf,
                              const std::vector<long>& data,
                              const distance_t& distance,
                              const range_t& range)
{
  if (distance.data.empty()) {
    return;
  }

  const size_t segments_first = segments.size();
  const size_t feature_points_first = feature_points.size();

  const size_t middle_offset = distance.data.size() / 2;
  const size_t middle_index = distance.range.first + middle_offset;

  vector<Point<long> > points;
  convert2d(points, lrf, distance);

  const double SegmentLength = 140.0; // [mm]
  const double ThinPollWidth = 150.0;
  const double IgnoreWidth = 0.0;
  double arc_length = arcLength(points);
  if (arc_length < IgnoreWidth) {
    // 登録しない

  } else if (arc_length < ThinPollWidth) {
    // 点列の長さが短いときは、細い棒として扱い、点の重心位置の少し先を登録する
    featurePoint_t fp;
    fp.type = ThinPoll;

    Point<long> point = centerPoint(points);
    point += moveBackOffset(lrf, middle_index,
                            static_cast<long>(arc_length * 4 / 5));

#if 0
    // !!! 角度補正をする場合
    Position<long> rotated = rotate(Position<long>(point.x, point.y, deg(0)),
                                    position.angle);
    fp.x = rotated.x + position.x;
    fp.y = rotated.y + position.y;
#else
    fp.point.x = point.x + position.x;
    fp.point.y = point.y + position.y;
#endif

    feature_points.push_back(fp);

  } else {
    double each_radian = eachRadian(lrf);
    double arc_radian = SegmentLength / distance.data[middle_offset];
    int line_points = max(static_cast<int>(arc_radian / each_radian), 2);

    //vector<double> line_a;
    //vector<double> line_b;

    size_t points_size = points.size();
    for (size_t i = 0; i < points_size; i += line_points) {
      // 最小二乗法による直線検出は Y 軸方向の傾きの精度が悪いので、
      // 対象の最初の位置が -90 [deg] の位置にあるとして直線の傾き推定を行う
      int first_index = lrf.deg2index(-90);
      double a;
      double b;
      createLine(a, b, &distance.data[i],
                 first_index, line_points, lrf);
      //line_a.push_back(a);
      //line_b.push_back(b);

      // 線分を、最初の点から傾き a の直線として登録する
      size_t begin_index = distance.range.first + i;
      size_t last_index = min(i + line_points, points_size);
      if (last_index > ((line_points * 2 / 3) + begin_index)) {
        // 本来の点の個数の 2/3 以上なければ線分を登録しない
        break;
      }

      size_t end_index = distance.range.first + last_index - 1;
      segment_t segment;
      if (createSegment(segment, lrf, distance,
                        i, begin_index, end_index)) {
        segments.push_back(segment);
      }
    }

    if (similarCircle(points)) {
      // 点列の形状が円弧に近ければ、円として扱う

      // 線分の角度差と線分の長さから半径の長さを推定する
      featurePoint_t fp;
      if (estimateCircle(fp, segments, segments_first)) {
        feature_points.push_back(fp);
      }

    } else {
      // 線分間の角度差の大きさから凸エッジ、凹エッジを検出する
      detectEdges(feature_points, segments, segments_first,
                  distance, lrf, data, range);
    }

    // position を特徴点の位置に反映させる
    size_t n = feature_points.size();
    for (size_t i = feature_points_first; i < n; ++i) {
      // !!! イテレータにする
      feature_points[i].point.x += position.x;
      feature_points[i].point.y += position.y;
    }

    n = segments.size();
    for (size_t i = segments_first; i < n; ++i) {
      // !!! イテレータにする
      segments[i].begin.x += position.x;
      segments[i].begin.y += position.y;
    }
  }
}
