/* ***********************************************************************
//
//   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 "operator.h"
#include "4DSTARTypes.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,
  const double index) :

  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::finalize(const mfem::Vector &initTheta) {
    if (m_isFinalized) {
        return;
    }

    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());

    // Remove the essential dofs from the constant matrices
    for (const auto& dof: m_theta_ess_tdofs.first) {
        m_Mmat->EliminateRow(dof);
        m_Qmat->EliminateCol(dof);
    }

    for (const auto& dof: m_phi_ess_tdofs.first) {
        m_Mmat->EliminateCol(dof);
        m_Qmat->EliminateRow(dof);
        m_Dmat->EliminateRowCol(dof);
    }

    m_negM_mat = std::make_unique<mfem::ScaledOperator>(m_Mmat.get(), -1.0);
    m_negQ_mat = std::make_unique<mfem::ScaledOperator>(m_Qmat.get(), -1.0);

    m_schurCompliment = std::make_unique<SchurCompliment>(*m_Qmat, *m_Dmat, *m_Mmat);

    // Set up the constant parts of the jacobian now

    // We use the assembled matrices with their boundary conditions already removed for the jacobian
    m_jacobian = std::make_unique<mfem::BlockOperator>(m_blockOffsets);
    m_jacobian->SetBlock(0, 1, m_negM_mat.get()); //<- -M (constant)
    m_jacobian->SetBlock(1, 0, m_negQ_mat.get()); //<- -Q (constant)
    m_jacobian->SetBlock(1, 1, m_Dmat.get());     //<-  D (constant)

    m_invSchurCompliment = std::make_unique<GMRESInverter>(*m_schurCompliment);

    m_isFinalized = true;

    // Build the initial preconditioner based on some initial guess
    const auto &grad = m_f->GetGradient(initTheta);
    updatePreconditioner(grad);

}

const mfem::BlockOperator &PolytropeOperator::GetJacobianOperator() 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::GetPreconditioner() 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;
}

void PolytropeOperator::Mult(const mfem::Vector &x, mfem::Vector &y) const {
    if (!m_isFinalized) {
        MFEM_ABORT("PolytropeOperator::Mult called before finalize");
    }
    // -- Create BlockVector views for input x and output y
    mfem::BlockVector x_block(const_cast<mfem::Vector&>(x), 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);

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

    // -- Apply essential boundary conditions --
    for (int i = 0; i < m_theta_ess_tdofs.first.Size(); i++) {
        if (int idx = m_theta_ess_tdofs.first[i]; idx >= 0 && idx < y_R0.Size()) {
            const double &targetValue = m_theta_ess_tdofs.second[i];
            y_block.GetBlock(0)[idx] = x_theta(idx) - targetValue; // inhomogenous essential bc.
        }
    }

    for (int i = 0; i < m_phi_ess_tdofs.first.Size(); i++) {
        if (int idx = m_phi_ess_tdofs.first[i]; idx >= 0 && idx < y_R1.Size()) {
            const double &targetValue = m_phi_ess_tdofs.second[i];
            y_block.GetBlock(1)[idx] = x_phi(idx) - targetValue; // inhomogenous essential bc.
        }
    }

    std::cout << "||r_θ|| = " << y_block.GetBlock(0).Norml2();
    std::cout << ", ||r_φ|| = " << y_block.GetBlock(1).Norml2() << std::endl;
}


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

void PolytropeOperator::updateInverseSchurCompliment() 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::updatePreconditioner(const mfem::Operator &grad) const {
    updateInverseNonlinearJacobian(grad);
    updateInverseSchurCompliment();
}

mfem::Operator& PolytropeOperator::GetGradient(const mfem::Vector &x) const {
    if (!m_isFinalized) {
        MFEM_ABORT("PolytropeOperator::GetGradient called before finalize");
    }
    // --- Get the gradient of f ---
    mfem::BlockVector x_block(const_cast<mfem::Vector&>(x), m_blockOffsets);
    const mfem::Vector& x_theta = x_block.GetBlock(0);

    auto &grad = m_f->GetGradient(x_theta);
    updatePreconditioner(grad);

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

    return *m_jacobian;
}
void PolytropeOperator::SetEssentialTrueDofs(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::SetEssentialTrueDofs(const SSE::MFEMArrayPairSet& ess_tdof_pair_set) {
    SetEssentialTrueDofs(ess_tdof_pair_set.first, ess_tdof_pair_set.second);
}

SSE::MFEMArrayPairSet PolytropeOperator::GetEssentialTrueDofs() 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
}