#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>

#include "mg2d.h"

#define AMPLITUDE 0.1
#define LOG2_GRIDSIZE 5
//#define GRIDSIZE ((1 << LOG2_GRIDSIZE) + 1)
#define GRIDSIZE 257
#define EXTENT 32.0
#define MAXITER 32

#define SQR(x) ((x) * (x))

static double findmax(double *arr, size_t len)
{
    double ret = 0.0;
    for (size_t i = 0; i < len; i++) {
        double val = fabs(*arr++);
        if (val > ret)
            ret = val;
    }
    return ret;
}

static double q(double a, double rho, double z)
{
    return a * SQR(rho) * exp(-(SQR(rho) + SQR(z)));
}

static double dq_rho(double a, double rho, double z)
{
    return a * 2 * rho * exp(-SQR(rho) - SQR(z)) * (1.0 - rho * (rho));
}

static double d2q_rho(double a, double rho, double z)
{
    double rho2 = SQR(rho);
    return a * 2 * exp(-SQR(rho) - SQR(z)) * (1.0 - 4.0 * rho * (rho) - rho2 + 2.0 * rho2 * SQR(rho));
}

static double d2q_rho_z(double a, double rho, double z)
{
    return a * 4 * z * rho * exp(-SQR(rho) - SQR(z)) * (rho * (rho) - 1.0);
}

static double dq_z(double a, double rho, double z)
{
    return -a * 2 * z * SQR(rho) * exp(-SQR(rho) - SQR(z));
}

static double d2q_z(double a, double rho, double z)
{
    return a * 2 * SQR(rho) * exp(-SQR(rho) - SQR(z)) * (2 * SQR(z) - 1.0);
}

int main(void)
{
    MG2DContext *ctx;
    int ret = 0;

    ctx = mg2d_solver_alloc(GRIDSIZE);
    if (!ctx) {
        fprintf(stderr, "Error allocating the solver context\n");
        return 1;
    }

    ctx->step[0] = EXTENT / (ctx->domain_size - 1);
    ctx->step[1] = EXTENT / (ctx->domain_size - 1);

    ctx->fd_stencil = 1;

    ctx->maxiter = MAXITER;
    ctx->nb_relax_pre  = 1;
    ctx->nb_cycles     = 2;
    ctx->nb_relax_post = 1;

    ctx->boundaries[MG2D_BOUNDARY_0L]->type = MG2D_BC_TYPE_FIXDIFF;
    ctx->boundaries[MG2D_BOUNDARY_1L]->type = MG2D_BC_TYPE_FIXDIFF;
    ctx->boundaries[MG2D_BOUNDARY_0U]->type = MG2D_BC_TYPE_FIXVAL;
    ctx->boundaries[MG2D_BOUNDARY_1U]->type = MG2D_BC_TYPE_FIXVAL;
    for (int i = 0; i < 4; i++) {
        memset(ctx->boundaries[i]->val, 0, ctx->boundaries[i]->val_len * sizeof(*ctx->boundaries[i]->val));
    }

    for (size_t z_idx = 0; z_idx < ctx->domain_size; z_idx++) {
        const double z = z_idx * ctx->step[1];

        memset(ctx->u + z_idx * ctx->u_stride, 0, sizeof(*ctx->u) * ctx->domain_size);

        for (size_t x_idx = 0; x_idx < ctx->domain_size; x_idx++) {
            const double x = x_idx * ctx->step[0];
            const double d2q = d2q_rho(AMPLITUDE, x, z) + d2q_z(AMPLITUDE, x, z);
            ctx->diff_coeffs[MG2D_DIFF_COEFF_02][ctx->diff_coeffs_stride * z_idx + x_idx] = 1.0;
            ctx->diff_coeffs[MG2D_DIFF_COEFF_20][ctx->diff_coeffs_stride * z_idx + x_idx] = x_idx ? 1.0 : 2.0;
            ctx->diff_coeffs[MG2D_DIFF_COEFF_10][ctx->diff_coeffs_stride * z_idx + x_idx] = x_idx ? 1.0 / x : 0.0;
            ctx->diff_coeffs[MG2D_DIFF_COEFF_00][ctx->diff_coeffs_stride * z_idx + x_idx] = 0.25 * d2q;
            ctx->rhs[ctx->rhs_stride * z_idx + x_idx] = -0.25 * d2q;
        }
        memset(ctx->diff_coeffs[MG2D_DIFF_COEFF_01] + z_idx * ctx->diff_coeffs_stride, 0,
               sizeof(*ctx->diff_coeffs[0]) * ctx->domain_size);
        memset(ctx->diff_coeffs[MG2D_DIFF_COEFF_11] + z_idx * ctx->diff_coeffs_stride, 0,
               sizeof(*ctx->diff_coeffs[0]) * ctx->domain_size);
    }

    ret = mg2d_solve(ctx);
    if (ret < 0) {
        fprintf(stderr, "Error solving the equation\n");
        ret = 1;
        goto fail;
    }

fail:
    mg2d_solver_free(&ctx);
    return ret;
}