#ifndef MATH_H
#define MATH_H

#define Sq(x) ((x)*(x))
#define Sqrt(x) sqrt((x))
#define PI 3.14159265358f
#define DegToRad(x) ((x)*(PI/180.0f))
#define RadToDeg(x) ((x)*(180.0f/PI))

#define PIVOT_X 0
#define PIVOT_Y 1
#define PIVOT_Z 2

typedef struct Vec2 {
  f32 x;
  f32 y;

  Vec2 operator-(Vec2 S)
  {
    Vec2 R = {0};
    R.x = x - S.x;
    R.y = y - S.y;
    return R;
  }

  Vec2 operator+(Vec2 S)
  {
    Vec2 R = {0};
    R.x = x + S.x;
    R.y = y + S.y;
    return R;
  }

  Vec2 operator*(Vec2 S)
  {
    Vec2 R = {0};
    R.x = x * S.x;
    R.y = y * S.y;
    return R;
  }

  Vec2 operator/(f32 s)
  {
    Vec2 R = {0};
    R.x = x/2;
    R.y = y/2;
    return R;
  }

  Vec2 operator/(Vec2 S)
  {
    Vec2 R = {0};
    R.x = x / S.x;
    R.y = y / S.y;
    return R;
  }
} Vec2;

typedef struct Vec3 {
    f32 x;
    f32 y;
    f32 z;

    Vec3 operator+(Vec3 S)
    {
      Vec3 R = {0};
      R.x = x + S.x;
      R.y = y + S.y;
      R.z = z + S.z;
      return R;
    }

    Vec3 operator+(f32 S)
    {
      Vec3 R = {0};
      R.x = x + S;
      R.y = y + S;
      R.z = z + S;
      return R;
    }

    Vec3 operator*(f32 S)
    {
      Vec3 R;
      R.x = x * S;
      R.y = y * S;
      R.z = z * S;
      
      return R;
    }

    Vec3 operator/(f32 S)
    {
      Vec3 R;
      R.x = x / S;
      R.y = y / S;
      R.z = z / S;
      
      return R;
    }
} Vec3;

typedef struct Vec4 {
    f32 x;
    f32 y;
    f32 z;
    f32 w;
} Vec4;

typedef struct Mat4 {
    f32 x0, x1, x2, x3;
    f32 y0, y1, y2, y3;
    f32 z0, z1, z2, z3;
    f32 w0, w1, w2, w3;
} Mat4;

Vec3 InitVec3(f32 Val);
Vec3 InitVec3(f32 x, f32 y, f32 z);
f32 LenVec3(Vec3 V);
Vec3 UnitVec3(Vec3 V);
f32 DotProductVec3(Vec3 S, Vec3 K);
Vec3 CrossProductVec3(Vec3 S, Vec3 K);

Vec4 InitVec4(f32 x, f32 y, f32 z, f32 w);
Vec4 ScalerAdd4(Vec4 Vec, f32 Scaler);
Vec4 ScalerMul4(Vec4 Vec, f32 Scaler);
Vec4 ScalerDiv4(Vec4 Vec, f32 Scaler);
Vec4 AddVec4(Vec4 V, Vec4 K);
f32 LenVec4(Vec4 V);
Vec4 UnitVec4(Vec4 V);
f32 DotProductVec4(Vec4 S, Vec4 K);
Vec4 Mul_Mat4Vec4(Mat4 Matrix, Vec4 S);
Mat4 IdentityMat();
Mat4 Mul_Mat4Mat4(Mat4 M1, Mat4 M2);
Mat4 CreateTranslationMat(Vec4 S);
Mat4 CreateScaleMat(Vec4 S);
Mat4 CreateRotationMat(f32 Theta, u8 Pivot);
Mat4 CreateFrustum(f32 left, f32 right, f32 bot, f32 top, f32 nearCam, f32 farCam);
Mat4 CreatePerspectiveUsingFrustum(f32 fov, f32 aspect, f32 nearCam, f32 farCam);
Mat4 CreateOrthographic(f32 left, f32 right, f32 bot, f32 top, f32 nearCam, f32 farCam);
Mat4 CreateOrthographicWithRatio(f32 scrWidth, f32 scrHeight, f32 nearCam, f32 farCam);
Mat4 CreateLookAtMat4(Vec3 CameraPos, Vec3 CameraTarget, Vec3 Up);

Vec3 InitVec3(f32 val)
{
  Vec3 R = Vec3{val,val,val};
  return R;
}

Vec3 InitVec3(f32 x, f32 y, f32 z)
{
  Vec3 R = Vec3{x,y,z};
  return R;
}

f32 LenVec3(Vec3 V)
{
    f32 L = Sqrt(Sq(V.x) + Sq(V.y) + Sq(V.z));
    return L;
}

Vec3 UnitVec3(Vec3 V)
{
    Vec3 R;
    f32 L = LenVec3(V);
    R.x = V.x/L;
    R.y = V.y/L;
    R.z = V.z/L;
    
    return R;
}

f32 DotProductVec3(Vec3 S, Vec3 K)
{
    Vec3 R;
    R.x = S.x*K.x;
    R.y = S.y*K.y;
    R.z = S.z*K.z;
    
    f32 DotProd = R.x + R.y + R.z;
    return DotProd;
}

Vec3 CrossProductVec3(Vec3 S, Vec3 K)
{
    Vec3 R;
    R.x = (S.y*K.z) - (S.z*K.y);
    R.y = (S.z*K.x) - (S.x*K.z);
    R.z = (S.x*K.y) - (S.y*K.x);
    
    return R;
}



// @note: I am creating vectors in many places so created a function to make initialising abit easier
Vec4 InitVec4(f32 x, f32 y, f32 z, f32 w)
{
    Vec4 V = {0};
    V.x = x;
    V.y = y;
    V.z = z;
    V.w = w;
    
    return V;
}


Vec4 ScalerAdd4(Vec4 Vec, f32 Scaler)
{
    Vec.x += Scaler;
    Vec.y += Scaler;
    Vec.z += Scaler;
    Vec.w += Scaler;
    
    return Vec;
}

Vec4 ScalerMul4(Vec4 Vec, f32 Scaler)
{
    Vec.x *= Scaler;
    Vec.y *= Scaler;
    Vec.z *= Scaler;
    Vec.w *= Scaler;
    
    return Vec;
}

Vec4 ScalerDiv4(Vec4 Vec, f32 Scaler)
{
    Vec.x = Vec.x/Scaler;
    Vec.y = Vec.y/Scaler;
    Vec.z = Vec.z/Scaler;
    Vec.w = Vec.w/Scaler;
    
    return Vec;
}

Vec4 AddVec4(Vec4 V, Vec4 K)
{
    Vec4 Res = {0};
    Res.x = V.x + K.x;
    Res.y = V.y + K.y;
    Res.z = V.z + K.z;
    Res.w = V.w + K.w;
    
    return Res;
}

f32 LenVec4(Vec4 V)
{
    f32 L = Sqrt(Sq(V.x) + Sq(V.y) + Sq(V.z) +Sq(V.w));
    return L;
}

Vec4 UnitVec4(Vec4 V)
{
    Vec4 R = {0};
    f32 L = LenVec4(V);
    R.x = V.x/L;
    R.y = V.y/L;
    R.z = V.z/L;
    R.w = V.w/L;
    
    return R;
}

f32 DotProductVec4(Vec4 S, Vec4 K)
{
    Vec4 R = {0};
    R.x = S.x*K.x;
    R.y = S.y*K.y;
    R.z = S.z*K.z;
    R.w = S.w*K.w;
    
    f32 DotProd = R.x + R.y + R.z + R.w;
    return DotProd;
}

// TODO: multiplication operations can be SIMD'd
Vec4 Mul_Mat4Vec4(Mat4 Matrix, Vec4 S)
{
    Vec4 Res = {0};
    Res.x = (Matrix.x0*S.x) + (Matrix.x1*S.y) + (Matrix.x2*S.z) + (Matrix.x3*S.w);
    Res.y = (Matrix.y0*S.y) + (Matrix.y1*S.y) + (Matrix.y2*S.z) + (Matrix.y3*S.w);
    Res.z = (Matrix.z0*S.z) + (Matrix.z1*S.y) + (Matrix.z2*S.z) + (Matrix.z3*S.w);
    Res.w = (Matrix.w0*S.w) + (Matrix.w1*S.y) + (Matrix.w2*S.z) + (Matrix.w3*S.w);
    
    return Res;
}

Mat4 IdentityMat()
{
  Mat4 M = {0};
  M.x0 = 1.0f;
  M.y1 = 1.0f;
  M.z2 = 1.0f;
  M.w3 = 1.0f;

  return M;
}

Mat4 Mul_Mat4Mat4(Mat4 M1, Mat4 M2)
{
    Mat4 Res = {0};
    // Row 0
    Res.x0 = (M1.x0*M2.x0) + (M1.x1*M2.y0) + (M1.x2*M2.z0) + (M1.x3*M2.w0);
    Res.x1 = (M1.x0*M2.x1) + (M1.x1*M2.y1) + (M1.x2*M2.z1) + (M1.x3*M2.w1);
    Res.x2 = (M1.x0*M2.x2) + (M1.x1*M2.y2) + (M1.x2*M2.z2) + (M1.x3*M2.w2);
    Res.x3 = (M1.x0*M2.x3) + (M1.x1*M2.y3) + (M1.x2*M2.z3) + (M1.x3*M2.w3);
    
    Res.y0 = (M1.y0*M2.x0) + (M1.y1*M2.y0) + (M1.y2*M2.z0) + (M1.y3*M2.w0);
    Res.y1 = (M1.y0*M2.x1) + (M1.y1*M2.y1) + (M1.y2*M2.z1) + (M1.y3*M2.w1);
    Res.y2 = (M1.y0*M2.x2) + (M1.y1*M2.y2) + (M1.y2*M2.z2) + (M1.y3*M2.w2);
    Res.y3 = (M1.y0*M2.x3) + (M1.y1*M2.y3) + (M1.y2*M2.z3) + (M1.y3*M2.w3);
    
    Res.z0 = (M1.z0*M2.x0) + (M1.z1*M2.y0) + (M1.z2*M2.z0) + (M1.z3*M2.w0);
    Res.z1 = (M1.z0*M2.x1) + (M1.z1*M2.y1) + (M1.z2*M2.z1) + (M1.z3*M2.w1);
    Res.z2 = (M1.z0*M2.x2) + (M1.z1*M2.y2) + (M1.z2*M2.z2) + (M1.z3*M2.w2);
    Res.z3 = (M1.z0*M2.x3) + (M1.z1*M2.y3) + (M1.z2*M2.z3) + (M1.z3*M2.w3);
    
    Res.w0 = (M1.w0*M2.x0) + (M1.w1*M2.y0) + (M1.w2*M2.z0) + (M1.w3*M2.w0);
    Res.w1 = (M1.w0*M2.x1) + (M1.w1*M2.y1) + (M1.w2*M2.z1) + (M1.w3*M2.w1);
    Res.w2 = (M1.w0*M2.x2) + (M1.w1*M2.y2) + (M1.w2*M2.z2) + (M1.w3*M2.w2);
    Res.w3 = (M1.w0*M2.x3) + (M1.w1*M2.y3) + (M1.w2*M2.z3) + (M1.w3*M2.w3);
    
    return Res;
}

/*
 * Matrix multiplication orders
 * Scale -> Rotate -> Translate
 */
Mat4 CreateTranslationMat(Vec4 S)
{
    Mat4 TM = {0};
    TM.x0=1.0f; TM.x3=S.x,
    TM.y1=1.0f; TM.y3=S.y,
    TM.z2=1.0f; TM.z3=S.z,
    TM.w3=1.0f;
    
    return TM;
}

Mat4 CreateScaleMat(Vec4 S)
{
    Mat4 SM = {0};
    SM.x0=S.x;
    SM.y1=S.y;
    SM.z2=S.z;
    SM.w3=1.0f;
    return SM;
}

Mat4 CreateRotationMat(f32 Theta, u8 Pivot)
{
    f32 CosTheta = cos(Theta);
    f32 SinTheta = sin(Theta);
    
    Mat4 RotMat = {0};
    if (Pivot == PIVOT_X)
    {
        RotMat.x0 = 1.0f;
        RotMat.y1 = CosTheta; RotMat.y2 = -SinTheta;
        RotMat.z1 = SinTheta; RotMat.z2 = CosTheta;
        RotMat.w3 = 1.0f;
    }
    else if (Pivot == PIVOT_Y)
    {
        RotMat.x0 = CosTheta; RotMat.x2 = SinTheta;
        RotMat.y1 = 1.0f;
        RotMat.z0 = -SinTheta; RotMat.z2 = CosTheta;
        RotMat.w3 = 1.0f;
    }
    else if (Pivot == PIVOT_Z)
    {
        RotMat.x0 = CosTheta; RotMat.x1 = -SinTheta;
        RotMat.y0 = SinTheta; RotMat.y1 = CosTheta;
        RotMat.z3 = 1.0f;
        RotMat.w3 = 1.0f;
    }
    
    return RotMat;
}

Mat4 CreateFrustum(f32 left, f32 right, f32 bot, f32 top, f32 nearCam, f32 farCam)
{
  Mat4 F = {0};
  F.x0 = 2.0f*nearCam/(right - left);    
  F.x3 = -nearCam*(right + left)/(right - left);
  F.y1 = 2.0f*nearCam/(top-bot);         
  F.y3 = -nearCam*(top + bot)/(top - bot);
  F.z2 = -(farCam+nearCam)/(farCam-nearCam); 
  F.z3 = 2.0f*farCam*nearCam/(nearCam - farCam); 
  F.w2 = -1.0f;                       
  F.w3 = 0.0f;

  return F;
}

Mat4 CreatePerspectiveUsingFrustum(f32 fov, f32 aspect, f32 nearCam, f32 farCam)
{
  f32 top = nearCam*tan(fov)/2;
  f32 bot = -top;
  f32 right = top*aspect;
  f32 left = -right;

  return CreateFrustum(left, right, bot, top, nearCam, farCam);
}

Mat4 CreateOrthographic(f32 left, f32 right, f32 bot, f32 top, f32 nearCam, f32 farCam)
{
  Mat4 F = {0};
  F.x0 = 2.0f/(right - left);
  F.w0 = -(right + left)/(right - left);

  F.y1 = 2.0f/(top - bot);
  F.w1 = -(top + bot)/(top - bot);

  F.z2 = -2.0f/(farCam - nearCam);
  F.w2 = -(farCam + nearCam)/(farCam - nearCam);

  F.w3 = 1.0f;

  return F;
}

Mat4 CreateOrthographicWithRatio(f32 scrWidth, f32 scrHeight, f32 nearCam, f32 farCam)
{
  f32 ratio_h = scrWidth/scrHeight;
  f32 left = -ratio_h;
  f32 right = ratio_h;

  f32 ratio_v = scrHeight/scrWidth;
  f32 top = ratio_v;
  f32 bot = -ratio_v;
  
  return CreateOrthographic(left, right, bot, top, nearCam, farCam);
}

// @research: the gram-schmidt process:
// https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process 
Mat4 CreateLookAtMat4(Vec3 CameraPos, Vec3 CameraTarget, Vec3 Up)
{
  // deriving the respective camera vectors
  Vec3 CameraDir = UnitVec3((CameraPos + (CameraTarget * -1.0f)));
  Vec3 CameraRight = UnitVec3(CrossProductVec3(Up, CameraDir));
  Vec3 CameraUp = CrossProductVec3(CameraRight, CameraDir);

  Mat4 DirMat = IdentityMat();
  DirMat.x0 = CameraRight.x; DirMat.x1 = CameraRight.y; DirMat.x2 = CameraRight.z;
  DirMat.y0 = CameraUp.x; DirMat.y1 = CameraUp.y; DirMat.y2 = CameraUp.z;
  DirMat.z0 = CameraDir.x; DirMat.z1 = CameraDir.y; DirMat.z2 = CameraDir.z;

  // make camera position -ve
  Mat4 TranslationMat = IdentityMat();
  TranslationMat.x3 = -CameraPos.x;
  TranslationMat.y3 = -CameraPos.y;
  TranslationMat.z3 = -CameraPos.z;

  Mat4 LookAt = Mul_Mat4Mat4(DirMat, TranslationMat); 
  return LookAt;
}

#endif