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

  \author Satofumi KAMIMURA

  $Id: detectLines.cpp 1573 2009-12-07 23:14:05Z satofumi $
*/

#include "detectLines.h"
#include "RangeFinder.h"
#include "leastSquare.h"
#include "PointUtils.h"
#include "Rotate.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));

    double direction = atan2(line_end.y - line_begin.y,
                             line_end.x - line_begin.x);

    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)));
    return true;
  }
}


void qrk::detectLines(std::vector<line_t>& lines,
                      const RangeFinder& lrf,
                      const std::vector<long>& data,
                      const std::vector<range_t>& ranges,
                      const Position<long>& position,
                      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 base_index = lrf.deg2index(-90);

  // 一定の点の数に対して直線をあてはめる
  for (vector<range_t>::const_iterator it = ranges.begin();
       it != ranges.end(); ++it) {

    int line_points = 1;
    long points_size = it->last - it->first + 1;
    for (long i = 0; i < points_size; i += line_points) {

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

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

      // !!! 直線かの判定処理を追加する必要がある
      // !!! - 同じ傾きの線分が連続したら、直線とみなす、など

      // 線分を、最初の点から傾き a の直線として登録する
      size_t last_index = it->first + i + line_points;
      line_t line;
      if (createLine(line, a, lrf, data, first_index, last_index)) {
        // position 位置を加算する
        (void)position;
        line.begin = rotate<long>(line.begin, position.angle);
        line.begin += Point<long>(position.x, position.y);
        line.direction += position.angle;
        lines.push_back(line);
      }
    }
  }
}
