// ---------------------------------------------------------------------------
// - Quaternion.cpp                                                          -
// - afnix:geo service - 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-2019 amaury darsch                                   -
// ---------------------------------------------------------------------------

#include "Real.hpp"
#include "Math.hpp"
#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_x, q.d_y, -q.d_z, -q.d_w};
      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_x + qb.d_x, qa.d_y + qb.d_y, qa.d_z + qb.d_z, qa.d_w + qb.d_w
      };
      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_x - qb.d_x, qa.d_y - qb.d_y, qa.d_z - qb.d_z, qa.d_w - qb.d_w
      };
      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 ax = qa.d_x;
      t_real ay = qa.d_y;
      t_real az = qa.d_z;
      t_real aw = qa.d_w;
      t_real bx = qb.d_x;
      t_real by = qb.d_y;
      t_real bz = qb.d_z;
      t_real bw = qb.d_w;
      t_real rx = aw * bx + bw * ax + ay * bz - az * by;
      t_real ry = aw * by + bw * ay + az * bx - ax * bz;
      t_real rz = aw * bz + bw * az + ax * by - ay * bx;
      t_real rw = aw * bw - ax * bx - ay * by - az * bz;
      Quaternion result = {rx, ry, rz, rw};
      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 x, const t_real y, const t_real z,
			  const t_real w) {
    d_x = x;
    d_y = y;
    d_z = z;
    d_w = w;
  }

  // copy construct this quaternion

  Quaternion::Quaternion (const Quaternion& that) {
    that.rdlock ();
    try {
      d_x = that.d_x;
      d_y = that.d_y;
      d_z = that.d_z;
      d_w = that.d_w;
      unlock ();
    } catch (...) {
      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_x = that.d_x;
      d_y = that.d_y;
      d_z = that.d_z;
      d_w = that.d_w;
      unlock ();
      that.unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // add a quaternion with another one

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

  Quaternion& Quaternion::operator *= (const t_real s) {
    wrlock ();
    try {
      d_x *= s;
      d_y *= s;
      d_z *= s;
      d_w *= 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) {
	d_x /= s;
	d_y /= s;
	d_z /= s;
	d_w /= s;
      } else {
	reset ();
      }
      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);
  }

  // reset this quaternion

  void Quaternion::reset (void) {
    wrlock ();
    try {
      d_x = 0.0;
      d_y = 0.0;
      d_z = 0.0;
      d_w = 1.0;
      unlock ();
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // get the quaternion x component
  
  t_real Quaternion::getx (void) const {
    rdlock ();
    try {
      t_real result = d_x;
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // get the quaternion y component

  t_real Quaternion::gety (void) const {
    rdlock ();
    try {
      t_real result = d_y;
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // get the quaternion z component

  t_real Quaternion::getz (void) const {
    rdlock ();
    try {
      t_real result = d_z;
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // get the quaternion w component

  t_real Quaternion::getw (void) const {
    rdlock ();
    try {
      t_real result = d_w;
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // set the quaternion at once

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

  // set the quaternion by angle and vector

  Quaternion& Quaternion::set (const t_real a, const Rvi& v) {
    wrlock ();
    try {
      // check the vector size
      long size = v.getsize ();
      if ((size < 1) || (size > 4)) {
	throw Exception ("quaternion-error", "invalid vector direction in set");
      }
      // normalize a vector argument
      t_real x = v.get (0);
      t_real y = (size > 1) ? v.get (1) : 0.0;
      t_real z = (size > 2) ? v.get (2) : 0.0;
      Vector3 vn = {x, y, z}; vn.normalize ();
      // compute angle and set
      t_real ha = a / 2.0;
      t_real sa = Math::sin (ha);
      t_real ca = Math::cos (ha);
      d_x = vn.nlget (0) * sa;
      d_y = vn.nlget (1) * sa;
      d_z = vn.nlget (2) * sa;
      d_w = ca;
      unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // compute the quaternion norm

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

  // normalize this quaternion

  Quaternion& Quaternion::normalize (void) {
    wrlock ();
    try {
      *this /= norm ();
      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

  Vector3 Quaternion::rotate (const Rvi& v) const {
    rdlock ();
    try {
      // check the vector argument
      long size = v.getsize ();
      if ((size < 1 ) || (size > 4)) {
	throw Exception ("quaternion-error", "invalid vector to rotate");
      }
      t_real x = v.get (0);
      t_real y = (size > 1) ? v.get (1) : 0.0;
      t_real z = (size > 2) ? v.get (2) : 0.0;
      t_real w = 0.0;
      // get a normalize quaternion
      Quaternion qn = *this; qn.normalize ();
      // create a vector quaternion
      Quaternion qv = {x, y, z, w};
      // compute qn*qv*qn[-1]
      Quaternion qp = qn * qv;
      Quaternion qr = qp * qn.conjugate ();
      // create the result vector
      Vector3 result = {qr.d_x, qr.d_y, qr.d_z};
      unlock ();
      return result;
    } 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 {
      Vector3 v = {1.0, 0.0, 0.0};
      set (a * Math::M_PI / 180.0, v);
      unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // set the rotation y angle

  Quaternion& Quaternion::setry (const t_real a) {
    wrlock ();
    try {
      Vector3 v = {0.0, 1.0, 0.0};
      set (a * Math::M_PI / 180.0, v);
      unlock ();
      return *this;
    } catch (...) {
      unlock ();
      throw;
    }
  }

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

  // set the quaternion axis angle at once

  void Quaternion::setra (const t_real rx, const t_real ry, const t_real rz) {
    wrlock ();
    try {
      if (rx != 0.0) {
	if ((ry != 0.0) || (rz != 0.0)) {
	  throw Exception ("quaternion-error",
			   "unimplemented multiple angles setra");
	}
	setrx (rx);
      }
      if (ry != 0.0) {
	if ((rx != 0.0) || (rz != 0.0)) {
	  throw Exception ("quaternion-error",
			   "unimplemented multiple angles setra");
	}
	setry (ry);
      }
      if (rz != 0.0) {
	if ((rx != 0.0) || (ry != 0.0)) {
	  throw Exception ("quaternion-error",
			   "unimplemented multiple angles setra");
	}
	setrz (rz);
      }
      unlock ();
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // get the quaternion rotation angle

  t_real Quaternion::getra (void) const {
    rdlock ();
    try {
      t_real n = norm ();
      t_real a = (n == 0.0) ? 0.0 : 2.0 * Math::acos (d_w / n);
      // adjust to degres
      t_real result = a * 180.0 / Math::M_PI;
      unlock ();
      return result;
    } catch (...) {
      unlock ();
      throw;
    }
  }

  // get the axis rotation angle

  t_real Quaternion::getxa (void) const {
    rdlock ();
    try {
      // get the quaternion angle in radian
      t_real a = getra ();
      // check for negative axis
      if ((d_x < 0.0) || (d_y < 0.0) || (d_z < 0.0)) a = -a;
      unlock ();
      return a;
    } catch (...) {
      unlock ();
      throw;
    }
  }
  
  // -------------------------------------------------------------------------
  // - object section                                                        -
  // -------------------------------------------------------------------------

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

  // the object supported quarks
  static const long QUARK_SET   = zone.intern ("set");
  static const long QUARK_GETX  = zone.intern ("get-x");
  static const long QUARK_GETY  = zone.intern ("get-y");
  static const long QUARK_GETZ  = zone.intern ("get-z");
  static const long QUARK_GETW  = zone.intern ("get-w");
  static const long QUARK_NORM  = zone.intern ("norm");
  static const long QUARK_GETRA = zone.intern ("get-angle");
  static const long QUARK_GETXA = zone.intern ("get-axis-angle");
  static const long QUARK_RESET = zone.intern ("reset");
  static const long QUARK_ROTAT = zone.intern ("rotate");
  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_SETRA = zone.intern ("set-rotation-angle");

  // 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 x = argv->getrint (0);
      t_real y = argv->getrint (1);
      t_real z = argv->getrint (2);
      t_real w = argv->getrint (3);
      return new Quaternion (x, y, z, w);
    }
    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 (Runnable* robj, 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_GETX) return new Real (getx ());
      if (quark == QUARK_GETY) return new Real (gety ());
      if (quark == QUARK_GETZ) return new Real (getz ());
      if (quark == QUARK_GETW) return new Real (getw ());
      if (quark == QUARK_NORM) return new Real (norm ());
      if (quark == QUARK_GETRA) return new Real (getra ());
      if (quark == QUARK_GETXA) return new Real (getxa ());
      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_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_ROTAT) {
	Object* obj = argv->get (0);
	Rvi*   vobj = dynamic_cast <Rvi*> (obj);
	if (vobj == nullptr) {
	  throw Exception ("type-error", "invalid object with rotate",
			   Object::repr (vobj));
	}
	return new Vector3 (rotate (*vobj));
      }
      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);
	Rvi*   vobj = dynamic_cast <Rvi*> (obj);
	if (vobj == nullptr) {
	  throw Exception ("type-error", "invalid object with set",
			   Object::repr (vobj));
	}
	set (a, *vobj);
	return nullptr;
      }
    }
    // dispatch 3 arguments
    if (argc == 3) {
      if (quark == QUARK_SETRA) {
	t_real rx = argv->getreal (0);
	t_real ry = argv->getreal (1);
	t_real rz = argv->getreal (2);
	setra (rx, ry, rz);
	return nullptr;
      }
    }
    // dispatch 4 arguments
    if (argc == 4) {
      if (quark == QUARK_SET) {
	t_real x = argv->getreal (0);
	t_real y = argv->getreal (1);
	t_real z = argv->getreal (2);
	t_real w = argv->getreal (3);
	set (x, y, z, w);
	return nullptr;
      }
    }
    // call the object method
    return Object::apply (robj, nset, quark, argv);
  }
}
