/*!
  \file
  \brief 連続直線の追従モジュール

  \author Satofumi KAMIMURA

  $Id: FollowLines.cpp 1574 2009-12-13 03:53:55Z satofumi $
*/

#include "FollowLines.h"
#include "Running.h"
#include "PointUtils.h"
#include "PositionUtils.h"
#include "AngleUtils.h"
#include <algorithm>

using namespace qrk;
using namespace std;

namespace
{
  typedef vector<Point<long> > Points;
}


struct FollowLines::pImpl
{
  bool first_follow_;
  size_t current_index_;
  Points normalized_points_;
  size_t points_size_;
  Running& run_;

  Position<long> check_point_;


  pImpl(const vector<Point<long> >& points, Running& running)
    : first_follow_(true), current_index_(0),
      normalized_points_(points), run_(running)
  {
    // 同じ位置が連続したら、その点を除去する
    unique(normalized_points_.begin(), normalized_points_.end());
    points_size_ = normalized_points_.size();

    // 最後の直線を走行させるために、もう１点追加する
    if (points_size_ >= 2) {
      Point<long> last_point(normalized_points_[points_size_ - 1]);
      Angle direction = lineDirection(normalized_points_[points_size_ - 2],
                                      normalized_points_[points_size_ - 1]);
      double radian = direction.to_rad();
      last_point.x += static_cast<long>(16.0 * cos(radian));
      last_point.y += static_cast<long>(16.0 * sin(radian));
      normalized_points_.push_back(last_point);
    }
  }


  // 次の直線に乗り換える位置を通過したかの判定
  bool passedNextPoint(void)
  {
    Position<long> position = run_.position(NULL);
    long length =
      lengthToLine(Point<long>(position.x, position.y), check_point_);
    if (length < 0) {
      return false;
    }

    ++current_index_;
    return true;
  }


  // 次の直線に乗り換える位置を更新
  void updateNextPoint(void)
  {
    if ((current_index_ + 2) >= normalized_points_.size()) {
      return;
    }

    Point<long>&a = normalized_points_[current_index_];
    Point<long>&b = normalized_points_[current_index_ + 1];
    Point<long>&c = normalized_points_[current_index_ + 2];

    // 直線の乗り換え位置は、１点目と２点目と間になる
    // ここで、２点目からの距離 l を計算する
    Angle narrow_angle = narrowAngle(a, b, c);

    double p;
    if (narrow_angle.to_deg() == 0.0) {
      // まったく同じ点で折り返すときは、その点は無視する
      p = 1.0;

    } else {
      // !!! 実際は run_ から追従時の曲率を取得すること
      double l = 300 * tan((M_PI + narrow_angle.to_rad()) / 2.0);

      double first_length = length(a, b);
      p = fabs(l / first_length);

      // check_point_ が線分の外側になる場合、曲率と並進速度を減少させる
      // ２つの線分の短い方の長さを基準に曲率を設定する
      if (p > 1.0) {
        // !!!
      }
    }

    Angle direction = lineDirection(a, b);
    check_point_ =
      Position<long>(static_cast<long>(b.x - ((b.x - a.x) * p)),
                     static_cast<long>(b.y - ((b.y - a.y) * p)), direction);
  }


  // 現在の直線に追従
  void followCurrentLine(void)
  {
    run_.followLine(check_point_, NULL);
  }
};


FollowLines::FollowLines(const std::vector<Point<long> >& points,
                         Running& running)
  : pimpl(new pImpl(points, running))
{
}


FollowLines::~FollowLines(void)
{
}


bool FollowLines::run(void)
{
  // 次の直線がなければ、終了
  // 点が n 個のとき、線分は (n - 1) 個だけ存在する
  if ((pimpl->current_index_ + 1) >= pimpl->points_size_) {
    return false;
  }

  // 最初の直線追従か、次の直線に乗り換える位置を通過したら
  // 走行コマンドを発行する
  if (pimpl->first_follow_ || pimpl->passedNextPoint()) {
    pimpl->first_follow_ = false;

    // 次の直線に乗り換える位置を更新
    pimpl->updateNextPoint();

    // 現在の直線に追従
    pimpl->followCurrentLine();
  }

  return true;
}


Position<long> FollowLines::currentLine(void) const
{
  return pimpl->check_point_;
}


void FollowLines::setPointIndex(size_t index)
{
  if (index + 1 >= pimpl->points_size_) {
    // 範囲外の場合は、更新しない
    return;
  }

  pimpl->current_index_ = index;
  pimpl->first_follow_ = true;
}


size_t FollowLines::pointIndex(void) const
{
  return pimpl->current_index_;
}
