/*!
  \file
  \brief URG データの描画ウィジット

  \author Satofumi KAMIMURA

  $Id: UrgDrawWidget.cpp 1300 2009-09-15 06:36:54Z satofumi $

  \todo Intensity 描画の色を指定できるようにする
  \todo 接続できないときに、ボタンが戻るように追加する
  \todo 強度データ用の表示切替えコンボボックスを付加
  \todo ポリゴン形式での表示方法を実装する
  \todo グリッドに数値と単位系を描画。画面左端と、画面下
  \todo 描画する色を変更できるようにする
*/

#include "UrgDrawWidget.h"
#include "RangeSensor.h"
#include "convert2d.h"
#include <QMouseEvent>
#include <deque>
#include <limits>

using namespace qrk;
using namespace std;


namespace
{
  const double DefaultPixelPerMm = 20.0;

  struct DrawData
  {
    vector<Point<long> > point_data;
    int timestamp;
    Color line_color;


    DrawData(vector<Point<long> >& point_data_,
             int timestamp_, const Color& line_color_)
      : timestamp(timestamp_), line_color(line_color_)
    {
      swap(point_data, point_data_);
    }
  };
}


struct UrgDrawWidget::pImpl
{
  enum {
    MinimumWidth = 100,
    MinimumHeight = 100,

    PointLength = 30 * 1000,     // [mm]
  };

  UrgDrawWidget* widget_;

  Point<double> view_center_;
  double pixel_per_mm_;
  DrawMode draw_mode_;
  short draw_period_;

  typedef deque<DrawData> DataArray;
  DataArray draw_data_;
  int last_timestamp_;
  int current_timestamp_;
  size_t width_;
  size_t height_;

  QColor clear_color_;
  Position<long> rotate_offset_;
  QPoint clicked_position_;
  bool now_pressed_;

  Color line_color_;


  pImpl(UrgDrawWidget* widget)
    : widget_(widget),
      pixel_per_mm_(DefaultPixelPerMm), draw_mode_(Lines), draw_period_(0),
      last_timestamp_(0), current_timestamp_(0),
      width_(MinimumWidth), height_(MinimumHeight),
      clear_color_(Qt::white), rotate_offset_(Position<long>(0, 0, deg(90))),
      now_pressed_(false),
      line_color_(0.2, 0.2, 1.0, 0.6)
  {
  }


  void initializeForm(void)
  {
    widget_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
    widget_->setMinimumSize(MinimumWidth, MinimumHeight);

    widget_->setMouseTracking(true);
  }


  void removeOldData(int timestamp)
  {
    int index = 0;
    for (DataArray::iterator it = draw_data_.begin();
         it != draw_data_.end(); ++it, ++index) {
      short difference = it->timestamp - timestamp;
      if (difference >= draw_period_) {
        break;
      }
    }

    if (index > 0) {
      DataArray::iterator it = draw_data_.begin();
      draw_data_.erase(it, it + index);
    }
  }


  void initializeGL(void)
  {
    widget_->qglClearColor(clear_color_);
    glEnable(GL_CULL_FACE);
    glEnable(GL_TEXTURE_2D);
    glEnable(GL_BLEND);
    glDisable(GL_DEPTH_TEST);

    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  }


  void resizeGL(int width, int height)
  {
    glViewport(0, 0, width, height);

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();

    double aspect = 1.0 * width / height;
    glOrtho(-1.0 * aspect, +1.0 * aspect, -1.0, +1.0,
            numeric_limits<int>::min(), numeric_limits<int>::max());

    glMatrixMode(GL_MODELVIEW);
    width_ = width;
    height_ = height;
  }


  void paintGL(void)
  {
    widget_->qglClearColor(clear_color_);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glLoadIdentity();

    // 軸の描画
    drawAxis();

    // データの描画
    drawData();
  }


  void drawAxis(void)
  {
    double zoom_ratio = zoomRatio();
    Point<double> offset = drawOffset(zoom_ratio);

    // 補助線の描画
    drawSubAxis(offset);

    glColor3d(0.3, 0.3, 0.3);
    glBegin(GL_LINES);

    // Y 軸
    glVertex2d(-offset.x, -1.0);
    glVertex2d(-offset.x, +1.0);

    // X 軸
    double aspect = 1.0 * width_ / height_;
    glVertex2d(-aspect, -offset.y);
    glVertex2d(+aspect, -offset.y);

    // 背後 45 [deg] の補助線
    glVertex2d(-offset.x, -offset.y);
    glVertex2d((-PointLength * zoom_ratio) - offset.x,
               (-PointLength * zoom_ratio) - offset.y);
    glVertex2d(-offset.x, -offset.y);
    glVertex2d((+PointLength * zoom_ratio) - offset.x,
               (-PointLength * zoom_ratio) - offset.y);
    glEnd();

    // 円の補助線を描画
    for (int i = 0; i <= PointLength; i += 1000) {
      drawSubCircle(i, offset);
    }

    // 単位の表示
    // !!!
  }


  void drawSubCircle(int radius, const Point<double>& offset)
  {
    double pixel_radius = (radius / pixel_per_mm_) / (height_ / 2.0);

    glBegin(GL_LINE_STRIP);
    for (int theta = -45; theta <= (180 + 45); theta += 2) {

      double radian = M_PI * theta / 180.0;
      double x = pixel_radius * cos(radian);
      double y = pixel_radius * sin(radian);
      glVertex2d(x - offset.x, y - offset.y);
    }
    glEnd();
  }


  void drawSubAxis(const Point<double>& offset)
  {
    Point<int> center(width_ / 2, height_ / 2);
    Point<long> first(static_cast<int>((-center.x * pixel_per_mm_)
                                      - view_center_.x),
                     static_cast<int>((-center.y * pixel_per_mm_)
                                      - view_center_.y));
    Point<long> last(static_cast<int>((+center.x * pixel_per_mm_)
                                      - view_center_.x),
                     static_cast<int>((+center.y * pixel_per_mm_)
                                      - view_center_.y));

    const double threshold[] = { 4.0, 16.0, 32.0 };
    const int interval[] = { 10, 100, 1000 };
    const double color[] = { 0.8, 0.6, 0.3 };
    size_t n = sizeof(threshold) / sizeof(threshold[0]);
    for (size_t i = 0; i < n; ++i) {
      const double draw_threshold = threshold[i];
      if (pixel_per_mm_ > draw_threshold) {
        continue;
      }
      double alpha = 1.0 - (pixel_per_mm_ / draw_threshold);
      glColor4d(color[i], color[i], color[i], alpha);
      drawSubPointLine(interval[i], offset, first, last);
    }
  }


  void drawSubPointLine(int interval, const Point<double>& offset,
                       const Point<long>& first, const Point<long>& last)
  {
    glBegin(GL_LINES);
    for (int x = (first.x / interval) - 1; x < ((last.x / interval) + 1); ++x) {
      double draw_x = (interval * x / pixel_per_mm_) / (height_ / 2.0);
      glVertex2d(draw_x - offset.x, -1.0);
      glVertex2d(draw_x - offset.x, +1.0);
    }

    double aspect = 1.0 * width_ / height_;
    for (int y = (first.y / interval) - 1; y < ((last.y / interval) + 1); ++y) {
      double draw_y = (interval * y / pixel_per_mm_) / (height_ / 2.0);
      glVertex2d(-aspect, draw_y - offset.y);
      glVertex2d(+aspect, draw_y - offset.y);
    }
    glEnd();
  }


  void drawData(void)
  {
    // !!! 時間が経過するほど、薄い色で表示するようにする

    double zoom_ratio = zoomRatio();
    Point<double> offset = drawOffset(zoom_ratio);

    if (draw_mode_ == Lines) {
      // 中心から測定点への直線を描画
      glBegin(GL_LINES);
      for (DataArray::iterator line_it = draw_data_.begin();
           line_it != draw_data_.end(); ++line_it) {

        Color& line_color = line_it->line_color;
        glColor4d(line_color.r, line_color.g, line_color.b, line_color.a);

        vector<Point<long> >& line_data = line_it->point_data;;
        vector<Point<long> >::iterator end_it = line_data.end();
        for (vector<Point<long> >::iterator it = line_data.begin();
             it != end_it; ++it) {

          double x = it->x * zoom_ratio;
          double y = it->y * zoom_ratio;
          glVertex2d(-offset.x, -offset.y);
          glVertex2d(x - offset.x, y - offset.y);
        }
      }
      glEnd();

    } else if (draw_mode_ == Polygon) {
      // !!! これを実現するには、convert2d 時に捨ててしまった情報が必要
      // !!! 実装方法を見直すべき
    }

    // 測定点の描画
    glColor3d(1.0, 0.0, 0.0);
    double base_size = 1.4;
    double mm_pixel = max(base_size / pixel_per_mm_, base_size);
    glPointSize(mm_pixel);

    glBegin(GL_POINTS);
    for (DataArray::iterator line_it = draw_data_.begin();
         line_it != draw_data_.end(); ++line_it) {

      vector<Point<long> >::iterator end_it = line_it->point_data.end();
      for (vector<Point<long> >::iterator it = line_it->point_data.begin();
           it != end_it; ++it) {

        double x = it->x * zoom_ratio;
        double y = it->y * zoom_ratio;
        glVertex2d(x - offset.x, y - offset.y);
      }
    }
    glEnd();
  }


  // [mm] -> [pixel]
  double zoomRatio(void)
  {
    return (1.0 / pixel_per_mm_ / (height_ / 2.0));
  }


  Point<double> drawOffset(double zoom_ratio)
  {
    return Point<double>(-view_center_.x * zoom_ratio,
                        -view_center_.y * zoom_ratio);
  }


  void setClickedPosition(QMouseEvent* event)
  {
    clicked_position_ = event->pos();
  }


  void updateZoomRatio(int steps)
  {
    double zoom = widget_->zoomRatio();
    zoom *= pow(1.1, steps);

    if (zoom > 500.0) {
      zoom = 500.0;
    } else if (zoom < 0.05) {
      zoom = 0.05;
    }
    widget_->setZoomRatio(zoom);
  }


  bool storeUrgData(int timestamp, vector<Point<long> >& point_data,
                    vector<Point<long> >* intensity_point_data = NULL)
  {
    removeOldData(timestamp);
    draw_data_.push_back(DrawData(point_data, timestamp, line_color_));
    if (intensity_point_data) {
      draw_data_.push_back(DrawData(*intensity_point_data, timestamp,
                                    Color(1.0, 0.0, 1.0, 0.7)));
    }

    // 取得頻度の表示を更新
    emit widget_->scansInterval(timestamp - last_timestamp_);

    last_timestamp_ = timestamp;

    return true;
  }
};


UrgDrawWidget::UrgDrawWidget(QWidget* parent)
  : QGLWidget(parent), pimpl(new pImpl(this))
{
  pimpl->initializeForm();
}


UrgDrawWidget::~UrgDrawWidget(void)
{
}


void UrgDrawWidget::clear(void)
{
  pimpl->draw_data_.clear();
  redraw();
}


void UrgDrawWidget::initializeView(void)
{
  pimpl->pixel_per_mm_ = DefaultPixelPerMm;
  pimpl->view_center_ = Point<double>(0.0, 0.0);
  redraw();
}


void UrgDrawWidget::redraw(int timestamp)
{
  pimpl->current_timestamp_ =
    (timestamp == NoneSpecified) ? pimpl->last_timestamp_ : timestamp;
  updateGL();
}


bool UrgDrawWidget::setUrgData(qrk::RangeSensor* sensor)
{
  vector<long> data;
  vector<long> intensity_data;
  vector<Point<long> > intensity_point_data;
  long timestamp = 0;

  RangeCaptureMode capture_mode = sensor->captureMode();

  int n = 0;
  if (capture_mode != IntensityCapture) {
    n = sensor->capture(data, &timestamp);
  } else {
    n = sensor->captureWithIntensity(data, intensity_data, &timestamp);
  }
  if (n <= 0) {
    return false;
  }

  vector<Point<long> > point_data;
  qrk::convert2d(point_data, sensor, data, pimpl->rotate_offset_);
  if (capture_mode == IntensityCapture) {
    qrk::convert2d(intensity_point_data, sensor, intensity_data,
                   pimpl->rotate_offset_, numeric_limits<int>::max());
  }

  if (point_data.empty()) {
    return false;
  }
  return pimpl->storeUrgData(timestamp, point_data, &intensity_point_data);
}


bool UrgDrawWidget::setUrgData(vector<long>& data,
                               const RangeSensor* sensor, int timestamp)
{
  vector<Point<long> > point_data;
  qrk::convert2d(point_data, sensor, data, pimpl->rotate_offset_);

  if (point_data.empty()) {
    return false;
  }
  return pimpl->storeUrgData(timestamp, point_data);
}


bool UrgDrawWidget::setUrgData(vector<Point<long> >& data, int timestamp)
{
  if (data.empty()) {
    return false;
  }
  return pimpl->storeUrgData(timestamp, data);
}


bool UrgDrawWidget::setUrgIntensityData(vector<long>& data,
                                        const qrk::RangeSensor* sensor,
                                        int timestamp)
{
  vector<Point<long> > point_data;
  qrk::convert2d(point_data, sensor, data, pimpl->rotate_offset_,
                 numeric_limits<int>::max());

  if (point_data.empty()) {
    return false;
  }

  pimpl->draw_data_.push_back(DrawData(point_data, timestamp,
                                       Color(1.0, 0.0, 1.0, 0.7)));
  return true;
}


bool UrgDrawWidget::setUrgIntensityData(vector<qrk::Point<long> >& data,
                                        int timestamp)
{
  if (data.empty()) {
    return false;
  }

  pimpl->draw_data_.push_back(DrawData(data, timestamp,
                                       Color(1.0, 0.0, 1.0, 0.7)));
  return true;
}


void UrgDrawWidget::setDrawColor(const qrk::Color& line_color)
{
  pimpl->line_color_ = line_color;
}


void UrgDrawWidget::setDrawMode(DrawMode mode)
{
  pimpl->draw_mode_ = mode;
}


void UrgDrawWidget::setDrawPeriod(size_t msec)
{
  pimpl->draw_period_ = msec;
}


void UrgDrawWidget::setZoomRatio(double pixel_per_mm)
{
  pimpl->pixel_per_mm_ = pixel_per_mm;
  redraw();
}


double UrgDrawWidget::zoomRatio(void)
{
  return pimpl->pixel_per_mm_;
}


void UrgDrawWidget::updateZoomRatio(int steps)
{
  pimpl->updateZoomRatio(steps);
}


void UrgDrawWidget::setViewCenter(const Point<long>& point)
{
  pimpl->view_center_.x = point.x;
  pimpl->view_center_.y = point.y;
}


Point<long> UrgDrawWidget::viewCenter(void)
{
  return Point<long>(static_cast<long>(pimpl->view_center_.x),
                    static_cast<long>(pimpl->view_center_.y));
}


void UrgDrawWidget::initializeGL(void)
{
  pimpl->initializeGL();
}


void UrgDrawWidget::resizeGL(int width, int height)
{
  pimpl->resizeGL(width, height);
}


void UrgDrawWidget::paintGL(void)
{
  pimpl->paintGL();
}


void UrgDrawWidget::mousePressEvent(QMouseEvent* event)
{
  pimpl->now_pressed_ = true;
  pimpl->setClickedPosition(event);
}


void UrgDrawWidget::mouseMoveEvent(QMouseEvent* event)
{
  int x = event->x();
  int y = event->y();

  if (pimpl->now_pressed_) {
    int dx = x - pimpl->clicked_position_.x();
    int dy = y - pimpl->clicked_position_.y();

    pimpl->view_center_.x += dx * pimpl->pixel_per_mm_;
    pimpl->view_center_.y -= dy * pimpl->pixel_per_mm_;

    pimpl->setClickedPosition(event);
  }

  // カーソル位置の座標をシグナルで送信する
  if ((x < 0) || (x >= static_cast<int>(pimpl->width_)) ||
      (y < 0) || (y >= static_cast<int>(pimpl->height_))) {
    emit position(false, -1, -1);
    return;
  }

  int center_x = pimpl->width_ / 2;
  int center_y = pimpl->height_ / 2;
  int x_mm = static_cast<int>(((x - center_x) * pimpl->pixel_per_mm_)
                              - pimpl->view_center_.x);
  int y_mm = static_cast<int>((-(y - center_y) * pimpl->pixel_per_mm_)
                              - pimpl->view_center_.y);
  emit position(true, x_mm, y_mm);
}


void UrgDrawWidget::mouseReleaseEvent(QMouseEvent* event)
{
  static_cast<void>(event);

  pimpl->now_pressed_ = false;
  redraw();
}


void UrgDrawWidget::wheelEvent(QWheelEvent* event) {

  int degrees = event->delta() / 8;
  int steps = degrees / 15;

  event->accept();
  updateZoomRatio(steps);
}
