#include "../include/solvers.hpp"
#include<iostream>

std::vector<double> general_algorithm(const std::vector<double>& a, const std::vector<double>& b, const std::vector<double>& c, const std::vector<double>& g) {
    int n = g.size();
    std::vector<double> c_prime(n-1, 0.0);
    std::vector<double> g_prime(n, 0.0);

    // Forward elimination
    c_prime[0] = c[0] / b[0];
    for(int i = 1; i < n - 1; i++){
        c_prime[i] = c[i] / (b[i] - a[i] * c_prime[i - 1]);
    }
    g_prime[0] = g[0] / b[0];
    for (int i = 1; i < n; i++) {
        g_prime[i] = (g[i] - a[i] * g_prime[i - 1]) / (b[i] - a[i] * c_prime[i - 1]);
    }

    // Back substitution
    std::vector<double> v(n, 0.0);
    v[n - 1] = g_prime[n - 1];
    for (int i = n - 2; i >= 0; i--) {
        v[i] = g_prime[i] - c_prime[i] * v[i + 1];
    }
    return v;
}

std::vector<double> special_algorithm(const std::vector<double>& g){
    int n = g.size();
    std::vector<double> v(n, 0.0);
    std::vector<double> g_tilde(n, 0.0);
    
    g_tilde[0] = g[0] / 2.0;
    for(int i = 1; i <= n - 1; i++){
        g_tilde[i] = (g[i] + g_tilde[i - 1]) * double(i + 1) / double(i + 2);
    }
    v[n - 1] = g_tilde[n - 1];
    for(int i = n - 2; i >= 0; i--){
        v[i] = g_tilde[i] + double(i + 1) / double(i + 2) * v[i + 1];
    }
    return v;
}

std::vector<double> optimal_algorithm_opt(const std::vector<double>& g) {
    // Trying to shave off the last few bits here and there
    
    const int n = static_cast<int>(g.size());
    std::vector<double> v(n);

    // Use g_tilde in-place (reuse v as workspace to save one allocation)
    double prev = g[0] * 0.5;
    v[0] = prev;

    // Forward sweep
    for (int i = 1; i < n; i++) {
        const double factor = double(i + 1) / double(i + 2);
        prev = (g[i] + prev) * factor;
        v[i] = prev;
    }

    // Backward sweep
    double next = v[n - 1];
    for (int i = n - 2; i >= 0; i--) {
        const double factor = double(i + 1) / double(i + 2);
        next = v[i] + factor * next;
        v[i] = next;
    }

    return v;
}


std::vector<double> add_boundaries(const std::vector<double>& v){
    std::vector<double> v_ast;
    // Add 0.0 to the beginning and end
    v_ast.push_back(0.0);
    for (int i = 0; i < v.size(); i++){
        v_ast.push_back(v[i]);
    }
    v_ast.push_back(0.0);
    return v_ast;
}

