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

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 "rs/core/video_module_interface.h"
#include "rs/core/video_module_control_interface.h"
#include "slam_core.h"

namespace rs
{
namespace slam
{
class slam_impl;

class slam : virtual public rs::core::video_module_interface, rs::core::video_module_control_interface, slam_core
{
public:
    slam(const slam &) = delete;
    slam& operator=(const slam &) = delete;

    /**
     * Module ID of slam
     */
    static const int32_t UID;

    slam();
    ~slam();



    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///// Module APIs documented in rs/core/video_module_interface.h and rs/core/video_module_control_interface.h /////
    ///// APIs that differs from definitions in rs/core/*.h are explicitly documented here ////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    int32_t query_module_uid()  override;

    rs::core::status query_supported_module_config(int32_t idx, supported_module_config &supported_config) override;

    rs::core::status query_current_module_config(actual_module_config &module_config) override;

    /**
      * @brief: To configure slam.
      *
      * @param[in] module_config: Specifies configuration of input stream.
      * Containing valid values for following:
      *
      * 1. Fisheye intrinsics in actual_config[stream_type::fisheye].intrinsics
      * 2. Depth intrinsics in actual_config[stream_type::depth].intrinsics
      * 3. Accelerometer intrinsics in actual_config[motion_type::accel].intrinsics.acc
      * 4. Gyro intrinsics in actual_config[motion_type::gyro].intrinsics.gyro
      *
      * +NOTE: For fisheye and depth image intrinsics and conversion see rs::device::get_stream_intrinsics(rs::stream)
      * and rs::utils::convert_intrinsics(...) respectively
      *
      * + NOTE: For accelerometer and gyro intrinsics and conversion see rs::device::get_motion_intrinsics()
      * and rs::utils::convert_motion_device_intrinsic(...)
      *
      * 5. Fisheye extrinsics in actual_config[stream_type::fisheye].extrinsics should
      * contain extrinsics from fisheye to motion sensor
      * +NOTE: For Fisheye to motion sensor extrinsics and conversion see rs::device::get_motion_extrinsics_from(rs::stream::fisheye))
      * and rs::utils::convert_extrinsics
      *
      * 6. Depth extrinsics in actual_config[stream_type::depth].extrinsics should contain extrinsics
      * from depth to fisheye
      * +NOTE: For Depth to fisheye sensor extrinsics and conversion see rs::device::get_extrinsics(rs::stream::depth, rs::stream::fisheye)
      * and rs::utils::convert_extrinsics
      *
      * @returns: If successful it returns status_no_error,
      * Otherwise error code will be returned.
      */
    rs::core::status set_module_config(const actual_module_config &module_config) override;

    /**
      * @brief: To submit sample to slam for asynchronous processing.
      *
      * @param[in] sample_set: correlated_sample_set containing streams required for slam.
      * Reference count of samples submitted to slam will be incremented.
      *
      * @returns: If successful it returns status_no_error,
      * Otherwise error code will be returned.
      */
    rs::core::status process_sample_set_async(rs::core::correlated_sample_set *sample_set) override;

    rs::core::status register_event_hander(processing_event_handler *handler) override;

    rs::core::status unregister_event_hander(processing_event_handler *handler) override;

    rs::core::video_module_control_interface *query_video_module_control() override;

    rs::core::status register_event_hander(control_event_handler * handler) override;

    rs::core::status unregister_event_hander(control_event_handler * handler) override;

    /**
      * @brief: Stops streaming of samples to slam tracking component.
      * All the samples submitted to slam through process_sample_set_async(...)
      * after calling reset() will be not be processed. slam module is not released
      * therefore all the output functions (get_...(...) and save_...(...)) will remain
      * available until the slam object is destroyed through destructor.
      *
      * @returns: If successful it returns status_no_error,
      * Otherwise error code will be returned.
      */
    rs::core::status reset() override;



    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /// Not implemented////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    rs::core::status process_sample_set_sync(rs::core::correlated_sample_set *sample_set) override;

    rs::core::status pause() override;

    rs::core::status resume() override;



    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /// Core APIs documented in  slam_core.h /////////////////////////////////////////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    std::shared_ptr<occupancy_map> create_occupancy_map(const int32_t tile_count) const override;
    std::shared_ptr<occupancy_map> create_occupancy_map(int32_t *pre_allocated, const int32_t tile_count) const override;
    rs::core::status get_camera_pose(PoseMatrix4f &pose) const override;
    rs::core::status set_camera_pose(const PoseMatrix4f &pose) override;
    rs::core::status set_auto_occupancy_map_building(const bool enable_map_building) override;
    bool is_auto_occupancy_map_building_enabled() const override;
    rs::core::status set_occupancy_map_height_of_interest(const float min_height, const float max_height) override;
    rs::core::status get_occupancy_map_height_of_interest(float &min_height, float &max_height) const override;
    rs::core::status set_occupancy_map_resolution(const float resolution) override;
    float get_occupancy_map_resolution() const override;
    rs::core::status update_occupancy_map(const PoseMatrix4f &pose, rs::core::correlated_sample_set &sample_set) override;
    rs::core::status get_occupancy_map(std::shared_ptr<occupancy_map> occ_map) const override;
    rs::core::status get_occupancy_map(std::shared_ptr<occupancy_map> occ_map, const rs::core::pointI32 &minPoint, const rs::core::pointI32 &maxPoint) const override;
    rs::core::status get_occupancy_map_update(std::shared_ptr<occupancy_map> occ_map) const override;
    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 override;
    rs::core::status save_occupancy_map(const std::string &file_name) const override;
    rs::core::status save_occupancy_map_as_ppm(const std::string &fileName, const bool draw_camera_trajectory) const override;
    rs::core::status load_occupancy_map(const std::string &fileName) override;
    rs::core::status start_relocalization_mapping() override;
    rs::core::status stop_relocalization_mapping() override;
    rs::core::status load_relocalization_map(const std::string &file_name) override;
    rs::core::status save_relocalization_map(const std::string &file_name) const override;
    tracking_accuracy get_tracking_accuracy() const override;
    rs::core::status enable_debug_image(const bool bEnableFlag) override;
    rs::core::status restart(const PoseMatrix4f &pose) override;
    rs::core::status restart() override;
    rs::core::status register_tracking_event_handler(tracking_event_handler* handler) override;
    rs::core::status unregister_event_hander() override;
    bool get_relocalization_pose(PoseMatrix4f &pose) const override;
    rs::core::status force_relocalization_pose(const bool enable) override;


private:
    slam_impl *m_pSLAM;
};
}
}
