// Copyright (C) 2026 Kiyotsugu Arai
// SPDX-License-Identifier: LGPL-3.0-or-later
#pragma once
//
// robust_lp.hpp — Self-contained, robust small-scale dense linear-programming solver (Mehrotra primal-dual interior-point method).
//
// Motivation: the existing 2 solvers were confirmed to have *real bugs* in a codex audit.
//   - linear_programming.hpp's 2-phase simplex: Phase 1 incorrectly deletes "rows where an
//     artificial variable remained in the basis", dropping a real constraint and solving a different LP → returns a constraint-violating solution with status=Optimal.
//   - InteriorPoint.hpp's solve_lp: no finite-value guards at all (proceeds even when x/s etc. are NaN/Inf), and on a degenerate LP
//     it spins up to max_iter without converging. It is also weak to the null directions created by the caller's x⁺−x⁻ split.
//   → This header is a robust replacement implementation. Standard form min cᵀx s.t. A x = b, x ≥ 0.
//
// ★Design (codex's minimal-robust approach):
//   - Mehrotra predictor-corrector primal-dual. The normal equations M = A·diag(x/s)·Aᵀ + reg·I are SPD, so
//     they are solved with an in-house Cholesky (no singular pivots). The reg regularization keeps it always factorizable.
//   - **Finite-value guards** at every step (NaN/Inf → abort on numerical failure, no hang). Maintains x,s>0.
//   - Residual-based stopping (primal/dual residual + duality gap < tol), max_iter, and blow-up detection to
//     decide unbounded/infeasible.
//   - Zero dependencies (only std::vector · header-only · no mp · no matrix.cpp). For dense problems with m≲a few hundred.
//
// Usage: A is m×n given as a list of row vectors. solveStandardLP(c,A,b) → {x,y,s,objective,status,ok}.
//   The caller converts inequalities and free variables into standard form (adding slacks, box shifts).

#include <algorithm>
#include <cmath>
#include <cstddef>
#include <vector>

namespace sangi {
namespace rlp {

enum class Status { Optimal, MaxIter, Infeasible, Unbounded, Numerical, BadInput };

struct Result {
    std::vector<double> x, y, s;     // primal / equality dual / dual slack
    double objective = 0.0;
    int    iterations = 0;
    bool   ok = false;               // achieved Optimal within tol
    Status status = Status::Numerical;
};

struct Options {
    double tol      = 1e-8;          // residual/gap stopping tolerance
    int    maxIter  = 120;
    double reg      = 1e-9;          // regularization of M = ADAᵀ + reg·I
    double bigBound = 1e13;          // if |x| or |s| exceeds this → unbounded/infeasible
    double eta      = 0.95;          // fraction-to-boundary
};

namespace detail {
// Overwrite the SPD matrix M (m×m, row-major) with its lower-triangular Cholesky. Returns false if not positive definite.
inline bool chol(std::vector<double>& M, int m) {
    for (int j = 0; j < m; ++j) {
        double d = M[(std::size_t)j * m + j];
        for (int k = 0; k < j; ++k) { const double v = M[(std::size_t)j * m + k]; d -= v * v; }
        if (!(d > 0.0) || !std::isfinite(d)) return false;
        d = std::sqrt(d);
        M[(std::size_t)j * m + j] = d;
        for (int i = j + 1; i < m; ++i) {
            double v = M[(std::size_t)i * m + j];
            for (int k = 0; k < j; ++k) v -= M[(std::size_t)i * m + k] * M[(std::size_t)j * m + k];
            M[(std::size_t)i * m + j] = v / d;
        }
    }
    return true;
}
// Solve M z = r using the lower-triangular Cholesky L (stored in M).
inline void cholSolve(const std::vector<double>& L, int m,
                      const std::vector<double>& r, std::vector<double>& z) {
    std::vector<double> w(m);
    for (int i = 0; i < m; ++i) {
        double v = r[i];
        for (int k = 0; k < i; ++k) v -= L[(std::size_t)i * m + k] * w[k];
        w[i] = v / L[(std::size_t)i * m + i];
    }
    z.assign(m, 0.0);
    for (int i = m - 1; i >= 0; --i) {
        double v = w[i];
        for (int k = i + 1; k < m; ++k) v -= L[(std::size_t)k * m + i] * z[k];
        z[i] = v / L[(std::size_t)i * m + i];
    }
}
inline bool allFinite(const std::vector<double>& v) {
    for (double z : v) if (!std::isfinite(z)) return false;
    return true;
}
}  // namespace detail

inline Result solveStandardLP(const std::vector<double>& c,
                              const std::vector<std::vector<double>>& A,
                              const std::vector<double>& b,
                              const Options& opt = Options{}) {
    using namespace detail;
    Result R;
    const int m = static_cast<int>(A.size());
    const int n = static_cast<int>(c.size());
    if (m == 0 || n == 0 || static_cast<int>(b.size()) != m) { R.status = Status::BadInput; return R; }
    for (const auto& row : A) if (static_cast<int>(row.size()) != n) { R.status = Status::BadInput; return R; }

    auto Ax = [&](const std::vector<double>& v) {
        std::vector<double> r(m, 0.0);
        for (int i = 0; i < m; ++i) { const auto& Ai = A[i]; double t = 0; for (int j = 0; j < n; ++j) t += Ai[j] * v[j]; r[i] = t; }
        return r;
    };
    auto Aty = [&](const std::vector<double>& v) {
        std::vector<double> r(n, 0.0);
        for (int i = 0; i < m; ++i) { const auto& Ai = A[i]; const double vi = v[i]; for (int j = 0; j < n; ++j) r[j] += Ai[j] * vi; }
        return r;
    };
    double nb = 1.0, nc = 1.0;
    for (double v : b) nb = std::max(nb, std::fabs(v));
    for (double v : c) nc = std::max(nc, std::fabs(v));

    std::vector<double> x(n, 1.0), s(n, 1.0), y(m, 0.0);
    int it = 0;
    for (; it < opt.maxIter; ++it) {
        const auto AxX = Ax(x);
        std::vector<double> rb(m);  for (int i = 0; i < m; ++i) rb[i] = AxX[i] - b[i];
        const auto Atyv = Aty(y);
        std::vector<double> rc(n);  for (int j = 0; j < n; ++j) rc[j] = Atyv[j] + s[j] - c[j];
        double mu = 0; for (int j = 0; j < n; ++j) mu += x[j] * s[j]; mu /= n;
        double pr = 0; for (double v : rb) pr = std::max(pr, std::fabs(v)); pr /= nb;
        double dr = 0; for (double v : rc) dr = std::max(dr, std::fabs(v)); dr /= nc;
        if (pr < opt.tol && dr < opt.tol && mu < opt.tol) { R.status = Status::Optimal; R.ok = true; break; }
        double big = 0; for (double v : x) big = std::max(big, v); for (double v : s) big = std::max(big, v);
        if (big > opt.bigBound) { R.status = (pr < opt.tol ? Status::Unbounded : Status::Infeasible); break; }

        std::vector<double> D(n); for (int j = 0; j < n; ++j) D[j] = x[j] / s[j];
        // M = A diag(D) Aᵀ + reg I  (symmetric · SPD)
        std::vector<double> M((std::size_t)m * m, 0.0);
        for (int i = 0; i < m; ++i) {
            const auto& Ai = A[i];
            for (int k = i; k < m; ++k) {
                const auto& Ak = A[k];
                double t = 0; for (int j = 0; j < n; ++j) t += Ai[j] * D[j] * Ak[j];
                M[(std::size_t)i * m + k] = t; M[(std::size_t)k * m + i] = t;
            }
            M[(std::size_t)i * m + i] += opt.reg;
        }
        std::vector<double> L = M;
        if (!chol(L, m)) {                                   // retry while progressively strengthening the regularization (against end-stage ill-conditioning)
            bool okc = false;
            for (double extra : {1e-6, 1e-3, 1.0, 1e3}) {
                L = M;
                for (int i = 0; i < m; ++i) L[(std::size_t)i * m + i] += extra;
                if (chol(L, m)) { okc = true; break; }
            }
            if (!okc) { R.status = Status::Numerical; break; }
        }
        // Direction computation (given rxs): MΔy=-rb-A(D rc - rxs/s), Δx=D(AᵀΔy+rc)-rxs/s, Δs=-(rxs+sΔx)/x
        auto solveDir = [&](const std::vector<double>& rxs,
                            std::vector<double>& dx, std::vector<double>& dy, std::vector<double>& ds) -> bool {
            std::vector<double> tmp(n); for (int j = 0; j < n; ++j) tmp[j] = D[j] * rc[j] - rxs[j] / s[j];
            const auto Atmp = Ax(tmp);
            std::vector<double> rhs(m); for (int i = 0; i < m; ++i) rhs[i] = -rb[i] - Atmp[i];
            cholSolve(L, m, rhs, dy);
            const auto Atdy = Aty(dy);
            dx.assign(n, 0.0); ds.assign(n, 0.0);
            for (int j = 0; j < n; ++j) {
                dx[j] = D[j] * (Atdy[j] + rc[j]) - rxs[j] / s[j];
                ds[j] = -(rxs[j] + s[j] * dx[j]) / x[j];
            }
            return allFinite(dx) && allFinite(dy) && allFinite(ds);
        };
        auto maxStep = [&](const std::vector<double>& v, const std::vector<double>& dv) {
            double a = 1.0; for (int j = 0; j < n; ++j) if (dv[j] < 0) a = std::min(a, -v[j] / dv[j]); return a;
        };
        // 1) affine
        std::vector<double> rxs(n); for (int j = 0; j < n; ++j) rxs[j] = x[j] * s[j];
        std::vector<double> dxa, dya, dsa;
        if (!solveDir(rxs, dxa, dya, dsa)) { R.status = Status::Numerical; break; }
        const double apx = maxStep(x, dxa), aps = maxStep(s, dsa);
        double muAff = 0; for (int j = 0; j < n; ++j) muAff += (x[j] + apx * dxa[j]) * (s[j] + aps * dsa[j]); muAff /= n;
        double sigma = (mu > 0) ? std::pow(muAff / mu, 3.0) : 0.0;
        sigma = std::min(std::max(sigma, 0.0), 1.0);
        // 2) corrector
        for (int j = 0; j < n; ++j) rxs[j] = x[j] * s[j] + dxa[j] * dsa[j] - sigma * mu;
        std::vector<double> dx, dy, ds;
        if (!solveDir(rxs, dx, dy, ds)) { R.status = Status::Numerical; break; }
        double ap = std::min(1.0, opt.eta * maxStep(x, dx));
        double ad = std::min(1.0, opt.eta * maxStep(s, ds));
        if (!(ap > 0) || !(ad > 0)) { R.status = Status::Numerical; break; }
        for (int j = 0; j < n; ++j) { x[j] += ap * dx[j]; s[j] += ad * ds[j]; }
        for (int i = 0; i < m; ++i) y[i] += ad * dy[i];
        for (int j = 0; j < n; ++j) {                        // maintain strict positivity (guard)
            if (!(x[j] > 0) || !std::isfinite(x[j])) x[j] = 1e-12;
            if (!(s[j] > 0) || !std::isfinite(s[j])) s[j] = 1e-12;
        }
    }
    if (R.status == Status::Numerical && false) { /* keep */ }
    if (it >= opt.maxIter && R.status != Status::Optimal && R.status != Status::Unbounded
        && R.status != Status::Infeasible && R.status != Status::Numerical)
        R.status = Status::MaxIter;
    R.x = x; R.y = y; R.s = s; R.iterations = it;
    double obj = 0; for (int j = 0; j < n; ++j) obj += c[j] * x[j]; R.objective = obj;
    return R;
}

}  // namespace rlp
}  // namespace sangi