clear all
close all
clc

ThetaOffset  = [ pi/2, pi/2,     0,    0,     0,   0];
Alpha        = [ pi/2,    0, -pi/2, pi/2, -pi/2,  pi];
R            = [  100,  250,     0,    0,     0,   0];
D            = [  320,    0,     0, -250,     0, -65];
JointsLimits = [ -170*pi/180, 170*pi/180; 
                 -160*pi/180,  65*pi/180; 
                  -51*pi/180, 225*pi/180; 
                 -200*pi/180, 200*pi/180; 
                 -135*pi/180, 135*pi/180; 
                 -360*pi/180, 360*pi/180];
             
             %% forbidden angles
             i=1;
range = 0.05
for q2 = -160:1:65
    for q3 = -51:1:225
        q2rad = q2*pi/180;
        q3rad = q3*pi/180;
        deter = 5*cos(q2rad+q3rad)-5*sin(q2rad)+2;
        if ((deter>-range)&&(deter<range))
            table(i,:) = [ deter, q2, q3 ];
            plot3(q2rad,q3rad,deter,'.','color','r')
            hold on
            xlabel('Q2');ylabel('Q3'),zlabel('det');
            i=i+1
            
            
        end    
    end
end
             
             %% HOMOGENEOUS TRANSFORMATION MATRIX:


%%
N = 100000;
range = 2; % +-degrees
Q1 = JointsLimits(1,1) + (JointsLimits(1,2) - JointsLimits(1,1))*rand(N,1);
Q2 = JointsLimits(2,1) + (JointsLimits(2,2) - JointsLimits(2,1))*rand(N,1);
Q3 = JointsLimits(3,1) + (JointsLimits(3,2) - JointsLimits(3,1))*rand(N,1);
Q4 = JointsLimits(4,1) + (JointsLimits(4,2) - JointsLimits(4,1))*rand(N,1);
Q5 = JointsLimits(5,1) + (JointsLimits(5,2) - JointsLimits(5,1))*rand(N,1);
Q6 = JointsLimits(6,1) + (JointsLimits(6,2) - JointsLimits(6,1))*rand(N,1);

figure(1)
j = 1; a = 1;
for i = 1:1:N
    HTM1 = DH_to_HTM( Q1(i), Alpha(1), R(1), D(1), ThetaOffset(1));
    HTM2 = DH_to_HTM( Q2(i), Alpha(2), R(2), D(2), ThetaOffset(2));
    HTM3 = DH_to_HTM( Q3(i), Alpha(3), R(3), D(3), ThetaOffset(3));
    HTM4 = DH_to_HTM( Q4(i), Alpha(4), R(4), D(4), ThetaOffset(4));
    HTM5 = DH_to_HTM( Q5(i), Alpha(5), R(5), D(5), ThetaOffset(5));
    HTM6 = DH_to_HTM( Q6(i), Alpha(6), R(6), D(6), ThetaOffset(6));
    HTM0E = HTM1*HTM2*HTM3*HTM4*HTM5*HTM6;
    X = HTM0E(1,4);
    Y = HTM0E(2,4);
    Z = HTM0E(3,4);
    i
    if(Z>0) %cannot go under the table
        
        %theta3 = 90+-, prvy factor
        if((Q3(i)>(90-range)*pi/180)&&(Q3(i)<(90+range)*pi/180)) %workspace boundary
            data90(j,:) = [X Y Z];
            plot3(X,Y,Z,'.','color','r')
            view(3);
            hold on
            j=j+1;
        end
        
        % treti faktor
        for k=1:1:length(table)
            if (round(Q2(i)*180/pi)==table(k,2))
                if (round(Q3(i)*180/pi)==table(k,3))
                    if ( Z>300 ) % cannot go through the base
                    trefa(a,:) = [X Y Z];
                    plot3(X,Y,Z,'.','color','b')
                    set(gcf,'color','w');
                    hold on
                    a = a+1;
                    end
                end
            end
        end
       % plot3(X,Y,Z,'.','color','b') %vykresli vsetko
    end
end

% view(3);
% title('Shoulder (blue) and Wrist (red) singularity workspace');
% xlabel('X-Axis (mm)');
% ylabel('Y-Axis (mm)');
% zlabel('Z-Axis (mm)');
view([0 0 1]); 
title('Side view, XY plane');
xlabel('X-Axis (mm)');
ylabel('Y-Axis (mm)');
