JorvikTotalStation.h

#pragma once
#include "DLLInfo.h"
#include "JorvikConnection.h"
#include "Collimator.h"
#include <NDEVR/Scanner3D.h>
#include <NDEVR/ApplicationOptions.h>
#include <NDEVR/Matrix.h>
#include <NDEVR/Time.h>
#include <NDEVR/Scene.h>
#include <NDEVR/TranslatedString.h>
#include <NDEVR/Pointer.h>
#include <NDEVR/Thread.h>
#include <NDEVR/INIInterface.h>
#include <NDEVR/ApplicationResource.h>
#include <NDEVR/NDEVRChartModel.h>
#include <QPointer>
 
struct JorvikData;
namespace NDEVR
{
 
    class PointMeasureModel;
    struct FileRequest;
    class DesignObjectLookup;
    class Log;
    class CarlsonScannerOrientation;
    class CarlsonScannerConnection;
    class CarlsonScannerBattery;
    class CarlsonScannerMotor;
    class PositionSensor;
    class SocketIOChannel;
    class CalibrationData;
    class MadgwickOrientationSensor;
    class JorvikTotalStation final : public Scanner3D, public INIInterface
    {
    public:
        JorvikTotalStation(JorvikConnection* connection, DesignObjectLookup* manager, QObject* parent = nullptr);

        ~JorvikTotalStation();

        bool isConnected() const override;
 
        // Children Sensors
        [[nodiscard]] virtual Light* light() const final override; 
        [[nodiscard]] virtual Light* redDot() const final override; 
        [[nodiscard]] virtual OrientationSensor* orientationSensor() const final override; 
        [[nodiscard]] virtual Connection* connection() const final override; 
        [[nodiscard]] virtual PositionSensor* positionSensor() const final override; 
        [[nodiscard]] virtual RangeSensor* rangeSensor() const final override; 
        [[nodiscard]] virtual TranslatedString scannerInfo() const final override; 
        [[nodiscard]] virtual Buffer<Battery*> batteries() const final override; 
        [[nodiscard]] virtual Buffer<TemperatureSensor*> temperatureSensors() const final override; 
        [[nodiscard]] virtual DistanceEncoder* distanceEncoder() const final override; 

        void setConstantLaserFire(bool constant_fire);

        void setModelFile(const File& file);
 
        // Scanner method implementations
        virtual void endUpdates() override; 
        virtual void startScan(const Scanner3DScanSettings& settings) final override; 
        virtual RangeSensor* createRangeSensor() final override; 
        virtual void stopScan() final override; 
        virtual void pauseScan() final override; 
        virtual void resumeScan() final override; 
        virtual void createScanner() final override; 

        void addReverseOrientation();

        void addGridOrientation(sint04 range_pitch, sint04 range_roll, const Angle<fltp08>& step_pitch, const Angle<fltp08>& step_roll, const Angle<fltp08>& offset);

        void addModelsToScan();

        void checkScanStep();

        void recordLocation();

        void orientHead(const Vector<3, Angle<fltp08>>& head_orientation) final override;

        bool executeManualCommand(const String& command) final override;
 
        [[nodiscard]] virtual bool supportsState(ScannerState state) const final override; 
        [[nodiscard]] virtual TimeSpan updateInterval() const final override; 
        [[nodiscard]] virtual TimeSpan getScanTime(const Scanner3DScanSettings& settings) const final override; 
        [[nodiscard]] bool useCalibratedScan() const final override; 
        [[nodiscard]] ScannerState scannerState() const final override; 
        [[nodiscard]] virtual bool supportsScanImageRaster() const final override; 
        [[nodiscard]] virtual RasterInfo defaultRasterInfo(const UUID& profile) const final override; 
        [[nodiscard]] virtual Buffer<ScanProfile> availableScanProfiles() const final override; 
        [[nodiscard]] Bounds<2, Angle<fltp08>> scanBounds() const final override; 
        [[nodiscard]] virtual Matrix<fltp08> cameraTransform() const final override; 
        [[nodiscard]] virtual bool usingPrism() const override; 

        fltp08 calculateDistance();

        String defaultCameraFeed();

        Vertex<3, fltp08> targetPosition() const;

        void saveDeviations(File file);

        virtual void getINI(INIFactory& factory) override;
 
    protected:
        virtual FileRequest scanModelFile() const override; 
        Matrix<fltp08> scanModelFileTransform() const override; 
        Matrix<fltp08> prismModelFileTransform() const override; 
        virtual void update(Time time) override; 
        void onConnectedUpdate(const Time& current); 
        void processMessages(Time time); 
        void processData(Time time, const JorvikData& data); 
        void parkDevice() override; 
        void initMeasureModel(); 
        void initCameraModel(); 
        void initMotors(); 
        Matrix<fltp08> laserTransform(const Vector<3, Angle<fltp08>>& head_angle) const override; 
        virtual Matrix<fltp08> scanTransform() const override; 
        Vector<3, Angle<fltp08>> pointToHeadAngle(Vertex<3, fltp08> point) const override; 
        void addDataToChartModel(); 
        void initCharts(); 
        fltp08 chartTime(); 
        void recordDeviation(); 
        void recordChartDeviation(); 
    private:
        void setScanProfile(const Scanner3DScanSettings& settings); 
    public:
        // Static application options
        static ApplicationOption<String> connection_address; 
        static ApplicationOption<bool> calculate_motor_offset; 
        static ApplicationOption<Angle<fltp08>> motor_epsilon; 
        static ApplicationOption<Angle<fltp08>> compensator_offset_pitch; 
        static ApplicationOption<Angle<fltp08>> compensator_offset_roll; 
        static ApplicationOption<Angle<fltp08>> encoder_angle_offset_pitch; 
        static ApplicationOption<Angle<fltp08>> encoder_angle_offset_roll; 
        static ApplicationOption<Angle<fltp08>> compensator_to_encoder_offset_pitch; 
        static ApplicationOption<Angle<fltp08>> compensator_to_encoder_offset_roll; 
 
    protected:
        // Member variables
        TranslatedString m_test_name; 
        TimeSpan m_reading_wait_time = TimeSpan(20.0); 
        Time m_last_compensator_update_time = Constant<Time>::Invalid; 
        Time m_last_angle_update_time = Constant<Time>::Invalid; 
        Time m_last_distance_update_time = Time(0); 
        Vector<2, Angle<fltp08>> m_motor_offset = Constant<Vector<2, Angle<fltp08>>>::Invalid; 
        Vector<2, Angle<fltp08>> m_current_target = Constant<Vector<2, Angle<fltp08>>>::Invalid; 
        Vector<2, Angle<fltp08>> m_motor_target = Constant<Vector<2, Angle<fltp08>>>::Invalid; 
        Vector<2, Angle<fltp08>> m_raw_angles = Constant<Vector<2, Angle<fltp08>>>::Invalid; 
        Vector<2, Angle<fltp08>> m_raw_compensator = Constant<Vector<2, Angle<fltp08>>>::Invalid; 
        Buffer<CollimatorReading> m_collimator_readings; 
        Buffer<Vector<2, Angle<fltp08>>> m_angle_targets; 
        File m_model_file; 
        fltp08 m_max_range = 10.0; 
        uint04 m_measure_initial_size = Constant<uint04>::Invalid; 
 
        // Hardware components
        JorvikConnection* m_connection; 
        PointMeasureModel* m_measure_model; 
        DynamicPointer<Camera> m_camera_model; 
        Model m_camera_target_model; 
        CalibrationData* m_calibration_data; 
        OrientationSensor* m_orientation = nullptr; 
        PositionSensor* m_position_sensor = nullptr; 
        DistanceEncoder* m_distance_encoder = nullptr; 
        Light* m_red_dot = nullptr; 
        Buffer<Model> m_model_targets; 
        Scene m_measurement_points; 
 
        // Options
        bool m_use_gnss = false; 
        bool m_constant_laser_fire = false; 
        bool m_auto_update_model = true; 
        bool m_collimator_test = false; 
        bool m_manual_step = false; 
        bool m_has_removed_bulb = false; 
        bool m_manually_drive = false; 
        bool m_infinite_test = false; 
        Time m_last_head_update_time = Time(0); 
        Time m_last_settings_time = Time(0); 
        Time m_last_head_move_time = Time(0); 
        NDEVRChartModel* m_deviation_chart_model[2]; 
        NDEVRChartModel* m_orientation_chart_model[2]; 
        NDEVRChartModel* m_angle_chart_model[2]; 
        Scene m_chart_scene; 
        fltp08 m_chart_offset_time = 0.0; 
        UUID m_scan_profile; 
    };
}