/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.

Developer: Chunran Zheng <zhengcr@connect.hku.hk>

For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.

This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/

#ifndef LIVO_POINT_H_
#define LIVO_POINT_H_

#include <boost/noncopyable.hpp>
#include <bitset>
#include "common_lib.h"
#include "frame.h"

class Feature;

// Cross-camera bookkeeping for a VisualPoint, embedded directly in the point
// (rather than a separate map keyed by point) to avoid lookups on the hot
// path. This is the core state behind Omni-LIVO's Cross-View Temporal
// Migration: it tracks which camera(s) currently/previously see the point and
// how many times the point's "primary" observing camera has changed.
#define MAX_CAMERAS 10 // implementation ceiling for the visibility bitsets, not a geometry assumption

struct CrossCameraData {
  std::bitset<MAX_CAMERAS> currently_visible;  // visibility in the current frame
  std::bitset<MAX_CAMERAS> previously_visible; // visibility in the previous frame

  int8_t primary_cam_idx;      // camera currently treated as this point's primary observer (-1 if none yet)
  int8_t previous_cam_idx;     // primary camera as of the previous frame
  int8_t migration_source_cam; // camera the point migrated from, if any
  uint8_t cross_camera_migrations; // number of times the primary camera has changed
  bool has_migration_history;

  int last_seen_frame_id;

  CrossCameraData() :
      primary_cam_idx(-1),
      previous_cam_idx(-1),
      migration_source_cam(-1),
      cross_camera_migrations(0),
      last_seen_frame_id(-1),
      has_migration_history(false)
  {
    currently_visible.reset();
    previously_visible.reset();
  }
};

/// A visual map point on the surface of the scene.
class VisualPoint : boost::noncopyable
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  static const int MAX_OBS = 15; //!< Maximum number of observations to keep per point (bounds obs_ growth).

  Vector3d pos_;                //!< 3d pos of the point in the world coordinate frame.
  Vector3d normal_;             //!< Surface normal at point.
  Matrix3d normal_information_; //!< Inverse covariance matrix of normal estimation.
  Vector3d previous_normal_;    //!< Last updated normal vector.
  list<Feature *> obs_;         //!< Reference patches which observe the point.
  Eigen::Matrix3d covariance_;  //!< Covariance of the point.
  bool is_converged_;           //!< True if the point is converged.
  bool is_normal_initialized_;  //!< True if the normal is initialized.
  bool has_ref_patch_;          //!< True if the point has a reference patch.
  Feature *ref_patch;           //!< Reference patch of the point.

  CrossCameraData cross_cam_data_; //!< Cross-camera visibility/migration bookkeeping for this point.

  std::bitset<MAX_CAMERAS> visible_cameras_cache_; //!< Cache of which cameras recently saw this point.
  int cache_frame_id_;                             //!< Frame id the above cache was computed for.

  VisualPoint(const Vector3d &pos);
  ~VisualPoint();
  void findMinScoreFeature(const Vector3d &framepos, Feature *&ftr) const;
  void deleteNonRefPatchFeatures();
  void deleteFeatureRef(Feature *ftr);
  void addFrameRef(Feature *ftr);
  bool getCloseViewObs(const Vector3d &pos, Feature *&obs, const Vector2d &cur_px) const;
};

#endif // LIVO_POINT_H_
