/* ***********************************************************************
//
//   Copyright (C) 2025 -- The 4D-STAR Collaboration
//   File Author: Emily Boudreaux
//   Last Modified: April 21, 2025
//
//   4DSSE is free software; you can use it and/or modify
//   it under the terms and restrictions the GNU General Library Public
//   License version 3 (GPLv3) as published by the Free Software Foundation.
//
//   4DSSE is distributed in the hope that it will be useful,
//   but WITHOUT ANY WARRANTY; without even the implied warranty of
//   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
//   See the GNU Library General Public License for more details.
//
//   You should have received a copy of the GNU Library General Public License
//   along with this software; if not, write to the Free Software
//   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
//
// *********************************************************************** */
#include "polytropeOperator.h"
#include "4DSTARTypes.h"
#include "utilities.h"

#include "mfem.hpp"
#include "mfem_smout.h"
#include <memory>


PolytropeOperator::PolytropeOperator(

  std::unique_ptr<mfem::MixedBilinearForm> M,
  std::unique_ptr<mfem::MixedBilinearForm> Q,
  std::unique_ptr<mfem::BilinearForm> D,
  std::unique_ptr<mfem::NonlinearForm> f,
  const mfem::Array<int> &blockOffsets) :

  // TODO: Need to update this so that the size is that of the reduced system operator
  mfem::Operator(blockOffsets.Last()), // Initialize the base class with the total size of the block offset vector
  m_blockOffsets(blockOffsets),
  m_jacobian(nullptr) {

    m_M = std::move(M);
    m_Q = std::move(Q);
    m_D = std::move(D);
    m_f = std::move(f);

    // Use Gauss-Seidel smoother to approximate the inverse of the matrix
    // t = 0 (symmetric Gauss-Seidel), 1 (forward Gauss-Seidel), 2 (backward Gauss-Seidel)
    // iterations = 3
    m_invNonlinearJacobian = std::make_unique<mfem::GSSmoother>(0, 3);
}

void PolytropeOperator::populate_free_dof_array() {
    m_freeDofs.SetSize(0);
    for (int i = 0; i < m_blockOffsets.Last(); i++) {
        const int thetaSearchIndex = i;
        const int phiSearchIndex = i - m_blockOffsets[1];
        if (phiSearchIndex < 0){
            if (m_theta_ess_tdofs.first.Find(thetaSearchIndex) == -1) {
                m_freeDofs.Append(i);
            }
        } else {
            if (m_phi_ess_tdofs.first.Find(phiSearchIndex) == -1) {
                m_freeDofs.Append(i);
            }
        }
    }
}

void PolytropeOperator::scatter_boundary_conditions() {
    mfem::Vector thetaStateValues(m_theta_ess_tdofs.first.Size());
    for (int i = 0; i < m_theta_ess_tdofs.first.Size(); i++) {
        thetaStateValues[i] = m_theta_ess_tdofs.second[i];
    }
    mfem::Vector phiStateValues(m_phi_ess_tdofs.first.Size());
    for (int i = 0; i < m_phi_ess_tdofs.first.Size(); i++) {
        phiStateValues[i] = m_phi_ess_tdofs.second[i];
    }

    mfem::Array<int> phiDofIndices(m_phi_ess_tdofs.first.Size());
    for (int i = 0; i < m_phi_ess_tdofs.first.Size(); i++) {
        phiDofIndices[i] = m_phi_ess_tdofs.first[i] + m_blockOffsets[1];
    }

    m_state.SetSize(m_blockOffsets.Last());
    m_state = 0.0;
    m_state.SetSubVector(m_theta_ess_tdofs.first, thetaStateValues);
    m_state.SetSubVector(phiDofIndices, phiStateValues);

}

void PolytropeOperator::construct_matrix_representations() {
    m_Mmat = std::make_unique<mfem::SparseMatrix>(m_M->SpMat());
    m_Qmat = std::make_unique<mfem::SparseMatrix>(m_Q->SpMat());
    m_Dmat = std::make_unique<mfem::SparseMatrix>(m_D->SpMat());

    m_MReduced = std::make_unique<mfem::SparseMatrix>(serif::utilities::buildReducedMatrix(*m_Mmat, m_phi_ess_tdofs.first, m_theta_ess_tdofs.first));
    m_QReduced = std::make_unique<mfem::SparseMatrix>(serif::utilities::buildReducedMatrix(*m_Qmat, m_theta_ess_tdofs.first, m_phi_ess_tdofs.first));
    m_DReduced = std::make_unique<mfem::SparseMatrix>(serif::utilities::buildReducedMatrix(*m_Dmat, m_phi_ess_tdofs.first, m_phi_ess_tdofs.first));

    m_negQ_mat = std::make_unique<mfem::ScaledOperator>(m_QReduced.get(), -1.0);
}

void PolytropeOperator::construct_reduced_block_offsets() {
    m_reducedBlockOffsets.SetSize(3);
    m_reducedBlockOffsets[0] = 0; // R0 block (theta)
    m_reducedBlockOffsets[1] = m_MReduced->Height(); // R1 block (theta)
    m_reducedBlockOffsets[2] = m_QReduced->Height() + m_reducedBlockOffsets[1]; // R2 block (phi)
}

void PolytropeOperator::construct_jacobian_constant_terms() {
    m_jacobian = std::make_unique<mfem::BlockOperator>(m_reducedBlockOffsets);
    m_jacobian->SetBlock(0, 1, m_MReduced.get());     //<-  M (constant)
    m_jacobian->SetBlock(1, 0, m_negQ_mat.get());     //<- -Q (constant)
    m_jacobian->SetBlock(1, 1, m_DReduced.get());     //<-  D (constant)
}

void PolytropeOperator::finalize(const mfem::Vector &initTheta) {
    using serif::utilities::buildReducedMatrix;

    if (m_isFinalized) {
        return;
    }

    // These functions must be called in this order since they depend on each others post state
    // TODO: Refactor this so that either there are explicit checks to make sure the order is correct or make
    //       them pure functions
    construct_matrix_representations();
    construct_reduced_block_offsets();
    construct_jacobian_constant_terms();
    scatter_boundary_conditions();
    populate_free_dof_array();

    // Override the size based on the reduced system
    height = m_reducedBlockOffsets.Last();
    width = m_reducedBlockOffsets.Last();

    m_isFinalized = true;

}

const mfem::BlockOperator &PolytropeOperator::get_jacobian_operator() const {
    if (m_jacobian == nullptr) {
        MFEM_ABORT("Jacobian has not been initialized before GetJacobianOperator() call.");
    }
    if (!m_isFinalized) {
        MFEM_ABORT("PolytropeOperator not finalized prior to call to GetJacobianOperator().");
    }

    return *m_jacobian;
}

mfem::BlockDiagonalPreconditioner& PolytropeOperator::get_preconditioner() const {
    if (m_schurPreconditioner == nullptr) {
        MFEM_ABORT("Schur preconditioner has not been initialized before GetPreconditioner() call.");
    }
    if (!m_isFinalized) {
        MFEM_ABORT("PolytropeOperator not finalized prior to call to GetPreconditioner().");
    }
    return *m_schurPreconditioner;
}

int PolytropeOperator::get_reduced_system_size() const {
    if (!m_isFinalized) {
        MFEM_ABORT("PolytropeOperator not finalized prior to call to GetReducedSystemSize().");
    }
    return m_reducedBlockOffsets.Last();
}

const mfem::Vector &PolytropeOperator::reconstruct_full_state_vector(const mfem::Vector &reducedState) const {
    m_state.SetSubVector(m_freeDofs, reducedState); // Scatter the reduced state vector into the full state vector
    return m_state;
}

void PolytropeOperator::Mult(const mfem::Vector &xFree, mfem::Vector &yFree) const {
    if (!m_isFinalized) {
        MFEM_ABORT("PolytropeOperator::Mult called before finalize");
    }

    // TODO: confirm that the vectors xFree and m_freeDofs are always parallel
    m_state.SetSubVector(m_freeDofs, xFree); // Scatter the free dofs from the input vector xFree into the state vector
    mfem::Vector y;
    y.SetSize(m_blockOffsets.Last());

    // -- Create BlockVector views for input x and output y
    mfem::BlockVector x_block(const_cast<mfem::Vector&>(m_state), m_blockOffsets);
    mfem::BlockVector y_block(y, m_blockOffsets);

    // -- Get Vector views for individual blocks
    const mfem::Vector &x_theta = x_block.GetBlock(0);
    const mfem::Vector &x_phi = x_block.GetBlock(1);
    mfem::Vector &y_R0 = y_block.GetBlock(0); // Residual Block 0 (theta)
    mfem::Vector &y_R1 = y_block.GetBlock(1); // Residual Block 1 (phi)

    int theta_size = m_blockOffsets[1] - m_blockOffsets[0];
    int phi_size = m_blockOffsets[2] - m_blockOffsets[1];

    mfem::Vector f_term(theta_size);
    mfem::Vector Mphi_term(theta_size);
    mfem::Vector Dphi_term(phi_size);
    mfem::Vector Qtheta_term(phi_size);

    // Calculate R0 and R1 terms
    // R0 = f(θ) + Mɸ
    // R1 = Dɸ - Qθ

    MFEM_ASSERT(m_f.get() != nullptr, "NonlinearForm m_f is null in PolytropeOperator::Mult");

    m_f->Mult(x_theta, f_term);
    m_M->Mult(x_phi, Mphi_term);
    m_D->Mult(x_phi, Dphi_term);
    m_Q->Mult(x_theta, Qtheta_term);

    add(f_term, Mphi_term, y_R0);
    subtract(Dphi_term, Qtheta_term, y_R1);

    yFree.SetSize(m_reducedBlockOffsets.Last());
    MFEM_ASSERT(m_freeDofs.Size() == m_reducedBlockOffsets.Last(), "PolytropeOperator::Mult: Size of free dofs does not match reduced block offsets size.");
    for (int i = 0, j = 0; i < y.Size(); ++i) {
        if (m_freeDofs.Find(i) != -1) {
            yFree[j] = y[i];
            j++;
        }
    }
}


void PolytropeOperator::update_inverse_nonlinear_jacobian(const mfem::Operator &grad) const {
    m_invNonlinearJacobian->SetOperator(grad);
}

void PolytropeOperator::update_inverse_schur_compliment() const {
    // TODO: This entire function could probably be refactored out
    if (!m_isFinalized) {
        MFEM_ABORT("PolytropeOperator::updateInverseSchurCompliment called before finalize");
    }
    if (m_invNonlinearJacobian == nullptr) {
        MFEM_ABORT("PolytropeOperator::updateInverseSchurCompliment called before updateInverseNonlinearJacobian");
    }
    if (m_schurCompliment == nullptr) {
        MFEM_ABORT("PolytropeOperator::updateInverseSchurCompliment called before updateInverseSchurCompliment");
    }

    m_schurCompliment->updateInverseNonlinearJacobian(*m_invNonlinearJacobian);

    if (m_schurPreconditioner == nullptr) {
        m_schurPreconditioner = std::make_unique<mfem::BlockDiagonalPreconditioner>(m_blockOffsets);
    }

    m_schurPreconditioner->SetDiagonalBlock(0, m_invNonlinearJacobian.get());
    m_schurPreconditioner->SetDiagonalBlock(1, m_invSchurCompliment.get());

}

void PolytropeOperator::update_preconditioner(const mfem::Operator &grad) const {
    update_inverse_nonlinear_jacobian(grad);
    update_inverse_schur_compliment();
}

mfem::Operator& PolytropeOperator::GetGradient(const mfem::Vector &xFree) const {
    //TODO: This now needs to be updated to deal with the reduced system size
    if (!m_isFinalized) {
        MFEM_ABORT("PolytropeOperator::GetGradient called before finalize");
    }
    m_state.SetSubVector(m_freeDofs, xFree); // Scatter the free dofs from the input vector xFree into the state vector
    // --- Get the gradient of f ---
    mfem::BlockVector x_block(const_cast<mfem::Vector&>(m_state), m_blockOffsets);
    const mfem::Vector& x_theta = x_block.GetBlock(0);

    // PERF: There are a lot of copies and loops here, probably performance could be gained by flattering some of these.
    auto &grad = m_f->GetGradient(x_theta);
    // updatePreconditioner(grad);
    const auto gradMatrix = dynamic_cast<mfem::SparseMatrix*>(&grad);

    if (gradMatrix == nullptr) {
        MFEM_ABORT("PolytropeOperator::GetGradient: Gradient is not a SparseMatrix.");
    }

    mfem::SparseMatrix reducedGrad = serif::utilities::buildReducedMatrix(*gradMatrix, m_theta_ess_tdofs.first, m_theta_ess_tdofs.first);

    m_jacobian->SetBlock(0, 0, &reducedGrad);

    return *m_jacobian;
}
void PolytropeOperator::set_essential_true_dofs(const SSE::MFEMArrayPair& theta_ess_tdofs, const SSE::MFEMArrayPair& phi_ess_tdofs) {
    m_isFinalized = false;
    m_theta_ess_tdofs = theta_ess_tdofs;
    m_phi_ess_tdofs = phi_ess_tdofs;

    if (m_f) {
        m_f->SetEssentialTrueDofs(theta_ess_tdofs.first);
    } else {
        MFEM_ABORT("m_f is null in PolytropeOperator::SetEssentialTrueDofs");
    }
}

void PolytropeOperator::set_essential_true_dofs(const SSE::MFEMArrayPairSet& ess_tdof_pair_set) {
    set_essential_true_dofs(ess_tdof_pair_set.first, ess_tdof_pair_set.second);
}

SSE::MFEMArrayPairSet PolytropeOperator::get_essential_true_dofs() const {
     return std::make_pair(m_theta_ess_tdofs, m_phi_ess_tdofs);
}
GMRESInverter::GMRESInverter(const SchurCompliment &op) :
mfem::Operator(op.Height(), op.Width()),
m_op(op) {
    m_solver.SetOperator(m_op);
    m_solver.SetMaxIter(100);
    m_solver.SetRelTol(1e-1);
    m_solver.SetAbsTol(1e-1);
}

void GMRESInverter::Mult(const mfem::Vector &x, mfem::Vector &y) const {
    m_solver.Mult(x, y); // Approximates m_op^-1 * x
}


SchurCompliment::SchurCompliment(
    const mfem::SparseMatrix &QOp,
    const mfem::SparseMatrix &DOp,
    const mfem::SparseMatrix &MOp,
    const mfem::Solver &GradInvOp) :
mfem::Operator(DOp.Height(), DOp.Width())
{
    SetOperator(QOp, DOp, MOp, GradInvOp);
    m_nPhi = m_DOp->Height();
    m_nTheta = m_MOp->Height();
}

SchurCompliment::SchurCompliment(
    const mfem::SparseMatrix &QOp,
    const mfem::SparseMatrix &DOp,
    const mfem::SparseMatrix &MOp) :
mfem::Operator(DOp.Height(), DOp.Width())
{
    updateConstantTerms(QOp, DOp, MOp);
    m_nPhi = m_DOp->Height();
    m_nTheta = m_MOp->Height();
}

void SchurCompliment::SetOperator(const mfem::SparseMatrix &QOp, const mfem::SparseMatrix &DOp, const mfem::SparseMatrix &MOp, const mfem::Solver &GradInvOp) {
    updateConstantTerms(QOp, DOp, MOp);
    updateInverseNonlinearJacobian(GradInvOp);
}

void SchurCompliment::updateInverseNonlinearJacobian(const mfem::Solver &gradInv) {
    m_GradInvOp = &gradInv;
}

void SchurCompliment::updateConstantTerms(const mfem::SparseMatrix &QOp, const mfem::SparseMatrix &DOp, const mfem::SparseMatrix &MOp) {
    m_QOp = &QOp;
    m_DOp = &DOp;
    m_MOp = &MOp;
}

void SchurCompliment::Mult(const mfem::Vector &x, mfem::Vector &y) const {
    // Check that the input vector is the correct size
    if (x.Size() != m_nPhi) {
        MFEM_ABORT("Input vector x has size " + std::to_string(x.Size()) + ", expected " + std::to_string(m_nPhi));
    }
    if (y.Size() != m_nPhi) {
        MFEM_ABORT("Output vector y has size " + std::to_string(y.Size()) + ", expected " + std::to_string(m_nPhi));
    }

    // Check that the operators are set
    if (m_QOp == nullptr) {
        MFEM_ABORT("QOp is null in SchurCompliment::Mult");
    }
    if (m_DOp == nullptr) {
        MFEM_ABORT("DOp is null in SchurCompliment::Mult");
    }
    if (m_MOp == nullptr) {
        MFEM_ABORT("MOp is null in SchurCompliment::Mult");
    }
    if (m_GradInvOp == nullptr) {
        MFEM_ABORT("GradInvOp is null in SchurCompliment::Mult");
    }

    mfem::Vector v1(m_nTheta); // M * x
    m_MOp -> Mult(x, v1); // M * x

    mfem::Vector v2(m_nTheta); // GradInv * M * x
    m_GradInvOp -> Mult(v1, v2); // GradInv * M * x

    mfem::Vector v3(m_nPhi); // Q * GradInv * M * x
    m_QOp -> Mult(v2, v3); // Q * GradInv * M * x

    mfem::Vector v4(m_nPhi); // D * x
    m_DOp -> Mult(x, v4); // D * x

    subtract(v4, v3, y); // (D - Q * GradInv * M) * x
}