/*
    Copyright (C) 2020 Akaneyu

    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 <http://www.gnu.org/licenses/>.
*/

#include "math_util.h"

#include <math.h>
#include <iostream>

using namespace std;

void multiplyMatrix4fVector4f(float *out, float *mat, float *v)
{
    float x = v[0];
    float y = v[1];
    float z = v[2];

    out[0] = x * mat[0] + y * mat[1] + z * mat[2] + mat[3] * v[3];
    out[1] = x * mat[4] + y * mat[5] + z * mat[6] + mat[7] * v[3];
    out[2] = x * mat[8] + y * mat[9] + z * mat[10] + mat[11] * v[3];
    out[3] = x * mat[12] + y * mat[13] + z * mat[14] + mat[15] * v[3];
}

void multiplyMatrix4fVector3f(float *out, float *mat, float *v)
{
    float x = v[0];
    float y = v[1];
    float z = v[2];

    out[0] = x * mat[0] + y * mat[1] + z * mat[2] + mat[3];
    out[1] = x * mat[4] + y * mat[5] + z * mat[6] + mat[7];
    out[2] = x * mat[8] + y * mat[9] + z * mat[10] + mat[11];
}

void multiplyVector4fValue(float *out, float *v, float val)
{
    out[0] = v[0] * val;
    out[1] = v[1] * val;
    out[2] = v[2] * val;
    out[3] = v[3] * val;
}

void multiplyVector3fValue(float *out, float *v, float val)
{
    out[0] = v[0] * val;
    out[1] = v[1] * val;
    out[2] = v[2] * val;
}

void copyVector4f(float *out, float *v)
{
    out[0] = v[0];
    out[1] = v[1];
    out[2] = v[2];
    out[3] = v[3];
}

void copyVector4fValue(float *out, float val)
{
    out[0] = val;
    out[1] = val;
    out[2] = val;
    out[3] = val;
}

void copyVector3f(float *out, float *v)
{
    out[0] = v[0];
    out[1] = v[1];
    out[2] = v[2];
}

void copyVector3fValue(float *out, float val)
{
    out[0] = val;
    out[1] = val;
    out[2] = val;
}

void copyVector3i(int *out, int *v)
{
    out[0] = v[0];
    out[1] = v[1];
    out[2] = v[2];
}

void copyVector3iValue(int *out, int val)
{
    out[0] = val;
    out[1] = val;
    out[2] = val;
}

void copyVector2f(float *out, float *v)
{
    out[0] = v[0];
    out[1] = v[1];
}

void copyVector2i(int *out, int *v)
{
    out[0] = v[0];
    out[1] = v[1];
}

void copyVector2iValue(int *out, int val)
{
    out[0] = val;
    out[1] = val;
}

void subtractVector3f(float *out, float *v1, float *v2)
{
    out[0] = v1[0] - v2[0];
    out[1] = v1[1] - v2[1];
    out[2] = v1[2] - v2[2];
}

void normalizeVector3f(float *v)
{
    float d = (float) sqrt(dotVector3f(v, v));

    if (d == 0) {
        copyVector3fValue(v, 0);
    } else {
        multiplyVector3fValue(v, v, 1.0f / d);
    }
}

float dotVector3f(float *v1, float *v2)
{
    return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}

float crossTriVector2f(float *v1, float *v2, float *v3)
{
    return (v1[0] - v2[0]) * (v2[1] - v3[1]) + (v1[1] - v2[1]) * (v3[0] - v2[0]);
}

float lenSquaredVector2f(float *v1, float *v2)
{
    float dx = v2[0] - v1[0];
    float dy = v2[1] - v1[1];

    return dx * dx + dy * dy;
}

void printVector2f(float *v) {
    cout << "(" << v[0] << ", " << v[1] << ")" << endl;
}

void printVector4f(float *v) {
    cout << "(" << v[0] << ", " << v[1] << ", " << v[2] << ", " << v[3] << ")" << endl;
}

void printMatrix4f(float *mat) {
    int i;

    cout << "[" << endl;

    for (i = 0; i < 16; i += 4) {
        cout << mat[i] << ", " << mat[i + 1] << ", " << mat[i + 2] << ", " << mat[i + 3] << endl;
    }

    cout << "]" << endl;
}
