/* ================================================================
Quat
````````````````
This file defines the Quaternion class
================================================================ */
#include "Quat.hpp"
#include <math.h>
#define PIOVER180 0.01745329252
Quat::Quat() {
  x = y = z = w = 0.0f;
};
Quat::Quat(float x_, float y_, float z_, float w_) {
  x = x_;
  y = y_;
  z = z_;
  w = w_;
}
// Tait-bryan angles to Quat constructor
Quat::Quat(float pitch, float yaw, float roll) {
  float cp = (float)cos(pitch * PIOVER180 / 2.0f);
  float cy = (float)cos(-yaw * PIOVER180 / 2.0f);
  float cr = (float)cos(-roll * PIOVER180 /  2.0f);
  float cycr = cy * cr;
  float sp = (float)sin(pitch * PIOVER180 /  2.0f);
  float sy = (float)sin(-yaw * PIOVER180 / 2.0f);
  float sr = (float)sin(-roll * PIOVER180 /  2.0f);
  float srsy = sr * sy;

  x = cycr *  sp + srsy * cp;
  y = cr * sy * cp - sr * cy * sp;
  z = sr * cy * cp + cr * sy * sp;
  w = cycr * cp - srsy * sp;
  normalize();
}
Quat::Quat(const Vec3 &v, float angle) {
  angle *= 0.5f;
  Vec3 vn(v);
  vn.normalize();

  float sin_angle = sin(angle);

  x = vn.x * sin_angle;
  y = vn.y * sin_angle;
  z = vn.z * sin_angle;
  w = cos(angle);
}
Quat Quat::operator*(const Quat &quat_) const {
  Quat new_Quat;
  new_Quat.x = w*quat_.x + x*quat_.w + y*quat_.z - z*quat_.y;
  new_Quat.y = w*quat_.y + y*quat_.w + z*quat_.x - x*quat_.z;
  new_Quat.z = w*quat_.z + z*quat_.w + x*quat_.y - y*quat_.x;
  new_Quat.w = w*quat_.w - x*quat_.x - y*quat_.y - z*quat_.z;

  return new_Quat;
}
Quat::~Quat() {}
// rotate Vec3 by Quat
Vec3 Quat::operator*(const Vec3 &vec) const {
  Vec3 newvec(vec);
  newvec.normalize();

  Quat vec_quat, res_quat;
  vec_quat.x = newvec.x;
  vec_quat.y = newvec.y;
  vec_quat.z = newvec.z;
  vec_quat.w = 0.0f;

  res_quat = vec_quat * getConjugate();
  res_quat = *this * res_quat;
  return (Vec3(res_quat.x, res_quat.y, res_quat.z));
}
void Quat::operator*=(const Quat &quat_) {
  w = w*quat_.w - x*quat_.x - y*quat_.y - z*quat_.z;
  x = w*quat_.x + x*quat_.w + y*quat_.z - z*quat_.y;
  y = w*quat_.y - x*quat_.z + y*quat_.w + z*quat_.x;
  z = w*quat_.z + x*quat_.y - y*quat_.x + z*quat_.w;
}

void Quat::normalize() {
  float magnitude = sqrt(w*w + x*x + y*y + z*z);
  x /= magnitude;
  y /= magnitude;
  z /= magnitude;
  w /= magnitude;
}

Quat Quat::getConjugate() const {
  return Quat(-x, -y, -z, w);
}
float Quat::getPitch() {
  return asin(2.0f * (w * x - z * y));
}
float Quat::getYaw() {
  return atan2(2.0f * (w * y + x * z), 1.0f - 2.0f * (x * x + y * y));
}
float Quat::getRoll() {
  return atan2(2.0f * (w * z + y * x), 1.0f - 2.0f * (x * x + z * z));
}
Mat4 Quat::toMat4() const {
  float x2 = x * x;
  float y2 = y * y;
  float z2 = z * z;
  float xy = x * y;
  float xz = x * z;
  float yz = y * z;
  float wx = w * x;
  float wy = w * y;
  float wz = w * z;

  return Mat4(
    Vec4(1.0f - 2.0f * (y2 + z2), 2.0f * (xy - wz), 2.0f * (xz + wy), 0.0f),
    Vec4(2.0f * (xy + wz), 1.0f - 2.0f * (x2 + z2), 2.0f * (yz - wx), 0.0f),
    Vec4(2.0f * (xz - wy), 2.0f * (yz + wx), 1.0f - 2.0f * (x2 + y2), 0.0f),
    Vec4(0.0f, 0.0f, 0.0f, 1.0f)
  );
}
float *Quat::toMatrix(float matrix[]) const {
  float x2 = x * x;
  float y2 = y * y;
  float z2 = z * z;
  float xy = x * y;
  float xz = x * z;
  float yz = y * z;
  float wx = w * x;
  float wy = w * y;
  float wz = w * z;

  matrix[0] = 1.0f - 2.0f * (y2 + z2); matrix[1] = 2.0f * (xy - wz); matrix[2] = 2.0f * (xz + wy); matrix[3] = 0.0f;
  matrix[4] = 2.0f * (xy + wz); matrix[5] = 1.0f - 2.0f * (x2 + z2); matrix[6] = 2.0f * (yz - wx); matrix[7] = 0.0f;
  matrix[8] = 2.0f * (xz - wy); matrix[9] = 2.0f * (yz + wx); matrix[10] = 1.0f - 2.0f * (x2 + y2); matrix[11] = 0.0f;
  matrix[12] = 0.0f; matrix[13] = 0.0f; matrix[14] = 0.0f; matrix[15] = 1.0f;
  return matrix;
}