#ifndef __RAY_MATH_TOOLKIT_H
#define __RAY_MATH_TOOLKIT_H

#include <math.h>
#include <stdio.h>
#include <assert.h>

static inline __attribute__((always_inline))
void normalize(double *v)
{
    double d = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
    assert(d != 0.0 && "Error calculating normal");

    v[0] /= d;
    v[1] /= d;
    v[2] /= d;
}

static inline __attribute__((always_inline))
double length(const double *v)
{
    return sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
}

static inline __attribute__((always_inline))
void add_vector(const double *a, const double *b, double *out)
{
    for (int i = 0; i < 3; i++)
        out[i] = a[i] + b[i];
}

static inline __attribute__((always_inline))
void subtract_vector(const double *a, const double *b, double *out)
{
    /*for (int i = 0; i < 3; i++)
        out[i] = a[i] - b[i];*/
    int i=0;
    out[i] = a[i] - b[i];
    out[i+1] = a[i+1] - b[i+1];
    out[i+2] = a[i+2] - b[i+2];
}

static inline __attribute__((always_inline))
void multiply_vectors(const double *a, const double *b, double *out)
{
    for (int i = 0; i < 3; i++)
        out[i] = a[i] * b[i];
}

static inline __attribute__((always_inline))
void multiply_vector(const double *a, double b, double *out)
{
    /*for (int i = 0; i < 3; i++)
        out[i] = a[i] * b;*/
    int i=0;
    out[i] = a[i] * b;
    out[i+1] = a[i+1] * b;
    out[i+2] = a[i+2] * b;

}

static inline __attribute__((always_inline))
void cross_product(const double *v1, const double *v2, double *out)
{
    out[0] = v1[1] * v2[2] - v1[2] * v2[1];
    out[1] = v1[2] * v2[0] - v1[0] * v2[2];
    out[2] = v1[0] * v2[1] - v1[1] * v2[0];
}

static inline __attribute__((always_inline))
double dot_product(const double *v1, const double *v2)
{
    double dp = 0.0;
    /*for (int i = 0; i < 3; i++)
        dp += v1[i] * v2[i];*/
    int i=0;
    dp += v1[i] * v2[i];
    dp += v1[i+1] * v2[i+1];
    dp += v1[i+2] * v2[i+2];
    return dp;
}

static inline __attribute__((always_inline))
void scalar_triple_product(const double *u, const double *v, const double *w,
                           double *out)
{
    cross_product(v, w, out);
    multiply_vectors(u, out, out);
}

static inline __attribute__((always_inline))
double scalar_triple(const double *u, const double *v, const double *w)
{
    double tmp[3];
    cross_product(w, u, tmp);
    return dot_product(v, tmp);
}

#endif
