/* @file KalmanFilter.hpp */

#pragma once

#include "KalmanFilterSpec.hpp"

namespace xo {
    namespace kalman {
        /* Specification for an ordinary discrete linear kalman filter.
         *
         * The filter generates estimates for a process observed at a discrete
         * set of times tk in {t0, t1, .., tn}
         *
         * At each time tk we have the following:
         *
         * 0. x(0) initial estimate at t(0)
         *    P(0) initial priors: error covariance matrix for x(0)
         *
         * 1. x_(k), [n x 1] vector:
         *    system state, denoted by vector.
         *    (state is not directly observable,
         *     filter will attempt to estimate it)
         *
         * 2. w_(k), [n x 1] vector
         *    Q(k),  [n x n] matrix
         *
         *    w_(k) denotes system noise,
         *    gaussian with covariance Q(k).
         *    noise w_(k) is not directly observable.
         *
         * 3. z(k), [m x 1] vector:
         *
         *    observation vector for time tk
         *
         * 4. v_(k), [m x 1] vector
         *    R(k), [m x m] matrix
         *
         *    v_(k) denotes observation errors,
         *    gaussian with covariance R(k).
         *    noise v_(k) is not directly observable.
         *
         * 5. F(k), [n x n] matrix
         *    state transition matrix
         *    model system evolves according to:
         *
         *    x_(k+1) = F(x).x_(k) + w_(k)
         *
         * 6. observations z(k) depend on system state:
         *
         *    z(k) = H(k).x_(k) + v_(k)
         *
         * 7. Kalman filter outputs:
         *    x(k), [n x 1] vector
         *    Q(k), [n x n] matrix
         *
         *    x(k) is optimal estimate for system state x_(k)
         *    P(k) is covariance matrix specifying confidence intervals
         *         for pairs (x(k)[i], x(k)[j])
         *
         * filter specification consists of:
         *    n, m, x(0), P(0), F(k), Q(k), H(k), R(k)
         * The cardinality of observations z(k) can vary over time,
         * so to be precise,  m can vary with tk;   write as m(k)
         *
         * More details:
         * - avoid having to specify t(k) in advance;
         *   instead defer until observation available
         *   so t(k) can be taken from polling timestamp
         */

        /* encapsulate a (linear) kalman filter
         * together with event publishing
         */
        class KalmanFilter {
        public:
            using MatrixXd = Eigen::MatrixXd;
            using VectorXd = Eigen::VectorXd;
            using utc_nanos = xo::time::utc_nanos;

        public:
            /* create filter with specification given by spec,  and initial state s0 */
            explicit KalmanFilter(KalmanFilterSpec spec);

            uint32_t step_no() const { return state_ext_->step_no(); }
            utc_nanos tm() const { return state_ext_->tm(); }
            KalmanFilterSpec const & filter_spec() const { return filter_spec_; }
            KalmanFilterStep const & step() const { return step_; }
            rp<KalmanFilterStateExt> const & state_ext() const { return state_ext_; }

            /* notify kalman filter with input for time t(k+1) = input_kp1.tkp1()
             * Require: input.tkp1() >= .current_tm()
             * Promise:
             * - .tm() = input_kp1.tkp1()
             * - .step_no() = old .step_no() + 1
             * - .filter_spec_k, .step_k, .state_k updated
             *   for observations in input_kp1
             */
            void notify_input(rp<KalmanFilterInput> const & input_kp1);

            void display(std::ostream & os) const;
            std::string display_string() const;

        private:
            /* specification for kalman filter;
             * produces process/observation matrices on demand
             */
            KalmanFilterSpec filter_spec_;

            /* filter step for most recent observation */
            KalmanFilterStep step_;

            /* filter state as of most recent observation;
             * result of applying KalmanFilterEngine::step() to contents of .step
             */
            rp<KalmanFilterStateExt> state_ext_;
        }; /*KalmanFilter*/

        inline std::ostream &
        operator<<(std::ostream & os, KalmanFilter const & x) {
            x.display(os);
            return os;
        } /*operator<<*/

    } /*namespace kalman*/
} /*namespace xo*/

/* end KalmanFilter.hpp */
