#include "bench_funcs.h"
#include <cmath>
#include <cassert>
#include <limits>

double dot_basic(const std::vector<double>& x, const std::vector<double>& y) {
    assert(x.size() == y.size());
    double s = 0.0;
    std::size_t N = x.size();
    for (std::size_t i = 0; i < N; ++i) {
        s += x[i] * y[i];
    }
    return s;
}

double dot_kahan(const std::vector<double>& x, const std::vector<double>& y) {
    assert(x.size() == y.size());
    double sum = 0.0;
    double c = 0.0; 
    std::size_t N = x.size();
    for (std::size_t i = 0; i < N; ++i) {
        double prod = x[i] * y[i];
        double yk = prod - c;
        double t = sum + yk;
        c = (t - sum) - yk;
        sum = t;
    }
    return sum;
}

double norm_basic(const std::vector<double>& x) {
    double s = 0.0;
    for (std::size_t i = 0; i < x.size(); ++i) {
    double v = x[i];
    s += v * v;
    }
    return std::sqrt(s);
}

void matvec_rowmajor(const std::vector<double>& A, std::size_t M, std::size_t N,
                     const std::vector<double>& x, std::vector<double>& b) {
    assert(A.size() == M * N);
    assert(x.size() == N);
    b.assign(M, 0.0);
    for (std::size_t i = 0; i < M; ++i) { //over rows
        double sum = 0.0;
        std::size_t base = i * N; //the start index of row 1
        for (std::size_t j = 0; j < N; ++j) { //dot product of that row with x
            sum += A[base + j] * x[j];
        }
        b[i] = sum;
    }
}

void matmul_rowmajor(const std::vector<double>& A, std::size_t M, std::size_t L,
                     const std::vector<double>& B, std::size_t N,
                     std::vector<double>& C) {
    assert(A.size() == M * L);
    assert(B.size() == L * N);
    C.assign(M * N, 0.0);
    for (std::size_t i = 0; i < M; ++i) {
        for (std::size_t k = 0; k < L; ++k) {
            double a = A[i * L + k]; //a=A[i,k]
            std::size_t baseB = k * N;
            std::size_t baseC = i * N;
            for (std::size_t j = 0; j < N; ++j) {
                C[baseC + j] += a * B[baseB + j];
            }
        }
    }
}

void polyp_horner(const std::vector<double>& a, const std::vector<double>& x,
                  std::vector<double>& y) {
    std::size_t p = (a.size() == 0) ? 0 : a.size() - 1; //if a.size=0, then p=0, otherwise p=a.soze-1
    std::size_t N = x.size();
    y.assign(N, 0.0);
    for (std::size_t i = 0; i < N; ++i) { //loops over all evaluation points x[i]
        double xi = x[i];
        double acc = a[p];
        for (std::size_t k = p; k-- > 0;) { // from p-1 down to 0
            acc = acc * xi + a[k];
        }
        y[i] = acc;
    }
}

void jacobi_csr(const CSR& K, const std::vector<double>& f, std::vector<double>& u,
                std::size_t max_iter, double omega, double tol) {
    std::size_t n = K.n;
    assert(f.size() == n);
    u.assign(n, 0.0);
    std::vector<double> u_new(n, 0.0);
    for (std::size_t iter = 0; iter < max_iter; ++iter) { //Jacobi iteration
        double max_err = 0.0;
        for (std::size_t i = 0; i < n; ++i) { 
            double diag = 0.0;
            double sum = 0.0;
            for (std::size_t idx = K.row_ptr[i]; idx < K.row_ptr[i+1]; ++idx) { //iterates over the non zero entries of row i
                std::size_t j = K.col[idx];
                double v = K.val[idx];
                if (j == i) diag = v;
                else sum += v * u[j]; //accomulates sum = Σ_{j≠i} K_ij * u[j]
            }
            double uk1 = u[i] + omega * (f[i] - sum - diag * u[i]) / diag; //stoers the diagonal separately 
            u_new[i] = uk1;
            double err = std::fabs(uk1 - u[i]);
            if (err > max_err) max_err = err;
        }
        u.swap(u_new);
        if (max_err < tol) break;
    }
}
