function sol = Kin_IKRaw(mzad, dim)	
    IK_SOLNUM  = 16;
    JOINTS_MAX = 6;
    d1 = dim(1); a2 = dim(2); a3 = dim(3); d4 = dim(4); d5 = dim(5); d6 = dim(6);

    % --- prealokacje
    Qval  = zeros(IK_SOLNUM, JOINTS_MAX);  % [q0..q5]
    x     = zeros(IK_SOLNUM,1);
    y     = zeros(IK_SOLNUM,1);
    theta = zeros(IK_SOLNUM,1);

    % *************** Obliczanie joint 0 i joint 4 ***************
    M5 = mzad * HT(0,0,-d6);
    R  = sqrt(M5(1,4)^2 + M5(2,4)^2);
    alpha11 = -atan2(M5(1,4), M5(2,4));
    alpha12 = alpha11 + pi;
    alpha   = atan2(d4, sqrt(max(R*R - d4*d4, 0)));

    if alpha12 > pi
        alpha12 = alpha12 - 2*pi;
    elseif alpha12 < -pi
        alpha12 = 2*pi + alpha12;
    end
	
    Q0a = alpha11 + alpha + pi/2;
    Q0b = alpha11 - alpha + pi/2;
    Q0c = alpha12 + alpha + pi/2;
    Q0d = alpha12 - alpha + pi/2;

    Q4a = acos(((mzad(1,4)*sin(Q0a) - mzad(2,4)*cos(Q0a)) - d4) / d6);
    Q4b = acos(((mzad(1,4)*sin(Q0b) - mzad(2,4)*cos(Q0b)) - d4) / d6);
    Q4c = acos(((mzad(1,4)*sin(Q0c) - mzad(2,4)*cos(Q0c)) - d4) / d6);
    Q4d = acos(((mzad(1,4)*sin(Q0d) - mzad(2,4)*cos(Q0d)) - d4) / d6);
	
	if ~isreal(Q4a) Q4a = nan; end
	if ~isreal(Q4b) Q4b = nan; end
	if ~isreal(Q4c)	Q4c = nan; end
	if ~isreal(Q4d)	Q4d = nan; end

    idx = 1;
    for i=1:(IK_SOLNUM/4)
        Qval(idx,1) = Q0a; Qval(idx,5) = Q4a; idx = idx+1;
    end
    for i=1:(IK_SOLNUM/4)
        Qval(idx,1) = Q0b; Qval(idx,5) = Q4b; idx = idx+1;
    end
    for i=1:(IK_SOLNUM/4)
        Qval(idx,1) = Q0c; Qval(idx,5) = Q4c; idx = idx+1;
    end
    for i=1:(IK_SOLNUM/4)
        Qval(idx,1) = Q0d; Qval(idx,5) = Q4d; idx = idx+1;
    end
	
    for i=1:IK_SOLNUM
        if mod(i-1, IK_SOLNUM/4) >= (IK_SOLNUM/8)
            Qval(i,5) = -Qval(i,5);
        end
    end


    % *************** Obliczanie joint 5 ***************
	for i = 1:IK_SOLNUM
		if Kin_SolIsReal(Qval(i,:))
			Qval(i,6) = atan2((-mzad(1,2) * sin(Qval(i,1)) + mzad(2,2) * cos(Qval(i,1))) / sin(Qval(i,5)), -((-mzad(1,1) * sin(Qval(i,1)) + mzad(2,1) * cos(Qval(i,1))) / sin(Qval(i,5))));
		else
			Qval(i,6) = nan;
		end
	end
	
	
    % *************** Obliczanie joint 1, 2 i 3 ***************
    for i=1:IK_SOLNUM
        if Kin_SolIsReal(Qval(i,:))
            Mx1 = HRZ(Qval(i,1)) * HT(0,0,d1) * HRX(pi/2);
            Mx5 = HRZ(Qval(i,5)) * HT(0,0,d5) * HRX(-pi/2);
            Mx6 = HRZ(Qval(i,6)) * HT(0,0,d6);
            mtemp = inv4(Mx1) * mzad;
            M4    = mtemp * inv4(Mx5 * Mx6);
            M5a   = mtemp * inv4(Mx6);
            x(i) = M4(1,4);
            y(i) = M4(2,4);
            theta(i) = atan2( M5a(2,4) - M4(2,4), M5a(1,4) - M4(1,4) );
            c3 = (x(i)^2 + y(i)^2 - a2^2 - a3^2) / (2*a2*a3);
            if mod(i-1,2)==0
                Qval(i,3) = acos(c3);
            else
                Qval(i,3) = -acos(c3);
            end
			if ~isreal(Qval(i,3)) Qval(i,3) = nan; end
            Qval(i,2) = atan2(y(i), x(i)) - atan2( a3*sin(Qval(i,3)), (a2 + a3*cos(Qval(i,3))) );
            Qval(i,4) = theta(i) - Qval(i,2) - Qval(i,3) + pi/2;
			if ~isreal(Qval(i,2)) Qval(i,2) = nan; end
			if ~isreal(Qval(i,4)) Qval(i,4) = nan; end
		end
    end

    % *************** Ograniczenie wartosci do +-pi ***************
    for i=1:IK_SOLNUM
        if Kin_SolIsReal(Qval(i,:))
            for j=1:JOINTS_MAX
                if Qval(i,j) > pi
                    Qval(i,j) = Qval(i,j) - 2*pi;
                elseif Qval(i,j) < -pi
                    Qval(i,j) = 2*pi + Qval(i,j);
                end
            end
        end
    end
	
	% *************** Zwrócenie tylko rozwiązań rzeczywistych ***************
    realMask = false(IK_SOLNUM, 1);
    for i=1:IK_SOLNUM
        realMask(i) = Kin_SolIsReal(Qval(i,:));
    end
    sol = Qval(realMask, 1:JOINTS_MAX);
end