// ---------------------------------------------------------------------------
// - Quaternion.cpp                                                          -
// - afnix:mth module - quaternion class implementation                      -
// ---------------------------------------------------------------------------
// - This program is free software;  you can redistribute it  and/or  modify -
// - it provided that this copyright notice is kept intact.                  -
// -                                                                         -
// - This program  is  distributed in  the hope  that it will be useful, but -
// - without  any  warranty;  without  even   the   implied    warranty   of -
// - merchantability or fitness for a particular purpose.  In no event shall -
// - the copyright holder be liable for any  direct, indirect, incidental or -
// - special damages arising in any way out of the use of this software.     -
// ---------------------------------------------------------------------------
// - copyright (c) 1999-2023 amaury darsch                                   -
// ---------------------------------------------------------------------------

#include "Real.hpp"
#include "Math.hpp"
#include "Mthsid.hxx"
#include "Vector.hpp"
#include "Boolean.hpp"
#include "QuarkZone.hpp"
#include "Exception.hpp"
#include "Quaternion.hpp"

namespace afnix {
  
  // -------------------------------------------------------------------------
  // - public section                                                       -
  // -------------------------------------------------------------------------

  // negate a quaternion

  Quaternion operator - (const Quaternion& q) {
    q.rdlock ();
    try {
      Quaternion result = {-q.d_w, -q.d_x, -q.d_y, -q.d_z};
      q.unlock ();
      return result;
    } catch (...) {
      q.unlock ();
      throw;
    }
  }
  
  // add two quaternions

  Quaternion operator + (const Quaternion& qa, const Quaternion& qb) {
    qa.rdlock ();
    qb.rdlock ();
    try {
      Quaternion result = {
	qa.d_w + qb.d_w, qa.d_x + qb.d_x, qa.d_y + qb.d_y, qa.d_z + qb.d_z
      };
      qa.unlock ();
      qb.unlock ();
      return result;
    } catch (...) {
      qa.unlock ();
      qb.unlock ();
      throw;
    }
  }

  // substract two quaternions

  Quaternion operator - (const Quaternion& qa, const Quaternion& qb) {
    qa.rdlock ();
    qb.rdlock ();
    try {
      Quaternion result = {
	qa.d_w - qb.d_w, qa.d_x - qb.d_x, qa.d_y - qb.d_y, qa.d_z - qb.d_z
      };
      qa.unlock ();
      qb.unlock ();
      return result;
    } catch (...) {
      qa.unlock ();
      qb.unlock ();
      throw;
    }
  }

  // multiply two quaternions

  Quaternion operator * (const Quaternion& qa, const Quaternion& qb) {
    qa.rdlock ();
    qb.rdlock ();
    try {
      t_real aw = qa.d_w;
      t_real ax = qa.d_x;
      t_real ay = qa.d_y;
      t_real az = qa.d_z;
      t_real bw = qb.d_w;
      t_real bx = qb.d_x;
      t_real by = qb.d_y;
      t_real bz = qb.d_z;
      t_real rw = aw * bw - ax * bx - ay * by - az * bz;
      t_real rx = aw * bx + ax * bw + ay * bz - az * by;
      t_real ry = aw * by - ax * bz + ay * bw + az * bx;
      t_real rz = aw * bz + ax * by - ay * bx + az * bw;
      Quaternion result = {rw, rx, ry, rz};
      qa.unlock ();
      qb.unlock ();
      return result;
    } catch (...) {
      qa.unlock ();
      qb.unlock ();
      throw;
    }
  }
  
  // -------------------------------------------------------------------------
  // - class section                                                         -
  // -------------------------------------------------------------------------

  // create a default quaternion

  Quaternion::Quaternion (void) {
    reset ();
  }
  
  // create a quaternion by components

  Quaternion::Quaternion (const t_real w,
			  const t_real x, const t_real y, const t_real z) {
    d_w = w;
    d_x = x;
    d_y = y;
    d_z = z;
  }

  // copy construct this quaternion

  Quaternion::Quaternion (const Quaternion& that) {
    that.rdlock ();
    try {
      d_w = that.d_w;
      d_x = that.d_x;
      d_y = that.d_y;
      d_z = that.d_z;
      that.unlock ();
    } catch (...) {
      that.unlock ();
      throw;
    }  
  }

  // assign a quaternion to this one

  Quaternion& Quaternion::operator = (const Quaternion& that) {
    // check for self-assignation
    if (this == &that) return *this;
    // lock and assign
    wrlock ();
    that.rdlock ();
    try {
      d_w = that.d_w;
      d_x = that.d_x;
      d_y = that.d_y;
      d_z = that.d_z;
      unlock ();
      that.unlock ();
      return *this;
    } catch (...) {
      unlock ();
      that.unlock ();
      throw;
    }
  }
  
  // add a quaternion with another one

  Quaternion& Quaternion::operator += (const Quaternion& q) {
    wrlock ();
    try {
      d_w *= q.d_w;
      d_x += q.d_x;
      d_y *= q.d_y;
      d_z *= q.d_z;
      unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // multiply a quaternion by a scalar

  Quaternion& Quaternion::operator *= (const t_real s) {
    wrlock ();
    try {
      d_w *= s;
      d_x *= s;
      d_y *= s;
      d_z *= s;
      unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // divide a quaternion by a scalar

  Quaternion& Quaternion::operator /= (const t_real s) {
    wrlock ();
    try {
      if (s == 0.0) {
	throw Exception ("quaternion-error", "null scalar divide");
      }
      d_w /= s;
      d_x /= s;
      d_y /= s;
      d_z /= s;
      unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // return the class name

  String Quaternion::repr (void) const {
    return "Quaternion";
  }

  // return a clone of this object

  Object* Quaternion::clone (void) const {
    return new Quaternion (*this);
  }

  // return the serial did

  t_word Quaternion::getdid (void) const {
    return SRL_DEOD_MTH;
  }

  // return the serial sid

  t_word Quaternion::getsid (void) const {
    return SRL_QTRN_SID;
  }
 
  // serialize this object
  
  void Quaternion::wrstream (OutputStream& os) const {
    rdlock ();
    try {
      // write the object data
      Serial::wrreal (d_w, os);
      Serial::wrreal (d_x, os);
      Serial::wrreal (d_y, os);
      Serial::wrreal (d_z, os);
      unlock ();
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // deserialize this object

  void Quaternion::rdstream (InputStream& is) {
    wrlock ();
    try {
      d_w = Serial::rdreal (is);
      d_x = Serial::rdreal (is);
      d_y = Serial::rdreal (is);
      d_z = Serial::rdreal (is);
      unlock ();
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // reset this quaternion

  void Quaternion::reset (void) {
    wrlock ();
    try {
      d_w = 1.0;
      d_x = 0.0;
      d_y = 0.0;
      d_z = 0.0;
      unlock ();
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // set a quaternion by vector

  Quaternion& Quaternion::set (const Rvector3& v) {
    wrlock ();
    try {
      d_w = 0.0;
      d_x = v.getx ();
      d_y = v.gety ();
      d_z = v.getz ();
      unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // set the quaternion by angle and vector

  Quaternion& Quaternion::set (const t_real a, const t_real v[3]) {
    wrlock ();
    try {
      // get the imaginary norm
      t_real x = v[0];
      t_real y = v[1];
      t_real z = v[2];
      t_real n = Math::sqrt (x*x + y*y + z*z);
      // compute angle and set
      t_real ha = a / 2.0;
      t_real sa = Math::sin (ha);
      t_real ca = Math::cos (ha);
      d_w = ca;
      d_x = (n == 0.0) ? 0.0 : x * sa / n;
      d_y = (n == 0.0) ? 0.0 : y * sa / n;
      d_z = (n == 0.0) ? 0.0 : z * sa / n;
      unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // set the quaternion by angle and vector

  Quaternion& Quaternion::set (const t_real a, const Rvector3& v) {
    wrlock ();
    try {
      t_real rv[3] = {v.getx(), v.gety(), v.getz()};
      Quaternion& result = set (a, rv);
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // set the quaternion at once

  Quaternion& Quaternion::set (const t_real w,
			       const t_real x, const t_real y, const t_real z) {
    wrlock ();
    try {
      d_w = w;
      d_x = x;
      d_y = y;
      d_z = z;
      unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // check for a x-axis rotation

  bool Quaternion::isrx (void) const {
    rdlock ();
    try {
      bool result = (d_x != 0.0) && (d_y == 0.0) && (d_z == 0.0);
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }	
  }

  // check for a y-axis rotation

  bool Quaternion::isry (void) const {
    rdlock ();
    try {
      bool result = (d_x == 0.0) && (d_y != 0.0) && (d_z == 0.0);
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }	
  }

  // check for a z-axis rotation

  bool Quaternion::isrz (void) const {
    rdlock ();
    try {
      bool result = (d_x == 0.0) && (d_y == 0.0) && (d_z != 0.0);
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }	
  }
  
  // set the rotation x angle

  Quaternion& Quaternion::setrx (const t_real a) {
    wrlock ();
    try {
      t_real v[3] = {1.0, 0.0, 0.0};
      set (a, v);
      unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // set the rotation y angle

  Quaternion& Quaternion::setry (const t_real a) {
    wrlock ();
    try {
      t_real v[3] = {0.0, 1.0, 0.0};
      set (a, v);
      unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // set the rotation z angle
  
  Quaternion& Quaternion::setrz (const t_real a) {
    wrlock ();
    try {
      t_real v[3] = {0.0, 0.0, 1.0};
      set (a, v);
      unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // get the quaternion real part

  t_real Quaternion::getrval (void) const {
    rdlock ();
    try {
      t_real result = d_w;
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // get the quaternion imaginary part

  Rvector3 Quaternion::getival (void) const {
    rdlock ();
    try {
      Rvector3 result = {d_x, d_y, d_z};
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // compute the quaternion module

  t_real Quaternion::tomod (void) const {
    rdlock ();
    try {
      t_real result =
	Math::sqrt (d_w * d_w + d_x * d_x + d_y * d_y + d_z * d_z);
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // get the quaternion argument

  t_real Quaternion::toarg (void) const {
    rdlock ();
    try {
      t_real   rval = getrval ();
      Rvector3 ival = getival ();
      t_real result = 2.0 * Math::atan (ival.norm (), rval);
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // get the quaternion direction

  Rvector3 Quaternion::todir (void) const {
    rdlock ();
    try {
      Rvector3 result = {d_x, d_y, d_z}; result.normalize ();
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // get the quaternion rotation matrix

  Rmatrix3 Quaternion::torot (void) const {
    rdlock ();
    try {
      // compute inverse square norm
      t_real s = 1.0 / (d_w*d_w + d_x*d_x + d_y*d_y + d_z*d_z);
      // collect matrix elements
      t_real m00 = 1 - 2.0*s*(d_y*d_y + d_z*d_z);
      t_real m01 = 2.0*s*(d_x*d_y - d_z*d_w);
      t_real m02 = 2.0*s*(d_x*d_z + d_y*d_w);
      t_real m10 = 2.0*s*(d_x*d_y + d_z*d_w);
      t_real m11 = 1.0 - 2.0*s*(d_x*d_x + d_z*d_z);
      t_real m12 = 2.0*s*(d_y*d_z - d_x*d_w);
      t_real m20 = 2.0*s*(d_x*d_z - d_y*d_w);
      t_real m21 = 2.0*s*(d_y*d_z + d_x*d_w);
      t_real m22 = 1.0 - 2.0*s*(d_x*d_x + d_y*d_y);
      // create the result matrix
      Rmatrix3 result;
      result.set (0, 0, m00); result.set (0, 1, m01); result.set (0, 2, m02);
      result.set (1, 0, m10); result.set (1, 1, m11); result.set (1, 2, m12);
      result.set (2, 0, m20); result.set (2, 1, m21); result.set (2, 2, m22);
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // normalize this quaternion

  Quaternion& Quaternion::normalize (void) {
    wrlock ();
    try {
      *this /= tomod ();
      unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // conjugate this quaternion

  Quaternion& Quaternion::conjugate (void) {
    wrlock ();
    try {
      d_x = -d_x;
      d_y = -d_y;
      d_z = -d_z;
      unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // rotate a vector with this quaternion

  Rvector3 Quaternion::rotate (const Rvector3& v) const {
    rdlock ();
    try {
      // collect quaternion oeprators
      Quaternion qn = *this; qn.normalize ();
      Quaternion qi = qn;    qi.conjugate ();
      Quaternion qv;         qv.set (v);
      // compute rotated quaternion
      Quaternion qr = qn * (qv * qi);
      // extract result
      Rvector3 result = qr.getival ();
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // -------------------------------------------------------------------------
  // - object section                                                        -
  // -------------------------------------------------------------------------

  // the quark zone
  static const long QUARK_ZONE_LENGTH = 17;
  static QuarkZone  zone (QUARK_ZONE_LENGTH);

  // the object supported quarks
  static const long QUARK_SET     = zone.intern ("set");
  static const long QUARK_TOMOD   = zone.intern ("module");
  static const long QUARK_TOARG   = zone.intern ("argument");
  static const long QUARK_TODIR   = zone.intern ("direction");
  static const long QUARK_TOROT   = zone.intern ("to-rotation");
  static const long QUARK_RESET   = zone.intern ("reset");
  static const long QUARK_CONJT   = zone.intern ("conjugate");
  static const long QUARK_NORMZ   = zone.intern ("normalize");
  static const long QUARK_ISRXP   = zone.intern ("rotation-x-p");
  static const long QUARK_ISRYP   = zone.intern ("rotation-y-p");
  static const long QUARK_ISRZP   = zone.intern ("rotation-z-p");
  static const long QUARK_SETRX   = zone.intern ("set-rotation-x");
  static const long QUARK_SETRY   = zone.intern ("set-rotation-y");
  static const long QUARK_SETRZ   = zone.intern ("set-rotation-z");
  static const long QUARK_ROTATE  = zone.intern ("rotate");
  static const long QUARK_GETRVAL = zone.intern ("get-real-part");
  static const long QUARK_GETIVAL = zone.intern ("get-imaginary-part");

  // create a new object in a generic way

  Object* Quaternion::mknew (Vector* argv) {
    long argc = (argv == nullptr) ? 0 : argv->length ();
    // check for 0 argument
    if (argc == 0) return new Quaternion;
    // check for 4 arguments
    if (argc == 4) {
      t_real w = argv->getrint (0);
      t_real x = argv->getrint (1);
      t_real y = argv->getrint (2);
      t_real z = argv->getrint (3);
      return new Quaternion (w, x, y, z);
    }
    throw Exception ("argument-error", 
                     "invalid arguments with with quaternion"); 
  }

  // return true if the given quark is defined
  
  bool Quaternion::isquark (const long quark, const bool hflg) const {
    rdlock ();
    try {
      if (zone.exists (quark) == true) {
	unlock ();
	return true;
      }
      bool result = hflg ? Object::isquark (quark, hflg) : false;
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // apply this object with a set of arguments and a quark

  Object* Quaternion::apply (Evaluable* zobj, Nameset* nset, const long quark,
			     Vector* argv) {
    // get the number of arguments
    long argc = (argv == nullptr) ? 0 : argv->length ();
    
    // dispatch 0 argument
    if (argc == 0) {
      if (quark == QUARK_TOMOD)   return new Real (tomod ());
      if (quark == QUARK_TOARG)   return new Real (toarg ());
      if (quark == QUARK_TODIR)   return new Rvector3 (todir ());
      if (quark == QUARK_TOROT)   return new Rmatrix3 (torot ());
      if (quark == QUARK_ISRXP)   return new Boolean (isrx ());
      if (quark == QUARK_ISRYP)   return new Boolean (isry ());
      if (quark == QUARK_ISRZP)   return new Boolean (isrz ());
      if (quark == QUARK_GETRVAL) return new Real (getrval ());
      if (quark == QUARK_GETIVAL) return new Rvector3 (getival ());
      if (quark == QUARK_RESET) {
	reset ();
	return nullptr;
      }
      if (quark == QUARK_CONJT) {
	conjugate ();
	return nullptr;
      }
      if (quark == QUARK_NORMZ) {
	normalize ();
	return nullptr;
      }
    }
    // dispatch 1 argument
    if (argc == 1) {
      if (quark == QUARK_ROTATE) {
	Object* obj = argv->get (0);
	auto v = dynamic_cast<Rvector3*>(obj);
	if (v == nullptr) {
	  throw Exception ("type-error", "invalid object to rotate",
			   Object::repr (obj));
	}
	return new Rvector3 (rotate (*v));
      }
      if (quark == QUARK_SET) {
	Object* obj = argv->get (0);
	auto v = dynamic_cast<Rvector3*>(obj);
	if (v == nullptr) {
	  throw Exception ("type-error", "invalid object to rotate",
			   Object::repr (obj));
	}
	set (*v);
	return this;
      }
      if (quark == QUARK_SETRX) {
	t_real a = argv->getreal (0);
	setrx (a);
	return nullptr;
      }
      if (quark == QUARK_SETRY) {
	t_real a = argv->getreal (0);
	setry (a);
	return nullptr;
      }
      if (quark == QUARK_SETRZ) {
	t_real a = argv->getreal (0);
	setrz (a);
	return nullptr;
      }
    }
    // dispatch 2 arguments
    if (argc == 2) {
      if (quark == QUARK_SET) {
	t_real    a = argv->getreal (0);
	Object* obj = argv->get (1);
	auto v = dynamic_cast<Rvector3*>(obj);
	if (v == nullptr) {
	  throw Exception ("type-error", "invalid object as vector",
			   Object::repr (obj));
	}
	set (a, *v);
	return this;
      }
    }
    // dispatch 4 arguments
    if (argc == 4) {
      if (quark == QUARK_SET) {
	t_real w = argv->getreal (0);
	t_real x = argv->getreal (1);
	t_real y = argv->getreal (2);
	t_real z = argv->getreal (3);
	set (w, x, y, z);
	return this;
      }
    }
    // call the object method
    return Object::apply (zobj, nset, quark, argv);
  }
}
