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

  \author Satofumi KAMIMURA

  $Id: Draw3DWidget.cpp 1970 2011-11-09 06:53:29Z satofumi $

  \todo X, Y, Z 軸を描画する
*/

#include "Draw3DWidget.h"
#include <QMouseEvent>
#include <deque>
#include <limits>

#include <cstdio>

using namespace qrk;
using namespace std;


namespace {
  class ScanData
  {
  public:
    size_t timestamp_;
    std::vector<qrk::Point3d<int> > points_;

    ScanData(size_t timestamp, std::vector<qrk::Point3d<int> > points)
      : timestamp_(timestamp), points_(points)
    {
    }
  };
}


struct Draw3DWidget::pImpl
{
  enum {
    DefaultDrawPeriod = 8000,   // [msec]
  };

  QColor clear_color_;
  Point3d<int> rotation_;

  std::deque<ScanData> plot_data_;
  std::deque<ScanData> additional_data_;

  QPoint last_position_;
  Point3d<int> draw_position_;
  size_t draw_period_;
  size_t current_timestamp_;

  double zoom_;

  std::vector<Point<long> > draw_line_points_;
  int h_offset_;


  pImpl(void)
    : clear_color_(Qt::black), draw_period_(DefaultDrawPeriod),
      current_timestamp_(0), zoom_(1.0), h_offset_(0)
  {
  }


  void initializeGL(Draw3DWidget* parent)
  {
    parent->qglClearColor(clear_color_);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_CULL_FACE);
    glEnable(GL_TEXTURE_2D);
  }


  void paintGL(Draw3DWidget* parent)
  {
    parent->qglClearColor(clear_color_);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glLoadIdentity();

    glRotated(rotation_.x, 1.0, 0.0, 0.0);
    glRotated(rotation_.y, 0.0, 1.0, 0.0);
    glRotated(rotation_.z, 0.0, 0.0, 1.0);

    // 追加の直線を描画
    drawAdditionalLines();

    // 最新データのレーザ範囲を描画
    if (! plot_data_.empty()) {
      drawLaserLine(plot_data_.front());
    }

    // 取得データを時間経過に従って、薄くしながら描画
    glBegin(GL_POINTS);
    std::deque<ScanData>::iterator additional_it = additional_data_.begin();
    for (std::deque<ScanData>::iterator it = plot_data_.begin();
         it != plot_data_.end(); ++it, ++additional_it) {
        //fprintf(stderr, "%d, %d\n", current_timestamp_, it->timestamp_);
      size_t diff_time = current_timestamp_ - it->timestamp_;
      if (diff_time > draw_period_) {
        // 以降のデータを削除
        plot_data_.erase(it, plot_data_.end());
        additional_data_.erase(additional_it, additional_data_.end());
        break;
      }

      // データの描画
      double alpha = 1.0 * (draw_period_ - diff_time) / draw_period_;
      drawScanData(*additional_it, alpha, 0.0, 0.0);
      drawScanData(*it, alpha, alpha, alpha);
    }
    glEnd();
  }


  void drawAdditionalLines(void)
  {
    if (draw_line_points_.size() <= 1) {
      return;
    }
    glColor3d(0.0, 0.5, 0.0);

    std::vector<Point<long> >::iterator first = draw_line_points_.begin();
    for (std::vector<Point<long> >::iterator it = first + 1;
         it != draw_line_points_.end(); ++it, ++first) {

      glBegin(GL_LINE_STRIP);
      glVertex3d(+(first->x - draw_position_.x) * zoom_,
                 +(first->y - draw_position_.y - h_offset_) * zoom_, 0.0);
      glVertex3d(+(it->x - draw_position_.x) * zoom_,
                 +(it->y - draw_position_.y - h_offset_) * zoom_, 0.0);
      glEnd();

    }
  }


  void drawScanData(const ScanData& line_data, double r, double g, double b)
  {
    if (line_data.points_.empty()) {
      return;
    }

    std::vector<Point3d<int> >::const_iterator it_end = line_data.points_.end();
    for (std::vector<Point3d<int> >::const_iterator it =
           line_data.points_.begin(); it != it_end; ++it) {

      glColor3d(r, g, b);
      glVertex3d(+(it->x - draw_position_.x) * zoom_,
                 +(it->y - draw_position_.y) * zoom_,
                 -(it->z - draw_position_.z) * zoom_);
    }
  }


  void drawLaserLine(const ScanData& line_data)
  {
    glColor3d(0, 0, 0.4);

    int index = 0;
    const int skip_line = 5;

    std::vector<Point3d<int> >::const_iterator it_end = line_data.points_.end();
    for (std::vector<Point3d<int> >::const_iterator it =
           line_data.points_.begin(); it != it_end; ++it, ++index) {
      if ((index % skip_line) != 0) {
        continue;
      }

      glBegin(GL_LINE_STRIP);
      glVertex3d(0.0, 0.0, 0.0);
      glVertex3d(+(it->x - draw_position_.x) * zoom_,
                 +(it->y - draw_position_.y) * zoom_,
                 -(it->z - draw_position_.z) * zoom_);
      glEnd();
    }
  }


  void setXRotation(int angle)
  {
    normalizeAngle(angle);
    if (angle != rotation_.x) {
      rotation_.x = angle;
    }
  }


  void setYRotation(int angle)
  {
    normalizeAngle(angle);
    if (angle != rotation_.y) {
      rotation_.y = angle;
    }
  }


  void setZRotation(int angle)
  {
    normalizeAngle(angle);
    if (angle != rotation_.z) {
      rotation_.z = angle;
    }
  }


  void normalizeAngle(int& angle)
  {
    while (angle < 0) {
      angle += 360 * 16;
    }

    while (angle > 360 * 16) {
      angle -= 360 * 16;
    }
  }
};


Draw3DWidget::Draw3DWidget(QGLWidget* parent)
  : QGLWidget(parent), pimpl(new pImpl)
{
}


Draw3DWidget::~Draw3DWidget(void)
{
}


void Draw3DWidget::initializeGL(void)
{
  pimpl->initializeGL(this);
}


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

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();

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

  glMatrixMode(GL_MODELVIEW);
}


void Draw3DWidget::paintGL(void)
{
  pimpl->paintGL(this);
}


void Draw3DWidget::mousePressEvent(QMouseEvent *event)
{
  pimpl->last_position_ = event->pos();
}


void Draw3DWidget::mouseMoveEvent(QMouseEvent *event)
{
  int dx = (event->x() - pimpl->last_position_.x()) / 2;
  int dy = (event->y() - pimpl->last_position_.y()) / 2;

  if (event->buttons() & Qt::LeftButton) {
    pimpl->setXRotation(pimpl->rotation_.x + dy);
    pimpl->setYRotation(pimpl->rotation_.y + dx);

  } else if (event->buttons() & Qt::RightButton) {
    pimpl->setXRotation(pimpl->rotation_.x + dy);
    pimpl->setZRotation(pimpl->rotation_.z - dx);
  }
  pimpl->last_position_ = event->pos();
}


void Draw3DWidget::redraw(size_t timestamp, const Point3d<int>& draw_position)
{
  pimpl->current_timestamp_ = timestamp;
  pimpl->draw_position_ = draw_position;
  updateGL();
}


void Draw3DWidget::setDrawLineData(const std::vector<qrk::Point<long> >&
                                   draw_points, int h)
{
  pimpl->draw_line_points_ = draw_points;
  pimpl->h_offset_ = h;
}


void Draw3DWidget::clear(void)
{
  pimpl->plot_data_.clear();
  pimpl->additional_data_.clear();
}


void Draw3DWidget::addData(size_t timestamp,
                           const std::vector<qrk::Point3d<int> >& points)
{
  pimpl->plot_data_.push_front(ScanData(timestamp, points));
}


void Draw3DWidget::addAdditionalData(size_t timestamp,
                                     const std::vector<qrk::Point3d<int> >&
                                     points)
{
  pimpl->additional_data_.push_front(ScanData(timestamp, points));
}



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


void Draw3DWidget::setViewRotation(const Point3d<int>& rotation)
{
  pimpl->rotation_ = rotation;
}


void Draw3DWidget::viewUp(void)
{
  pimpl->rotation_.x += 4;
}


void Draw3DWidget::viewDown(void)
{
  pimpl->rotation_.x -= 4;
}


void Draw3DWidget::viewLeft(void)
{
  pimpl->rotation_.y -= 4;
}


void Draw3DWidget::viewRight(void)
{
  pimpl->rotation_.y += 4;
}


qrk::Point3d<int> Draw3DWidget::viewRotation(void)
{
  return pimpl->rotation_;
}


void Draw3DWidget::setViewZoom(const double ratio)
{
  pimpl->zoom_ = ratio;
}


void Draw3DWidget::zoomIn(void)
{
  pimpl->zoom_ *= 1.2;
}


void Draw3DWidget::zoomOut(void)
{
  pimpl->zoom_ *= 0.8;
  if (pimpl->zoom_ < 0.0) {
    pimpl->zoom_ = 0.0;
  }
}


double Draw3DWidget::zoomRatio(void)
{
  return pimpl->zoom_;
}
