// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2016/02/03
//  filename:   Quaternion.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Class defining a quaternion
//
//
/*********************************************************************/

#include "Quaternion.h"
#include "Euler.h"
#include "RotationMatrix.h"
#include <math.h>
#include <stdio.h>

namespace flair {
namespace core {

Quaternion::Quaternion(float inQ0, float inQ1, float inQ2, float inQ3)
    : q0(inQ0), q1(inQ1), q2(inQ2), q3(inQ3) {}

Quaternion::~Quaternion() {}

Quaternion &Quaternion::operator=(const Quaternion &quaternion) {
  q0 = quaternion.q0;
  q1 = quaternion.q1;
  q2 = quaternion.q2;
  q3 = quaternion.q3;
  return (*this);
}

float Quaternion::GetNorm(void) const {
  return sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
}

void Quaternion::Normalize(void) {
  float n = GetNorm();
  if (n != 0) {
    q0 = q0 / n;
    q1 = q1 / n;
    q2 = q2 / n;
    q3 = q3 / n;
  }
}

void Quaternion::Conjugate(void) {
  q1 = -q1;
  q2 = -q2;
  q3 = -q3;
}

Quaternion Quaternion::GetConjugate(void) {
  return Quaternion(q0, -q1, -q2, -q3);
}

void Quaternion::GetLogarithm(Vector3Df &logarithm) {
  Normalize();
  float v_norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3);

  if (v_norm != 0) {
    float v_arccos = acosf(q0);
    logarithm.x = (q1 * v_arccos) / v_norm;
    logarithm.y = (q2 * v_arccos) / v_norm;
    logarithm.z = (q3 * v_arccos) / v_norm;
  } else {
    logarithm.x = 0;
    logarithm.y = 0;
    logarithm.z = 0;
  }
}

Vector3Df Quaternion::GetLogarithm(void) {
  Vector3Df vector;
  GetLogarithm(vector);
  return vector;
}

Quaternion Quaternion::GetDerivative(const Vector3Df &angularSpeed) const {
  const Quaternion Qw(0, angularSpeed.x, angularSpeed.y, angularSpeed.z);
  return 0.5 * (*this) * Qw;
}

void Quaternion::Derivate(const Vector3Df &angularSpeed) {
  Quaternion Q = GetDerivative(angularSpeed);
  (*this) = Q;
}

void Quaternion::ToEuler(Euler &euler) const {
  euler.roll = atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
  euler.pitch = asin(2 * (q0 * q2 - q1 * q3));
  euler.yaw = atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
}

Euler Quaternion::ToEuler(void) const {
  Euler euler;
  ToEuler(euler);
  return euler;
}

void Quaternion::ToRotationMatrix(RotationMatrix &matrix) const {
  float aSq = q0 * q0;
  float bSq = q1 * q1;
  float cSq = q2 * q2;
  float dSq = q3 * q3;
  matrix.m[0][0] = aSq + bSq - cSq - dSq;
  matrix.m[0][1] = 2.0f * (q1 * q2 - q0 * q3);
  matrix.m[0][2] = 2.0f * (q0 * q2 + q1 * q3);
  matrix.m[1][0] = 2.0f * (q1 * q2 + q0 * q3);
  matrix.m[1][1] = aSq - bSq + cSq - dSq;
  matrix.m[1][2] = 2.0f * (q2 * q3 - q0 * q1);
  matrix.m[2][0] = 2.0f * (q1 * q3 - q0 * q2);
  matrix.m[2][1] = 2.0f * (q0 * q1 + q2 * q3);
  matrix.m[2][2] = aSq - bSq - cSq + dSq;
}

Quaternion &Quaternion::operator+=(const Quaternion &quaternion) {
  q0 += quaternion.q0;
  q1 += quaternion.q1;
  q2 += quaternion.q2;
  q3 += quaternion.q3;
  return (*this);
}

Quaternion &Quaternion::operator-=(const Quaternion &quaternion) {
  q0 -= quaternion.q0;
  q1 -= quaternion.q1;
  q2 -= quaternion.q2;
  q3 -= quaternion.q3;
  return (*this);
}

Quaternion operator+(const Quaternion &quaternionA,
                     const Quaternion &quaterniontB) {
  return Quaternion(
      quaternionA.q0 + quaterniontB.q0, quaternionA.q1 + quaterniontB.q1,
      quaternionA.q2 + quaterniontB.q2, quaternionA.q3 + quaterniontB.q3);
}

Quaternion operator-(const Quaternion &quaterniontA,
                     const Quaternion &quaterniontB) {
  return Quaternion(
      quaterniontA.q0 - quaterniontB.q0, quaterniontA.q1 - quaterniontB.q1,
      quaterniontA.q2 - quaterniontB.q2, quaterniontA.q3 - quaterniontB.q3);
}

Quaternion operator-(const Quaternion &quaternion) {
  return Quaternion(-quaternion.q0, -quaternion.q1, -quaternion.q2,
                    -quaternion.q3);
}

Quaternion operator*(const Quaternion &quaterniontA,
                     const Quaternion &quaterniontB) {
  return Quaternion(
      quaterniontA.q0 * quaterniontB.q0 - quaterniontA.q1 * quaterniontB.q1 -
          quaterniontA.q2 * quaterniontB.q2 - quaterniontA.q3 * quaterniontB.q3,
      quaterniontA.q0 * quaterniontB.q1 + quaterniontA.q1 * quaterniontB.q0 +
          quaterniontA.q2 * quaterniontB.q3 - quaterniontA.q3 * quaterniontB.q2,
      quaterniontA.q0 * quaterniontB.q2 - quaterniontA.q1 * quaterniontB.q3 +
          quaterniontA.q2 * quaterniontB.q0 + quaterniontA.q3 * quaterniontB.q1,
      quaterniontA.q0 * quaterniontB.q3 + quaterniontA.q1 * quaterniontB.q2 -
          quaterniontA.q2 * quaterniontB.q1 + quaterniontA.q3 * quaterniontB.q0);
}

Quaternion operator*(float coeff, const Quaternion &quaternion) {
  return Quaternion(coeff * quaternion.q0, coeff * quaternion.q1,
                    coeff * quaternion.q2, coeff * quaternion.q3);
}

Quaternion operator*(const Quaternion &quaternion, float coeff) {
  return Quaternion(coeff * quaternion.q0, coeff * quaternion.q1,
                    coeff * quaternion.q2, coeff * quaternion.q3);
}

} // end namespace core
} // end namespace flair