classdef Quaternion
    
    properties
        rot = 1;
        x   = 0;
        y   = 0;
        z   = 0;
    end
    
    methods
        function obj = Quaternion(rot, x, y, z)
            if (nargin > 0)
                obj.rot = rot;
                obj.x   = x;
                obj.y   = y;
                obj.z   = z;
            end
        end
        
        function obj = normalize(q)
            obj = Quaternion(1,0,0,0);
            res = q.rot*q.rot+q.x*q.x+q.y*q.y+q.z*q.z;
            if((res > 1.001) || (res < 0.999))
                mag = sqrt(res);
                if (mag ~= 0)
                    obj.rot = q.rot/mag;
                    obj.x   = q.x/mag;
                    obj.y   = q.y/mag;
                    obj.z   = q.z/mag;
                end
            else
                 obj = q;
            end
        end
    end  
    
    methods(Static = true)
        function [angles, qCurrent] = updateAngles(radX, radY, radZ, qCurrent)
            qDelta = Quaternion.qFromEuler(radX, radY, radZ);
            qDelta = qDelta.normalize();
            
            qCurrent = Quaternion.multiQuat(qCurrent, qDelta);
            qCurrent = qCurrent.normalize();
            angles = Quaternion.eulerFromQ(qCurrent);
        end
        
        function q = multiQuat(q1, q2)
            q = Quaternion(1,0,0,0);
            q.rot = (q1.rot*q2.rot - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z);
            q.x   = (q1.rot*q2.x + q1.x*q2.rot + q1.y*q2.z - q1.z*q2.y);
            q.y   = (q1.rot*q2.y - q1.x*q2.z + q1.y*q2.rot + q1.z*q2.x);
            q.z   = (q1.rot*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.rot);
        end
        
        function q = qFromEuler(radX, radY, radZ)
            % angles are in radians.
            q = Quaternion(1,0,0,0);
            c1 = cos(radZ/2);
            s1 = sin(radZ/2);
            c2 = cos(radY/2);
            s2 = sin(radY/2);
            c3 = cos(radX/2);
            s3 = sin(radX/2);
            
            q.rot = c1*c2*c3 - s1*s2*s3;
            q.x   = c1*c2*s3 + s1*s2*c3;
            q.y   = s1*c2*c3 + c1*s2*s3;
            q.z   = c1*s2*c3 - s1*c2*s3;
        end
        
        function angles = eulerFromQ(q1)
            angles = zeros(1,3);
            test = q1.x*q1.y + q1.z*q1.rot;
            if (test > 0.499)  % singularity at north pole
                angles(1) = 2 * atan2(q1.x,q1.rot);
                angles(2) = pi/2;
                angles(3) = 0;
                return;
            end
            if (test < -0.499) % singularity at south pole
                angles(1) = -2 * atan2(q1.x,q1.rot);
                angles(2) = - pi/2;
                angles(3) = 0;
                return;
            end
            sqx = q1.x*q1.x;
            sqy = q1.y*q1.y;
            sqz = q1.z*q1.z;
            angles(1) = atan2(2*q1.y*q1.rot-2*q1.x*q1.z , 1 - 2*sqy - 2*sqz); %heading
            angles(2) = asin(2*test); % pitch
            angles(3) = atan2(2*q1.x*q1.rot-2*q1.y*q1.z , 1 - 2*sqx - 2*sqz); %roll
            angles = angles.*180/pi;
        end
        
        function DCM = dcmFromQ(q)
            sqRot = q.rot*q.rot;
            sqX   = q.x*q.x;
            sqY   = q.y*q.y;
            sqZ   = q.z*q.z;
            DCM = [ sqRot-sqX-sqY+sqZ     2*(q.rot*q.x+q.y*q.z)   2*(q.rot*q.y-q.x*q.z);
                    2*(q.rot*q.x-q.y*q.z) -sqRot+sqX-sqY+sqZ      2*(q.x*q.y+q.rot*q.z);
                    2*(q.rot*q.y+q.x*q.z) 2*(q.x*q.y+q.rot*q.z)   -sqRot-sqX+sqY+sqZ];
                
%             DCM = [ -1+2*sqRot+2*sqZ     2*(q.rot*q.x-q.y*q.z)   2*(q.rot*q.y+q.x*q.z);
%                     2*(q.rot*q.x+q.y*q.z) -1+2*sqX+2*sqZ      2*(q.x*q.y-q.rot*q.z);
%                     2*(q.rot*q.y-q.x*q.z) 2*(q.rot*q.z+q.x*q.y)   -1+2*sqY+2*sqZ];     
        end
        
        function qnb = qnbFromQ(q)
            rotHalf = q.rot*0.5;
            qnb =  Quaternion( cos(rotHalf), q.x/q.rot*sin(rotHalf), q.y/q.rot*sin(rotHalf), q.z/q.rot*sin(rotHalf));
        end
        
        function qNew = matrixQ(m, q)
            qNew = Quaternion( m*[q.rot q.x q.y q.z]);
        end
    end
    
end