/*
 * Copyright (c) 1999-2019 Hartmut Seichter
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */
#ifndef PW_CORE_AXISANGLE_HPP
#define PW_CORE_AXISANGLE_HPP

#include <pw/core/vector.hpp>

namespace pw {

template <typename T>
struct axisangle_ {

    typedef T value_type;
    typedef vector3_<T> axis_type;

    axis_type axis;
    T angle;

    axisangle_()
        : axis(vector3_<T>::up()),
          angle(0)
    {}

    axisangle_(const vector3_<T> &axis,const T &angle)
        : axis(axis)
        , angle(angle)
    {
    }

	static const axisangle_ from_matrix(const matrix_<4,4,T>& m)
	{
		using std::acos;
		using std::sqrt;

		axisangle_ aa_res;
		aa_res.angle = acos((m(0,0) + m(1,1) + m(2,2) - 1) / T(2));
		const T m2112 = m(2,1) - m(1,2);
		const T m0220 = m(0,2) - m(2,0);
		const T m1001 = m(1,0) - m(0,1);
		// no singularity check here ...
		const T mrot_denom = sqrt( m2112 * m2112 + m0220 * m0220 + m1001 * m1001 );

		aa_res.axis.x() = m2112 / mrot_denom;
		aa_res.axis.y() = m0220 / mrot_denom;
		aa_res.axis.z() = m1001 / mrot_denom;

		return aa_res;
	}

    matrix_<4,4,T> to_matrix() const
    {
        using std::cos;
        using std::sin;

        // result
        matrix_<4,4,T> rot_mat; rot_mat.set_identity();
		axis_type axis_n = axis.normalized(); // always normalize

		const T cos_a = cos(angle);
		const T sin_a = sin(angle);
		const T cos_1_a = T(1) - cos_a;

		rot_mat(0,0) = cos_a + axis_n.x() * axis_n.x() * cos_1_a;
		rot_mat(1,1) = cos_a + axis_n.y() * axis_n.y() * cos_1_a;
		rot_mat(2,2) = cos_a + axis_n.z() * axis_n.z() * cos_1_a;

		T v1 = axis_n.x() * axis_n.y() * cos_1_a;
		T v2 = axis_n.z() * sin_a;
		rot_mat(1,0) = v1 + v2;
		rot_mat(0,1) = v1 - v2;

		v1 = axis_n.x() * axis_n.z() * cos_1_a;
		v2 = axis_n.y() * sin_a;
		rot_mat(2,0) = v1 - v2;
		rot_mat(0,2) = v1 + v2;

		v1 = axis_n.y() * axis_n.z() * cos_1_a;
		v2 = axis_n.x() * sin_a;
		rot_mat(2,1) = v1 + v2;
		rot_mat(1,2) = v1 - v2;

        return rot_mat;
    }
};


using axisangle = axisangle_<real_t> ;
using axisanglef = axisangle_<float> ;
using axisangled = axisangle_<double> ;

}





#endif
