/******************************************************************************
* Copyright (c) ATTX INC 2025. All Rights Reserved.
*
* This software and associated documentation (the "Software") are the 
* proprietary and confidential information of ATTX, INC. The Software is 
* furnished under a license agreement between ATTX and the user organization 
* and may be used or copied only in accordance with the terms of the agreement.
* Refer to 'license/attx_license.adoc' for standard license terms.
*
* EXPORT CONTROL NOTICE: THIS SOFTWARE MAY INCLUDE CONTENT CONTROLLED UNDER THE
* INTERNATIONAL TRAFFIC IN ARMS REGULATIONS (ITAR) OR THE EXPORT ADMINISTRATION 
* REGULATIONS (EAR99). No part of the Software may be used, reproduced, or 
* transmitted in any form or by any means, for any purpose, without the express 
* written permission of ATTX, INC.
******************************************************************************/
/*
Bias and noise model header file

Author: James Tabony
*/
/*
Metadata for MS GUI:
imdata = {"exclude" : True}
*/

#ifndef MODELS_SUPPORT_MARKOV_UNCERTAINTY_MODEL_H
#define MODELS_SUPPORT_MARKOV_UNCERTAINTY_MODEL_H

#include "simulation/Model.h"
#include "simulation/NormalRandom.hpp"

namespace modelspace {

    /**
     * @brief Markov Uncertainty Model
     * 
     * This model is a higher fidelity model for implementing noise, bias, and other 
     * sources of error into an input, with an implied emphesis on sensor measurements.
     * 
     * The foundational equation of this model is:
     *      y_meas = (1+s)*y_truth + \eta + b
     *      db/dt  = \xi
     * Where:
     *      \eta = Measurement noise which follows some zero mean Gaussian distribution with standard deviation \sigma_n
     *      b    = Measurement bias and is not necessarily constant
     *      \xi  = Bias random walk which follows some zero mean Gaussian distribution with standard deviation \sigma_b
     *      s    = Scale factor which follows some zero mean Gaussian distribution with standard deviation \sigma_s
     * 
     * The Stochastic Differential Equation (SDE) governing the brownian motion of the bias random-walk (or drift) can be 
     * discretized using the Euler-Maruyama Method. The SDE in Itô calculus notation is
     *      db(t) = \sigma_b * dW(t)
     * Where:
     *      W(t) = Wiener process (standard brownian motion)
     *      dW(t) follows a zero mean Gaussian distribution with standard deviation of sqrt(discretization time step)
     * 
     * The Euler-Maruyama Method would suggest a discrete time update (with variable step size) for SDE's of this form to be 
     *      b_(k+1) = b_(k) + \sigma_b * sqrt(dt_(k)) * \zeta
     * Where:
     *      \zeta follows a zero mean Gaussian distribution with standard deviation of one (white process noise)
     * 
     * This can be reduced into a final discrete bias update by relating random varibale multiplication with varaince scaling
     *      b_(k+1) = b_(k) + w_(k)
     * Where:
     *      w_(k) follows a zero mean Gaussian distribution with standard deviation of \sigma_b * sqrt(dt_(k))
     * 
     * 
     * WHEN TO USE CERTAIN NOISE TERMS
     * 1). Measurement noise describes the random fluctuations in measurements (or any input) and is the most basic form of error.
     *  
     * 2). Bias is a additive off-set of the measurment or the noise. Bias along with noise describe a Guassian process that has
     * a mean equal to the bias and a standard deviation equal to the that of the noise. Sensors such as accelerometers, GPS, or
     * gyroscopes often have bias.
     * 
     * 3). Bias drift describes the random fluctuations in that additive purtabative term over time. This is a good way to model
     * the bias of a sensor drifting with age, temperature, radiative effects, etc. For long operation missions, it is typical 
     * for the bias (if present) to drift in a seemingly random manner.
     * 
     * 4). Scale factor is a multiplicative error term that is present in a lot of sensors. This error can be thought of a small
     * increments or decrements in the input (e.g. a 0.05 percent increase/decrease). Sensors that convert analog external properties
     * such as pressure/temperature/strain/force into a voltage that then gets converted into a digital signal are almost always prone
     * to scale factor errors to due slight calibration error. Some gyroscopes have scale factor errors provided in degrees per second
     * per volt or degrees per second per bit.
     * 
     * TODO: The model only takes in single value inputs, so for quantities in R^3 like acceleration or angular velocity vectors, the model cannot apply misalignment error
     * 
     * @author James Tabony <james.tabony@attx.tech>
    */
    MODEL(MarkovUncertaintyModel)
    public:
        // Model params
        //         NAME                     TYPE                    DEFAULT VALUE
        START_PARAMS
            /** The initial bias at startup in the object. This bias is non constant unless the random-walk
             *  standard deviation is set to zero. Must be set before model startup. (input units) */
            SIGNAL(bias_initial,            double,                 0.0)
            /** The standard deviation of bias random-walk as a gaussian random noise. (input units not units*srqt(Hz)) */
            SIGNAL(bias_random_walk_std,    double,                 0.0)
            /** The standard deviation of noise in the object as a gaussian random noise. (input units not units*srqt(Hz)) */
            SIGNAL(noise_std,               double,                 0.0)
            /** The standard deviation of the scale factor as a gaussian random noise. (input units not units*srqt(Hz)) */
            SIGNAL(scale_factor_std,        double,                 0.0)
            /** Rate at which model is used. This value is used for the bias brownian motion. (Hz) */
            SIGNAL(rate_hz,                 double,                 0.0)
            /** Value to seed the internal RNG for this model. */
            SIGNAL(seed_value,              int,                    0)
        END_PARAMS

        // Model inputs
        //         NAME                     TYPE                    DEFAULT VALUE
        START_INPUTS
            /** The input value to be perturbed by noise and bias. (input units) */
            SIGNAL(input_val,               double,                 0.0)
        END_INPUTS

        // Model outputs
        //         NAME                     TYPE                    DEFAULT VALUE
        START_OUTPUTS
            /** The output value as perturbed by noise and bias. (input units) */
            SIGNAL(output_val,              double,                 0.0)
        END_OUTPUTS

        /// Default constructor
        MarkovUncertaintyModel();

        /// Function to force the bias to be a specific value after some amount of random-walk.
        void setBias(double new_bias);
        /// Function to retrieve the bias after some amount of random-walk.
        double getBias();

    protected:
        int16 start() override;
        int16 execute() override;

        /// @brief RNG to produce our distribution.
        NormalRandom<double> *_rng = nullptr;

        /// Internal variable for holding the bias and its deviations from the initial value
        double _bias;
        double _bias_drift;

        /// Internal variables for the different noise sources
        double _scale_factor;
        double _noise;

        /// Internal variable for 1/sqrt(Hz)
        double _sqrt_time_step;
    };

}

#endif