#include "Utility/Math/AffineMatrix.h"

#include <bit>
#include <cstring>

// STATIC: 0x01312438
// GLOBAL: gl_21913
const float g_identityMatrix[12] = {1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0};

namespace {
  // INLINED
  void Rotate(float* values, const float* rotation) {
    for (unsigned row = 0; row < 4; ++row) {
      const float x = values[row * 3];
      const float y = values[row * 3 + 1];
      const float z = values[row * 3 + 2];
      const float first = (x * rotation[0] + y * rotation[3]) + z * rotation[6];
      const float second = (x * rotation[1] + y * rotation[4]) + z * rotation[7];
      const float third = (x * rotation[2] + y * rotation[5]) + z * rotation[8];

      values[row * 3] = row == 3 ? first + rotation[9] : first;
      values[row * 3 + 1] = row == 3 ? second + rotation[10] : second;
      values[row * 3 + 2] = row == 3 ? third + rotation[11] : third;
    }
  }
}

// INLINED
void AffineMatrix::Identity() {
  std::memcpy(this->values, g_identityMatrix, sizeof(g_identityMatrix));
}

// OFFSET: 0x00555360
// FUNCTION: fn_02034
void AffineMatrix::SetXRotation(float radians) {
  this->Identity();

  const float sine = ClientSin(radians);
  const float cosine = ClientCos(radians);
  this->values[4] = cosine;
  this->values[8] = cosine;
  this->values[5] = sine;
  this->values[7] = -sine;
}

// OFFSET: 0x005553D0
// FUNCTION: fn_02035
void AffineMatrix::SetYRotation(float radians) {
  this->Identity();

  const float sine = ClientSin(radians);
  const float cosine = ClientCos(radians);
  this->values[0] = cosine;
  this->values[8] = cosine;
  this->values[2] = -sine;
  this->values[6] = sine;
}

// OFFSET: 0x00555440
// FUNCTION: fn_02036
void AffineMatrix::SetZRotation(float radians) {
  this->Identity();

  const float sine = ClientSin(radians);
  const float cosine = ClientCos(radians);
  this->values[0] = cosine;
  this->values[4] = cosine;
  this->values[3] = -sine;
  this->values[1] = sine;
}

// OFFSET: 0x00552F70
// FUNCTION: fn_02016
void AffineMatrix::RotateX(float radians) {
  AffineMatrix rotation;
  rotation.SetXRotation(radians);
  Rotate(this->values, rotation.values);
}

// OFFSET: 0x00553270
// FUNCTION: fn_02017
void AffineMatrix::RotateY(float radians) {
  AffineMatrix rotation;
  rotation.SetYRotation(radians);
  Rotate(this->values, rotation.values);
}

// OFFSET: 0x00553570
// FUNCTION: fn_02018
void AffineMatrix::RotateZ(float radians) {
  AffineMatrix rotation;
  rotation.SetZRotation(radians);
  Rotate(this->values, rotation.values);
}

// OFFSET: 0x005550C0
// FUNCTION: fn_02032
void AffineMatrix::SetTranslation(float x, float y, float z) {
  std::memcpy(this->values, g_identityMatrix, sizeof(g_identityMatrix));
  this->values[9] = x;
  this->values[10] = y;
  this->values[11] = z;
}

// OFFSET: 0x00554B40
// FUNCTION: fn_02028
void AffineMatrix::SetScale(float x, float y, float z) {
  std::memcpy(this->values, g_identityMatrix, sizeof(g_identityMatrix));
  this->values[0] = x;
  this->values[4] = y;
  this->values[8] = z;
}

// OFFSET: 0x005529C0
// FUNCTION: fn_02014
void AffineMatrix::Scale(float x, float y, float z) {
  AffineMatrix scale;
  scale.SetScale(x, y, z);
  Rotate(this->values, scale.values);
}

// OFFSET: 0x00552CA0
// FUNCTION: fn_02015
void AffineMatrix::Translate(const float* position) {
  AffineMatrix translation;
  translation.SetTranslation(position[0], position[1], position[2]);
  Rotate(this->values, translation.values);
}

// OFFSET: 0x005554B0
// FUNCTION: fn_02037
void AffineMatrix::Compose(const AffineMatrix& first, const AffineMatrix& second) {
  const float* a = first.values;
  const float* b = second.values;
  float* output = this->values;

  output[0] = (a[0] * b[0] + b[3] * a[1]) + b[6] * a[2];
  output[3] = (a[4] * b[3] + a[3] * b[0]) + a[5] * b[6];
  output[6] = (a[7] * b[3] + a[6] * b[0]) + a[8] * b[6];
  output[9] = ((a[10] * b[3] + a[9] * b[0]) + a[11] * b[6]) + b[9];

  output[1] = (b[4] * a[1] + b[1] * a[0]) + b[7] * a[2];
  output[4] = (a[4] * b[4] + a[3] * b[1]) + a[5] * b[7];
  output[7] = (a[7] * b[4] + a[6] * b[1]) + a[8] * b[7];
  output[10] = ((a[10] * b[4] + a[9] * b[1]) + a[11] * b[7]) + b[10];

  output[2] = (b[5] * a[1] + b[2] * a[0]) + a[2] * b[8];
  output[5] = (b[5] * a[4] + b[2] * a[3]) + a[5] * b[8];
  output[8] = (b[5] * a[7] + b[2] * a[6]) + a[8] * b[8];
  output[11] = ((b[5] * a[10] + b[2] * a[9]) + a[11] * b[8]) + b[11];
}

// OFFSET: 0x00554920
// FUNCTION: fn_02027
void AffineMatrix::Inverse(const AffineMatrix& source) {
  const float* a = source.values;
  float* output = this->values;
  const float minor = a[4] * a[8] - a[5] * a[7];
  const float inverse = 1.0f / ((minor * a[0] - (a[3] * a[8] - a[6] * a[5]) * a[1]) + (a[3] * a[7] - a[6] * a[4]) * a[2]);
  const float negative = -inverse;
  output[0] = minor * inverse;
  const float firstY = (a[1] * a[8] - a[7] * a[2]) * negative;
  output[1] = firstY;
  const float firstZ = (a[1] * a[5] - a[2] * a[4]) * inverse;
  output[2] = firstZ;
  const float secondX = (a[3] * a[8] - a[6] * a[5]) * negative;
  output[3] = secondX;
  const float secondY = (a[0] * a[8] - a[2] * a[6]) * inverse;
  output[4] = secondY;
  const float secondZ = (a[0] * a[5] - a[2] * a[3]) * negative;
  output[5] = secondZ;
  const float thirdX = (a[7] * a[3] - a[6] * a[4]) * inverse;
  output[6] = thirdX;
  const float thirdY = (a[7] * a[0] - a[1] * a[6]) * negative;
  output[7] = thirdY;
  const float thirdZ = (a[4] * a[0] - a[1] * a[3]) * inverse;
  output[8] = thirdZ;

  output[9] = -((secondX * a[10] + (minor * inverse) * a[9]) + thirdX * a[11]);
  output[10] = -((firstY * a[9] + secondY * a[10]) + thirdY * a[11]);
  output[11] = -((secondZ * a[10] + firstZ * a[9]) + thirdZ * a[11]);
}

// FUNCTION: fn_02026
int AffineMatrix::IsIdentity() const {
  const auto absolute = [](float value) {
    return std::bit_cast<float>(std::bit_cast<unsigned>(value) & 0x7FFFFFFFu);
  };
  const float tolerance = 1.0e-5f;
  if (absolute(this->values[0] - 1.0f) > tolerance || absolute(this->values[4] - 1.0f) > tolerance ||
      absolute(this->values[8] - 1.0f) > tolerance || absolute(this->values[1]) > tolerance ||
      absolute(this->values[2]) > tolerance || absolute(this->values[3]) > tolerance ||
      absolute(this->values[5]) > tolerance || absolute(this->values[6]) > tolerance ||
      absolute(this->values[7]) > tolerance || absolute(this->values[9]) > tolerance ||
      absolute(this->values[10]) > tolerance || absolute(this->values[11]) > tolerance) {
    return 0;
  }
  return 1;
}

// FUNCTION: fn_02031
const float* AffineMatrix::SetAxisRotation(const float* axis, float radians) {
  const float halfAngle = radians * 0.5f;
  const float sine = ClientSin(halfAngle);
  const float x = sine * axis[0];
  const float y = sine * axis[1];
  const float z = sine * axis[2];
  const float cosine = ClientCos(halfAngle);
  this->values[9] = 0.0f;
  this->values[10] = 0.0f;
  this->values[11] = 0.0f;
  const float twiceX = x + x;
  const float twiceY = y + y;
  const float twiceZ = z + z;
  const float wz = cosine * twiceZ;
  const float wx = cosine * twiceX;
  const float wy = cosine * twiceY;
  const float xx = twiceX * x;
  const float xy = twiceY * x;
  const float yy = twiceY * y;
  const float xz = twiceZ * x;
  const float zz = twiceZ * z;
  const float yz = twiceZ * y;
  this->values[0] = 1.0f - (zz + yy);
  this->values[1] = xy + wz;
  this->values[3] = xy - wz;
  this->values[2] = xz - wy;
  this->values[6] = xz + wy;
  this->values[4] = 1.0f - (zz + xx);
  this->values[5] = yz + wx;
  this->values[7] = yz - wx;
  this->values[8] = 1.0f - (yy + xx);
  return axis;
}