/*!
  \file
  \brief 線分の検出

  \author Satofumi KAMIMURA

  $Id: detectLines.cpp 1450 2009-10-25 21:15:53Z satofumi $
*/

#include "detectLines.h"
#include "RangeFinder.h"
#include "range_t.h"
#include "leastSquare.h"
#include "Position.h"
#include "PointUtils.h"

using namespace qrk;
using namespace std;


namespace
{
  void setLengthRange(range_t& length_range,
                      long& min_length, long& max_length,
                      const RangeFinder& lrf)
  {
    if (min_length == LinesDefaultLength) {
      min_length = lrf.minDistance();
    }
    if (max_length == LinesDefaultLength) {
      max_length = lrf.maxDistance();
    }
    length_range.first = min_length;
    length_range.last = max_length;
  }


  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 matchLine(double& line_a,
                 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) {
      double radian = lrf.index2rad(first_index + i);
      points.push_back(calculateXy<long>(data[i], radian));
    }

    double line_b;
    leastSquare<long>(line_a, line_b, &points[0], size);
  }


  bool createLine(line_t& line, double line_a,
                  const RangeFinder& lrf,
                  const vector<long>& data,
                  size_t first_index, size_t last_index)
  {
    int width = last_index -1 - first_index;
    if (width <= 0) {
      return false;
    }

    (void)line_a;

    Point<long> line_begin =
      calculateXy<long>(data[first_index], lrf.index2rad(first_index));
    Point<long> line_end =
      calculateXy<long>(data[last_index -1],
                        lrf.index2rad(last_index -1));
    //fprintf(stderr, "[%d, %d], ", first_index, last_index);
    //fprintf(stderr, "(%d,%d)-(%d,%d), ", line_begin.x, line_begin.y, line_end.x, line_end.y);

#if 0
    double direction = atan2(line_end.y - line_begin.y,
                             line_end.x - line_begin.x);
#else
    double direction =
      atan2(line_a, 1.0) + lrf.index2rad(first_index) + (M_PI / 2.0);
#endif
    line.begin = Point<long>(line_begin.x, line_begin.y);
    line.direction = rad(direction);
    line.length = static_cast<long>(sqrt(pow(line_end.x - line_begin.x, 2) +
                                         pow(line_end.y - line_begin.y, 2)));
    //fprintf(stderr, "(%d, %d, %d)\n", line.begin.x, line.begin.y, line.direction.to_deg());
    return true;
  }
}


void qrk::detectLines(std::vector<line_t>& lines,
                      const RangeFinder& lrf,
                      const std::vector<long>& data,
                      const range_t& range,
                      long line_length_mm,
                      long min_length, long max_length)
{
  // 有効なデータの範囲の作成
  range_t length_range;
  setLengthRange(length_range, min_length, max_length, lrf);

  // 一定の点の数に対して直線をあてはめる
  double each_radian = eachRadian(lrf);
  int line_points = 1;
  int base_index = lrf.deg2index(-90);
  long points_size = range.last - range.first + 1;
  //fprintf(stderr, "points_size: %d, %d\n", points_size, range.first);
  for (long i = 0; i < points_size; i += line_points) {

    // line_points の更新
    double arc_radian = 1.0 * line_length_mm / data[range.first + i];
    line_points = max(static_cast<int>(arc_radian / each_radian), 4);
    size_t first_index = range.first + i;
    //fprintf(stderr, "<%d>, ", line_points);
    if ((i + line_points) > points_size) {
      //line_points = points_size - i;
      i = points_size - line_points;
      if (i < 0) {
        break;
      }
    }

    // 最小二乗法による直線検出は Y 軸方向の傾きの精度が悪いので、
    // 対象の最初の位置が -90 [deg] の位置にあるとして直線の傾き計算を行う
    double a;
    matchLine(a, &data[range.first + i],
              base_index, line_points, lrf);

    // 線分を、最初の点から傾き a の直線として登録する
    size_t last_index = range.first + i + line_points;
    line_t line;
    if (createLine(line, a, lrf, data, first_index, last_index)) {
      lines.push_back(line);
    }
  }
}
