#if !defined(MATH)
#define MATH

#import <math.h>

#define PI32 3.141592653589
#define TAU32 6.28318530718

typedef struct {
  float _00, _01, _02, _03;
  float _10, _11, _12, _13;
  float _20, _21, _22, _23;
  float _30, _31, _32, _33;
} Matrix;

typedef struct {
   float x,y,z;
} Vec3;

typedef struct {
  float x,y,z,w;
} Vec4;

Matrix mat_identity = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};

Matrix
mat_scale(f32 amnt) {
  Matrix r = {0};
  r._00 = amnt;
  r._11 = amnt;
  r._22 = amnt;
  r._33 = amnt;
  return r;
}

Matrix
make_proj_matrix(f32 fov, int width, int height, f32 near_plane, f32 far_plane) {
  Matrix r = {0};

  bool depth_01 = false;

  f32 aspect = (f32)height/(f32)width;

  // Y up, X right, -Z forward
  f32 fov_part = 1.0/tan(fov*0.5*PI32/180.0);
  f32 denom = far_plane - near_plane;
  r._00 = fov_part;
  r._11 = fov_part/aspect;
  r._22 = (far_plane + near_plane)/denom;
  r._23 = 2.0*far_plane*near_plane/denom;
  r._32 = 1.0;
  r._33 = 1.0;
  if(depth_01) {
    // VULKAN?
    // Makes depth range from 0 to 1
    // To map -1,1 depth range to 0,1 we transform z as follows: z' = z * 0.5 + 0.5
    r._22 = r._22 * 0.5 + r._32 * 0.5;
    r._23 = r._23 * 0.5 + r._33 * 0.5;
  }

  return r;
}

Matrix
make_translation(Vec3 v) {
  Matrix r;
  r = mat_identity;
  r._03 = v.x;
  r._13 = v.y;
  r._23 = v.z;
  return r;
}

Vec3
v3_normalize(Vec3 v) {
  f32 inv_denom = 1.0/sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
  v.x *= inv_denom;
  v.y *= inv_denom;
  v.z *= inv_denom;
  return v;
}

Matrix
make_rotation(Vec3 axis, f32 angle) {
  Matrix r = mat_identity;
  Vec3 a = v3_normalize(axis);
  f32 sin_theta = sin(angle);
  f32 cos_theta = cos(angle);
  f32 inv_cos = 1.0 - cos_theta;

  // Contributors to vector's X
  r._00 = (a.x*a.x*inv_cos) + cos_theta;
  r._01 = (a.x*a.y*inv_cos) + (a.z*sin_theta);
  r._02 = (a.x*a.z*inv_cos) - (a.y*sin_theta);

  // Contributors to vector's Y
  r._10 = (a.y*a.x*inv_cos) - (a.z*sin_theta);
  r._11 = (a.y*a.y*inv_cos) + cos_theta;
  r._12 = (a.y*a.z*inv_cos) + (a.x*sin_theta);

  // Contributors to vector's Z
  r._20 = (a.z*a.x*inv_cos) + (a.y*sin_theta);
  r._21 = (a.z*a.y*inv_cos) - (a.x*sin_theta);
  r._22 = (a.z*a.z*inv_cos) + cos_theta;

  return r;
}

Matrix
translate(Matrix from, Vec3 with) {
  Matrix r;
  r = from;
  r._03 += with.x*r._00 + with.y*r._01 + with.z*r._02;
  r._13 += with.x*r._10 + with.y*r._11 + with.z*r._12;
  r._23 += with.x*r._20 + with.y*r._21 + with.z*r._22;
  return r;
}

Vec3
cross(Vec3 a, Vec3 b) {
  Vec3 r;
  r.x = a.y*b.z - a.z*b.y;
  r.y = a.z*b.x - a.x*b.z;
  r.z = a.x*b.y - a.y*b.x;
  return r;
}

Matrix
make_lookat(Vec3 camera, Vec3 at, Vec3 up_vector) {
  Vec3 forward, right, up;
  Matrix look_at;
  // Z forward, Y up, X right
  forward.x = at.x - camera.x;
  forward.y = at.y - camera.y;
  forward.z = at.z - camera.z;
  forward = v3_normalize(forward);
  right = v3_normalize(cross(up_vector, forward)); // up_vector
  up = v3_normalize(cross(forward, right));
  memset(&look_at, 0x00, sizeof(Matrix)); // Todo: zero subroutine
  look_at._00 = right.x;
  look_at._01 = right.y;
  look_at._02 = right.z;
  look_at._10 = up.x;
  look_at._11 = up.y;
  look_at._12 = up.z;
  look_at._20 = forward.x;
  look_at._21 = forward.y;
  look_at._22 = forward.z;
  look_at._33 = 1;

  camera.x = -camera.x;
  camera.y = -camera.y;
  camera.z = -camera.z;
  look_at = translate(look_at, camera);

  return look_at;
}

Matrix
mat_mul(Matrix a, Matrix b) {
  Matrix r;
  r._00 = a._00*b._00 + a._01*b._10 + a._02*b._20 + a._03*b._30;
  r._01 = a._00*b._01 + a._01*b._11 + a._02*b._21 + a._03*b._31;
  r._02 = a._00*b._02 + a._01*b._12 + a._02*b._22 + a._03*b._32;
  r._03 = a._00*b._03 + a._01*b._13 + a._02*b._23 + a._03*b._33;
  r._10 = a._10*b._00 + a._11*b._10 + a._12*b._20 + a._13*b._30;
  r._11 = a._10*b._01 + a._11*b._11 + a._12*b._21 + a._13*b._31;
  r._12 = a._10*b._02 + a._11*b._12 + a._12*b._22 + a._13*b._32;
  r._13 = a._10*b._03 + a._11*b._13 + a._12*b._23 + a._13*b._33;
  r._20 = a._20*b._00 + a._21*b._10 + a._22*b._20 + a._23*b._30;
  r._21 = a._20*b._01 + a._21*b._11 + a._22*b._21 + a._23*b._31;
  r._22 = a._20*b._02 + a._21*b._12 + a._22*b._22 + a._23*b._32;
  r._23 = a._20*b._03 + a._21*b._13 + a._22*b._23 + a._23*b._33;
  r._30 = a._30*b._00 + a._31*b._10 + a._32*b._20 + a._33*b._30;
  r._31 = a._30*b._01 + a._31*b._11 + a._32*b._21 + a._33*b._31;
  r._32 = a._30*b._02 + a._31*b._12 + a._32*b._22 + a._33*b._32;
  r._33 = a._30*b._03 + a._31*b._13 + a._32*b._23 + a._33*b._33;
  return r;
}

Vec3
m4v3_mul(Matrix m, Vec3 v) {
  Vec3 result;
  result.x = m._00*v.x + m._01*v.y + m._02*v.z;
  result.y = m._10*v.x + m._11*v.y + m._12*v.z;
  result.z = m._20*v.x + m._21*v.y + m._22*v.z;
  return result;
}

void
transpose(Matrix *m) {
  #define SWAP(a, b) {f32 _t = a; a = b; b = _t;}
  SWAP(m->_01, m->_10);
  SWAP(m->_02, m->_20);
  SWAP(m->_03, m->_30);
  SWAP(m->_12, m->_21);
  SWAP(m->_13, m->_31);
  SWAP(m->_23, m->_32);
  #undef SWAP
}

#endif