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


void printMatrix (float matrix[16]) {
	printf("%5.1f %5.1f %5.1f %5.1f\n",matrix[0],matrix[4],matrix[8], matrix[12]);
	printf("%5.1f %5.1f %5.1f %5.1f\n",matrix[1],matrix[5],matrix[9], matrix[13]);
	printf("%5.1f %5.1f %5.1f %5.1f\n",matrix[2],matrix[6],matrix[10],matrix[14]);
	printf("%5.1f %5.1f %5.1f %5.1f\n",matrix[3],matrix[7],matrix[11],matrix[15]);
}

float lengthVector (float vector[]) {
	float xSquared = vector[0] * vector[0];
	float ySquared = vector[1] * vector[1];
	float zSquared = vector[2] * vector[2];
	return sqrt(xSquared + ySquared + zSquared);
}

void createMatrix (float matrix[16], float pos[3], float rot[3], float scale[3]) {
	matrix[12] = pos[0];
	matrix[13] = pos[1];
	matrix[14] = pos[2];
	matrix[15] = 1.0;
	
	float A = cos(rot[1]), B = sin(rot[1]);
	float C = cos(rot[0]), D = sin(rot[0]);
	float E = cos(rot[2]), F = sin(rot[2]);
	
	matrix[0] = ( A*E + B*D*F) * scale[0];
	matrix[1] = ( C*F        ) * scale[0];
	matrix[2] = (-B*E + A*D*F) * scale[0];
	matrix[3] = 0.0;
	
	matrix[4] = ( B*D*E - A*F) * scale[1];
	matrix[5] = ( C*E        ) * scale[1];
	matrix[6] = ( B*F + A*D*E) * scale[1];
	matrix[7] = 0.0;
	
	matrix[8] = ( B*C        ) * scale[2];
	matrix[9] = (-D          ) * scale[2];
	matrix[10]= ( A*C        ) * scale[2];
	matrix[11]= 0.0;
}

void invertMatrix (float A[], float B[]) {
	float sclX = lengthVector(&B[0]);
	float sclY = lengthVector(&B[4]);
	float sclZ = lengthVector(&B[8]);
	
	sclX *= sclX;
	sclY *= sclY;
	sclZ *= sclZ;
	
	if (sclX == 0) sclX = 0.00000001;
	if (sclY == 0) sclY = 0.00000001;
	if (sclZ == 0) sclZ = 0.00000001;
	
	A[0] = B[0]/sclX; A[4] = B[1]/sclX; A[8]  = B[2] /sclX;
	A[1] = B[4]/sclY; A[5] = B[5]/sclY; A[9]  = B[6] /sclY;
	A[2] = B[8]/sclZ; A[6] = B[9]/sclZ; A[10] = B[10]/sclZ;
	A[3] = 0.0;       A[7] = 0.0;       A[11] = 0.0;
	
	float posX = -B[12],
	posY = -B[13],
	posZ = -B[14];
	
	A[12] = A[0]*posX + A[4]*posY + A[8] *posZ;
	A[13] = A[1]*posX + A[5]*posY + A[9] *posZ;
	A[14] = A[2]*posX + A[6]*posY + A[10]*posZ;
	A[15] = 1.0;
}

void multiplyMatrix (float A[], float B[], float C[]) {
	A[0]  = B[0]*C[0]  + B[4]*C[1]  + B[8] *C[2]  + B[12]*C[3];
	A[1]  = B[1]*C[0]  + B[5]*C[1]  + B[9] *C[2]  + B[13]*C[3];
	A[2]  = B[2]*C[0]  + B[6]*C[1]  + B[10]*C[2]  + B[14]*C[3];
	A[3]  = B[3]*C[0]  + B[7]*C[1]  + B[11]*C[2]  + B[15]*C[3];
	
	A[4]  = B[0]*C[4]  + B[4]*C[5]  + B[8] *C[6]  + B[12]*C[7];
	A[5]  = B[1]*C[4]  + B[5]*C[5]  + B[9] *C[6]  + B[13]*C[7];
	A[6]  = B[2]*C[4]  + B[6]*C[5]  + B[10]*C[6]  + B[14]*C[7];
	A[7]  = B[3]*C[4]  + B[7]*C[5]  + B[11]*C[6]  + B[15]*C[7];
	
	A[8]  = B[0]*C[8]  + B[4]*C[9]  + B[8] *C[10] + B[12]*C[11];
	A[9]  = B[1]*C[8]  + B[5]*C[9]  + B[9] *C[10] + B[13]*C[11];
	A[10] = B[2]*C[8]  + B[6]*C[9]  + B[10]*C[10] + B[14]*C[11];
	A[11] = B[3]*C[8]  + B[7]*C[9]  + B[11]*C[10] + B[15]*C[11];
	
	A[12] = B[0]*C[12] + B[4]*C[13] + B[8] *C[14] + B[12]*C[15];
	A[13] = B[1]*C[12] + B[5]*C[13] + B[9] *C[14] + B[13]*C[15];
	A[14] = B[2]*C[12] + B[6]*C[13] + B[10]*C[14] + B[14]*C[15];
	A[15] = B[3]*C[12] + B[7]*C[13] + B[11]*C[14] + B[15]*C[15];
}

int main () {
	float matrix[16];
	float inverse[16];
	float identity[16];
	
	float position[3] = {0, 0, 0}; // X, Y, Z
	float rotation[3] = {0, 0, 0}; // X, Y, Z
	float scale[3]    = {1, 1, 3}; // X, Y, Z
	
	createMatrix(matrix,position,rotation,scale);
	invertMatrix(inverse,matrix);
	multiplyMatrix(identity,inverse,matrix);
	
	printf("Matrix\n");
	printMatrix(matrix);
	
	printf("Inverse\n");
	printMatrix(inverse);
	
	printf("Inverse * Matrix\n");
	printMatrix(identity);
	
	return 0;
}