/* Copyright (c) 2019, Anthony Latorre <tlatorre at uchicago>
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
 * Software Foundation, either version 3 of the License, or (at your option)
 * any later version.

 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
 * more details.

 * You should have received a copy of the GNU General Public License along with
 * this program. If not, see <https://www.gnu.org/licenses/>.
 */

#include <math.h>
#include <stdio.h>
#include "vector.h"
#include "mt19937ar.h"
#include "misc.h"

typedef int testFunction(char *err);

int isclose(double a, double b, double rel_tol, double abs_tol)
{
    /* Returns 1 if a and b are "close". This algorithm is taken from Python's
     * math.isclose() function.
     *
     * See https://www.python.org/dev/peps/pep-0485/. */
    return fabs(a-b) <= fmax(rel_tol*fmax(fabs(a),fabs(b)),abs_tol);
}

int test_dot(char *err)
{
    int i;
    double a[3], b[3], c, expected;

    init_genrand(0);

    for (i = 0; i < 100; i++) {
        a[0] = genrand_real2();
        a[1] = genrand_real2();
        a[2] = genrand_real2();

        b[0] = genrand_real2();
        b[1] = genrand_real2();
        b[2] = genrand_real2();

        c = DOT(a,b);

        expected = a[0]*b[0] + a[1]*b[1] + a[2]*b[2];

        if (!isclose(c, expected, 1e-9, 0)) {
            sprintf(err, "a \\dot b = %.5f, but expected %.5f", c, expected);
            return 1;
        }
    }

    return 0;
}

int test_norm(char *err)
{
    int i;
    double a[3], c, expected;

    init_genrand(0);

    for (i = 0; i < 100; i++) {
        a[0] = genrand_real2();
        a[1] = genrand_real2();
        a[2] = genrand_real2();

        c = NORM(a);

        expected = sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);

        if (!isclose(c, expected, 1e-9, 0)) {
            sprintf(err, "norm(a) = %.5f, but expected %.5f", c, expected);
            return 1;
        }
    }

    return 0;
}

int test_copy(char *err)
{
    int i, j;
    double a[3], b[3];

    init_genrand(0);

    for (i = 0; i < 100; i++) {
        b[0] = genrand_real2();
        b[1] = genrand_real2();
        b[2] = genrand_real2();

        COPY(a,b);

        for (j = 0; j < 3; j++) {
            if (!isclose(a[j], b[j], 1e-9, 0)) {
                sprintf(err, "a[%i] = %.5f, but expected %.5f", j, a[j], b[j]);
                return 1;
            }
        }
    }

    return 0;
}

int test_div(char *err)
{
    int i, j;
    double a[3], b[3], c;

    init_genrand(0);

    for (i = 0; i < 100; i++) {
        b[0] = genrand_real2();
        b[1] = genrand_real2();
        b[2] = genrand_real2();

        c = genrand_real2();

        COPY(a,b);
        DIV(a,c);

        for (j = 0; j < 3; j++) {
            if (!isclose(a[j], b[j]/c, 1e-9, 0)) {
                sprintf(err, "a[%i] = %.5f, but expected %.5f", j, a[j], b[j]/c);
                return 1;
            }
        }
    }

    return 0;
}

int test_add(char *err)
{
    int i;
    double a[3], b[3], c[3];

    init_genrand(0);

    for (i = 0; i < 100; i++) {
        a[0] = genrand_real2();
        a[1] = genrand_real2();
        a[2] = genrand_real2();

        b[0] = genrand_real2();
        b[1] = genrand_real2();
        b[2] = genrand_real2();

        ADD(c,a,b);

        if (!isclose(c[0], a[0]+b[0], 1e-9, 0)) {
            sprintf(err, "a[0] + b[0] = %.5f, but expected %.5f", c[0], a[0]+b[0]);
            return 1;
        }

        if (!isclose(c[1], a[1]+b[1], 1e-9, 0)) {
            sprintf(err, "a[1] + b[1] = %.5f, but expected %.5f", c[1], a[1]+b[1]);
            return 1;
        }

        if (!isclose(c[2], a[2]+b[2], 1e-9, 0)) {
            sprintf(err, "a[2] + b[2] = %.5f, but expected %.5f", c[2], a[2]+b[2]);
            return 1;
        }
    }

    return 0;
}

int test_sub(char *err)
{
    int i;
    double a[3], b[3], c[3];

    init_genrand(0);

    for (i = 0; i < 100; i++) {
        a[0] = genrand_real2();
        a[1] = genrand_real2();
        a[2] = genrand_real2();

        b[0] = genrand_real2();
        b[1] = genrand_real2();
        b[2] = genrand_real2();

        SUB(c,a,b);

        if (!isclose(c[0], a[0]-b[0], 1e-9, 0)) {
            sprintf(err, "a[0] - b[0] = %.5f, but expected %.5f", c[0], a[0]-b[0]);
            return 1;
        }

        if (!isclose(c[1], a[1]-b[1], 1e-9, 0)) {
            sprintf(err, "a[1] - b[1] = %.5f, but expected %.5f", c[1], a[1]-b[1]);
            return 1;
        }

        if (!isclose(c[2], a[2]-b[2], 1e-9, 0)) {
            sprintf(err, "a[2] - b[2] = %.5f, but expected %.5f", c[2], a[2]-b[2]);
            return 1;
        }
    }

    return 0;
}

int test_normalize(char *err)
{
    int i;
    double a[3], c;

    init_genrand(0);

    for (i = 0; i < 100; i++) {
        a[0] = genrand_real2();
        a[1] = genrand_real2();
        a[2] = genrand_real2();

        normalize(a);

        c = NORM(a);

        if (!isclose(c, 1.0, 1e-9, 0)) {
            sprintf(err, "norm(a) = %.5f, but expected %.5f", c, 1.0);
            return 1;
        }
    }

    return 0;
}

int test_cross_product(char *err)
{
    int i, j;
    double a[3], b[3], c[3], expected[3];

    init_genrand(0);

    for (i = 0; i < 100; i++) {
        a[0] = genrand_real2();
        a[1] = genrand_real2();
        a[2] = genrand_real2();

        b[0] = genrand_real2();
        b[1] = genrand_real2();
        b[2] = genrand_real2();

        CROSS(c,a,b);

        expected[0] = a[1]*b[2] - a[2]*b[1];
        expected[1] = a[2]*b[0] - a[0]*b[2];
        expected[2] = a[0]*b[1] - a[1]*b[0];

        for (j = 0; j < 3; j++) {
            if (!isclose(c[j], expected[j], 1e-9, 0)) {
                sprintf(err, "(a x b)[%i] = %.5f, but expected %.5f", j, c[j], expected[j]);
                return 1;
            }
        }
    }

    return 0;
}

int test_rotate(char *err)
{
    int i;
    double a[3], b[3], c[3], expected[3];

    a[0] = 1;
    a[1] = 0;
    a[2] = 0;

    b[0] = 0;
    b[1] = 0;
    b[2] = 1;

    rotate(c,a,b,M_PI/2);

    expected[0] = 0;
    expected[1] = 1;
    expected[2] = 0;

    for (i = 0; i < 3; i++) {
        if (!isclose(c[i], expected[i], 1e-9, 1e-9)) {
            sprintf(err, "rotate(a,b,pi/2)[%i] = %.5f, but expected %.5f", i, c[i], expected[i]);
            return 1;
        }
    }

    return 0;
}

struct tests {
    testFunction *test;
    char *name;
} tests[] = {
    {test_dot, "test_dot"},
    {test_norm, "test_norm"},
    {test_copy, "test_copy"},
    {test_div, "test_div"},
    {test_add, "test_add"},
    {test_sub, "test_sub"},
    {test_normalize, "test_normalize"},
    {test_cross_product, "test_cross_product"},
    {test_rotate, "test_rotate"}
};

int main(int argc, char **argv)
{
    int i;
    char err[256];
    int retval = 0;
    struct tests test;

    for (i = 0; i < LEN(tests); i++) {
        test = tests[i];

        if (!test.test(err)) {
            printf("[\033[92mok\033[0m] %s\n", test.name);
        } else {
            printf("[\033[91mfail\033[0m] %s: %s\n", test.name, err);
            retval = 1;
        }
    }

    return retval;
}