#include <complex.h>
#include <string.h>
#include <math.h>
#include "include/matrix.h"

float __matrix_identMatrix[4][4] = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1};

float lenVec4(vec4 vector) {
    return sqrt(vector.x*vector.x + vector.y*vector.y + vector.z*vector.z);
}
vec4 addVec4(vec4 vector1, vec4 vector2) {
    vec4 out = {
        vector1.x + vector2.x,
        vector1.y + vector2.y,
        vector1.z + vector2.z,
        vector1.w
    };
    return out;
}
vec4 subVec4(vec4 vector1, vec4 vector2) {
    vec4 out = {
        vector1.x - vector2.x,
        vector1.y - vector2.y,
        vector1.z - vector2.z,
        vector1.w
    };
    return out;
}

vec4 crossVec4(vec4 vector1, vec4 vector2) {
    vec4 out = {
        vector1.y*vector2.z - vector1.z*vector2.y,
        vector1.z*vector2.x - vector1.x*vector2.z,
        vector1.x*vector2.y - vector1.y*vector2.x
    };
    return out;
}


void loadIdentMatrix(float target[4][4]) {
    memcpy(target, __matrix_identMatrix, 16*sizeof(float));
}

void transposeMatrix(float target[4][4]) { // extremely could not be bothered to do inplace
    float out[4][4] = {
        target[0][0],target[1][0],target[2][0], target[3][0],
        target[0][1],target[1][1], target[2][1], target[3][1],
        target[0][2],target[1][2], target[2][2], target[3][2],
        target[0][3],target[1][3], target[2][3], target[3][3],
    };
    memcpy(target, out, 16*sizeof(float));
}

void multiplyMat4x4ByMat4x4(float mat1[4][4], float mat2[4][4]) {
    float out[4][4]={0};
    for (int i = 0; i<4; i++) {
        for (int j =0; j<4; j++) {
            for (int k=0; k<4; k++) {
                out[i][j] += mat1[i][k] * mat2[k][j];
            }
        }
    }
    memcpy(mat1, out, 16*sizeof(float));
}


vec4 multiplyMat4x4ByVec4(float mat[4][4], vec4 vector) {
    vec4 out = {
        vector.x*mat[0][0] + vector.y*mat[0][1] + vector.z*mat[0][2] + vector.w*mat[0][3],
        vector.x*mat[1][0] + vector.y*mat[1][1] + vector.z*mat[1][2] + vector.w*mat[1][3],
        vector.x*mat[2][0] + vector.y*mat[2][1] + vector.z*mat[2][2] + vector.w*mat[2][3],
        vector.x*mat[3][0] + vector.y*mat[3][1] + vector.z*mat[3][2] + vector.w*mat[3][3],

    }; 
    return out;
}

vec4 rotateVec4(vec4 original, vec4 angles) {
    float cosX=cos(angles.x), cosY = cos(angles.y), cosZ = cos(angles.z);
    float sinX=sin(angles.x), sinY = sin(angles.y), sinZ = sin(angles.z);
    float rotMat[4][4] = {
        cosY*cosZ, (sinX*sinY*cosZ)-(cosX*sinZ), (cosX*sinY*cosZ)+(sinX*sinZ),0,
        cosY*sinZ, (sinX*sinY*sinZ)+(cosX*cosZ), (cosX*sinY*sinZ)-(sinX*cosZ),0,
        -sinY, sinX*cosY, cosX*cosY, 0,
        0,0,0,1
    };
    return multiplyMat4x4ByVec4(rotMat, original);
}

void rotateMat4x4(float mat[4][4], vec4 angles) { // i just love this <3
    float cosX=cos(angles.x), cosY=cos(angles.y), cosZ= cos(angles.z);
    float sinX=sin(angles.x), sinY = sin(angles.y), sinZ = sin(angles.z);
    float rotMat[4][4] = {
        cosY*cosZ, (sinX*sinY*cosZ)-(cosX*sinZ), (cosX*sinY*cosZ)+(sinX*sinZ),0,
        cosY*sinZ, (sinX*sinY*sinZ)+(cosX*cosZ), (cosX*sinY*sinZ)-(sinX*cosZ),0,
        -sinY, sinX*cosY, cosX*cosY, 0,
        0,0,0,1
    };
    multiplyMat4x4ByMat4x4(mat, rotMat);
}

void translateMatrix(float target[4][4], vec4 vectorDiff) {
    float translationMatrix[4][4] = {
        1, 0, 0, vectorDiff.x,
        0, 1, 0, vectorDiff.y,
        0, 0, 1, vectorDiff.z,
        0, 0, 0, 1,
    };
    multiplyMat4x4ByMat4x4(target, translationMatrix);
}

vec4 scaleVec4(vec4 original, vec4 vectorDiff){
    original.x *= vectorDiff.x;
    original.y *= vectorDiff.y;
    original.z *= vectorDiff.z;
    return original;
}

void normalizeMat4x4(float mat[4][4]);
vec4 normalizeVec4(vec4 original) { // maybe possible nan?
    vec4 out = {
        original.x/lenVec4(original),
        original.y/lenVec4(original),
        original.z/lenVec4(original),
        original.w
    };
    return out;
}

void createProjectionMat4x4(float target[4][4], float fov, float aspect_ratio, float znear, float zfar) {
    float out[4][4] = {
        1/(aspect_ratio*tan(fov/2)), 0,0,0,
        0, 1/tan(fov/2), 0,0,
        0,0, -(zfar+znear)/(zfar-znear), -(2*zfar*znear)/(zfar-znear),
        0,0,-1, 0
    };
    memcpy(target, out, 16*sizeof(float));
}


vec4 __unit_vector = {1, 1, 1,1};

void lookAt(float out[4][4], vec4 cameraPosition, vec4 object, vec4 up_axis) { // up axis = world space up direction, up = camera space up translated
    vec4 right, forward, up;
    forward = normalizeVec4(subVec4(cameraPosition, object)); // opengl has Z+ toward the camera and Z- in front of
    right = normalizeVec4(crossVec4(up_axis,forward));
    up = crossVec4(forward, right);

    cameraPosition.x = -cameraPosition.x;
    cameraPosition.y = -cameraPosition.y;
    cameraPosition.z = -cameraPosition.z;
    
    float lookAtMatrix[4][4] = {
        right.x, right.y, right.z, 0,
        up.x, up.y, up.z, 0,
        forward.x, forward.y,forward.z, 0,
        0, 0,0,1
    };

    translateMatrix(lookAtMatrix, cameraPosition);
    
    memcpy(out, lookAtMatrix, 16*sizeof(float));
}