/*
 *  Copyright (c) 2000-2022 Inria
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *  this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *  this list of conditions and the following disclaimer in the documentation
 *  and/or other materials provided with the distribution.
 *  * Neither the name of the ALICE Project-Team nor the names of its
 *  contributors may be used to endorse or promote products derived from this
 *  software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Contact: Bruno Levy
 *
 *     https://www.inria.fr/fr/bruno-levy
 *
 *     Inria,
 *     Domaine de Voluceau,
 *     78150 Le Chesnay - Rocquencourt
 *     FRANCE
 *
 */

#ifndef GEOGRAM_BASIC_QUATERNION
#define GEOGRAM_BASIC_QUATERNION

#include <geogram/basic/common.h>
#include <geogram/basic/geometry.h>
#include <iostream>


/**
 * \file geogram/basic/quaternion.h
 * \brief a class that represents quaternions (eases
 *   manipulation of 3D rotations)
 */

namespace GEO {

    /**
     * \brief Quaternions are useful for representing rotations.
     * \details This class is inspired by an implementation written
     * by Paul Rademacher, in his glui library.
     */
    class GEOGRAM_API Quaternion {
    public:

    /**
     * \brief Constructs a new Quaternion.
     */
    Quaternion() : v_(0.0,0.0,0.0), s_(1.0) {
    }

    /**
     * \brief Copy-constructs a new Quaternion.
     * \param[in] rhs the Quaternion to be copied.
     */
    Quaternion(const Quaternion& rhs) : v_(rhs.v_), s_(rhs.s_) {
    }

    /**
     * \brief Constructs a new quaternion from its coefficients
     * \param[in] x a coefficient of the quaternion
     * \param[in] y a coefficient of the quaternion
     * \param[in] z a coefficient of the quaternion
     * \param[in] w a coefficient of the quaternion
     */
    Quaternion(
        double x, double y, double z, double w
    ) : v_(x,y,z), s_(w) {
    }

    /**
     * \brief Constructs a new Quaternion from a vector and
     *  a scalar.
     * \param[in] v a const reference to the vector
     * \param[in] s the scalalr
     */
    Quaternion( const vec3& v, double s ) : v_(v), s_(s) {
    }

    /**
     * \brief Copies a Quaternion
     * \param[in] q the Quaternion to be copied
     * \return a reference to this Quaternion
     */
    Quaternion& operator = ( const Quaternion &q ) {
        v_ = q.v_ ;
        s_ = q.s_ ;
        return *this ;
    }

    /**
     * \brief Sets the coefficients of this quaterion
     * \param[in] v a const reference to the vector components
     * \param[in] s the scalar component
     */
    void set( const vec3& v, double s ) {
        v_ = v;
        s_ = s;
    }

    /**
     * \brief Displays this Quaternion
     * \param[in] out a reference to the std::ostream
     *  where this Quaternion should be displayed
     */
    void print( std::ostream& out ) const {
        out << v_.x << " " << v_.y << " " << v_.z << " " << s_ ;
    }


    /**
     * \brief Converts this Quaternion into a matrix
     * \return a matrix (mat4) that represents this Quaternion
     */
    mat4 to_matrix() const;

    /**
     * \brief Sets the rotation angle.
     * \param[in] f the rotation angle
     */
    void  set_angle( double f ) {
        vec3 ax = axis();
        s_ = ::cos(f / 2.0);
        v_ = ax * ::sin(f / 2.0);
    }

    /**
     * \brief Scales the rotation angle.
     */
    void scale_angle( double f ) {
        set_angle( f * angle() );
    }

    /**
     * \brief Gets the rotation angle
     * \return the angle
     */
    double angle() const {
        return 2.0 * acos( s_ ) ;
    }

    /**
     * \brief Gets the axis.
     * \return The axis.
     */
    vec3 axis() const;

    /**
     * \brief Computes the interpolation between two quaternions
     * \param[in] from a const reference to the first quaternion
     * \param[in] to a const reference to the second quaternion
     * \param[in] t time, in [0.0,1.0]
     * \return a smooth interpolation between \p from and \p to
     *  parameterized by \p t
     */
    static Quaternion spherical_interpolation(
        const Quaternion& from, const Quaternion& to,
        double t
    );

    /**
     * \brief Gets the vector component
     * \return the vector component
     * \note the vector part is not the axis of rotation.
     *  The axis of rotation is obtained by calling axis().
     */
    const vec3& v() const {
        return v_;
    }

    /**
     * \brief Gets the scalar component
     * \return the scalar component
     * \note the scalar component is not the rotation angle.
     *  The rotation angle is obtained by calling angle().
     */
    double s() const {
        return s_;
    }

    private:
    vec3 v_ ;
    double s_ ;
    } ;


    /*************************************************************************/

    /**
     * \brief Writes a Quaternion to a stream
     * \param[in,out] out the stream
     * \param[in] q a const reference to the quaternion
     * \return a reference to the stream
     */
    inline std::ostream& operator<<(std::ostream& out, const Quaternion& q) {
        q.print(out) ;
        return out ;
    }


    /**
     * \brief Reads a Quaternion from a stream
     * \param[in,out] in the stream
     * \param[out] q a reference to the quaternion
     * \return a reference to the stream
     */
    inline std::istream& operator>>(std::istream& in, Quaternion& q) {
        double x=0.0,y=0.0,z=0.0,w=0.0 ;
        in >> x >> y >> z >> w ;
        q.set(vec3(x,y,z),w) ;
        return in ;
    }


    /**
     * \brief Computes the sum of two Quaternion
     * \param[in] a a const reference to the first Quaternion
     * \param[in] b a const reference to the second Quaternion
     * \return the sum of \p a and \p b
     */
    inline Quaternion operator + (const Quaternion& a, const Quaternion& b) {
        return Quaternion(
            a.v() + b.v(),
            a.s() + b.s()
        ) ;
    }

    /**
     * \brief Computes the difference between two Quaternion
     * \param[in] a a const reference to the first Quaternion
     * \param[in] b a const reference to the second Quaternion
     * \return the difference between \p a and \p b
     */
    inline Quaternion operator - (const Quaternion& a, const Quaternion& b) {
        return Quaternion(
            a.v() - b.v(),
            a.s() - b.s()
        ) ;
    }

    /**
     * \brief Computes the opposite of a Quaternion
     * \param[in] a a const reference to the Quaternion
     * \return the opposite of \p a
     */
    inline Quaternion operator - (const Quaternion& a ) {
        return Quaternion( -1.0 * a.v(), -a.s() );
    }

    /**
     * \brief Computes the product of two Quaternion
     * \param[in] a a const reference to the first Quaternion
     * \param[in] b a const reference to the second Quaternion
     * \return the product of \p a and \p b
     */
    inline Quaternion operator * ( const Quaternion& a, const Quaternion& b) {
        return Quaternion(
            a.s() * b.v() + b.s() * a.v() + cross(a.v(),b.v()),
            a.s() * b.s() - dot(a.v() , b.v())
        );
    }

    /**
     * \brief Computes the product of a Quaternion and a scalar
     * \param[in] a a const reference to the Quaternion
     * \param[in] t the scalar
     * \return the product of \p a and \p t
     */
    inline Quaternion operator * ( const Quaternion& a, double t ) {
        return Quaternion( t * a.v(), a.s() * t );
    }


    /**
     * \brief Computes the product of a scalar and a Quaternion
     * \param[in] t the scalar
     * \param[in] a a const reference to the second Quaternion
     * \return the product of \p t and \p a
     */
    inline Quaternion operator * ( double t, const Quaternion& a ) {
        return Quaternion( t * a.v(), a.s() * t );
    }

}

#endif
