#include "benchmarks.h"
#include "vdop.h"
#include <iostream>
#include <vector>
#include <cmath>
#include <cassert>       // assert()


#ifdef __INTEL_CLANG_COMPILER
#pragma message(" ##########  Use of MKL  ###############")
#include <mkl.h>
#else
#pragma message(" ##########  Use of CBLAS  ###############")

#include <cblas.h>               // cBLAS Library
#include <lapacke.h>             // Lapack

#endif



// (A) Inner product of two vectors (from skalar_stl)
double scalar(vector<double> const &x, vector<double> const &y)
{
    assert(x.size() == y.size());
    size_t const N = x.size();
    double sum = 0.0;
    for (size_t i = 0; i < N; ++i)
    {
        sum += x[i] * y[i];
    }
    return sum;
}

// (A) 5.(b) Kahan scalar product
double Kahan_skalar(vector<double> const &x, vector<double> const &y)
{
    double sum = 0.0;
    double c = 0.0;
    size_t n = x.size();
    for (size_t i = 0; i < n; ++i)
    {
        double z = x[i]*y[i] - c;    // c is the part that got lost in the last iteration
        double t = sum + z;          // when adding sum + z, the lower digits are lost if sum is large
        c = (t - sum) - z;           // now we recover the lower digits to add in the next iteration
        sum = t;
    }
    return sum;
}

// (A) 6. cBLAS scalar product
double scalar_cBLAS(vector<double> const &x, vector<double> const &y)
{
    return cblas_ddot(x.size(), x.data(), 1, y.data(), 1); // x.data() = &x[0]
}


// (B) Matrix-vector product (from intro_vector_densematrix)
vector<double> MatVec(vector<double> const &A, vector<double> const &x)
{
    size_t const nelem = A.size();
    size_t const N = x.size();
    assert(nelem % N == 0); // make sure multiplication is possible
    size_t const M = nelem/N;          


    vector<double> b(M);    

    for (size_t i = 0; i < M; ++i)
    {
        double tmp = 0.0;
        for (size_t j = 0; j < N; ++j)
            tmp += A[N*i + j] * x[j];
        b[i] = tmp;
    }

    return b;
}

// (B) cBLAS Matrix-vector product
vector<double> MatVec_cBLAS(vector<double> const &A, vector<double> const &x)
{
    size_t const nelem = A.size();
    size_t const N = x.size();
    assert(nelem % N == 0); // make sure multiplication is possible
    size_t const M = nelem/N;


    vector<double> b(M);

    cblas_dgemv(CblasRowMajor, CblasNoTrans, M, N, 1.0, A.data(), N, x.data(), 1, 0.0, b.data(), 1);

    return b;
}


// (C) Matrix-matrix product
vector<double> MatMat(vector<double> const &A, vector<double> const &B, size_t const &L)
{
    size_t const nelem_A = A.size();
    size_t const nelem_B = B.size();

    assert(nelem_A % L == 0 && nelem_B % L == 0);

    size_t const M = nelem_A/L;
    size_t const N = nelem_B/L;


    vector<double> C(M*N);

            

    for (size_t i = 0; i < M; ++i)
    {
        for (size_t j = 0; j < N; ++j)
        {
            double C_temp = 0;
            for (size_t k = 0; k < L; ++k)
            {
                C_temp += A[L*i + k]*B[N*k + j];
            }
            C[N*i + j] = C_temp;
        }
    }

    return C;
}

// (C) cBLAS matrix-matrix product
vector<double> MatMat_cBLAS(vector<double> const &A, vector<double> const &B, size_t const &L)
{
    size_t const nelem_A = A.size();
    size_t const nelem_B = B.size();

    assert(nelem_A % L == 0 && nelem_B % L == 0);

    size_t const M = nelem_A/L;
    size_t const N = nelem_B/L;


    vector<double> C(M*N);

    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, M, N, L, 1.0, A.data(), L, B.data(), N, 0.0, C.data(), N);

    return C;
}


// (D) Evaluation of a polynomial function
vector<double> poly(vector<double> const &a, vector<double> const &x)
{
    size_t const N = x.size();
    size_t const p = a.size() - 1;
    vector<double> y(N, 0);

    for (size_t i = 0; i < N; ++i)
    {
        double x_temp = x[i];
        double y_temp = 0;
        for (size_t k = 0; k < p + 1; ++k)
        {
            y_temp += x_temp*y_temp + a[p - k];
        }
        y[i] = y_temp;
    }

    return y;
}


// (E) Solves linear system of equations
void JacobiSolve(CRS_Matrix const &SK, vector<double> const &f, vector<double> &u)
{
    const double omega   = 1.0;
    const int    maxiter = 1000;
    const double tol  = 1e-5,                // tolerance
                 tol2 = tol * tol;           // tolerance^2

    int nrows = SK.Nrows();                  // number of rows == number of columns
    assert( nrows == static_cast<int>(f.size()) && f.size() == u.size() );

    cout << endl << " Start Jacobi solver for " << nrows << " d.o.f.s"  << endl;
    //  Choose initial guess
    for (int k = 0; k < nrows; ++k)
    {
        u[k] = 0.0;                          //  u := 0
    }

    vector<double> dd(nrows);                // matrix diagonal
    vector<double>  r(nrows);                // residual
    vector<double>  w(nrows);                // correction

    SK.GetDiag(dd);                          //  dd := diag(K)
    ////DebugVector(dd);{int ijk; cin >> ijk;}

    //  Initial sweep
    SK.Defect(r, f, u);                      //  r := f - K*u

    vddiv(w, r, dd);                         //  w := D^{-1}*r
    double sigma0 = dscapr(w, r);            // s0 := <w,r>

    // Iteration sweeps
    int iter  = 0;
    double sigma = sigma0;
    while ( sigma > tol2 * sigma0 && maxiter > iter)
    {
        ++iter;
        vdaxpy(u, u, omega, w );             //  u := u + om*w
        SK.Defect(r, f, u);                  //  r := f - K*u
        vddiv(w, r, dd);                     //  w := D^{-1}*r
        sigma = dscapr(w, r);                // s0 := <w,r>
//      	cout << "Iteration " << iter << " : " << sqrt(sigma/sigma0) << endl;
    }
    cout << "aver. Jacobi rate :  " << exp(log(sqrt(sigma / sigma0)) / iter) << "  (" << iter << " iter)" << endl;
    cout << "final error: " << sqrt(sigma / sigma0) << " (rel)   " << sqrt(sigma) << " (abs)\n";

    return;
}