/********************************************************************************

INTEL CORPORATION PROPRIETARY INFORMATION This software is supplied under the
terms of a license agreement or nondisclosure agreement with Intel Corporation
and may not be copied or disclosed except in accordance with the terms of that
agreement.
Copyright(c) 2011-2016 Intel Corporation. All Rights Reserved.

 *********************************************************************************/

#pragma once
#include <memory>
#include "slam_utils.h"
#include "rs/core/status.h"
#include "rs/core/correlated_sample_set.h"

namespace rs
{
namespace slam
{

enum class tracking_accuracy : int32_t
{
    failed,    /* No confidence in camera pose estimation      */
    low,	   /* Low confidence in camera pose estimation     */
    medium,	   /* Medium confidence in camera pose estimation  */
    high,	   /* High confidence in camera pose estimation    */
};


/**
 * Container to hold occupancy map data.
 * +NOTE: see create_occupancy_map(...) to allocate container
 * +NOTE: see get_occupancy_map(...) and get_occupancy_map_update(...) to update data stored in
 * 		  container with current reconstruction
 */
class occupancy_map
{
public:
    /**
     * @brief: Resizes the container to hold specified number of tiles.
     */
    virtual void resize(const int tile_count) = 0;

    /**
     * @brief: To get current number of tiles available in the container.
     */
    virtual int get_tile_count() const = 0;

    /**
     * @brief: To get tile co-ordinate array. Length of integer array is 3*get_tile_count().
     * Each tile is represented 3-integer tuple (x, y, occupancy value).
     * x and y are locations while occupancy value is in the range [0, 100].
     * Note that tile locations are defined in multiples of cell units, and hence, the actual dimension
     * can be calculated as (tile location * cell dimension), where cell dimension is set as the
     * resolution value by function get_occupancy_map_resolution.
     */
    virtual const int32_t *get_tile_coordinates() const = 0;

    /**
     * @brief: To get maximum number of tile can be store by container
     */
    virtual int32_t get_capacity() const = 0;

    /**
     * @brief: To remove all tiles present in the container.
     */
    virtual void clear() = 0;

    virtual ~occupancy_map(){}
};

/**
 * Interface to register for tracking state change events
 */
class tracking_event_handler
{
public:
    /**
     * @brief: Callback function invoked when slam is restarted due to restart(...) event
     */
    virtual void on_restart() = 0;
    virtual ~tracking_event_handler() {}
};

class slam_core
{

public:

    virtual ~slam_core(){}

    /**
     * @brief: To create container to hold occupancy map data.
     *
     * @param[in] tile_count: Specifies maximum number of tiles container can hold at a time.
     *
     * @returns: Allocated shared handle to the container.
     */
    virtual std::shared_ptr<occupancy_map> create_occupancy_map(const int tile_count) const = 0;

    /**
     * @brief: To create container to hold occupancy map data using specified pre-allocated tile coordinate array.
     * Memory of specified tile array is managed by client. Container dosn't take over ownership of the supplied memory array.
     *
     * @param[in] pre_allocated_tiles: Tile co-ordinate array pre-allocated by client.
     *
     * @param[in] tile_count: Tile capacity of pre_allocated_tiles [(count of number of int's that can be stored by pre_allocated_tiles) / 3 ].
     *
     * @returns: Allocated shared handle to the container.
     */
    virtual std::shared_ptr<occupancy_map> create_occupancy_map(int32_t *pre_allocated_tiles, const int32_t tile_count) const = 0;

    /**
     * @brief: To get current pose of the camera.
     *
     * @param[out] pose: Output camera pose.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status get_camera_pose(PoseMatrix4f &pose) const = 0;

    /**
     * @brief: Moves slam's internal camera to the specified pose.
     *
     * @param[in] pose: Camera pose to set. If occupancy mapping is used,
     * the camera pose must be aligned with gravity.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status set_camera_pose(const PoseMatrix4f &pose) = 0;

    /**
     * @brief: set_auto_occupancy_map_building allows enabling and disabling the automatic occupancy
     * map building. By default, automatic map building is enabled.
     * When it is enabled, passing inputs to process_sample_set_async
     * will also update the occupancy map. To manually build occupancy map with
     * update_occupancy_map, set_auto_occupancy_map_building should be called to disable
     * automatic mapping.
     *
     * @param[in] enable_map_building: If the value is false, map building will be
     * disabled; otherwise, it is enabled.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status set_auto_occupancy_map_building(const bool enable_map_building) = 0;

    /**
     * @brief: is_auto_occupancy_map_building_enabled indicates if the automatic occupancy map building is
     * enabled or disabled. When it is enabled, passing inputs to process_sample_set_sync or
     * process_sample_set_async will also update the occupancy map.
     *
     * @return: Flag indicating whether occupancy map is enabled.
     */
    virtual bool is_auto_occupancy_map_building_enabled() const = 0;

    /**
     * @brief: set_occupancy_map_height_of_interest sets the vertical range of interest for
     * the occupancy map. Regions outside this vertical range are ignored.
     *
     * @param[in] min_height: Minimum height of interest for construction occupancy map . Height is
     * measured from the current camera level, where negative numbers represent
     * the heights below the camera level, and positive numbers, above the camera level.
     *
     * @param[in] max_height: Maximum height of interest for construction occupancy map. Height is
     * measured from the current camera level, where negative numbers represent
     * the heights below the camera level, and positive numbers, above the camera level.
     * Value of max_height must be bigger than value of the parameter min_height.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status set_occupancy_map_height_of_interest(const float min_height, const float max_height) = 0;

    /**
     * @brief: get_occupancy_map_height_of_interest gets the thresholds for the height range of interest in
     * occupancy mapping. Regions whose height values are outside the range are ignored when
     * constructing the occupancy map.
     *
     * @param[out] min_height: To store current minimum height of interest for occupancy map construction.
     * Height is measured from the current camera level, where negative numbers represent
     * the heights below the camera level, and positive numbers, above the camera level.
     *
     * @param[out] max_height: To store maximum height of interest for  occupancy map construction.
     * Height is measured from the current camera level, where negative numbers represent
     * the heights below the camera level, and positive numbers, above the camera level.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status get_occupancy_map_height_of_interest(float &min_height, float &max_height) const = 0;

    /**
     * @brief set_occupancy_map_resolution sets the grid resolution of occupancy map construction.
     * The resolution defines the size of each square grid cell in the map.
     * The function should only be called prior to calling the first process_sample_set_async
     * to avoid corrupting the occupancy map.
     *
     * @param[in] resolution: Resolution for occupancy map construction in meters. Occupancy map Grid cells'
     * dimension is defined by the resolution value.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status set_occupancy_map_resolution(const float resolution) = 0;

    /**
     * @brief: get_occupancy_map_resolution gets the grid resolution of the occupancy map.
     * The resolution defines the size of each square grid cell in the map.
     *
     * @return resolution of occupancy map.
     */
    virtual float get_occupancy_map_resolution() const = 0;

    /**
     * @brief: update_occupancy_map updates occupancy map using the given depth image and pose
     * of the camera for the image. The function should be called if automatic occupancy mapping is disabled to force
     * occupancy map construction on demand.
     *
     * @param[in] pose: Camera pose associate with sample_set.
     * The pose must be aligned with gravity (vector) in order for occupancy mapping to work.
     *
     * @param[in] sample_set: Input stream sample containing depth image.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     *
     * \see set_auto_occupancy_map_building
     */
    virtual rs::core::status update_occupancy_map(const PoseMatrix4f &pose, rs::core::correlated_sample_set &sample_set) = 0;

    /**
     * @brief: get_occupancy_map returns ALL the tiles in the occupancy map construction that have occupancy values.
     *
     * @param[out] occ_map: Pre-allocated instance of occupancy_map. After successful
     * execution of the function this container will be updated with occupancy map data.
     *
     * @returns: If successful it returns status_no_error, error code will be on failure, warning
     * status_data_pending would be returned if all occupancy_map cannot accommodate all the available tiles,
     * in this case make additional call to get_occupancy_map(...) to fetch remaining tiles until status_no_error is returned.
     */
    virtual rs::core::status get_occupancy_map(std::shared_ptr<occupancy_map> occ_map) const = 0;

    /**
     * @brief: To get_occupancy_map within specified region of interest.
     *
     * @param[out] occ_map: pre-allocated instance of occupancy_map.
     *
     * @param[in] minPoint: minimum of x and y co-ordinate of region of interest.
     *
     * @param[in] maxPoint: maximum of x and y co-ordinate of region of interest.
     *
     * @returns: If successful it returns status_no_error, error code will be on failure, warning
     * status_data_pending would be returned if all occupancy_map cannot accommodate all the available tiles,
     * in this case make additional call to get_occupancy_map(...) to fetch remaining tiles until status_no_error is returned.
     *
     * Note that range values are in the same unit as the resolution value.
     */
    virtual rs::core::status get_occupancy_map(std::shared_ptr<occupancy_map> occ_map, const rs::core::pointI32 &minPoint, const rs::core::pointI32 &maxPoint) const = 0;

    /**
     * @brief: To obtain the occupancy map data that have
     * been modified since the last successful call to get_occupancy_map_update(...)
     * when is_auto_occupancy_map_building_enabled is enabled.
     *
     * @param[out] occ_map: Pre-allocated instance of occupancy_map. After successful
     * execution of the function this container will be updated with occupancy map data.
     *
     * @returns: If successful it returns status_no_error, error code will be on failure, warning
     * status_data_pending would be returned if all occupancy_map cannot accommodate all the available tiles,
     * in this case make additional call later to get_occupancy_map_update(...) to fetch remaining tiles until status_no_error is returned.
     */
    virtual rs::core::status get_occupancy_map_update(std::shared_ptr<occupancy_map> occ_map) const = 0;

    /**
     * @brief: To obtain get_occupancy_map_update(...) within specified region of interest.
     *
     * @param[out] occ_map: Pre-allocated instance of occupancy_map. After successful
     * execution of the function this container will be updated with occupancy map data.
     *
     * @param[in] minPoint: minimum of x and y co-ordinate of region of interest.
     *
     * @param[in] maxPoint: maximum of x and y co-ordinate of region of interest.
     *
     * @returns: If successful it returns status_no_error, error code will be on failure, warning
     * status_data_pending would be returned if all occupancy_map cannot accommodate all the available tiles,
     * in this case make additional call later to get_occupancy_map_update(...) to fetch remaining tiles until status_no_error is returned.
     *
     * Note that range values are in the same unit as the resolution value.
     */
    virtual rs::core::status get_occupancy_map_update(std::shared_ptr<occupancy_map> occ_map, const rs::core::pointI32 &minPoint, const rs::core::pointI32 &maxPoint) const = 0;

    /**
     * @brief: Saves the occupancy map to a specified file. Saved occupancy map can be loaded later
     * by calling load_ocupancy_map(...).
     *
     * @param[in] file_name The full path name of a file for storing values of the current
     * occupancy grid that can be later restored.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status save_occupancy_map(const std::string &file_name) const = 0;

    /**
     * @brief: Saves the occupancy map as a PPM file with following color codes.
     * 		Unknown cells are colored white(255, 255, 255)
     * 		100% occupied cells are colored red(255, 0, 0).
     * 		Black(0, 0, 0) pixels represent traversed but 0% occupied(unoccupied)cells.
     * 		All values are scaled linearly between 0 to 255.
     *
     * @param[in] fileName: full path name of a file to store values of the current
     * occupancy grid that can be later restored.
     *
     * @param[in] draw_camera_trajectory: Flag specifying whether to include camera trajectory
     * in the occupancy map image. If enabled, camera trajectory will be drawn in green(0, 255, 0) color.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status save_occupancy_map_as_ppm(const std::string &fileName, const bool draw_camera_trajectory) const = 0;

    /**
     * @brief: To load occupancy map from specified occupancy map file which was create using save_occupancy_map(...).
     *
     * @param[in] fileName This is the full path name of a file from which to load and restore the
     * occupancy map.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status load_occupancy_map(const std::string &fileName) = 0;

    /**
     * @brief: Starts the re-localization mapping process. The function
     * should be called before the first call to process_sample_set_async and
     * before any call to load_relocalization_map. Enabled by default.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     *
     * \see load_relocalization_map(...)
     */
    virtual rs::core::status start_relocalization_mapping() = 0;

    /**
     * @brief: Stops the re-localization mapping process.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status stop_relocalization_mapping() = 0;

    /**
     * @brief load_relocalization_map loads re-localization map data from a given file
     * which was created using save_relocalization_map(...).
     *
     * @param[in] file_name This is the full path name of a file from which to load and
     * restore re-localization map data.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status load_relocalization_map(const std::string &file_name) = 0;

    /**
     * @brief: Saves the current re-localization map data to a given file. which can be loaded
     * later using load_relocalization_map(...).
     *
     * @param[in] file_name: Full path name of a file where to save the current
     * re-localization map.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status save_relocalization_map(const std::string &file_name) const = 0;

    /**
     * @brief: To get tracking accuracy of camera pose estimation.
     *
     * @returns: tracking_accuracy indicates reliability of camera pose obtained using get_camera_pose(...).
     */
    virtual tracking_accuracy get_tracking_accuracy() const = 0;

    /**
     * @brief: To enable debug images. If enabled the images returned by on module finished processing
     * will contain debugging information overlaid on input images.
     *
     * @param[in] bEnableFlag: enable/disable flag for debug images.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status enable_debug_image(const bool bEnableFlag) = 0;

    /**
     * @brief: To restart slam with specified pose as initial pose.
     * +NOTE: Do not call from processing_event_handler::process_sample_complete(...) callback thread.
     *
     * @param[in] pose: initial pose of the camera for starting tracking,
     * If occupancy map is used, supplied pose should be with respect (aligned) to gravity.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status restart(const PoseMatrix4f &pose) = 0;

    /**
     * @brief: To restart slam with pose aligned to gravity.
     * +NOTE: Do not call from processing_event_handler::process_sample_complete(...) callback thread.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status restart() = 0;

    /**
     * @brief: To register tracking_event_handler.
     *
     * @param[in] handler: Handle to instance of tracking_event_handler.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status register_tracking_event_handler(tracking_event_handler* handler) = 0;

    /**
     * @brief: To unregister tracking_event_handler.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status unregister_event_hander() = 0;

    /**
     * @brief: Returns the relocalizer pose for the last tracked frame.
     *
     * @param[out] pose: Output relocalization pose.
     *
     * @returns: flag indicating whether relocalization happen or not,
     * If relocalization happens, then pose argument will be overwritten with relocalizer pose otherwise not.
     */
    virtual bool get_relocalization_pose(PoseMatrix4f &pose) const = 0;

    /**
     * @brief: Allows to set whether to use relocalization pose in tracking or not when relocalization mapping is
     * running (relocalization is not stopped using stop_relocalization_mapping(...))
     * By default relocalization will overwrite the tracker pose.
     * Only available before passing first sample to slam module.
     *
     * @param[in] enable: Flag indicating whether to overwrite tracker pose with pose from relocalization.
     *
     * @returns: If successful it returns status_no_error,
     * Otherwise error code will be returned.
     */
    virtual rs::core::status force_relocalization_pose(const bool enable) = 0;
};
}
}
