# Source code for filterpy.hinfinity.hinfinity_filter

    # -*- coding: utf-8 -*-
    # pylint: disable=C0103, R0913, R0902, C0326, R0914
    # disable snake_case warning, too many arguments, too many attributes,
    # one space before assignment, too many local variables

    """Copyright 2015 Roger R Labbe Jr.

    FilterPy library.
    http://github.com/rlabbe/filterpy

    Documentation at:
    https://filterpy.readthedocs.org

    Supporting book at:
    https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

    This is licensed under an MIT license. See the readme.MD file
    for more information.
    """

    from __future__ import absolute_import, division
    import copy
    import warnings
    import numpy as np
    from numpy import dot, zeros, eye
    import scipy.linalg as linalg
    from filterpy.common import pretty_str

    class HInfinityFilter(object):
        """
        H-Infinity filter. You are responsible for setting the
        various state variables to reasonable values; the defaults below will
        not give you a functional filter.

        Parameters
        ----------

        dim_x : int
            Number of state variables for the Kalman filter. For example, if
            you are tracking the position and velocity of an object in two
            dimensions, dim_x would be 4.

            This is used to set the default size of `P`, `Q`, and `u`

        dim_z : int
            Number of of measurement inputs. For example, if the sensor
            provides you with position in (x, y), `dim_z` would be 2.

        dim_u : int
            Number of control inputs for the Gu part of the prediction step.

        gamma : float

        .. warning::
            I do not believe this code is correct. DO NOT USE THIS.
            In particular, note that predict does not update the covariance
            matrix.
        """

        def __init__(self, dim_x, dim_z, dim_u, gamma):
            warnings.warn("This code is likely incorrect. DO NOT USE.", DeprecationWarning)

            self.dim_x = dim_x
            self.dim_z = dim_z
            self.dim_u = dim_u
            self.gamma = gamma

            self.x = zeros((dim_x, 1)) # state

            self.B = 0                     # control transition matrix
            self.F = eye(dim_x)            # state transition matrix
            self.H = zeros((dim_z, dim_x)) # Measurement function
            self.P = eye(dim_x)            # Uncertainty covariance.
            self.Q = eye(dim_x)

            self._V_inv = zeros((dim_z, dim_z)) # inverse measurement noise
            self._V = zeros((dim_z, dim_z))     #  measurement noise
            self.W = zeros((dim_x, dim_x))      # process uncertainty

            # gain and residual are computed during the innovation step. We
            # save them so that in case you want to inspect them for various
            # purposes

            self.K = 0 # H-infinity gain
            self.y = zeros((dim_z, 1))
            self.z = zeros((dim_z, 1))

            # identity matrix. Do not alter this.
            self._I = np.eye(dim_x)

        def update(self, z):
            """
            Add a new measurement `z` to the H-Infinity filter. If `z` is None,
            nothing is changed.

            Parameters
            ----------
            z : ndarray
                measurement for this update.
            """

            if z is None:
                return

            # rename for readability and a tiny extra bit of speed
            I = self._I
            gamma = self.gamma
            Q = self.Q
            H = self.H
            P = self.P
            x = self.x
            V_inv = self._V_inv
            F = self.F
            W = self.W

            # common subexpression H.T * V^-1
            HTVI = dot(H.T, V_inv)

            L = linalg.inv(I - gamma * dot(Q, P) + dot(HTVI, H).dot(P))

            # common subexpression P*L
            PL = dot(P, L)

            K = dot(F, PL).dot(HTVI)

            self.y = z - dot(H, x)

            # x = x + Ky
            # predict new x with residual scaled by the H-Infinity gain
            self.x = self.x + dot(K, self.y)
            self.P = dot(F, PL).dot(F.T) + W

            # force P to be symmetric
            self.P = (self.P + self.P.T) / 2

            # pylint: disable=bare-except
            try:
                self.z = np.copy(z)
            except:
                self.z = copy.deepcopy(z)

        def predict(self, u=0):
            """
            Predict next position.

            Parameters
            ----------
            u : ndarray
                Optional control vector. If non-zero, it is multiplied by `B`
                to create the control input into the system.
            """

            # x = Fx + Bu
            self.x = dot(self.F, self.x) + dot(self.B, u)

        def batch_filter(self, Zs,update_first=False, saver=False):
            """ Batch processes a sequences of measurements.

            Parameters
            ----------
            Zs : list-like
                list of measurements at each time step `self.dt` Missing
                measurements must be represented by 'None'.

            update_first : bool, default=False, optional,
                controls whether the order of operations is update followed by
                predict, or predict followed by update.

            saver : filterpy.common.Saver, optional
                filterpy.common.Saver object. If provided, saver.save() will be
                called after every epoch

            Returns
            -------
            means: ndarray ((n, dim_x, 1))
                array of the state for each time step. Each entry is an np.array.
                In other words `means[k,:]` is the state at step `k`.

            covariance: ndarray((n, dim_x, dim_x))
                array of the covariances for each time step. In other words
                `covariance[k, :, :]` is the covariance at step `k`.
            """

            n = np.size(Zs, 0)

            # mean estimates from H-Infinity Filter
            means = zeros((n, self.dim_x, 1))

            # state covariances from H-Infinity Filter
            covariances = zeros((n, self.dim_x, self.dim_x))

            if update_first:
                for i, z in enumerate(Zs):
                    self.update(z)
                    means[i, :] = self.x
                    covariances[i, :, :] = self.P
                    self.predict()

                    if saver is not None:
                        saver.save()
            else:
                for i, z in enumerate(Zs):
                    self.predict()
                    self.update(z)

                    means[i, :] = self.x
                    covariances[i, :, :] = self.P

                    if saver is not None:
                        saver.save()

            return (means, covariances)

        def get_prediction(self, u=0):
            """ Predicts the next state of the filter and returns it. Does not
            alter the state of the filter.

            Parameters
            ----------
            u : ndarray
                optional control input

            Returns
            -------
            x : ndarray
                State vector of the prediction.
            """
            return dot(self.F, self.x) + dot(self.B, u)

        def residual_of(self, z):
            """ returns the residual for the given measurement (z). Does not alter
            the state of the filter.
            """
            return z - dot(self.H, self.x)

        def measurement_of_state(self, x):
            """ Helper function that converts a state into a measurement.

            Parameters
            ----------
            x : ndarray
                H-Infinity state vector

            Returns
            -------
            z : ndarray
                measurement corresponding to the given state
            """
            return dot(self.H, x)

        @property
        def V(self):
            """ measurement noise matrix"""
            return self._V

        @V.setter
        def V(self, value):
            """ measurement noise matrix"""

            if np.isscalar(value):
                self._V = np.array([[value]], dtype=float)
            else:
                self._V = value
            self._V_inv = linalg.inv(self._V)

        def __repr__(self):
            return '\n'.join([
                'HInfinityFilter object',
                pretty_str('dim_x', self.dim_x),
                pretty_str('dim_z', self.dim_z),
                pretty_str('dim_u', self.dim_u),
                pretty_str('gamma', self.dim_u),
                pretty_str('x', self.x),
                pretty_str('P', self.P),
                pretty_str('F', self.F),
                pretty_str('Q', self.Q),
                pretty_str('V', self.V),
                pretty_str('W', self.W),
                pretty_str('K', self.K),
                pretty_str('y', self.y),
                ])