/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          https://www.mrpt.org/                            |
   |                                                                           |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file        |
   | See: https://www.mrpt.org/Authors - All rights reserved.                  |
   | Released under BSD License. See details in https://www.mrpt.org/License   |
   +---------------------------------------------------------------------------+
   */
#include "CGLWidget.h"

#include <mrpt/config/CConfigFile.h>
#include <mrpt/gui/CGlCanvasBase.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/maps/TMetricMapInitializer.h>
#include <mrpt/math/TLine3D.h>
#include <mrpt/math/TObject3D.h>
#include <mrpt/math/geometry.h>
#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/opengl/CTexturedPlane.h>
#include <mrpt/opengl/stock_objects.h>

#include <QApplication>
#include <QMouseEvent>
#include <cmath>

#include "CDocument.h"

// Include libraries in linking (needed for Windows)
#include <mrpt/config.h>
#if defined(_MSC_VER)
#pragma comment(lib, "opengl32.lib")
#endif

using namespace mrpt;
using namespace mrpt::opengl;

CGlWidget::CGlWidget(bool is2D, QWidget* parent) :
    CQtGlCanvasBase(parent),
    m_groundPlane(std::make_shared<CGridPlaneXY>(-200, 200, -200, 200, 0, 5)),
    m_doc(nullptr),
    m_miniMapSize(-1.0),
    m_minimapPercentSize(0.25),
    m_observationSize(10.),
    m_observationColor(mrpt::img::TColor::red()),
    m_selectedObsSize(15.0),
    m_selectedColor(mrpt::img::TColor::green()),
    m_isShowObs(false),
    m_visiblePoints(std::make_shared<CSetOfObjects>()),
    m_currentObs(opengl::stock_objects::CornerXYZSimple()),
    m_line(std::make_shared<CSetOfLines>()),
    m_is2D(is2D),
    m_showRobot(false),
    m_moveSelected(false),
    m_pressedPos(0, 0),
    m_lastPos(0, 0)
{
  // setClearColors(1.0, 1.0, 1.0);
  setMinimumZoom(1.0f);
  setMaximumZoom(3200.0f);

  if (m_is2D)
  {
    m_miniMapViewport = getOpenGLSceneRef()->createViewport("miniMap");
    m_miniMapViewport->setBorderSize(2);
    m_miniMapViewport->setViewportPosition(0.01, 0.01, m_minimapPercentSize, m_minimapPercentSize);
    m_miniMapViewport->setTransparent(false);

    setAzimuthDegrees(-90.0f);
    setElevationDegrees(90.0f);

    CCamera& camMiniMap = m_miniMapViewport->getCamera();
    updateCameraParams(camMiniMap);
    camMiniMap.setOrthogonal();
  }

  updateCamerasParams();
  m_groundPlane->setColor(0.4f, 0.4f, 0.4f);
  setVisibleGrid(true);
  setFocusPolicy(Qt::StrongFocus);
}

CGlWidget::~CGlWidget() = default;
void CGlWidget::fillMap(const CSetOfObjects::Ptr& renderizableMap)
{
  removeFromMap(m_map);
  insertToMap(renderizableMap);
  m_map = renderizableMap;
  float xMin = 0;
  float xMax = 0;
  float yMin = 0;
  float yMax = 0;

  if (m_is2D)
  {
    if (m_map->size() == 1)
    {
      CRenderizable* ren = m_map->begin()->get();
      auto* textured = dynamic_cast<CTexturedPlane*>(ren);
      if (textured)
      {
        textured->getPlaneCorners(xMin, xMax, yMin, yMax);
      }
      else
      {
        auto* points = dynamic_cast<CPointCloud*>(ren);
        if (points)
        {
          const std::vector<mrpt::math::TPoint3Df>& pts = points->getArrayPoints();

          for (const auto& pt : pts)
          {
            mrpt::keep_min(xMin, pt.x);
            mrpt::keep_min(yMin, pt.y);

            mrpt::keep_max(xMax, pt.x);
            mrpt::keep_max(yMax, pt.y);
          }
        }
      }
    }

    float xDist = xMax - xMin;
    float yDist = yMax - yMin;

    CCamera& camMiniMap = m_miniMapViewport->getCamera();
    updateCameraParams(camMiniMap);
    camMiniMap.setZoomDistance(std::max(xDist, yDist));
    updateCamerasParams();
  }

  if (m_isShowObs)
    setSelectedObservation(m_isShowObs);
  else
    update();
}

void CGlWidget::setSelected(const math::TPose3D& pose)
{
  if (!m_map) return;

  removeRobotDirection();

  m_showRobot = true;

  m_currentObs->setPose(pose);
  m_map->insert(m_currentObs);

  update();
}

void CGlWidget::setSelectedObservation(bool is)
{
  m_isShowObs = is;
  if (!m_doc || !m_map) return;

  if (is)
    m_map->insert(m_visiblePoints);

  else
  {
    m_map->removeObject(m_visiblePoints);
    deselectAll();
  }

  update();
}

void CGlWidget::setLaserScan(CPlanarLaserScan::Ptr laserScan)
{
  if (m_currentLaserScan) m_map->removeObject(m_currentLaserScan);

  m_currentLaserScan = laserScan;
  m_map->insert(m_currentLaserScan);

  update();
}

void CGlWidget::setDocument(CDocument* doc)
{
  m_doc = doc;

  if (m_isShowObs) m_map->removeObject(m_visiblePoints);

  deselectAll();
  m_visiblePoints->clear();

  size_t id = 0;
  for (const auto& poseSf : m_doc->simplemap())
  {
    math::TPose3D pose = poseSf.pose->getMeanVal().asTPose();
    CRobotPose::Ptr robotPose = std::make_shared<CRobotPose>(id);
    robotPose->setPose(pose);
    m_visiblePoints->insert(robotPose);
    ++id;
  }

  if (m_isShowObs) setSelectedObservation(m_isShowObs);
}

void CGlWidget::updateObservations()
{
  bool isShowObs = m_isShowObs;

  std::vector<CRobotPose::Ptr> selectedPoints = m_selectedPoints;

  if (isShowObs) setSelectedObservation(false);
  m_visiblePoints->clear();

  int id = 0;
  for (const auto& poseSf : m_doc->simplemap())
  {
    math::TPose3D pose = poseSf.pose->getMeanVal().asTPose();
    CRobotPose::Ptr robotPose = std::make_shared<CRobotPose>(id);
    robotPose->setPose(pose);
    m_visiblePoints->insert(robotPose);
    ++id;
  }

  double maxDist = maximumSizeObservation(QPoint(0, 0));
  ASSERT_(maxDist != -1.0);

  for (auto& it : selectedPoints) selectPoint(it->getId());

  emit selectedChanged(m_selectedPoints);

  if (isShowObs) setSelectedObservation(isShowObs);
}

void CGlWidget::setZoom(float zoom)
{
  CamaraParams params = cameraParams();
  params.cameraZoomDistance = zoom;
  setCameraParams(params);
  updateCamerasParams();

  update();
}

float CGlWidget::getZoom() const { return getZoomDistance(); }
void CGlWidget::setCameraParams(const gui::CGlCanvasBase::CamaraParams& params)
{
  CGlCanvasBase::CamaraParams cam = cameraParams();
  if (cam.cameraAzimuthDeg != params.cameraAzimuthDeg) emit azimuthChanged(params.cameraAzimuthDeg);

  if (cam.cameraElevationDeg != params.cameraElevationDeg)
    emit elevationChanged(params.cameraElevationDeg);

  CQtGlCanvasBase::setCameraParams(params);
}
void CGlWidget::setAzimuthDegrees(float ang)
{
  CQtGlCanvasBase::setAzimuthDegrees(ang);
  emit azimuthChanged(ang);
  updateCamerasParams();
}

void CGlWidget::setElevationDegrees(float ang)
{
  CQtGlCanvasBase::setElevationDegrees(ang);
  emit elevationChanged(ang);
  updateCamerasParams();
}

void CGlWidget::setBackgroundColor(float r, float g, float b, float a)
{
  // setClearColors(r, g, b, a);

  update();
}

void CGlWidget::setGridColor(double r, double g, double b, double a)
{
  m_groundPlane->setColor(r, g, b, a);
  update();
}

void CGlWidget::setVisibleGrid(bool is)
{
  if (is)
    insertToMap(m_groundPlane);
  else
    removeFromMap(m_groundPlane);
}

bool CGlWidget::setBot(int value)
{
  if (!m_map) return false;

  math::TPose3D pose;

  m_map->removeObject(m_currentObs);
  if (m_showRobot) pose = m_currentObs->getPose();

  switch (value)
  {
    case 0:
      m_currentObs = opengl::stock_objects::CornerXYZSimple();
      break;
    case 1:
      m_currentObs = opengl::stock_objects::CornerXYZ();
      break;
    case 2:
      m_currentObs = opengl::stock_objects::RobotGiraff();
      break;
    case 3:
      m_currentObs = opengl::stock_objects::RobotRhodon();
      break;
    case 4:
      m_currentObs = opengl::stock_objects::RobotPioneer();
      break;
    case 5:
      m_currentObs = opengl::stock_objects::BumblebeeCamera();
      break;
    default:
      m_currentObs = opengl::stock_objects::CornerXYZSimple();
      break;
  }
  if (m_showRobot)
  {
    m_currentObs->setPose(pose);
    m_map->insert(m_currentObs);
  }

  return true;
}

bool CGlWidget::setObservationSize(double s)
{
  if (m_observationSize != s)
  {
    m_observationSize = s;
    return true;
  }
  return false;
}

bool CGlWidget::setObservationColor(int type)
{
  img::TColorf color = typeToColor(type);
  if (color.R != m_observationColor.R || color.B != m_observationColor.B ||
      color.G != m_observationColor.G || color.A != m_observationColor.A)
  {
    m_observationColor = color;
    m_visiblePoints->setColor(m_observationColor);
    return true;
  }
  return false;
}

bool CGlWidget::setSelectedObservationSize(double s)
{
  if (m_selectedObsSize != s)
  {
    m_selectedObsSize = s;
    return true;
  }
  return false;
}

bool CGlWidget::setSelectedObservationColor(int type)
{
  img::TColorf color = typeToColor(type);
  if (color.R != m_selectedColor.R || color.B != m_selectedColor.B ||
      color.G != m_selectedColor.G || color.A != m_selectedColor.A)
  {
    m_selectedColor = color;
    return true;
  }
  return false;
}

void CGlWidget::updateSelectionWithoutSignals(const std::vector<size_t>& idx)
{
  blockSignals(true);
  deselectAll();

  for (auto& id : idx) selectPoint(id);

  blockSignals(false);
}

void CGlWidget::resizeGL(int width, int height)
{
  CQtGlCanvasBase::resizeGL(width, height);

  if (m_is2D)
  {
    GLint win_dims[4];
    glGetIntegerv(GL_VIEWPORT, win_dims);
    m_miniMapSize = std::min(win_dims[2], win_dims[3]) * m_minimapPercentSize;
  }
  updateMinimapPos();
}

void CGlWidget::updateCamerasParams()
{
  float zoom = getCameraZoomDistance();
  CQtGlCanvasBase::updateCamerasParams();

  if (m_is2D)
  {
    CCamera& camMiniMap = m_miniMapViewport->getCamera();
    float zoomMiniMap = camMiniMap.getZoomDistance();
    updateCameraParams(camMiniMap);
    camMiniMap.setProjectiveModel(false);
    camMiniMap.setZoomDistance(zoomMiniMap);
  }

  if (zoom != getZoomDistance()) emit zoomChanged(getZoomDistance());
}

void CGlWidget::insertToMap(const CRenderizable::Ptr& newObject)
{
  CQtGlCanvasBase::insertToMap(newObject);
  if (m_is2D)
  {
    ASSERT_(m_miniMapViewport);
    m_miniMapViewport->insert(newObject);
  }
}

void CGlWidget::removeFromMap(const CRenderizable::Ptr& newObject)
{
  CQtGlCanvasBase::removeFromMap(newObject);
  if (m_is2D)
  {
    ASSERT_(m_miniMapViewport);
    m_miniMapViewport->removeObject(newObject);
  }
}

void CGlWidget::mouseMoveEvent(QMouseEvent* event)
{
  CQtGlCanvasBase::mouseMoveEvent(event);

  {
    std::pair<bool, math::TPoint3D> scenePos = sceneToWorld(event->pos());
    if (scenePos.first) emit mousePosChanged(scenePos.second.x, scenePos.second.y);
  }

  if (m_moveSelected)
  {
    auto scenePos = sceneToWorld(m_lastPos);
    auto sceneOtherPos = sceneToWorld(event->pos());
    if (scenePos.first && sceneOtherPos.first)
    {
      double distX = scenePos.second.x - sceneOtherPos.second.x;
      double distY = scenePos.second.y - sceneOtherPos.second.y;
      if (distX != 0.0 || distY != 0.0)
      {
        for (auto& it : m_selectedPoints)
        {
          CRobotPose::Ptr robotPose = std::dynamic_pointer_cast<CRobotPose>(it);
          mrpt::math::TPose3D pose = robotPose->getPose();
          pose.x -= distX;
          pose.y -= distY;
          robotPose->setPose(pose);
        }
        update();
      }
    }
  }
  m_lastPos = event->pos();
}

void CGlWidget::mousePressEvent(QMouseEvent* event)
{
  CQtGlCanvasBase::mousePressEvent(event);

  if (!m_isShowObs) return;

  m_moveSelected = false;

  QPoint pos = event->pos();
  m_pressedPos = pos;
  m_lastPos = pos;
  bool isPressCtrl = event->modifiers() == Qt::ControlModifier;
  bool noModifier = event->modifiers() == Qt::NoModifier;
  QPoint otherPos(pos.x() + m_observationSize, pos.y() + m_observationSize);

  auto scenePos = sceneToWorld(pos);
  auto sceneOtherPos = sceneToWorld(otherPos);
  if (scenePos.first && sceneOtherPos.first)
  {
    bool needUnpressMouse = false;
    bool needUpdateScene = false;

    double maxDist = maximumSizeObservation(pos);
    int selectedIndex = searchSelectedPose(scenePos.second.x, scenePos.second.y, maxDist);

    if (selectedIndex != -1)
    {
      m_moveSelected = noModifier;

      if (isPressCtrl)
      {
        removePoseFromSelected(selectedIndex);
        needUpdateScene = true;
      }
      needUnpressMouse = noModifier || isPressCtrl;
    }
    else if (!isPressCtrl)
      needUpdateScene = deselectAll();

    if (!needUnpressMouse)
    {
      needUnpressMouse = selectPoint(scenePos.second.x, scenePos.second.y, maxDist);
      if (needUnpressMouse)
      {
        needUpdateScene = needUnpressMouse;
        m_moveSelected = needUnpressMouse;
      }
    }

    if (needUnpressMouse) unpressMouseButtons();

    if (needUpdateScene) update();
  }
}

void CGlWidget::mouseReleaseEvent(QMouseEvent* event)
{
  CQtGlCanvasBase::mouseReleaseEvent(event);

  if (m_moveSelected)
  {
    std::vector<size_t> idx;
    for (auto& it : m_selectedPoints) idx.push_back(it->getId());

    auto scenePos = sceneToWorld(m_pressedPos);
    auto sceneOtherPos = sceneToWorld(event->pos());
    if (scenePos.first && sceneOtherPos.first)
    {
      QPointF dist;
      dist.setX(sceneOtherPos.second.x - scenePos.second.x);
      dist.setY(sceneOtherPos.second.y - scenePos.second.y);
      if (dist.x() != 0.0 || dist.y() != 0.0) emit moveRobotPoses(idx, dist);
    }
    m_moveSelected = false;
  }
  m_pressedPos = event->pos();
}

void CGlWidget::keyPressEvent(QKeyEvent* event)
{
  CQtGlCanvasBase::keyPressEvent(event);
  if (event->key() == Qt::Key_Delete)
  {
    std::vector<size_t> idx;
    for (auto& it : m_selectedPoints) idx.push_back(it->getId());
    deselectAll();
    emit deleteRobotPoses(idx);
  }
}

img::TColorf CGlWidget::typeToColor(int type) const
{
  mrpt::img::TColor color = mrpt::img::TColor::red();
  switch (type)
  {
    case 0:
      color = mrpt::img::TColor::red();
      break;
    case 1:
      color = mrpt::img::TColor::green();
      break;
    case 2:
      color = mrpt::img::TColor::blue();
      break;
    case 3:
      color = mrpt::img::TColor::white();
      break;
    case 4:
      color = mrpt::img::TColor::black();
      break;
    case 5:
      color = mrpt::img::TColor::gray();
      break;
    default:
      break;
  }
  return mrpt::img::TColorf(color);
}

std::pair<bool, math::TPoint3D> CGlWidget::sceneToWorld(const QPoint& pos) const
{
  mrpt::math::TLine3D outRay;
  mainViewport()->get3DRayForPixelCoord(pos.x(), pos.y(), outRay);

  const mrpt::math::TPlane groundPlane(
      mrpt::math::TPoint3D(0, 0, 0), mrpt::math::TPoint3D(1, 0, 0), mrpt::math::TPoint3D(0, 1, 0));
  // Intersection of the line with the plane:
  mrpt::math::TObject3D inters;
  mrpt::math::intersect(outRay, groundPlane, inters);
  // Interpret the intersection as a point, if there is an intersection:
  mrpt::math::TPoint3D intersPt;
  bool converted = inters.getPoint(intersPt);
  return std::make_pair(converted, intersPt);
}

double CGlWidget::maximumSizeObservation(const QPoint& pos) const
{
  QPoint otherPos(pos.x() + m_observationSize, pos.y() + m_observationSize);

  auto scenePos = sceneToWorld(pos);
  auto sceneOtherPos = sceneToWorld(otherPos);
  if (!scenePos.first || !sceneOtherPos.first) return -1.0;

  double xDistPow = std::pow(sceneOtherPos.second.x - scenePos.second.x, 2);
  double yDistPow = std::pow(sceneOtherPos.second.y - scenePos.second.y, 2);
  double maxDist = xDistPow + yDistPow;

  return maxDist;
}

bool CGlWidget::selectPoint(float x, float y, double maxDist)
{
  int poseIndex = searchPose(x, y, maxDist);
  if (poseIndex != -1)
  {
    selectPoint((size_t)poseIndex);
    return true;
  }
  return false;
}

void CGlWidget::selectPoint(size_t poseIndex)
{
  auto robotPose = getRobotPose(poseIndex);
  robotPose->setSelected(true);
  m_selectedPoints.push_back(robotPose);
  removeRobotDirection();
  emit selectedChanged(m_selectedPoints);

  if (m_selectedPoints.size() == 1)
  {
    mrpt::math::TPose3D pose = robotPose->getPose();
    emit showPoseDirection(robotPose->getId(), pose.yaw, pose.pitch, pose.roll);
  }
}

template <class Container>
int CGlWidget::searchPoseFromList(float x, float y, double maxDist, Container list) const
{
  std::map<double, int> minDistPos;
  int i = 0;

  for (auto& it : list)
  {
    mrpt::math::TPose3D pos = it->getPose();
    double dist = std::pow(x - pos.x, 2) + std::pow(y - pos.y, 2);
    if (dist < maxDist) minDistPos.emplace(dist, i);
    ++i;
  }

  if (minDistPos.empty()) return -1;

  return minDistPos.begin()->second;
}

CRobotPose::Ptr CGlWidget::removePoseFromPointsCloud(CSetOfObjects::Ptr points, int index) const
{
  auto it = points->begin() + index;
  ASSERT_(it != points->end());
  CRobotPose::Ptr robotPose = std::dynamic_pointer_cast<CRobotPose>(*it);
  ASSERT_(robotPose);
  points->removeObject(*it);
  return robotPose;
}

void CGlWidget::removeRobotDirection()
{
  if (!m_map.get()) return;
  m_map->removeObject(m_currentObs);

  if (m_currentLaserScan) m_map->removeObject(m_currentLaserScan);

  m_showRobot = false;
}

void CGlWidget::updateMinimapPos()
{
  if (!m_is2D) return;

  GLint win_dims[4];
  glGetIntegerv(GL_VIEWPORT, win_dims);

  Viewport::Ptr miniMap = getOpenGLSceneRef()->getViewport("miniMap");
  float w = m_miniMapSize / win_dims[2];
  float h = m_miniMapSize / win_dims[3];
  miniMap->setViewportPosition(0.01, 0.01, w, h);
}

int CGlWidget::searchSelectedPose(float x, float y, double maxDist)
{
  return searchPoseFromList(x, y, maxDist, m_selectedPoints);
}

int CGlWidget::searchPose(float x, float y, double maxDist)
{
  return searchPoseFromList(x, y, maxDist, *m_visiblePoints);
}

void CGlWidget::removePoseFromSelected(int index)
{
  auto it = m_selectedPoints.begin() + index;
  ASSERT_(it != m_selectedPoints.end());
  CRobotPose::Ptr robotPose = std::dynamic_pointer_cast<CRobotPose>(*it);
  ASSERT_(robotPose);
  robotPose->setSelected(false);
  m_selectedPoints.erase(it);
  emit selectedChanged(m_selectedPoints);
}

void CGlWidget::selectPose(CRobotPose::Ptr robotPose)
{
  robotPose->setSelected(true);
  m_selectedPoints.push_back(robotPose);
  emit selectedChanged(m_selectedPoints);
  removeRobotDirection();
}

bool CGlWidget::deselectAll()
{
  bool changedVectors = false;

  for (auto& it : m_selectedPoints)
  {
    CRobotPose::Ptr robotPose = std::dynamic_pointer_cast<CRobotPose>(it);
    ASSERT_(robotPose);
    robotPose->setSelected(false);
    changedVectors = true;
  }

  m_selectedPoints.clear();
  emit selectedChanged(m_selectedPoints);
  removeRobotDirection();
  return changedVectors;
}

CRobotPose::Ptr CGlWidget::getRobotPose(size_t index)
{
  auto it = m_visiblePoints->begin() + index;
  ASSERT_(it != m_visiblePoints->end());
  CRobotPose::Ptr robotPose = std::dynamic_pointer_cast<CRobotPose>(*it);
  ASSERT_(robotPose);
  return robotPose;
}
