#pragma once

#include <math.h>
#include "typedef.h"

static const F32 PI    = 3.14159265359f;
static const F32 PIRAD = 0.01745329251f;
static const F32 RADPI = 57.2957795131f;

struct VEC2 {
  F32 x;
  F32 y;

  VEC2() { x = y = 0.0f; }
  VEC2( F32 X, F32 Y ) { x = X; y = Y; }
  VEC2( const F32* v ) { x = v[0]; y = v[1]; }
  VEC2( const VEC2& v ) { x = v.x; y = v.y; }

  bool operator==( const VEC2& v ) const { return ( x == v.x && y == v.y ); }
  VEC2& operator=( const VEC2& v ) { x = v.x; y = v.y; return *this; }
  F32& operator[]( I32 i ) { return ( (F32*)this )[i]; }
  F32 operator[]( I32 i ) const { return ( (const F32*)this )[i]; }

  VEC2& operator+=( const VEC2& v ) { x += v.x; y += v.y; return *this; }
  VEC2& operator-=( const VEC2& v ) { x -= v.x; y -= v.y; return *this; }
  VEC2& operator*=( const VEC2& v ) { x *= v.x; y *= v.y; return *this; }
  VEC2& operator/=( const VEC2& v ) { x /= v.x; y /= v.y; return *this; }

  VEC2 operator+( const VEC2& v ) const { return VEC2( x + v.x, y + v.y ); }
  VEC2 operator-( const VEC2& v ) const { return VEC2( x - v.x, y - v.y ); }
  VEC2 operator*( const VEC2& v ) const { return VEC2( x * v.x, y * v.y ); }
  VEC2 operator/( const VEC2& v ) const { return VEC2( x / v.x, y / v.y ); }

  VEC2& operator+=( const F32& v ) { x += v; y += v; return *this; }
  VEC2& operator-=( const F32& v ) { x -= v; y -= v; return *this; }
  VEC2& operator*=( const F32& v ) { x *= v; y *= v; return *this; }
  VEC2& operator/=( const F32& v ) { x /= v; y /= v; return *this; }

  VEC2 operator/( const F32& v ) const { return VEC2( x / v, y / v ); }
  VEC2 operator*( const F32& v ) const { return VEC2( x * v, y * v ); }
  VEC2 operator+( const F32& v ) const { return VEC2( x + v, y + v ); }
  VEC2 operator-( const F32& v ) const { return VEC2( x - v, y - v ); }
};

struct VEC3 {
  F32 x;
  F32 y;
  F32 z;

  VEC3() { x = y = z = 0.0f; }
  VEC3( F32 X, F32 Y, F32 Z ) { x = X; y = Y; z = Z; }
  VEC3( const F32* v ) { x = v[0]; y = v[1]; z = v[2]; }
  VEC3( const VEC3& v ) { x = v.x; y = v.y; z = v.z; }

  VEC3( const VEC2& v ) { x = v.x; y = v.y; z = 0.f; }

  bool operator==( const VEC3& v ) const { return ( x == v.x && y == v.y && z == v.z ); }
  VEC3& operator=( const VEC3& v ) { x = v.x; y = v.y; z = v.z; return *this; }
  F32& operator[]( I32 i ) { return ( (F32*)this )[i]; }
  F32 operator[]( I32 i ) const { return ( (F32*)this )[i]; }

  VEC3& operator+=( const VEC3& v ) { x += v.x; y += v.y; z += v.z; return *this; }
  VEC3& operator-=( const VEC3& v ) { x -= v.x; y -= v.y; z -= v.z; return *this; }
  VEC3& operator*=( const VEC3& v ) { x *= v.x; y *= v.y; z *= v.z; return *this; }
  VEC3& operator/=( const VEC3& v ) { x /= v.x; y /= v.y; z /= v.z; return *this; }

  VEC3 operator+( const VEC3& v ) const { return VEC3( x + v.x, y + v.y, z + v.z ); }
  VEC3 operator-( const VEC3& v ) const { return VEC3( x - v.x, y - v.y, z - v.z ); }
  VEC3 operator*( const VEC3& v ) const { return VEC3( x * v.x, y * v.y, z * v.z ); }
  VEC3 operator/( const VEC3& v ) const { return VEC3( x / v.x, y / v.y, z / v.z ); }

  VEC3& operator+=( const F32& v ) { x += v; y += v; z += v; return *this; }
  VEC3& operator-=( const F32& v ) { x -= v; y -= v; z -= v; return *this; }
  VEC3& operator*=( const F32& v ) { x *= v; y *= v; z *= v; return *this; }
  VEC3& operator/=( const F32& v ) { x /= v; y /= v; z /= v; return *this; }

  VEC3 operator/( const F32& v ) const { return VEC3( x / v, y / v, z / v ); }
  VEC3 operator*( const F32& v ) const { return VEC3( x * v, y * v, z * v ); }
  VEC3 operator+( const F32& v ) const { return VEC3( x + v, y + v, z + v ); }
  VEC3 operator-( const F32& v ) const { return VEC3( x - v, y - v, z - v ); }
};

struct VEC4 {
  F32 x, y, z, w;

  VEC4() { x = y = z = w = 0; }
  VEC4( F32 X, F32 Y, F32 Z, F32 W ) { x = X; y = Y; z = Z; w = W; }
  VEC4( const VEC4& v ) { x = v.x; y = v.y; z = v.z; w = v.w; }
  VEC4( const VEC3& v ) { x = v.x; y = v.y; z = v.z; w = 1.f; }
  VEC4( const VEC2& v ) { x = v.x; y = v.y; z = 0.f; w = 1.f; }

  VEC4( const VEC2& v, F32 W ) { x = v.x; y = v.y; z = 0.f; w = W; }
  VEC4( const VEC3& v, F32 W ) { x = v.x; y = v.y; z = v.z; w = W; }

  VEC4( F32* v ) { x = v[0]; y = v[1]; z = v[2]; w = v[3]; }

  VEC4 operator=( const VEC4& v ) { x = v.x; y = v.y; z = v.z; w = v.w; return *this; }
  F32& operator[]( const int i ) { return *(&x + i); }
  F32 operator[]( const int i ) const { return *(&x + i); }

  VEC4& operator+=( const VEC4& v ) { x += v.x; y += v.y; z += v.z; w += v.w; return *this; }
  VEC4& operator-=( const VEC4& v ) { x -= v.x; y -= v.y; z -= v.z; w -= v.w; return *this; }
  VEC4& operator*=( const VEC4& v ) { x *= v.x; y *= v.y; z *= v.z; w *= v.w; return *this; }
  VEC4& operator/=( const VEC4& v ) { x /= v.x; y /= v.y; z /= v.z; w /= v.w; return *this; }

  VEC4 operator+( const VEC4& v ) { return VEC4( x + v.x, y + v.y, z + v.z, w + v.w ); }
  VEC4 operator-( const VEC4& v ) { return VEC4( x - v.x, y - v.y, z - v.z, w - v.w ); }
  VEC4 operator*( const VEC4& v ) { return VEC4( x * v.x, y * v.y, z * v.z, w * v.w ); }
  VEC4 operator/( const VEC4& v ) { return VEC4( x / v.x, y / v.y, z / v.z, w / v.w ); }

  VEC4& operator+=( const F32& v ) { x += v; y += v; z += v; w += v; return *this; }
  VEC4& operator-=( const F32& v ) { x -= v; y -= v; z -= v; w -= v; return *this; }
  VEC4& operator*=( const F32& v ) { x *= v; y *= v; z *= v; w *= v; return *this; }
  VEC4& operator/=( const F32& v ) { x /= v; y /= v; z /= v; w /= v; return *this; }

  VEC4 operator+( const F32& v ) { return VEC4( x + v, y + v, z + v, w + v ); }
  VEC4 operator-( const F32& v ) { return VEC4( x - v, y - v, z - v, w - v ); }
  VEC4 operator*( const F32& v ) { return VEC4( x * v, y * v, z * v, w * v ); }
  VEC4 operator/( const F32& v ) { return VEC4( x / v, y / v, z / v, w / v ); }
};

inline U8 is_zero( VEC2 v ) { return v.x == 0.f && v.y == 0.f; }
inline U8 is_zero( VEC3 v ) { return v.x == 0.f && v.y == 0.f && v.z == 0.f; }
inline U8 is_zero( VEC4 v ) { return v.x == 0.f && v.y == 0.f && v.z == 0.f && v.w == 0.f; }

inline F32 vec_len( VEC2 v ) { return sqrtf( v.x * v.x + v.y * v.y ); }
inline F32 vec_lensq( VEC2 v ) { return v.x * v.x + v.y * v.y; }

inline F32 vec_len( VEC3 v ) { return sqrtf( v.x * v.x + v.y * v.y + v.z * v.z ); }
inline F32 vec_lensq( VEC3 v ) { return v.x * v.x + v.y * v.y + v.z * v.z; }
inline F32 vec_len2d( VEC3 v ) { return sqrtf( v.x * v.x + v.y * v.y ); }
inline F32 vec_len2dsq( VEC3 v ) { return v.x * v.x + v.y * v.y; }

inline F32 vec_len( VEC4 v ) { return sqrtf( v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w ); }
inline F32 vec_lensq( VEC4 v ) { return v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; }
inline F32 vec_len3d( VEC4 v ) { return sqrtf( v.x * v.x + v.y * v.y + v.z * v.z ); }
inline F32 vec_len3dsq( VEC4 v ) { return v.x * v.x + v.y * v.y + v.z * v.z; }

inline F32 vec_distsq( VEC2 v1, VEC2 v2 ) { return vec_lensq( v1 - v2 ); }
inline F32 vec_dist( VEC2 v1, VEC2 v2 ) { return sqrtf( vec_distsq( v1, v2 ) ); }
inline F32 vec_dot( VEC2 v1, VEC2 v2 ) { return v1.x * v2.x + v1.y * v2.y; }
inline F32 vec_cross( VEC2 v1, VEC2 v2 ) { return v1.x * v2.y - v1.y * v2.x; }

inline F32 vec_distsq( VEC3 v1, VEC3 v2 ) { return vec_lensq( v1 - v2 ); }
inline F32 vec_dist( VEC3 v1, VEC3 v2 ) { return sqrtf( vec_distsq( v1, v2 ) ); }
inline F32 vec_dist2dsq( VEC3 v1, VEC3 v2 ) { return vec_lensq( v1 - v2 ); }
inline F32 vec_dist2d( VEC3 v1, VEC3 v2 ) { return sqrtf( vec_dist2dsq( v1, v2 ) ); }
inline F32 vec_dot( VEC3 v1, VEC3 v2 ) { return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z; }
inline VEC3 vec_cross( VEC3 v1, VEC3 v2 ) { return { v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x }; }

inline void vec_normalize( VEC2* v ) { F32 l = vec_len( *v ); v->x /= l; v->y /= l; }
inline void vec_normalize( VEC3* v ) { F32 l = vec_len( *v ); v->x /= l; v->y /= l; v->z /= l; }

inline VEC2 vec_normalize( VEC2 v ) { vec_normalize( &v ); return v; };
inline VEC3 vec_normalize( VEC3 v ) { vec_normalize( &v ); return v; };

inline VEC3 vec_normangle( VEC3* v ) {
  v->x = remainderf( v->x, 360.f );
  v->y = remainderf( v->y, 360.f );
  v->z = remainderf( v->z, 360.f );

  v->x = fmaxf( fminf( v->x, 90.f ), -90.f );
  return *v;
}

inline VEC3 vec_normangle( VEC3 v ) {
  return vec_normangle( &v );
}

inline VEC2 vec_normangle( VEC2* v ) {
  v->x = remainderf( v->x, 360.f );
  v->y = remainderf( v->y, 360.f );

  v->x = fmaxf( fminf( v->x, 90.f ), -90.f );
  return *v;
}

inline VEC2 vec_normangle( VEC2 v ) {
  return vec_normangle( &v );
}

inline VEC3 vec_angles( VEC3 v1, VEC3 v2 ) {
  VEC3 d = v2 - v1;

  F32 m = sqrtf( d.x * d.x + d.y * d.y );
  F32 p = atan2f( -d.z, m ) * RADPI;
  F32 y = atan2f( d.y, d.x ) * RADPI;

  return vec_normangle( VEC3( p, y, 0.0f ) );
}

inline F32 vec_angle( VEC2 v1, VEC2 v2 ) {
  VEC2 d = v2 - v1;
  return remainderf( atan2f( d.y, d.x ) * RADPI, 360.f );
}

inline VEC3 angle_vectors( VEC3 angles ) {
  F32 sp, sy, cp, cy;
  sp = sinf( angles.x * PIRAD );
  cp = cosf( angles.x * PIRAD );
  sy = sinf( angles.y * PIRAD );
  cy = cosf( angles.y * PIRAD );

  return VEC3{ cp * cy, cp * sy, -sp };
}

inline void angle_vectors( const VEC3& angles, VEC3* forward, VEC3* right, VEC3* up ) {
  F32 sr, sp, sy, cr, cp, cy;

  sp = sinf( angles.x * PIRAD );
  cp = cosf( angles.x * PIRAD );
  sy = sinf( angles.y * PIRAD );
  cy = cosf( angles.y * PIRAD );
  sr = sinf( angles.z * PIRAD );
  cr = cosf( angles.z * PIRAD );

  if ( forward ) {
    forward->x = cp * cy;
    forward->y = cp * sy;
    forward->z = -sp;
  }

  if ( right ) {
    right->x = -1 * sr * sp * cy + -1 * cr * -sy;
    right->y = -1 * sr * sp * sy + -1 * cr * cy;
    right->z = -1 * sr * cp;
  }

  if ( up ) {
    up->x = cr * sp * cy + -sr * -sy;
    up->y = cr * sp * sy + -sr * cy;
    up->z = cr * cp;
  }
}