#ifndef CONCORD_UNIFIED_HPP
#define CONCORD_UNIFIED_HPP

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/LU>
#include <vector>
#include <memory>
#include <string>
#include <stdexcept>

namespace concord {

using namespace Eigen;

// Forward declarations
class ConcordResult;
class ConcordOptions;

/**
 * @brief Enumeration of available algorithms
 */
enum class Algorithm {
    COORDINATE_DESCENT,    // Standard coordinate descent (concordC)
    COORDINATE_HIGH_DIM,   // High-dimensional coordinate descent (concordC2) 
    COORDINATE_BLAS,       // BLAS-optimized coordinate descent (concordC3)
    ISTA,                  // Matrix-based ISTA algorithm
    SYMLASSO,              // Symmetric Lasso (symlassoC)
    SYMLASSO_ITERATES,     // Symmetric Lasso with iterate saving (symlassoC2)
    AUTO                   // Automatic algorithm selection
};

/**
 * @brief Configuration options for CONCORD algorithms
 */
class ConcordOptions {
public:
    Algorithm algorithm = Algorithm::AUTO;
    double tolerance = 1e-5;
    int max_iterations = 100;
    bool save_iterates = false;
    bool penalize_diagonal = false;
    double lambda_scale = 2.0;  // Lambda scaling factor (from original R interface)
    bool verbose = false;
    
    ConcordOptions() = default;
};

/**
 * @brief Result structure containing solution and diagnostics
 */
class ConcordResult {
public:
    SparseMatrix<double> omega;           // Solution matrix
    std::vector<double> objective_values; // Objective function values
    std::vector<double> convergence_metrics; // Convergence metrics per iteration
    std::vector<int> inner_iterations;    // Inner iteration counts (for ISTA)
    int total_iterations;                 // Total number of iterations
    Algorithm algorithm_used;             // Algorithm that was actually used
    bool converged;                       // Whether algorithm converged
    double final_tolerance;               // Final convergence metric
    std::string convergence_reason;       // Reason for termination
    
    // Optional: full iterate history
    std::vector<MatrixXd> iterates;       // Full solution path (if requested)
    
    ConcordResult() : total_iterations(0), converged(false), final_tolerance(0.0) {}
};

/**
 * @brief Utility functions for matrix operations
 */
namespace utils {
    
    /**
     * @brief Soft thresholding operator (element-wise)
     * @param x Input value
     * @param threshold Threshold value
     * @return Soft-thresholded value
     */
    inline double soft_threshold(double x, double threshold) {
        if (threshold < std::abs(x)) {
            return (x > 0) ? (x - threshold) : (x + threshold);
        } else {
            return 0.0;
        }
    }
    
    /**
     * @brief Matrix soft thresholding
     * @param X Input matrix (modified in-place)
     * @param tau Step size
     * @param Lambda Penalty matrix
     */
    void soft_threshold_matrix(MatrixXd& X, double tau, const MatrixXd& Lambda);
    
    /**
     * @brief Linear index for symmetric matrix storage
     * @param i Row index
     * @param j Column index  
     * @param p Matrix dimension
     * @return Linear index for lower triangular storage
     */
    inline int linear_index(int i, int j, int p) {
        int ii = (i < j) ? i : j;
        int jj = (i < j) ? j : i;
        return (p * ii) + jj - ((ii * (ii + 1)) / 2);
    }
    
    /**
     * @brief Check if matrix is positive definite
     * @param X Input matrix
     * @return True if positive definite
     */
    bool is_positive_definite(const MatrixXd& X);
    
         /**
      * @brief Compute ISTA objective function value: -log det(X) + 0.5*tr(X*S*X) + lambda*||X||_1
      * @param X Current solution
      * @param S Sample covariance matrix
      * @param Lambda Penalty matrix
      * @return Objective function value
      */
     double compute_objective(const MatrixXd& X, const MatrixXd& S, const MatrixXd& Lambda);
     
     /**
      * @brief Compute coordinate descent objective: -log det(X) + tr(S*X) + lambda*||X||_1
      * @param X Current solution
      * @param S Sample covariance matrix
      * @param Lambda Penalty matrix
      * @return Objective function value
      */
     double compute_objective_cd(const MatrixXd& X, const MatrixXd& S, const MatrixXd& Lambda);
     
     /**
      * @brief Compute gradient of smooth part h1(X) = -log det(X) + 0.5 * tr(X * S * X)
      */
     MatrixXd compute_h1_gradient(const MatrixXd& X, const MatrixXd& S);
     
     /**
      * @brief Compute h1 function value
      */
     double compute_h1(const MatrixXd& X, const MatrixXd& S);
     
     /**
      * @brief Compute subgradient of h2(X) = ||X||_1
      */
     MatrixXd compute_h2_subgradient(const MatrixXd& X, const MatrixXd& Lambda, const MatrixXd* grad_h1 = nullptr);
     
     /**
      * @brief Check convergence based on subgradient norm
      */
     bool check_subgradient_convergence(const MatrixXd& X, const MatrixXd& S, const MatrixXd& Lambda, double tolerance);
     
     /**
      * @brief Vectorized element access for symmetric matrices
      */
     void extract_lower_triangular(const MatrixXd& X, VectorXd& vec);
     void set_from_lower_triangular(MatrixXd& X, const VectorXd& vec);
     
} // namespace utils

/**
 * @brief Base class for CONCORD algorithms
 */
class ConcordSolver {
public:
    explicit ConcordSolver(const ConcordOptions& options = ConcordOptions());
    virtual ~ConcordSolver() = default;
    
    /**
     * @brief Solve the sparse precision matrix estimation problem
     * @param S Sample covariance matrix
     * @param Lambda Penalty matrix
     * @return ConcordResult containing solution and diagnostics
     */
    ConcordResult solve(const MatrixXd& S, const MatrixXd& Lambda);
    
    /**
     * @brief Solve with automatic lambda matrix construction
     * @param S Sample covariance matrix
     * @param lambda_scalar Scalar penalty parameter
     * @return ConcordResult containing solution and diagnostics
     */
    ConcordResult solve(const MatrixXd& S, double lambda_scalar);
    
    /**
     * @brief Solve from raw data matrix
     * @param data Raw data matrix (n x p)
     * @param lambda_scalar Scalar penalty parameter
     * @return ConcordResult containing solution and diagnostics
     */
    ConcordResult solve_from_data(const MatrixXd& data, double lambda_scalar);
    
    /**
     * @brief Get current options
     */
    const ConcordOptions& get_options() const { return options_; }
    
    /**
     * @brief Set options
     */
    void set_options(const ConcordOptions& options) { options_ = options; }

protected:
    ConcordOptions options_;
    
    /**
     * @brief Select algorithm based on problem characteristics
     * @param S Sample covariance matrix
     * @param data_matrix Raw data matrix (optional)
     * @return Selected algorithm
     */
    Algorithm select_algorithm(const MatrixXd& S, const MatrixXd* data_matrix = nullptr);
    
    /**
     * @brief Create lambda matrix from scalar
     * @param lambda_scalar Scalar penalty
     * @param p Matrix dimension
     * @return Penalty matrix
     */
    MatrixXd create_lambda_matrix(double lambda_scalar, int p);
    
private:
    // Algorithm implementations
    ConcordResult solve_coordinate_descent(const MatrixXd& S, const MatrixXd& Lambda);
    ConcordResult solve_coordinate_high_dim(const MatrixXd& data, const MatrixXd& S, const MatrixXd& Lambda);
    ConcordResult solve_coordinate_blas(const MatrixXd& S, const MatrixXd& Lambda);
    ConcordResult solve_ista(const MatrixXd& S, const MatrixXd& Lambda);
    ConcordResult solve_symlasso(const MatrixXd& S, const MatrixXd& Lambda);
    ConcordResult solve_symlasso_iterates(const MatrixXd& S, const MatrixXd& Lambda);
};

/**
 * @brief Convenience function for solving CONCORD problems
 * @param S Sample covariance matrix
 * @param lambda_scalar Scalar penalty parameter
 * @param options Solver options
 * @return ConcordResult containing solution and diagnostics
 */
ConcordResult concord(const MatrixXd& S, double lambda_scalar, const ConcordOptions& options = ConcordOptions());

/**
 * @brief Convenience function for solving from raw data
 * @param data Raw data matrix (n x p)
 * @param lambda_scalar Scalar penalty parameter
 * @param options Solver options
 * @return ConcordResult containing solution and diagnostics
 */
ConcordResult concord_from_data(const MatrixXd& data, double lambda_scalar, const ConcordOptions& options = ConcordOptions());

} // namespace concord

#endif // CONCORD_UNIFIED_HPP 