/*!
  \file
  \brief 描画

  \author Satofumi KAMIMURA

  $Id: plot_objects.cpp 1520 2009-11-16 12:35:30Z satofumi $
*/

#include "plot_objects.h"
#include <MarkerManager.h>
#include <convert2d.h>
#include <Rotate.h>

using namespace qrk;
using namespace std;


enum {
  MarkerWidth = 50,
};


void plotLrfData(MarkerManager& marker,
                 const RangeFinder& range_finder, const vector<long>& data,
                 const char* name)
{
  vector<Point<long> > points;
  Position<long> offset;
  convert2d(points, range_finder, data, offset);

  Color blue(0.0, 0.0, 1.0);
  marker.drawPoints(name, points, blue, MarkerWidth);
}


void plotOriginDirection(MarkerManager& marker, const Angle& direction)
{
  vector<Point<long> > points;

  points.push_back(Point<long>(0, 0));

  const double length = 1000.0;
  double radian = direction.to_rad();
  long x = static_cast<long>(length * cos(-radian));
  long y = static_cast<long>(length * sin(-radian));
  points.push_back(Point<long>(x, y));

  Color black(0.0, 0.0, 0.0);
  marker.drawLines("origin_direction", points, black, MarkerWidth);
}


void plotPoles(MarkerManager& marker, const vector<pole_t>& poles,
               const Position<long>& position,
               const char* name)
{
  Color blue(0.0, 0.0, 1.0);

  // !!! 単項演算子を定義すべきかを検討する
  Angle rotate_angle = deg(0) - position.angle;
  for (vector<pole_t>::const_iterator it = poles.begin();
       it != poles.end(); ++it) {
    Position<long> draw_point(it->center.x - position.x,
                              it->center.y - position.y, deg(0));
    draw_point = rotate<long>(draw_point, rotate_angle);

    marker.drawPoint(name,
                     Point<long>(draw_point.x, draw_point.y),
                     blue, it->radius);
  }
}


void plotLines(MarkerManager& marker, const vector<line_t>& lines,
               const char* name)
{
  if (lines.empty()) {
    return;
  }

  for (vector<line_t>::const_iterator it = lines.begin();
       it != lines.end(); ++it) {

    vector<Point<long> > points;
    points.push_back(it->begin);

    // 点列の最後の点を登録
    double length = it->length;
    double radian = it->direction.to_rad();
    Point<long> offset(static_cast<long>(length * cos(radian)),
                       static_cast<long>(length * sin(radian)));
    points.push_back(points.back() + offset);

    Color green(0.0, 1.0, 0.0);
    marker.drawLines(name, points, green, MarkerWidth);
  }
}
