Combiner.hpp

Go to the documentation of this file.

// This file is part of INSTINCT, the INS Toolkit for Integrated
// Navigation Concepts and Training by the Institute of Navigation of
// the University of Stuttgart, Germany.
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.

 
#pragma once
 
#include <limits>
#include <memory>
#include <unordered_set>
#include <map>
 
#include "Navigation/Time/InsTime.hpp"
#include "NodeData/NodeData.hpp"
#include "internal/Node/Node.hpp"
#include "internal/gui/widgets/DynamicInputPins.hpp"
 
#include "Navigation/Math/PolynomialRegressor.hpp"
 
#include "NodeData/GNSS/GnssCombination.hpp"
#include "NodeData/GNSS/GnssObs.hpp"
#include "NodeData/GNSS/RtklibPosObs.hpp"
#include "NodeData/GNSS/SppSolution.hpp"
#include "NodeData/IMU/ImuObs.hpp"
#include "NodeData/IMU/ImuObsSimulated.hpp"
#include "NodeData/IMU/ImuObsWDelta.hpp"
#include "NodeData/IMU/KvhObs.hpp"
#include "NodeData/IMU/VectorNavBinaryOutput.hpp"
#include "NodeData/State/InsGnssLCKFSolution.hpp"
#include "NodeData/State/InsGnssTCKFSolution.hpp"
#include "NodeData/State/PosVelAtt.hpp"
 
#include "util/Logger/CommonLog.hpp"
#include "util/Container/ScrollingBuffer.hpp"
#include "util/Container/Unordered_map.hpp"
 
namespace NAV
{
class Combiner : public Node, public CommonLog
{
  public:
    Combiner();
    ~Combiner() override;
    Combiner(const Combiner&) = delete;
    Combiner(Combiner&&) = delete;
    Combiner& operator=(const Combiner&) = delete;
    Combiner& operator=(Combiner&&) = delete;

    [[nodiscard]] static std::string typeStatic();

    [[nodiscard]] std::string type() const override;

    [[nodiscard]] static std::string category();

    void guiConfig() override;

    [[nodiscard]] json save() const override;

    void restore(const json& j) override;
 
  private:
    constexpr static size_t OUTPUT_PORT_INDEX_DYN_DATA = 0; 

    static inline std::vector<std::string> _dataIdentifier = { Pos::type(),
                                                               PosVel::type(),
                                                               PosVelAtt::type(),
                                                               InsGnssLCKFSolution::type(),
                                                               InsGnssTCKFSolution::type(),
                                                               GnssCombination::type(),
                                                               GnssObs::type(),
                                                               SppSolution::type(),
                                                               RtklibPosObs::type(),
                                                               ImuObs::type(),
                                                               ImuObsWDelta::type(),
                                                               ImuObsSimulated::type(),
                                                               KvhObs::type(),
                                                               VectorNavBinaryOutput::type() };

    struct Combination
    {
        struct Term
        {
            double factor = 1.0;                                         
            size_t pinIndex = 0;                                         
            std::variant<size_t, std::string> dataSelection = size_t(0); 
 
            PolynomialRegressor<double> polyReg{ 1, 2 };                   
            ScrollingBuffer<std::shared_ptr<const NodeData>> rawData{ 2 }; 

            [[nodiscard]] std::string description(const Combiner* node, const std::vector<std::string>& descriptors) const
            {
                if (std::holds_alternative<size_t>(dataSelection) && std::get<size_t>(dataSelection) < descriptors.size())
                {
                    return fmt::format("{} {} ({})", factor == 1.0 ? "+" : (factor == -1.0 ? "-" : fmt::format("{:.2f}", factor)),
                                       descriptors.at(std::get<size_t>(dataSelection)), node->inputPins.at(pinIndex).name);
                }
                if (std::holds_alternative<std::string>(dataSelection))
                {
                    return fmt::format("{} {} ({})", factor == 1.0 ? "+" : (factor == -1.0 ? "-" : fmt::format("{:.2f}", factor)),
                                       std::get<std::string>(dataSelection), node->inputPins.at(pinIndex).name);
                }
                return fmt::format("N/A ({})", node->inputPins.at(pinIndex).name);
            }
        };

        std::vector<Term> terms{ Term{ .factor = +1.0, .pinIndex = 0 },
                                 Term{ .factor = -1.0, .pinIndex = 1 } };

        [[nodiscard]] std::string description(const Combiner* node) const
        {
            std::string desc;
            for (const auto& term : terms)
            {
                auto descriptors = node->getDataDescriptors(term.pinIndex);
                auto termDescription = term.description(node, descriptors);
 
                if (!desc.empty())
                {
                    if (termDescription.starts_with("+ ") || termDescription.starts_with("- "))
                    {
                        desc += " ";
                    }
                    else
                    {
                        desc += " + ";
                    }
                }
                desc += termDescription;
            }
 
            return desc;
        }
    };

    std::vector<Combination> _combinations{ Combination() };

    struct PinData
    {
        InsTime lastTime;
        double minTimeStep = std::numeric_limits<double>::infinity();
        std::vector<std::string> dynDataDescriptors;
    };

    std::vector<PinData> _pinData;

    size_t _refPinIdx = 0;

    bool _outputMissingAsNaN = false;
 
    bool _noOutputIfTimeDiffLarge = true;         
    double _maxTimeDiffMultiplierFrequency = 2.1; 
 
    bool _noOutputIfTimeStepLarge = true;         
    double _maxTimeStepMultiplierFrequency = 5.0; 

    struct SendRequest
    {
        size_t combIndex = 0;                                                         
        std::unordered_set<size_t> termIndices;                                       
        double result = 0.0;                                                          
        bool termNullopt = false;                                                     
        std::vector<std::pair<std::string, std::shared_ptr<const NodeData>>> rawData; 
    };

    std::map<InsTime, std::vector<SendRequest>> _sendRequests;

    static void pinAddCallback(Node* node);
    static void pinDeleteCallback(Node* node, size_t pinIdx);

    gui::widgets::DynamicInputPins _dynamicInputPins{ 0, this, pinAddCallback, pinDeleteCallback };

    bool initialize() override;

    void deinitialize() override;

    [[nodiscard]] std::vector<std::string> getDataDescriptors(size_t pinIndex) const;

    void receiveData(InputPin::NodeDataQueue& queue, size_t pinIdx);

    friend void to_json(json& j, const Combination& data);
    friend void from_json(const json& j, Combination& data);
    friend void to_json(json& j, const Combination::Term& data);
    friend void from_json(const json& j, Combination::Term& data);
};
 
} // namespace NAV