/*!
  \file
  \brief 描画

  \author Satofumi KAMIMURA

  $Id: plot_objects.cpp 1451 2009-10-25 21:22:05Z satofumi $
*/

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

using namespace qrk;
using namespace std;


enum {
  MarkerWidth = 50,
};


void plotLrfData(MarkerManager& marker,
                 const RangeFinder& range_finder, const vector<long>& data)
{
  vector<Point<long> > points;

  Position<long> offset;
  convert2d(points, range_finder, data, offset);

  Color blue(0.0, 0.0, 1.0);
  marker.drawPoints("lrf_data", 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 plotPolls(MarkerManager& marker, vector<featurePoint_t>& polls)
{
  Color blue(0.0, 0.0, 1.0);

  for (vector<featurePoint_t>::iterator it = polls.begin();
       it != polls.end(); ++it) {
    marker.drawPoint("polls", it->point, blue, it->radius);
  }
}


void plotLines(MarkerManager& marker, vector<line_t>& lines)
{
  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("lines", points, green, MarkerWidth);
  }
}
