#include <cmath>

#include <hpc/matvec/axpy.hpp>
#include <hpc/matvec/copy.hpp>
#include <hpc/matvec/densevector.hpp>
#include <hpc/matvec/iterators.hpp>
#include <hpc/matvec/traits.hpp>
#include <hpc/matvec/print.hpp>

//------------------------------------------------------------------------------

using namespace hpc;
using namespace hpc::matvec;


template <typename T>
const T &
assertEqual(const T &a, const T &b)
{
    assert(a==b);
    return a;
}

//------------------------------------------------------------------------------

template <
    typename F, typename G,
    typename T, template<typename> class VectorF,
                template<typename> class VectorU,
                Require< Dense<VectorF<T>>,
                         Dense<VectorU<T>> > = true
>
void
discretizeProblem(const F &f, const G &g, VectorF<T> &f_h, VectorU<T> &u_h)
{
    auto N  = assertEqual(f_h.length(), u_h.length()) - 2;
    auto h  = T(1)/(N+1);

    u_h(0)   = g(0);
    u_h(N+1) = g(1);

    for (std::size_t i=1; i<N; ++i) {
        u_h(i) = 0;
        f_h(i) = f(i*h);
    }
    // actually we don't have to initialize f_h(0), f_h(N+1) so we store some
    // good and evil numbers there:
    f_h(0)   = 42;
    f_h(N+1) = 666;
}

//------------------------------------------------------------------------------

template <
    typename T, template<typename> class Vector,
                Require< Dense<Vector<T>> > = true
>
T
lInfNorm(const Vector<T> &x_h)
{
    T res = 0;

    for (auto [i, xi] : x_h) {
        i=i;
        if (std::abs(xi) > res) {
            res = std::abs(xi);
        }
    }
    return res;
}

//------------------------------------------------------------------------------

template <
    typename U,
    typename T, template<typename> class VectorU,
                template<typename> class VectorE,
                Require< Dense<VectorU<T>>,
                         Dense<VectorE<T>> > = true
>
void
evalError(const U &u, const VectorU<T> &u_h, VectorE<T> &e_h)
{
    auto N  = assertEqual(u_h.length(), e_h.length()) - 2;
    auto h  = T(1)/(N+1);

    for (auto [i, ei] : e_h) {
        ei = u(i*h) - u_h(i);
    }
}

//------------------------------------------------------------------------------

template <
    typename T, template<typename> class VectorU,
                template<typename> class VectorF,
                template<typename> class VectorR,
                Require< Dense<VectorF<T>>,
                         Dense<VectorU<T>>,
                         Dense<VectorR<T>> > = true
>
void
evalResidual(const VectorU<T> &u_h, const VectorF<T> &f_h, VectorR<T> &r_h)
{
    auto N  = assertEqual(u_h.length(), r_h.length()) - 2;
    auto h  = T(1)/(N+1);
    auto h2 = h*h;

    r_h(0) = r_h(N+1) = 0;
    for (std::size_t i=1; i<=N; ++i) {
        r_h(i) = f_h(i) - 1./h2 * (2*u_h(i) - u_h(i-1) - u_h(i+1));
    }
}


//------------------------------------------------------------------------------

template <
    typename T, template<typename> class VectorF,
                template<typename> class VectorU,
                Require< Dense<VectorF<T>>,
                         Dense<VectorU<T>> > = true
>
void
direct_solver(const VectorF<T> &f_h, VectorU<T> &u_h)
{
    auto N  = assertEqual(f_h.length(), u_h.length()) - 2;
    auto h  = T(1)/(N+1);
    auto h2 = h*h;

    // TODO: Your code
}

//------------------------------------------------------------------------------

int
main()
{
    auto f = [](double x) { return -2; };
    auto g = [](double x) { return 1 + 3*x; };
    auto u = [](double x) { return (x+1)*(x+1); };

    std::size_t N = 10;

    DenseVector<double>  f_h(N+2), u_h(N+2), r_h(N+2), e_h(N+2);

    discretizeProblem(f, g, f_h, u_h);

    fmt::printf("f_h =\n"); print(f_h);
    fmt::printf("u_h =\n"); print(u_h);

    fmt::printf("Calling solver: direct_solver(f_h, u_h);\n");
    direct_solver(f_h, u_h);
    evalResidual(u_h, f_h, r_h);

    fmt::printf("u_h =\n"); print(u_h);
    fmt::printf("r_h =\n"); print(r_h);

    fmt::printf("Eval approximation error of u_h:\n");
    evalError(u, u_h, e_h);
    fmt::printf("e_h =\n"); print(e_h);

    fmt::printf("\n\nIn short:\n");
    fmt::printf("lInfNorm(r_h) = %.5e\n", lInfNorm(r_h));
    fmt::printf("lInfNorm(e_h) = %.5e\n", lInfNorm(e_h));
}