#pragma once
#include "real.hpp"
#include "uvector.hpp"
#include "xarray.hpp"

namespace algoim
{

class Quaternion
{
public:
    real aw, ai, aj, ak;

    Quaternion(real aw_, real ai_, real aj_, real ak_) : aw(aw_), ai(ai_), aj(aj_), ak(ak_) {}

    Quaternion(real ai_, real aj_, real ak_) : aw(0), ai(ai_), aj(aj_), ak(ak_) {}

    Quaternion(const uvector3& axis, real angle)
    {
        uvector3 axisNorm  = axis / norm(axis);
        real     halfAngle = angle / 2;
        aw                 = cos(halfAngle);
        real sinHalfAngle  = sin(halfAngle);
        ai                 = axisNorm(0) * sinHalfAngle;
        aj                 = axisNorm(1) * sinHalfAngle;
        ak                 = axisNorm(2) * sinHalfAngle;
    }

    void getRotation(tensor2& rotation) const
    {
        assert(all(rotation.ext() == uvector2(3)));
        rotation.m(uvector2(0, 0)) = aw * aw + ai * ai - aj * aj - ak * ak;
        rotation.m(uvector2(0, 1)) = 2 * (ai * aj - aw * ak);
        rotation.m(uvector2(0, 2)) = 2 * (ai * ak + aw * aj);

        rotation.m(uvector2(1, 0)) = 2 * (ai * aj + aw * ak);
        rotation.m(uvector2(1, 1)) = aw * aw - ai * ai + aj * aj - ak * ak;
        rotation.m(uvector2(1, 2)) = 2 * (aj * ak - aw * ai);

        rotation.m(uvector2(2, 0)) = 2 * (ai * ak - aw * aj);
        rotation.m(uvector2(2, 1)) = 2 * (aj * ak + aw * ai);
        rotation.m(uvector2(2, 2)) = aw * aw - ai * ai - aj * aj + ak * ak;
    }
};
} // namespace algoim