/*!
  \file
  \brief 線分の連結

  \author Satofumi KAMIMURA

  $Id: mergeLines.cpp 1537 2009-11-19 21:28:03Z satofumi $
*/

#include "mergeLines.h"
#include "MathUtils.h"
#include "PointUtils.h"
#include <cstdlib>

using namespace qrk;
using namespace std;


void mergeLines(std::vector<qrk::line_t>& merged_lines,
                const std::vector<qrk::line_t>& lines)
{
  if (lines.empty()) {
    return;
  }

  // 始点が前の線分の終点と近く、傾きがほぼ同じときに、線分を連結する
  enum {
    NearLength = 200,         // [mm]
    NearDegree = 8,
  };
  line_t pre_line = lines.front();
  line_t merged_line;
  bool no_merged_line = true;

  for (vector<line_t>::const_iterator it = lines.begin() + 1;
       it != lines.end(); ++it) {
    // 終点の位置を計算する
    double radian = pre_line.direction.to_rad();
    long x_offset = pre_line.length * cos(radian);
    long y_offset = pre_line.length * sin(radian);
    Point<long> pre_line_end =
      pre_line.begin + Point<long>(x_offset, y_offset);

    int degree_diff = pre_line.direction.to_deg() - it->direction.to_deg();
    if (degree_diff > 180) {
      degree_diff -= 360;
    } else if (degree_diff < -180) {
      degree_diff += 360;
    }

    if ((length<long>(it->begin, pre_line_end) < NearLength) &&
        (abs(degree_diff) < NearDegree)) {
      // 連結した直線を作る
      if (no_merged_line) {
        merged_line.begin = pre_line.begin;
        merged_line.length = pre_line.length;
      }

      radian = it->direction.to_rad();
      x_offset = it->length * cos(radian);
      y_offset = it->length * sin(radian);
      Point<long> line_end =
        it->begin + Point<long>(x_offset, y_offset);

      merged_line.direction =
        rad(atan2(line_end.y - merged_line.begin.y,
                  line_end.x - merged_line.begin.x));
      merged_line.length += it->length;
      no_merged_line = false;
    } else {
      // 直線が連結されていれば、登録する
      if (! no_merged_line) {
        merged_lines.push_back(merged_line);
      }
      no_merged_line = true;
    }
    pre_line = *it;
  }

  // 直線が連結されていれば、登録する
  if (! no_merged_line) {
    merged_lines.push_back(merged_line);
  }
}
