CovarianceBase3Impl.hpp

/* This file is part of OpenKalman, a header-only C++ library for
 * Kalman filters and other recursive filters.
 *
 * Copyright (c) 2020-2021 Christopher Lee Ogden <ogden@gatech.edu>
 *
 * 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/.
 */

#ifndef OPENKALMAN_COVARIANCEBASE3IMPL_HPP
#define OPENKALMAN_COVARIANCEBASE3IMPL_HPP

#include <utility>

namespace OpenKalman::internal
{
  // ======================================CASE 3======================================
  template<typename Derived, typename NestedMatrix>
  struct CovarianceBase3Impl : AdapterBase<Derived, NestedMatrix>
  {
  private:

    using Base = AdapterBase<Derived, NestedMatrix>;

    using Scalar = scalar_type_of_t<NestedMatrix>;

  protected:

    using CholeskyNestedMatrix = std::conditional_t<
      diagonal_matrix<NestedMatrix>,
      typename MatrixTraits<std::decay_t<NestedMatrix>>::template DiagonalMatrixFrom<>,
      std::conditional_t<triangular_matrix<NestedMatrix>,
        typename MatrixTraits<std::decay_t<NestedMatrix>>::template SelfAdjointMatrixFrom<>,
        typename MatrixTraits<std::decay_t<NestedMatrix>>::template TriangularAdapterFrom<>>>;


    // The cholesky nested matrix one would expect given whether the covariance is a square root.
    mutable CholeskyNestedMatrix cholesky_nested;


    // The synchronization state. 0 == synchronized, 1 = forward, -1 = reverse.
    mutable int synch_direction;


    auto& cholesky_nested_matrix() & { return cholesky_nested; }

    const auto& cholesky_nested_matrix() const & { return cholesky_nested; }

    auto&& cholesky_nested_matrix() && { return std::move(cholesky_nested); }

    const auto&& cholesky_nested_matrix() const && { return std::move(cholesky_nested); }


    int synchronization_direction() const { return synch_direction; }


    void synchronize_forward() const &
    {
      cholesky_nested = to_covariance_nestable<CholeskyNestedMatrix>(Base::nested_object());
      synch_direction = 0;
    }


    void synchronize_forward() const &&
    {
      cholesky_nested = to_covariance_nestable<CholeskyNestedMatrix>(std::move(*this).nested_object());
      synch_direction = -1;
    }


    void mark_nested_matrix_changed() const { synch_direction = 1; }


    void mark_cholesky_nested_matrix_changed() const { synch_direction = -1; }


    void mark_synchronized() const { synch_direction = 0; }


#ifdef __cpp_concepts
    CovarianceBase3Impl() requires std::default_initializable<NestedMatrix>
#else
    template<typename T = NestedMatrix, std::enable_if_t<std::is_default_constructible_v<T>, int> = 0>
    CovarianceBase3Impl()
#endif
      : Base {}, synch_direction {} {}


    CovarianceBase3Impl(const CovarianceBase3Impl& other) = default;


    CovarianceBase3Impl(CovarianceBase3Impl&& other) noexcept = default;


#ifdef __cpp_concepts
    template<typename Arg> requires (triangular_matrix<Arg> == triangular_matrix<NestedMatrix>) and
      (not diagonal_matrix<Arg>)
#else
    template<typename Arg, std::enable_if_t<(triangular_matrix<Arg> == triangular_matrix<NestedMatrix>) and
      (not diagonal_matrix<Arg>), int> = 0>
#endif
    CovarianceBase3Impl(Arg&& arg, int sd)
      : Base {std::forward<Arg>(arg)}, cholesky_nested {}, synch_direction {sd} {}


#ifdef __cpp_concepts
    template<typename Arg> requires (triangular_matrix<Arg> != triangular_matrix<NestedMatrix>) or diagonal_matrix<Arg>
#else
    template<typename Arg, std::enable_if_t<(triangular_matrix<Arg> != triangular_matrix<NestedMatrix>) or
      diagonal_matrix<Arg>, int> = 0>
#endif
    CovarianceBase3Impl(Arg&& arg, int sd)
      : Base {}, cholesky_nested {std::forward<Arg>(arg)}, synch_direction {sd} {}


    template<typename N, typename CN>
    CovarianceBase3Impl(N&& n, CN&& cn, int sd)
      : Base {std::forward<N>(n)}, cholesky_nested {std::forward<CN>(cn)}, synch_direction {sd} {}


  public:

    auto& operator=(const CovarianceBase3Impl& other)
    {
      if constexpr (not zero<NestedMatrix> and not identity_matrix<NestedMatrix>) if (this != &other)
      {
        if (synch_direction >= 0) Base::nested_object() = other.nested_object();
        if (synch_direction <= 0) cholesky_nested = other.cholesky_nested_matrix();
        synch_direction = other.synch_direction;
      }
      return *this;
    }


    auto& operator=(CovarianceBase3Impl&& other) noexcept
    {
      if constexpr (not zero<NestedMatrix> and not identity_matrix<NestedMatrix>) if (this != &other)
      {
        if (synch_direction >= 0) Base::nested_object() = std::move(other.nested_object());
        if (synch_direction <= 0) cholesky_nested = std::move(other.cholesky_nested_matrix());
        synch_direction = other.synch_direction;
      }
      return *this;
    }


#ifdef __cpp_concepts
    template<covariance Arg> requires (not std::derived_from<std::decay_t<Arg>, CovarianceBase3Impl>)
#else
    template<typename Arg, std::enable_if_t<covariance<Arg> and
      (not std::is_base_of_v<CovarianceBase3Impl, std::decay_t<Arg>>), int> = 0>
#endif
    auto& operator=(Arg&& arg)
    {
      if constexpr(not (zero<nested_object_of_t<Arg>> and zero<NestedMatrix>) and
        not (identity_matrix<nested_object_of_t<Arg>> and identity_matrix<NestedMatrix>))
      {
        if constexpr (case1or2<Arg>)
        {
          // Arg is Case 1 or 2
          if constexpr (triangular_matrix<nested_object_of_t<Arg>> == triangular_matrix<NestedMatrix> and
            not diagonal_matrix<Arg>)
          {
            Base::nested_object() = to_covariance_nestable<NestedMatrix>(std::forward<Arg>(arg));
            mark_nested_matrix_changed();
          }
          else
          {
            cholesky_nested = to_covariance_nestable<CholeskyNestedMatrix>(std::forward<Arg>(arg));
            mark_cholesky_nested_matrix_changed();
          }
        }
        else
        {
          // Arg is Case 3 or 4
          if (synch_direction >= 0)
          {
            Base::nested_object() = to_covariance_nestable<NestedMatrix>(std::forward<Arg>(arg));
          }
          if (synch_direction <= 0)
          {
            cholesky_nested = to_covariance_nestable<CholeskyNestedMatrix>(std::forward<Arg>(arg));
          }
          synch_direction = arg.synchronization_direction();
        }
      }
      return *this;
    }


#ifdef __cpp_concepts
    template<covariance_nestable Arg>
#else
    template<typename Arg, std::enable_if_t<covariance_nestable<Arg>, int> = 0>
#endif
    auto& operator=(Arg&& arg)
    {
      if constexpr(not (zero<Arg> and zero<NestedMatrix>) and
        not (identity_matrix<Arg> and identity_matrix<NestedMatrix>))
      {
        if constexpr(triangular_matrix<Arg> == triangular_matrix<NestedMatrix> and not diagonal_matrix<Arg>)
        {
          Base::nested_object() = to_covariance_nestable<NestedMatrix>(std::forward<Arg>(arg));
          mark_nested_matrix_changed();
        }
        else
        {
          cholesky_nested = to_covariance_nestable<CholeskyNestedMatrix>(std::forward<Arg>(arg));
          mark_cholesky_nested_matrix_changed();
        }
      }
      return *this;
    }


    auto operator() (std::size_t i, std::size_t j)
    {
      if constexpr(writable_by_component<CholeskyNestedMatrix, 2>)
        return ElementAccessor(cholesky_nested, i, j,
          [this] { if (synch_direction > 0) synchronize_forward(); },
          [this] { mark_cholesky_nested_matrix_changed(); });
      else
        return ElementAccessor(cholesky_nested_matrix(), i, j,
          [this] { if (synch_direction > 0) synchronize_forward(); });
    }

    auto operator() (std::size_t i, std::size_t j) const
    {
      return ElementAccessor(cholesky_nested, i, j, [this] { if (synch_direction > 0) synchronize_forward(); });
    }


    auto operator[] (std::size_t i)
    {
      if constexpr(writable_by_component<CholeskyNestedMatrix, 1>)
        return ElementAccessor(cholesky_nested, i,
          [this] { if (synch_direction > 0) synchronize_forward(); },
          [this] { mark_cholesky_nested_matrix_changed(); });
      else
        return ElementAccessor(cholesky_nested, i,
          [this] { if (synch_direction > 0) synchronize_forward(); });
    }


    auto operator[] (std::size_t i) const
    {
      return ElementAccessor(cholesky_nested, i, [this] { if (synch_direction > 0) synchronize_forward(); });
    }


    void set_component(const Scalar s, const std::size_t i, const std::size_t j)
    {
      if (synch_direction > 0) synchronize_forward();
      OpenKalman::set_component(cholesky_nested, s, i, j);
      mark_cholesky_nested_matrix_changed();
    }


    void set_component(const Scalar s, const std::size_t i)
    {
      if (synch_direction > 0) synchronize_forward();
      OpenKalman::set_component(cholesky_nested, s, i);
      mark_cholesky_nested_matrix_changed();
    }

  };


}

#endif //OPENKALMAN_COVARIANCEBASE3IMPL_HPP