// Copyright (c), ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#include "colmap/ui/model_viewer_widget.h"

#include "colmap/math/math.h"
#include "colmap/ui/render_options.h"

using RGBAColor = Eigen::Matrix<uint8_t, 4, 1>;

// Color of a selected 3D point.
const RGBAColor kSelectedPointColor(0, 255, 0, 255);
const RGBAColor kSelectedPointToCamColor(0, 255, 0, 204);

// Color of the viewing frustum if the image is selected.
const RGBAColor kSelectedImagePlaneColor(255, 0, 255, 153);
const RGBAColor kSelectedImageFrameColor(204, 0, 204, 255);

// Color of the viewing frustum if another image in the same frame is selected.
const RGBAColor kSelectedFramePlaneColor(204, 0, 204, 77);
const RGBAColor kSelectedFrameFrameColor(153, 0, 153, 179);

const RGBAColor kMovieGrabberImagePlaneColor(0, 255, 255, 153);
const RGBAColor kMovieGrabberImageFrameColor(0, 204, 204, 255);

const RGBAColor kGridColor(51, 51, 51, 153);
const RGBAColor kXAxisColor(230, 0, 0, 128);
const RGBAColor kYAxisColor(0, 230, 0, 128);
const RGBAColor kZAxisColor(0, 0, 230, 128);

namespace colmap {
namespace {

inline uint8_t FloatToUint8(float v) {
  return static_cast<uint8_t>(std::lround(v * 255));
}

inline RGBAColor FloatColorToUint8(const Eigen::Vector4f& c) {
  return {FloatToUint8(c(0)),
          FloatToUint8(c(1)),
          FloatToUint8(c(2)),
          FloatToUint8(c(3))};
}

// Generate unique index from RGB color in the range [0, 256^3].
inline size_t RGBToIndex(const uint8_t r, const uint8_t g, const uint8_t b) {
  return static_cast<size_t>(r) + static_cast<size_t>(g) * 256 +
         static_cast<size_t>(b) * 65536;
}

// Derive color from unique index, generated by `RGBToIndex`.
inline RGBAColor IndexToRGB(const size_t index) {
  RGBAColor color;
  color(0) = (index & 0x000000FF) >> 0;
  color(1) = (index & 0x0000FF00) >> 8;
  color(2) = (index & 0x00FF0000) >> 16;
  color(3) = 255;
  return color;
}

void BuildCameraModel(const std::optional<Rigid3d>& cam_from_world,
                      const Camera& camera,
                      const float image_size,
                      const RGBAColor& plane_color,
                      const RGBAColor& frame_color,
                      const Eigen::Vector3d& model_origin,
                      const double model_scale,
                      std::vector<TrianglePainter::Data>* triangle_data,
                      std::vector<LinePainter::Data>* line_data) {
  // Updating the reconstruction in the viewer (e.g., deleting an image or a
  // point) is not thread-safe when the mapper is running, where some images may
  // be in a partial, incorrect state. In rare circumstances, an image may be
  // registered but not yet have a pose. Instead of crashing the viewer, we
  // simply skip the visualization of these images and warn the user.
  if (!cam_from_world) {
    LOG(WARNING) << "Failed to render camera because it has no pose.";
    return;
  }

  // Generate camera dimensions in OpenGL (world) coordinate space.
  const float kBaseCameraWidth = 1024.0f;
  const float image_width = image_size * camera.width / kBaseCameraWidth;
  const float image_height = image_width * static_cast<float>(camera.height) /
                             static_cast<float>(camera.width);
  const float image_extent = std::max(image_width, image_height);
  const float camera_extent = std::max(camera.width, camera.height);
  const float camera_extent_normalized =
      static_cast<float>(camera.CamFromImgThreshold(camera_extent));
  const float focal_length = 2.0f * image_extent / camera_extent_normalized;

  Rigid3d world_from_cam = Inverse(*cam_from_world);
  world_from_cam.translation() += model_origin;
  world_from_cam.translation() *= model_scale;

  const Eigen::Matrix<float, 3, 4> world_from_cam_mat =
      world_from_cam.ToMatrix().cast<float>();

  // Projection center, top-left, top-right, bottom-right, bottom-left corners.

  const Eigen::Vector3f pc = world_from_cam_mat.rightCols<1>();
  const Eigen::Vector3f tl =
      world_from_cam_mat *
      Eigen::Vector4f(-image_width, image_height, focal_length, 1);
  const Eigen::Vector3f tr =
      world_from_cam_mat *
      Eigen::Vector4f(image_width, image_height, focal_length, 1);
  const Eigen::Vector3f br =
      world_from_cam_mat *
      Eigen::Vector4f(image_width, -image_height, focal_length, 1);
  const Eigen::Vector3f bl =
      world_from_cam_mat *
      Eigen::Vector4f(-image_width, -image_height, focal_length, 1);

  // Image plane as two triangles.
  if (triangle_data != nullptr) {
    triangle_data->emplace_back(PointPainter::Data(tl(0),
                                                   tl(1),
                                                   tl(2),
                                                   plane_color(0),
                                                   plane_color(1),
                                                   plane_color(2),
                                                   plane_color(3)),
                                PointPainter::Data(tr(0),
                                                   tr(1),
                                                   tr(2),
                                                   plane_color(0),
                                                   plane_color(1),
                                                   plane_color(2),
                                                   plane_color(3)),
                                PointPainter::Data(bl(0),
                                                   bl(1),
                                                   bl(2),
                                                   plane_color(0),
                                                   plane_color(1),
                                                   plane_color(2),
                                                   plane_color(3)));

    triangle_data->emplace_back(PointPainter::Data(bl(0),
                                                   bl(1),
                                                   bl(2),
                                                   plane_color(0),
                                                   plane_color(1),
                                                   plane_color(2),
                                                   plane_color(3)),
                                PointPainter::Data(tr(0),
                                                   tr(1),
                                                   tr(2),
                                                   plane_color(0),
                                                   plane_color(1),
                                                   plane_color(2),
                                                   plane_color(3)),
                                PointPainter::Data(br(0),
                                                   br(1),
                                                   br(2),
                                                   plane_color(0),
                                                   plane_color(1),
                                                   plane_color(2),
                                                   plane_color(3)));
  }

  if (line_data != nullptr) {
    // Frame around image plane and connecting lines to projection center.

    line_data->emplace_back(PointPainter::Data(pc(0),
                                               pc(1),
                                               pc(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)),
                            PointPainter::Data(tl(0),
                                               tl(1),
                                               tl(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)));

    line_data->emplace_back(PointPainter::Data(pc(0),
                                               pc(1),
                                               pc(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)),
                            PointPainter::Data(tr(0),
                                               tr(1),
                                               tr(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)));

    line_data->emplace_back(PointPainter::Data(pc(0),
                                               pc(1),
                                               pc(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)),
                            PointPainter::Data(br(0),
                                               br(1),
                                               br(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)));

    line_data->emplace_back(PointPainter::Data(pc(0),
                                               pc(1),
                                               pc(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)),
                            PointPainter::Data(bl(0),
                                               bl(1),
                                               bl(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)));

    line_data->emplace_back(PointPainter::Data(tl(0),
                                               tl(1),
                                               tl(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)),
                            PointPainter::Data(tr(0),
                                               tr(1),
                                               tr(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)));

    line_data->emplace_back(PointPainter::Data(tr(0),
                                               tr(1),
                                               tr(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)),
                            PointPainter::Data(br(0),
                                               br(1),
                                               br(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)));

    line_data->emplace_back(PointPainter::Data(br(0),
                                               br(1),
                                               br(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)),
                            PointPainter::Data(bl(0),
                                               bl(1),
                                               bl(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)));

    line_data->emplace_back(PointPainter::Data(bl(0),
                                               bl(1),
                                               bl(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)),
                            PointPainter::Data(tl(0),
                                               tl(1),
                                               tl(2),
                                               frame_color(0),
                                               frame_color(1),
                                               frame_color(2),
                                               frame_color(3)));
  }
}

}  // namespace

ModelViewerWidget::ModelViewerWidget(QWidget* parent, OptionManager* options)
    : QOpenGLWidget(parent),
      options_(options),
      point_viewer_widget_(new PointViewerWidget(parent, this, options)),
      image_viewer_widget_(
          new DatabaseImageViewerWidget(parent, this, options)),
      movie_grabber_widget_(new MovieGrabberWidget(parent, this)),
      mouse_is_pressed_(false),
      focus_distance_(kInitFocusDistance),
      selected_image_id_(kInvalidImageId),
      selected_point3D_id_(kInvalidPoint3DId),
      coordinate_grid_enabled_(true),
      near_plane_(kInitNearPlane) {
  background_color_[0] = 1.0f;
  background_color_[1] = 1.0f;
  background_color_[2] = 1.0f;

  QSurfaceFormat format;
  format.setDepthBufferSize(24);
  format.setMajorVersion(3);
  format.setMinorVersion(2);
  format.setSamples(4);
  format.setRenderableType(QSurfaceFormat::OpenGL);
  format.setProfile(QSurfaceFormat::CoreProfile);
#ifdef DEBUG
  format.setOption(QSurfaceFormat::DebugContext);
#endif
  setFormat(format);
  QSurfaceFormat::setDefaultFormat(format);

  SetPointColormap(std::make_unique<PointColormapPhotometric>());
  SetImageColormap(std::make_unique<ImageColormapUniform>());

  image_size_ = static_cast<float>(devicePixelRatio() * image_size_);
  point_size_ = static_cast<float>(devicePixelRatio() * point_size_);
}

ModelViewerWidget::~ModelViewerWidget() {
  // Make the OpenGL context current so that painter destructors can
  // cleanly release VAOs and VBOs.
  makeCurrent();
}

void ModelViewerWidget::initializeGL() {
  initializeOpenGLFunctions();
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  glEnable(GL_VERTEX_PROGRAM_POINT_SIZE);
  SetupPainters();
  SetupView();
}

void ModelViewerWidget::paintGL() {
  glClearColor(
      background_color_[0], background_color_[1], background_color_[2], 1.0f);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  const QMatrix4x4 pmv_matrix = projection_matrix_ * model_view_matrix_;

  // Model view matrix for center of view
  QMatrix4x4 model_view_center_matrix = model_view_matrix_;
  const Eigen::Vector4f rot_center =
      QMatrixToEigen(model_view_matrix_).inverse() *
      Eigen::Vector4f(0, 0, -focus_distance_, 1);
  model_view_center_matrix.translate(
      rot_center(0), rot_center(1), rot_center(2));

  // Coordinate system
  if (coordinate_grid_enabled_) {
    const QMatrix4x4 pmvc_matrix =
        projection_matrix_ * model_view_center_matrix;
    coordinate_axes_painter_.Render(pmv_matrix, width(), height(), 2);
    coordinate_grid_painter_.Render(pmvc_matrix, width(), height(), 1);
  }

  // Points
  point_painter_.Render(pmv_matrix, point_size_);
  point_cloud_painter_.Render(pmv_matrix, point_size_);
  point_connection_painter_.Render(pmv_matrix, width(), height(), 1);

  // Mesh
  mesh_painter_.Render(pmv_matrix,
                       model_view_matrix_,
                       options_->render->mesh_wireframe,
                       options_->render->mesh_color);

  // Images
  image_line_painter_.Render(pmv_matrix, width(), height(), 1);
  image_triangle_painter_.Render(pmv_matrix);
  image_connection_painter_.Render(pmv_matrix, width(), height(), 1);

  // Movie grabber cameras
  movie_grabber_path_painter_.Render(pmv_matrix, width(), height(), 1.5);
  movie_grabber_line_painter_.Render(pmv_matrix, width(), height(), 1);
  movie_grabber_triangle_painter_.Render(pmv_matrix);
}

void ModelViewerWidget::resizeGL(int width, int height) {
  glViewport(0, 0, width, height);
  ComposeProjectionMatrix();
  UploadCoordinateGridData();
}

void ModelViewerWidget::ReloadReconstruction() {
  if (reconstruction == nullptr) {
    ComputeModelOriginAndScale();
    UploadPointCloudData();
    UploadSurfaceMeshData();
    update();
    return;
  }

  rigs = reconstruction->Rigs();
  cameras = reconstruction->Cameras();
  frames = reconstruction->Frames();
  points3D = reconstruction->Points3D();

  frames.clear();
  images.clear();
  reg_image_ids.clear();
  for (const frame_t frame_id : reconstruction->RegFrameIds()) {
    Frame& frame = frames[frame_id];
    frame = reconstruction->Frame(frame_id);
    frame.SetRigPtr(&rigs[frame.RigId()]);
    for (const data_t& data_id : frame.ImageIds()) {
      Image& image = images[data_id.id];
      image = reconstruction->Image(data_id.id);
      image.SetCameraPtr(&cameras[image.CameraId()]);
      image.SetFramePtr(&frame);
      reg_image_ids.push_back(data_id.id);
    }
  }

  std::sort(reg_image_ids.begin(), reg_image_ids.end());

  if (selected_point3D_id_ != kInvalidPoint3DId &&
      points3D.count(selected_point3D_id_) == 0) {
    selected_point3D_id_ = kInvalidPoint3DId;
  }

  if (selected_image_id_ != kInvalidImageId &&
      !std::binary_search(
          reg_image_ids.begin(), reg_image_ids.end(), selected_image_id_)) {
    selected_image_id_ = kInvalidImageId;
  }

  statusbar_status_label->setText(
      QString().asprintf("%d Frames - %d Images - %d Points",
                         static_cast<int>(reconstruction->NumRegFrames()),
                         static_cast<int>(reg_image_ids.size()),
                         static_cast<int>(points3D.size())));

  Upload();
}

void ModelViewerWidget::ClearReconstruction() {
  cameras.clear();
  images.clear();
  points3D.clear();
  reg_image_ids.clear();
  reconstruction = nullptr;
  point_cloud.reset();
  surface_mesh.reset();
  surface_texture_data.clear();
  surface_texture_width = 0;
  surface_texture_height = 0;
  selected_image_id_ = kInvalidImageId;
  selected_point3D_id_ = kInvalidPoint3DId;
  Upload();
}

int ModelViewerWidget::GetProjectionType() const {
  return options_->render->projection_type;
}

void ModelViewerWidget::SetPointColormap(
    std::unique_ptr<PointColormapBase> colormap) {
  point_colormap_ = std::move(colormap);
}

void ModelViewerWidget::SetImageColormap(
    std::unique_ptr<ImageColormapBase> colormap) {
  image_colormap_ = std::move(colormap);
}

void ModelViewerWidget::UpdateMovieGrabber() {
  UploadMovieGrabberData();
  update();
}

void ModelViewerWidget::EnableCoordinateGrid() {
  coordinate_grid_enabled_ = true;
  update();
}

void ModelViewerWidget::DisableCoordinateGrid() {
  coordinate_grid_enabled_ = false;
  update();
}

void ModelViewerWidget::ChangeFocusDistance(const float delta) {
  if (delta == 0.0f) {
    return;
  }
  const float prev_focus_distance = focus_distance_;
  float diff = delta * ZoomScale() * kFocusSpeed;
  focus_distance_ -= diff;
  if (focus_distance_ < kMinFocusDistance) {
    focus_distance_ = kMinFocusDistance;
    diff = prev_focus_distance - focus_distance_;
  } else if (focus_distance_ > kMaxFocusDistance) {
    focus_distance_ = kMaxFocusDistance;
    diff = prev_focus_distance - focus_distance_;
  }
  const Eigen::Matrix4f vm_mat = QMatrixToEigen(model_view_matrix_).inverse();
  const Eigen::Vector3f tvec(0, 0, diff);
  const Eigen::Vector3f tvec_rot = vm_mat.block<3, 3>(0, 0) * tvec;
  model_view_matrix_.translate(tvec_rot(0), tvec_rot(1), tvec_rot(2));
  ComposeProjectionMatrix();
  UploadCoordinateGridData();
  update();
}

void ModelViewerWidget::ChangeNearPlane(const float delta) {
  if (delta == 0.0f) {
    return;
  }
  near_plane_ *= (1.0f + delta / 100.0f * kNearPlaneScaleSpeed);
  near_plane_ = std::max(kMinNearPlane, std::min(kMaxNearPlane, near_plane_));
  ComposeProjectionMatrix();
  UploadCoordinateGridData();
  update();
}

void ModelViewerWidget::ChangePointSize(const float delta) {
  if (delta == 0.0f) {
    return;
  }
  point_size_ *= (1.0f + delta / 100.0f * kPointScaleSpeed);
  point_size_ = std::max(kMinPointSize, std::min(kMaxPointSize, point_size_));
  update();
}

void ModelViewerWidget::RotateView(const float x,
                                   const float y,
                                   const float prev_x,
                                   const float prev_y) {
  if (x - prev_x == 0 && y - prev_y == 0) {
    return;
  }

  // Rotation according to the Arcball method "ARCBALL: A User Interface for
  // Specifying Three-Dimensional Orientation Using a Mouse", Ken Shoemake,
  // University of Pennsylvania, 1992.

  // Determine Arcball vector on unit sphere.
  const Eigen::Vector3f u = PositionToArcballVector(x, y);
  const Eigen::Vector3f v = PositionToArcballVector(prev_x, prev_y);

  // Angle between vectors.
  const float angle = 2.0f * std::acos(std::min(1.0f, u.dot(v)));

  const float kMinAngle = 1e-3f;
  if (angle > kMinAngle) {
    const Eigen::Matrix4f vm_mat = QMatrixToEigen(model_view_matrix_).inverse();

    // Rotation axis.
    Eigen::Vector3f axis = vm_mat.block<3, 3>(0, 0) * v.cross(u);
    axis = axis.normalized();
    // Center of rotation is current focus.
    const Eigen::Vector4f rot_center =
        vm_mat * Eigen::Vector4f(0, 0, -focus_distance_, 1);
    // First shift to rotation center, then rotate and shift back.
    model_view_matrix_.translate(rot_center(0), rot_center(1), rot_center(2));
    model_view_matrix_.rotate(RadToDeg(angle), axis(0), axis(1), axis(2));
    model_view_matrix_.translate(
        -rot_center(0), -rot_center(1), -rot_center(2));
    update();
  }
}

void ModelViewerWidget::TranslateView(const float x,
                                      const float y,
                                      const float prev_x,
                                      const float prev_y) {
  if (x - prev_x == 0 && y - prev_y == 0) {
    return;
  }

  Eigen::Vector3f tvec(x - prev_x, prev_y - y, 0.0f);

  if (options_->render->projection_type ==
      RenderOptions::ProjectionType::PERSPECTIVE) {
    tvec *= ZoomScale();
  } else if (options_->render->projection_type ==
             RenderOptions::ProjectionType::ORTHOGRAPHIC) {
    tvec *= 2.0f * OrthographicWindowExtent() / height();
  }

  const Eigen::Matrix4f vm_mat = QMatrixToEigen(model_view_matrix_).inverse();

  const Eigen::Vector3f tvec_rot = vm_mat.block<3, 3>(0, 0) * tvec;
  model_view_matrix_.translate(tvec_rot(0), tvec_rot(1), tvec_rot(2));

  update();
}

void ModelViewerWidget::ChangeCameraSize(const float delta) {
  if (delta == 0.0f) {
    return;
  }
  image_size_ *= (1.0f + delta / 100.0f * kImageScaleSpeed);
  image_size_ = std::max(kMinImageSize, std::min(kMaxImageSize, image_size_));
  UploadImageData();
  UploadMovieGrabberData();
  update();
}

void ModelViewerWidget::ResetView() {
  SetupView();
  Upload();
}

QMatrix4x4 ModelViewerWidget::ModelViewMatrix() const {
  return model_view_matrix_;
}

void ModelViewerWidget::SetModelViewMatrix(const QMatrix4x4& matrix) {
  model_view_matrix_ = matrix;
  update();
}

void ModelViewerWidget::SelectObject(const int x, const int y) {
  makeCurrent();

  // Ensure that anti-aliasing does not change the colors of objects.
  glDisable(GL_MULTISAMPLE);

  glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  // Upload data in selection mode (one color per object).
  UploadImageData(true);
  UploadPointData(true);

  // Render in selection mode, with larger points to improve selection accuracy.
  const QMatrix4x4 pmv_matrix = projection_matrix_ * model_view_matrix_;
  image_triangle_painter_.Render(pmv_matrix);
  point_painter_.Render(pmv_matrix, 2 * point_size_);

  const int scaled_x = devicePixelRatio() * x;
  const int scaled_y = devicePixelRatio() * (height() - y - 1);

  QOpenGLFramebufferObjectFormat fbo_format;
  fbo_format.setSamples(0);
  QOpenGLFramebufferObject fbo(1, 1, fbo_format);

  glBindFramebuffer(GL_READ_FRAMEBUFFER, defaultFramebufferObject());
  glBindFramebuffer(GL_DRAW_FRAMEBUFFER, fbo.handle());
  glBlitFramebuffer(scaled_x,
                    scaled_y,
                    scaled_x + 1,
                    scaled_y + 1,
                    0,
                    0,
                    1,
                    1,
                    GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT,
                    GL_NEAREST);

  fbo.bind();
  std::array<uint8_t, 3> color;
  glReadPixels(0, 0, 1, 1, GL_RGB, GL_UNSIGNED_BYTE, color.data());
  fbo.release();

  const size_t index = RGBToIndex(color[0], color[1], color[2]);

  if (index < selection_buffer_.size()) {
    const SelectionType buffer_type = selection_buffer_[index].second;
    if (buffer_type == SelectionType::kImage) {
      selected_image_id_ = static_cast<image_t>(selection_buffer_[index].first);
      selected_point3D_id_ = kInvalidPoint3DId;
      ShowImageInfo(selected_image_id_);
    } else if (buffer_type == SelectionType::kPoint) {
      selected_image_id_ = kInvalidImageId;
      selected_point3D_id_ = selection_buffer_[index].first;
      ShowPointInfo(selection_buffer_[index].first);
    } else {
      selected_image_id_ = kInvalidImageId;
      selected_point3D_id_ = kInvalidPoint3DId;
      image_viewer_widget_->hide();
    }
  } else {
    selected_image_id_ = kInvalidImageId;
    selected_point3D_id_ = kInvalidPoint3DId;
    image_viewer_widget_->hide();
  }

  // Re-enable, since temporarily disabled above.
  glEnable(GL_MULTISAMPLE);

  selection_buffer_.clear();

  UploadPointData();
  UploadImageData();
  UploadPointConnectionData();
  UploadImageConnectionData();

  update();
}

void ModelViewerWidget::SelectMoviewGrabberView(const size_t view_idx) {
  selected_movie_grabber_view_ = view_idx;
  UploadMovieGrabberData();
  update();
}

QImage ModelViewerWidget::GrabImage() {
  makeCurrent();

  DisableCoordinateGrid();

  paintGL();

  const int scaled_width = static_cast<int>(devicePixelRatio() * width());
  const int scaled_height = static_cast<int>(devicePixelRatio() * height());

  QOpenGLFramebufferObjectFormat fbo_format;
  fbo_format.setSamples(0);
  QOpenGLFramebufferObject fbo(scaled_width, scaled_height, fbo_format);

  glBindFramebuffer(GL_READ_FRAMEBUFFER, defaultFramebufferObject());
  glBindFramebuffer(GL_DRAW_FRAMEBUFFER, fbo.handle());
  glBlitFramebuffer(0,
                    0,
                    scaled_width,
                    scaled_height,
                    0,
                    0,
                    scaled_width,
                    scaled_height,
                    GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT,
                    GL_NEAREST);

  fbo.bind();
  QImage image(scaled_width, scaled_height, QImage::Format_RGB888);
  glReadPixels(0,
               0,
               scaled_width,
               scaled_height,
               GL_RGB,
               GL_UNSIGNED_BYTE,
               image.bits());
  fbo.release();

  EnableCoordinateGrid();

#if QT_VERSION >= QT_VERSION_CHECK(6, 9, 0)
  return image.flipped(Qt::Orientation::Vertical);
#else
  return image.mirrored();
#endif
}

void ModelViewerWidget::GrabMovie() { movie_grabber_widget_->show(); }

void ModelViewerWidget::ShowPointInfo(const point3D_t point3D_id) {
  point_viewer_widget_->Show(point3D_id);
}

void ModelViewerWidget::ShowImageInfo(const image_t image_id) {
  image_viewer_widget_->ShowImageWithId(image_id);
}

float ModelViewerWidget::PointSize() const { return point_size_; }

float ModelViewerWidget::ImageSize() const { return image_size_; }

void ModelViewerWidget::SetPointSize(const float point_size) {
  point_size_ = point_size;
}

void ModelViewerWidget::SetImageSize(const float image_size) {
  image_size_ = image_size;
  UploadImageData();
}

void ModelViewerWidget::SetBackgroundColor(const float r,
                                           const float g,
                                           const float b) {
  background_color_[0] = r;
  background_color_[1] = g;
  background_color_[2] = b;
  update();
}

void ModelViewerWidget::mousePressEvent(QMouseEvent* event) {
  if (mouse_press_timer_.isActive()) {  // Select objects (2. click)
    mouse_is_pressed_ = false;
    mouse_press_timer_.stop();
    selection_buffer_.clear();
    SelectObject(event->pos().x(), event->pos().y());
  } else {  // Set timer to remember 1. click
    mouse_press_timer_.setSingleShot(true);
    mouse_press_timer_.start(kDoubleClickInterval);
    mouse_is_pressed_ = true;
    prev_mouse_pos_ = event->pos();
  }
  event->accept();
}

void ModelViewerWidget::mouseReleaseEvent(QMouseEvent* event) {
  mouse_is_pressed_ = false;
  event->accept();
}

void ModelViewerWidget::mouseMoveEvent(QMouseEvent* event) {
  if (mouse_is_pressed_) {
    if (event->buttons() & Qt::RightButton ||
        (event->buttons() & Qt::LeftButton &&
         event->modifiers() & Qt::ControlModifier)) {
      TranslateView(event->pos().x(),
                    event->pos().y(),
                    prev_mouse_pos_.x(),
                    prev_mouse_pos_.y());
    } else if (event->buttons() & Qt::LeftButton) {
      RotateView(event->pos().x(),
                 event->pos().y(),
                 prev_mouse_pos_.x(),
                 prev_mouse_pos_.y());
    }
  }
  prev_mouse_pos_ = event->pos();
  event->accept();
}

void ModelViewerWidget::wheelEvent(QWheelEvent* event) {
  // We don't mind whether horizontal or vertical scroll.
  const float delta = event->angleDelta().x() + event->angleDelta().y();
  if (event->modifiers().testFlag(Qt::ControlModifier)) {
    ChangePointSize(delta);
  } else if (event->modifiers().testFlag(Qt::AltModifier)) {
    ChangeCameraSize(delta);
  } else if (event->modifiers().testFlag(Qt::ShiftModifier)) {
    ChangeNearPlane(delta);
  } else {
    ChangeFocusDistance(delta);
  }
  event->accept();
}

void ModelViewerWidget::SetupPainters() {
  makeCurrent();

  coordinate_axes_painter_.Setup();
  coordinate_grid_painter_.Setup();

  point_painter_.Setup();
  point_connection_painter_.Setup();

  image_line_painter_.Setup();
  image_triangle_painter_.Setup();
  image_connection_painter_.Setup();

  movie_grabber_path_painter_.Setup();
  movie_grabber_line_painter_.Setup();
  movie_grabber_triangle_painter_.Setup();

  point_cloud_painter_.Setup();
  mesh_painter_.Setup();
}

void ModelViewerWidget::SetupView() {
  point_size_ = kInitPointSize;
  image_size_ = kInitImageSize;
  focus_distance_ = kInitFocusDistance;
  model_view_matrix_.setToIdentity();
  model_view_matrix_.translate(0, 0, -focus_distance_);
  model_view_matrix_.rotate(225, 1, 0, 0);
  model_view_matrix_.rotate(-45, 0, 1, 0);
}

void ModelViewerWidget::ComputeModelOriginAndScale() {
  std::vector<double> x_coords;
  std::vector<double> y_coords;
  std::vector<double> z_coords;

  if (!reg_image_ids.empty()) {
    x_coords.resize(reg_image_ids.size());
    y_coords.resize(reg_image_ids.size());
    z_coords.resize(reg_image_ids.size());
    for (size_t i = 0; i < reg_image_ids.size(); ++i) {
      const Image& image = images[reg_image_ids[i]];
      const Eigen::Vector3d proj_center = image.ProjectionCenter();
      x_coords[i] = proj_center.x();
      y_coords[i] = proj_center.y();
      z_coords[i] = proj_center.z();
    }
  } else if (point_cloud.has_value() && !point_cloud->empty()) {
    x_coords.reserve(point_cloud->size());
    y_coords.reserve(point_cloud->size());
    z_coords.reserve(point_cloud->size());
    for (const auto& point : *point_cloud) {
      x_coords.push_back(point.x);
      y_coords.push_back(point.y);
      z_coords.push_back(point.z);
    }
  } else if (surface_mesh.has_value() && !surface_mesh->mesh.vertices.empty()) {
    x_coords.reserve(surface_mesh->mesh.vertices.size());
    y_coords.reserve(surface_mesh->mesh.vertices.size());
    z_coords.reserve(surface_mesh->mesh.vertices.size());
    for (const auto& vertex : surface_mesh->mesh.vertices) {
      x_coords.push_back(vertex.x);
      y_coords.push_back(vertex.y);
      z_coords.push_back(vertex.z);
    }
  } else {
    model_origin_.setZero();
    model_scale_ = 1;
    return;
  }

  model_origin_ =
      -Eigen::Vector3d(Median(x_coords), Median(y_coords), Median(z_coords));
  model_scale_ =
      1. / std::max({Percentile(x_coords, 95) - Percentile(x_coords, 5),
                     Percentile(y_coords, 95) - Percentile(y_coords, 5),
                     Percentile(z_coords, 95) - Percentile(z_coords, 5),
                     1e-6});
}

void ModelViewerWidget::Upload() {
  point_colormap_->Prepare(cameras, images, points3D, reg_image_ids);
  image_colormap_->Prepare(cameras, images, points3D, reg_image_ids);

  ComposeProjectionMatrix();

  ComputeModelOriginAndScale();

  UploadPointData();
  UploadImageData();
  UploadMovieGrabberData();
  UploadPointConnectionData();
  UploadImageConnectionData();
  UploadPointCloudData();
  UploadSurfaceMeshData();

  update();
}

void ModelViewerWidget::UploadCoordinateGridData() {
  makeCurrent();

  const float zoom_scale = ZoomScale();
  const Eigen::Vector3d scaled_origin = model_scale_ * model_origin_;

  // View center grid.
  constexpr double kCenterGridSize = 20;
  coordinate_grid_painter_.Upload(
      {LinePainter::Data(
           PointPainter::Data(scaled_origin(0) - kCenterGridSize * zoom_scale,
                              scaled_origin(1),
                              scaled_origin(2),
                              kGridColor(0),
                              kGridColor(1),
                              kGridColor(2),
                              kGridColor(3)),
           PointPainter::Data(scaled_origin(0) + kCenterGridSize * zoom_scale,
                              scaled_origin(1),
                              scaled_origin(2),
                              kGridColor(0),
                              kGridColor(1),
                              kGridColor(2),
                              kGridColor(3))),
       LinePainter::Data(
           PointPainter::Data(scaled_origin(0),
                              scaled_origin(1) - kCenterGridSize * zoom_scale,
                              scaled_origin(2),
                              kGridColor(0),
                              kGridColor(1),
                              kGridColor(2),
                              kGridColor(3)),
           PointPainter::Data(scaled_origin(0),
                              scaled_origin(1) + kCenterGridSize * zoom_scale,
                              scaled_origin(2),
                              kGridColor(0),
                              kGridColor(1),
                              kGridColor(2),
                              kGridColor(3))),
       LinePainter::Data(
           PointPainter::Data(scaled_origin(0),
                              scaled_origin(1),
                              scaled_origin(2) - kCenterGridSize * zoom_scale,
                              kGridColor(0),
                              kGridColor(1),
                              kGridColor(2),
                              kGridColor(3)),
           PointPainter::Data(scaled_origin(0),
                              scaled_origin(1),
                              scaled_origin(2) + kCenterGridSize * zoom_scale,
                              kGridColor(0),
                              kGridColor(1),
                              kGridColor(2),
                              kGridColor(3)))});

  // Coordinate axes.
  constexpr double kCoordAxesSize = 50;
  coordinate_axes_painter_.Upload(
      {LinePainter::Data(
           PointPainter::Data(scaled_origin(0),
                              scaled_origin(1),
                              scaled_origin(2),
                              kXAxisColor(0),
                              kXAxisColor(1),
                              kXAxisColor(2),
                              kXAxisColor(3)),
           PointPainter::Data(scaled_origin(0) + kCoordAxesSize * zoom_scale,
                              scaled_origin(1),
                              scaled_origin(2),
                              kXAxisColor(0),
                              kXAxisColor(1),
                              kXAxisColor(2),
                              kXAxisColor(3))),
       LinePainter::Data(
           PointPainter::Data(scaled_origin(0),
                              scaled_origin(1),
                              scaled_origin(2),
                              kYAxisColor(0),
                              kYAxisColor(1),
                              kYAxisColor(2),
                              kYAxisColor(3)),
           PointPainter::Data(scaled_origin(0),
                              scaled_origin(1) + kCoordAxesSize * zoom_scale,
                              scaled_origin(2),
                              kYAxisColor(0),
                              kYAxisColor(1),
                              kYAxisColor(2),
                              kYAxisColor(3))),
       LinePainter::Data(
           PointPainter::Data(scaled_origin(0),
                              scaled_origin(1),
                              scaled_origin(2),
                              kZAxisColor(0),
                              kZAxisColor(1),
                              kZAxisColor(2),
                              kZAxisColor(3)),
           PointPainter::Data(scaled_origin(0),
                              scaled_origin(1),
                              scaled_origin(2) + kCoordAxesSize * zoom_scale,
                              kZAxisColor(0),
                              kZAxisColor(1),
                              kZAxisColor(2),
                              kZAxisColor(3)))});
}

void ModelViewerWidget::UploadPointData(const bool selection_mode) {
  makeCurrent();

  std::vector<PointPainter::Data> data;

  // Assume we want to display the majority of points
  data.reserve(points3D.size());

  const size_t min_track_len =
      static_cast<size_t>(options_->render->min_track_len);

  if (selected_image_id_ == kInvalidImageId &&
      images.count(selected_image_id_) == 0) {
    for (const auto& [point3D_id, point3D] : points3D) {
      if (point3D.error <= options_->render->max_error &&
          point3D.track.Length() >= min_track_len) {
        const Eigen::Vector3f xyz =
            (model_scale_ * (point3D.xyz + model_origin_)).cast<float>();

        RGBAColor color;
        if (selection_mode) {
          const size_t index = selection_buffer_.size();
          selection_buffer_.push_back(
              std::make_pair(point3D_id, SelectionType::kPoint));
          color = IndexToRGB(index);

        } else if (point3D_id == selected_point3D_id_) {
          color = kSelectedPointColor;
        } else {
          color = FloatColorToUint8(
              point_colormap_->ComputeColor(point3D_id, point3D));
        }

        data.emplace_back(
            xyz(0), xyz(1), xyz(2), color(0), color(1), color(2), color(3));
      }
    }
  } else {  // Image selected
    const auto& selected_image = images[selected_image_id_];
    for (const auto& [point3D_id, point3D] : points3D) {
      if (point3D.error <= options_->render->max_error &&
          point3D.track.Length() >= min_track_len) {
        const Eigen::Vector3f xyz =
            (model_scale_ * (point3D.xyz + model_origin_)).cast<float>();

        RGBAColor color;
        if (selection_mode) {
          const size_t index = selection_buffer_.size();
          selection_buffer_.push_back(
              std::make_pair(point3D_id, SelectionType::kPoint));
          color = IndexToRGB(index);
        } else if (selected_image.HasPoint3D(point3D_id)) {
          color = kSelectedImagePlaneColor;
        } else if (point3D_id == selected_point3D_id_) {
          color = kSelectedPointColor;
        } else {
          color = FloatColorToUint8(
              point_colormap_->ComputeColor(point3D_id, point3D));
        }

        data.emplace_back(
            xyz(0), xyz(1), xyz(2), color(0), color(1), color(2), color(3));
      }
    }
  }

  point_painter_.Upload(data);
}

void ModelViewerWidget::UploadPointConnectionData() {
  makeCurrent();

  std::vector<LinePainter::Data> line_data;

  if (selected_point3D_id_ == kInvalidPoint3DId) {
    // No point selected, so upload empty data
    point_connection_painter_.Upload(line_data);
    return;
  }

  const auto& point3D = points3D[selected_point3D_id_];
  const Eigen::Vector3f xyz =
      (model_scale_ * (point3D.xyz + model_origin_)).cast<float>();

  // 3D point position.
  LinePainter::Data line;
  line.point1 = PointPainter::Data(xyz(0),
                                   xyz(1),
                                   xyz(2),
                                   kSelectedPointToCamColor(0),
                                   kSelectedPointToCamColor(1),
                                   kSelectedPointToCamColor(2),
                                   kSelectedPointToCamColor(3));

  // All images in which 3D point is observed.
  for (const auto& track_el : point3D.track.Elements()) {
    const Image& conn_image = images[track_el.image_id];
    const Eigen::Vector3f conn_proj_center =
        (model_scale_ * (conn_image.ProjectionCenter() + model_origin_))
            .cast<float>();
    line.point2 = PointPainter::Data(conn_proj_center(0),
                                     conn_proj_center(1),
                                     conn_proj_center(2),
                                     kSelectedPointToCamColor(0),
                                     kSelectedPointToCamColor(1),
                                     kSelectedPointToCamColor(2),
                                     kSelectedPointToCamColor(3));
    line_data.push_back(line);
  }

  point_connection_painter_.Upload(line_data);
}

void ModelViewerWidget::UploadImageData(const bool selection_mode) {
  makeCurrent();

  std::vector<LinePainter::Data> line_data;
  line_data.reserve(8 * reg_image_ids.size());

  std::vector<TrianglePainter::Data> triangle_data;
  triangle_data.reserve(2 * reg_image_ids.size());

  const frame_t selected_frame_id = selected_image_id_ == kInvalidImageId
                                        ? kInvalidFrameId
                                        : images[selected_image_id_].FrameId();

  for (const image_t image_id : reg_image_ids) {
    const Image& image = images[image_id];
    const Camera& camera = cameras[image.CameraId()];

    RGBAColor plane_color;
    RGBAColor frame_color;
    if (selection_mode) {
      const size_t index = selection_buffer_.size();
      selection_buffer_.push_back(
          std::make_pair(image_id, SelectionType::kImage));
      plane_color = frame_color = IndexToRGB(index);
    } else {
      if (image_id == selected_image_id_) {
        plane_color = kSelectedImagePlaneColor;
        frame_color = kSelectedImageFrameColor;
      } else if (selected_frame_id != kInvalidFrameId &&
                 image.FrameId() == selected_frame_id) {
        plane_color = kSelectedFramePlaneColor;
        frame_color = kSelectedFrameFrameColor;
      } else {
        Eigen::Vector4f plane_colorf;
        Eigen::Vector4f frame_colorf;
        image_colormap_->ComputeColor(image, &plane_colorf, &frame_colorf);
        plane_color = FloatColorToUint8(plane_colorf);
        frame_color = FloatColorToUint8(frame_colorf);
      }
    }

    // Lines are not colored with the indexed color in selection mode, so do not
    // show them, so they do not block the selection process
    BuildCameraModel(image.CamFromWorld(),
                     camera,
                     image_size_,
                     plane_color,
                     frame_color,
                     model_origin_,
                     model_scale_,
                     &triangle_data,
                     selection_mode ? nullptr : &line_data);
  }

  image_line_painter_.Upload(line_data);
  image_triangle_painter_.Upload(triangle_data);
}

void ModelViewerWidget::UploadImageConnectionData() {
  makeCurrent();

  std::vector<LinePainter::Data> line_data;
  std::vector<image_t> image_ids;

  if (selected_image_id_ != kInvalidImageId) {
    // Show connections to selected images
    image_ids.push_back(selected_image_id_);
  } else if (options_->render->image_connections) {
    // Show all connections
    image_ids = reg_image_ids;
  } else {  // Disabled, so upload empty data
    image_connection_painter_.Upload(line_data);
    return;
  }

  for (const image_t image_id : image_ids) {
    const Image& image = images.at(image_id);

    const Eigen::Vector3f proj_center =
        (model_scale_ * (image.ProjectionCenter() + model_origin_))
            .cast<float>();

    // Collect all connected images
    std::unordered_set<image_t> conn_image_ids;

    for (const Point2D& point2D : image.Points2D()) {
      if (point2D.HasPoint3D()) {
        const Point3D& point3D = points3D[point2D.point3D_id];
        for (const auto& track_elem : point3D.track.Elements()) {
          conn_image_ids.insert(track_elem.image_id);
        }
      }
    }

    // Selected image in the center.
    LinePainter::Data line;
    line.point1 = PointPainter::Data(proj_center(0),
                                     proj_center(1),
                                     proj_center(2),
                                     kSelectedImageFrameColor(0),
                                     kSelectedImageFrameColor(1),
                                     kSelectedImageFrameColor(2),
                                     204);

    // All connected images to the selected image.
    for (const image_t conn_image_id : conn_image_ids) {
      const Image& conn_image = images[conn_image_id];
      const Eigen::Vector3f conn_proj_center =
          (model_scale_ * (conn_image.ProjectionCenter() + model_origin_))
              .cast<float>();
      line.point2 = PointPainter::Data(conn_proj_center(0),
                                       conn_proj_center(1),
                                       conn_proj_center(2),
                                       kSelectedImageFrameColor(0),
                                       kSelectedImageFrameColor(1),
                                       kSelectedImageFrameColor(2),
                                       204);
      line_data.push_back(line);
    }
  }

  image_connection_painter_.Upload(line_data);
}

void ModelViewerWidget::UploadMovieGrabberData() {
  makeCurrent();

  const size_t num_frames = movie_grabber_widget_->frames.size();

  std::vector<LinePainter::Data> path_data;
  path_data.reserve(num_frames);

  std::vector<LinePainter::Data> line_data;
  line_data.reserve(4 * num_frames);

  std::vector<TrianglePainter::Data> triangle_data;
  triangle_data.reserve(2 * num_frames);

  if (num_frames > 0) {
    const Frame& frame0 = movie_grabber_widget_->frames[0];
    Eigen::Vector3f prev_proj_center =
        frame0.RigFromWorld().TgtOriginInSrc().cast<float>();

    for (size_t i = 1; i < num_frames; ++i) {
      const Frame& frame = movie_grabber_widget_->frames[i];
      const Eigen::Vector3f curr_proj_center =
          frame.RigFromWorld().TgtOriginInSrc().cast<float>();
      LinePainter::Data path;
      path.point1 = PointPainter::Data(prev_proj_center(0),
                                       prev_proj_center(1),
                                       prev_proj_center(2),
                                       kSelectedImagePlaneColor(0),
                                       kSelectedImagePlaneColor(1),
                                       kSelectedImagePlaneColor(2),
                                       kSelectedImagePlaneColor(3));
      path.point2 = PointPainter::Data(curr_proj_center(0),
                                       curr_proj_center(1),
                                       curr_proj_center(2),
                                       kSelectedImagePlaneColor(0),
                                       kSelectedImagePlaneColor(1),
                                       kSelectedImagePlaneColor(2),
                                       kSelectedImagePlaneColor(3));
      path_data.push_back(path);
      prev_proj_center = curr_proj_center;
    }

    // Setup dummy camera with same settings as current OpenGL viewpoint.
    const float kDefaultImageWdith = 2048.0f;
    const float kDefaultImageHeight = 1536.0f;
    const float focal_length =
        -2.0f * std::tan(DegToRad(kFieldOfView) / 2.0f) * kDefaultImageWdith;
    Camera camera =
        Camera::CreateFromModelId(kInvalidCameraId,
                                  SimplePinholeCameraModel::model_id,
                                  focal_length,
                                  kDefaultImageWdith,
                                  kDefaultImageHeight);

    // Build all camera models
    for (size_t i = 0; i < num_frames; ++i) {
      const Frame& frame = movie_grabber_widget_->frames[i];
      RGBAColor plane_color;
      RGBAColor frame_color;
      if (i == selected_movie_grabber_view_) {
        plane_color = kSelectedImagePlaneColor;
        frame_color = kSelectedImageFrameColor;
      } else {
        plane_color = kMovieGrabberImagePlaneColor;
        frame_color = kMovieGrabberImageFrameColor;
      }

      BuildCameraModel(frame.MaybeRigFromWorld(),
                       camera,
                       image_size_,
                       plane_color,
                       frame_color,
                       /*model_origin=*/Eigen::Vector3d::Zero(),
                       /*model_scale=*/1.,
                       &triangle_data,
                       &line_data);
    }
  }

  movie_grabber_path_painter_.Upload(path_data);
  movie_grabber_line_painter_.Upload(line_data);
  movie_grabber_triangle_painter_.Upload(triangle_data);
}

void ModelViewerWidget::UploadPointCloudData() {
  makeCurrent();

  if (!point_cloud.has_value() || point_cloud->empty()) {
    point_cloud_painter_.Upload({});
    return;
  }

  std::vector<PointPainter::Data> data;
  data.reserve(point_cloud->size());

  for (const auto& point : *point_cloud) {
    const float x =
        static_cast<float>(model_scale_ * (point.x + model_origin_(0)));
    const float y =
        static_cast<float>(model_scale_ * (point.y + model_origin_(1)));
    const float z =
        static_cast<float>(model_scale_ * (point.z + model_origin_(2)));
    data.emplace_back(x, y, z, point.r, point.g, point.b, 255);
  }

  point_cloud_painter_.Upload(data);
}

void ModelViewerWidget::UploadSurfaceMeshData() {
  makeCurrent();

  if (!surface_mesh.has_value() || surface_mesh->mesh.faces.empty()) {
    mesh_painter_.Upload({});
    return;
  }

  const PlyMesh& mesh = surface_mesh->mesh;
  const bool has_uvs = !surface_mesh->face_uvs.empty();

  std::vector<MeshPainter::Data> data;
  data.reserve(mesh.faces.size() * 3);

  for (size_t fi = 0; fi < mesh.faces.size(); ++fi) {
    const PlyMeshFace& face = mesh.faces[fi];
    const PlyMeshVertex& v0 = mesh.vertices.at(face.vertex_idx1);
    const PlyMeshVertex& v1 = mesh.vertices.at(face.vertex_idx2);
    const PlyMeshVertex& v2 = mesh.vertices.at(face.vertex_idx3);

    // Transform vertices by model_origin_ and model_scale_
    const Eigen::Vector3f p0(
        static_cast<float>(model_scale_ * (v0.x + model_origin_(0))),
        static_cast<float>(model_scale_ * (v0.y + model_origin_(1))),
        static_cast<float>(model_scale_ * (v0.z + model_origin_(2))));
    const Eigen::Vector3f p1(
        static_cast<float>(model_scale_ * (v1.x + model_origin_(0))),
        static_cast<float>(model_scale_ * (v1.y + model_origin_(1))),
        static_cast<float>(model_scale_ * (v1.z + model_origin_(2))));
    const Eigen::Vector3f p2(
        static_cast<float>(model_scale_ * (v2.x + model_origin_(0))),
        static_cast<float>(model_scale_ * (v2.y + model_origin_(1))),
        static_cast<float>(model_scale_ * (v2.z + model_origin_(2))));

    // Compute face normal from cross product
    const Eigen::Vector3f edge1 = p1 - p0;
    const Eigen::Vector3f edge2 = p2 - p0;
    const Eigen::Vector3f normal = edge1.cross(edge2).normalized();

    if (has_uvs) {
      const float* uvs = &surface_mesh->face_uvs[fi * 6];
      data.emplace_back(p0.x(),
                        p0.y(),
                        p0.z(),
                        normal.x(),
                        normal.y(),
                        normal.z(),
                        uvs[0],
                        uvs[1],
                        v0.r,
                        v0.g,
                        v0.b);
      data.emplace_back(p1.x(),
                        p1.y(),
                        p1.z(),
                        normal.x(),
                        normal.y(),
                        normal.z(),
                        uvs[2],
                        uvs[3],
                        v1.r,
                        v1.g,
                        v1.b);
      data.emplace_back(p2.x(),
                        p2.y(),
                        p2.z(),
                        normal.x(),
                        normal.y(),
                        normal.z(),
                        uvs[4],
                        uvs[5],
                        v2.r,
                        v2.g,
                        v2.b);
    } else {
      data.emplace_back(p0.x(),
                        p0.y(),
                        p0.z(),
                        normal.x(),
                        normal.y(),
                        normal.z(),
                        v0.r,
                        v0.g,
                        v0.b);
      data.emplace_back(p1.x(),
                        p1.y(),
                        p1.z(),
                        normal.x(),
                        normal.y(),
                        normal.z(),
                        v1.r,
                        v1.g,
                        v1.b);
      data.emplace_back(p2.x(),
                        p2.y(),
                        p2.z(),
                        normal.x(),
                        normal.y(),
                        normal.z(),
                        v2.r,
                        v2.g,
                        v2.b);
    }
  }

  mesh_painter_.Upload(data);

  if (!surface_texture_data.empty()) {
    mesh_painter_.UploadTexture(std::move(surface_texture_data),
                                surface_texture_width,
                                surface_texture_height,
                                3);
  }
}

void ModelViewerWidget::ComposeProjectionMatrix() {
  projection_matrix_.setToIdentity();
  if (options_->render->projection_type ==
      RenderOptions::ProjectionType::PERSPECTIVE) {
    projection_matrix_.perspective(
        kFieldOfView, AspectRatio(), near_plane_, kFarPlane);
  } else if (options_->render->projection_type ==
             RenderOptions::ProjectionType::ORTHOGRAPHIC) {
    const float extent = OrthographicWindowExtent();
    projection_matrix_.ortho(-AspectRatio() * extent,
                             AspectRatio() * extent,
                             -extent,
                             extent,
                             near_plane_,
                             kFarPlane);
  }
}

float ModelViewerWidget::ZoomScale() const {
  // "Constant" scale factor w.r.t. zoom-level.
  return 2.0f * std::tan(static_cast<float>(DegToRad(kFieldOfView)) / 2.0f) *
         std::abs(focus_distance_) / height();
}

float ModelViewerWidget::AspectRatio() const {
  return static_cast<float>(width()) / static_cast<float>(height());
}

float ModelViewerWidget::OrthographicWindowExtent() const {
  return std::tan(DegToRad(kFieldOfView) / 2.0f) * focus_distance_;
}

Eigen::Vector3f ModelViewerWidget::PositionToArcballVector(
    const float x, const float y) const {
  Eigen::Vector3f vec(2.0f * x / width() - 1, 1 - 2.0f * y / height(), 0.0f);
  const float norm2 = vec.squaredNorm();
  if (norm2 <= 1.0f) {
    vec.z() = std::sqrt(1.0f - norm2);
  } else {
    vec = vec.normalized();
  }
  return vec;
}

}  // namespace colmap
