RasterScanner.h

#pragma once
#include "DLLInfo.h"
#include "RasterFrame.h"
#include "RasterPointScanner/Headers/KeyFramePointRaster.hpp"
#include "PointScanner/Headers/Scanner3D.h"
#include "BlockModel/Headers/SegmentedBlockModel.h"
#include "Design/Headers/Camera.h"
#include "Base/Headers/Time.h"
#include "Base/Headers/Pointer.hpp"
#include "Base/Headers/Thread.h"
#include "Base/Headers/INIInterface.h"
#include <QObject>
class ScannerDriver;
namespace NDEVR
{
    class ColorPositionShaderLogic;
    class SLAMParameters;
    struct FileRequest;
    class OrbSLAM;
    class SLAMEngine;
    class DesignObjectLookup;
    class Log;
    class Battery;
    template<class t_type, class t_smooth_type>
    class PointSmootherBlockFilter;
    class MadgwickOrientationSensor;
    class RangeSensor;
    template<class t_type>
    class SegmentedBlockModel;
    class ColorNode;
    template<class t_type>
    class ViewportPointSmoother;
    struct PointPipeline
    {
        static constexpr uint04 s_pipeline_frame_count = 5; 
        String id;                                          
        RasterFrame frames[s_pipeline_frame_count];         
    };

    struct FilterSettings
    {
        uint08 scan_density_cuttoff = 10;                                   
        fltp08 scan_range_from_origin = Constant<fltp08>::Invalid;          
    };

    struct RasterPacketInfo
    {
        UUID scanner_id;                                                
        Matrix<fltp08> mat = Constant<Matrix<fltp08>>::Invalid;     
        Buffer<Vertex<3, fltp04>> active_point_data;                
        Buffer<RGBColor> active_color_data;                         
 
        Buffer<Vertex<3, fltp04>> inactive_point_data;              
        Buffer<RGBColor> inactive_color_data;                       
        void clear()
        {
            active_point_data.clear();
            active_color_data.clear();
            inactive_point_data.clear();
            inactive_color_data.clear();
        }
        sint02 gyro_status = 0;     
        Time frame_time;            
        uint01 packet_num = 0;      
    };

    class RASTER_POINT_SCANNER_API RasterScanner : public Scanner3D
    {
    public:
        RasterScanner(DesignObjectLookup* lookup, const Model& model, LogPtr log, QObject* parent = nullptr);
        RasterScanner(DesignObjectLookup* manager, LogPtr log, QObject* parent = nullptr);
        void setSpacialTransform(const Matrix<fltp08>& spacial_transform);
        virtual ~RasterScanner();
        virtual TimeSpan updateInterval() const override;
        void updateModel() override;
        void updateCameraModel();
        virtual void startScan(const Scanner3DScanSettings& settings) override;
        virtual void pauseScan() override;
        virtual void stopScan() override;
        virtual void resumeScan() override;
        virtual void createScanner() override; 
        virtual bool isOrientationValid() const;
        PositionSensor* positionSensor() const;
        virtual SLAMParameters createSLAMParameters() const;
        Matrix<fltp08> slamTransform() const { return m_slam_transform; }
        Matrix<fltp08> spacialTransform() const { return m_spacial_transform; }
        void setSLAMTransform(const Matrix<fltp08>& matrix);
        virtual void setIsNetworkServer(bool is_server);
        void startProcessingThreads();
        [[nodiscard]] virtual bool supportsState(ScannerState state) const override;
        void startTriggerScan(fltp08 step, fltp08 speed);
        void setFilterSettings(const FilterSettings& filter);
        [[nodiscard]] FilterSettings filterSettings() const;
        [[nodiscard]] uint04 meshBlendFactor() const override { return 1U; }
        [[nodiscard]] Matrix<fltp08> scannerMatrix() const override;
        [[nodiscard]] Bounds<2, Angle<fltp08>> maxScanBounds() const;
        virtual Buffer<ScanProfile> availableScanProfiles() const override;
        virtual Bounds<2, Angle<fltp08>> scanBounds() const override;
        [[nodiscard]] virtual OrientationSensor* orientationSensor() const override;
        void requestDisconnect() override;
        [[nodiscard]] [[nodiscard]] virtual RangeSensor* rangeSensor() const override;
        [[nodiscard]] virtual Matrix<fltp08> orientationAt(const Time& time, bool offset_with_station) const;
        [[nodiscard]] RasterInfo defaultRasterInfo(const UUID& id) const override;
        [[nodiscard]] virtual Buffer<TemperatureSensor*> temperatureSensors() const override;
        [[nodiscard]] virtual DistanceEncoder* distanceEncoder() const override;
        virtual void setSmoother(ViewportPointSmoother<ColorNormalNode>* smoother);
        virtual void setupCameraModel();
        void setIgnoreCameraSetup(bool ignore);
        bool snapViewportToScanner() const override;
        SegmentedBlockModel<ColorNormalOffsetNode>* createSegmentedBlockModel(fltp08 range, fltp08 step);
        void invalidateTime();
        void setPositionStation(bool position_station);
        void setLiveCloudVisible(bool visible, bool auto_toggle = false);
        void setCurrentPacket(const RasterPacketInfo& packet);
        bool getCurrentPacket(RasterPacketInfo& packet) const;
    protected:
        virtual bool collectPoints() = 0;
        void setToGPSPosition();
        void updateSLAMVisuals();
        void writeCalibrationYaml(fltp08 spacing) const;
        virtual void updateIOState() {}
        virtual void setupFilters() {}
        void showLiveCloud(bool show_cloud);
        void collectPointData(RasterFrame& frame);
        bool finishActiveFrame();
        void finishSmoothLogic();
        void updateLiveDisplay();
        virtual bool filterFrame(RasterFrame&) { return true; }
        virtual bool calculateColorsAndPositions(RasterFrame& frame);
        virtual bool smoothPositions(RasterFrame& frame);
        virtual bool performSLAM(RasterFrame& frame);
        virtual bool addRecords(RasterFrame& frame);
        void saveCalibrationData(RasterFrame& frame);
        virtual Matrix<fltp08> scanModelFileTransform() const override;
        virtual void update(Time time) override;
        Matrix<fltp08> laserTransform(const Vector<3, Angle<fltp08>>& head_angle) const override;
        [[nodiscard]] Matrix<fltp08> initialScanTransform() const override;
        virtual Matrix<fltp08> scanTransform() const override;
        void initDevice();
        void updateCamera(const DynamicPointer<Camera>& camera);
        bool processPipeline(ProcessStage stage);
        ProcessStage nextProcessStage(ProcessStage current, bool success) const;
        const VideoFrameDetails& colorFrameDetails() const { return m_color_frame_details; };
        const VideoFrameDetails& depthFrameDetails() const { return m_depth_frame_details; };
        const RasterFrame& activeFrame() const { return m_active_frame; };
    protected:
        void init();
        virtual bool attemptConnection() { return true; }
        bool updateState();
        bool isAddingToScan() const;
        bool isAddingToLive() const;
        bool isDeploying() const;
    protected:
        Time m_last_sound_time = Time(0);
        RasterPacketInfo m_live_data; 
        RasterPacketInfo m_inactive_data;
        VideoFrameDetails m_color_frame_details;
        VideoFrameDetails m_depth_frame_details;
        Matrix<fltp08> m_last_calculated_matrix = Constant<Matrix<fltp08>>::Invalid;
        fltp04 m_depth_scale = 0.001f;
        Matrix<fltp08> m_slam_calculated = Constant<Matrix<fltp08>>::Invalid;
        CollectionMode m_collection_mode = CollectionMode::e_off;
        fltp04 m_quality = 0.5f;
        fltp08 m_density = Constant<fltp08>::Invalid;
        RasterFrame m_active_frame;
        uint04 m_last_tracking_state = 0;
        RasterFrame m_pending_slam_frame;
        RasterFrame m_slam_process_frame;
        FilterSettings m_filter_settings;
        PointPipeline m_rs_pipeline;
        PositionSensor* m_position_sensor = nullptr;
        ViewportPointSmoother<ColorNormalNode>* m_smoother = nullptr;
        FramePointScan* m_frame_scan = nullptr;
        PointSmootherBlockFilter<ColorNormalOffsetNode, ColorNormalNode>* m_block_filter = nullptr;
        DynamicPointer<Camera> m_camera_device;
        OrientationSensor* m_orientation_sensor = nullptr;
        DistanceEncoder* m_distance_encoder = nullptr;
        OrbSLAM* m_slam_engine = nullptr;
        Matrix<fltp08> m_spacial_transform = Matrix<fltp08>(1.0);
        Matrix<fltp08> m_slam_transform = Constant<Matrix<fltp08>>::Invalid;
        SegmentedBlockModel<ColorNormalOffsetNode>* m_segmented_block_model = nullptr;
        Model* m_live_model = nullptr;
        Model* m_inactive_model = nullptr;
        ColorPositionShaderLogic* m_color_position_shader = nullptr;
        Thread* m_pipeline_threads[ProcessStage::e_stage_size];
        Bounds<1, fltp04> m_laser_range;
        Time m_frame_offset_time = Constant<Time>::Invalid;
        Time m_last_rx_time = Time(0U);
        uint04 m_trigger_count = 0;
        uint04 m_last_map_id = 0;
        uint04 m_active_slam_map = Constant<uint04>::Invalid;
        uint04 m_slam_process_count = 0;
        uint04 m_collection_process_count = 0;
        Dictionary<uint04, Matrix<fltp08>> m_slam_map_transforms;
        mutable bool m_has_packet_data = false;
        bool m_perform_calibration = false;
        bool m_auto_reduce = true;
        bool m_add_to_grid = false;
        bool m_use_imu_for_slam = false;
        bool m_has_camera = false;
        bool m_is_init = false;
        bool m_has_scan_data = false;
        bool m_ignore_camera_setup = false;
        bool m_are_cameras_snapped = false;
        bool m_live_display = false;
        bool m_is_network_server = false;
        bool m_has_live_data = false;
        bool m_request_slam_reset = false;
        bool m_is_trigger_scan = false;
        bool m_is_remote = false;
        bool m_position_station = true;
        bool m_auto_toggle_live_display = true;
        bool m_show_keyframe_display = true;
        bool m_show_keypoint_display = true;
        bool m_show_live_display = true;
        bool m_show_raw_cloud = true;
        bool m_use_camera_model_frames = false;
        bool m_needs_keyframe_raster_update = false;
    };
}