#-- Draw the PP configuration
#-- q1,q2: Joint angles (in degrees)
function PP(q1,q2)

#-- Robot parameters
L=1;
l1 = L/2;
l2 = L;
l3 = L/2;
l = l1+l2+l3;

#-- Joints (in Radians)
q1=q1*pi/180;
q2=q2*pi/180;

#-------------------
#-- Calculations
#-------------------
#-- Calculate the orientation of pose1
q01 = atan(-(sin(q1))/(cos(q1)+l1/l2));

#-- calculate the orientation of pose2
q02 = atan( -(l3*sin(q1+q2)+l2*sin(q1)) / (l3*cos(q1+q2)+l2*cos(q1)+l1 ) );

#-- Orientation of pose 3
q03 = atan( -(l3*sin(q1+q2)+l2*sin(q1)) / (l3*cos(q1+q2)+l2*cos(q1)) );

#-- Calculate the orientation of pose 4
q04 = -q1;

f1 = -q01 - q1;
f2 = atan(-sin(q2)/(cos(q2)+l2/l3 ));

#-- Determine the current pose
if (q1<0 && q2>=f1)

  #-- Pose 1 ----
  pose=1;
  q0 = q01;

elseif (q2<0 && q1>=f2)

  #-- Pose 3 ---
  pose=3;
  q0 = q03;

elseif (q1>=0 && q2>=0)

  #-- Pose 4 ---
  pose=4;
  q0=q04;

else
  pose=2;
  q0 = q02;
endif;

#disp("Pose "), disp(pose);

#-------------------------------------------
#-- Define the homogeneous transformations
#-------------------------------------------
#-- Transformation between the frames 0 and 1
A01 = RotxTrasy(q0,l1); 
A12 = RotxTrasy(q1,l2);
A23 = RotxTrasy(q2,l3);

#-- Origin of the local frames
org = [0 0 0 1];

pm0 = A01*RotxTrasy(q1,l2/2)*org';

#-- Points refered to the Frame 0 (the world)
if (pose==3 || pose==4)

  offset=[1   0   0     0;
          0   1   0     -pm0(2);
          0   0   1  -l1*sin(q0);
          0   0   0     1];

else

  offset = [1  0  0  0;
            0  1  0  -pm0(2);
            0  0  1  0;
            0  0  0  1];

endif;




p00 = offset*org';            
p01 = offset*A01*org';         
p02 = offset*A01*A12*org';      
p03 = offset*A01*A12*A23*org';

#-- Draw the robot

x = [p00(1) p01(1) p02(1) p03(1)];
y = [p00(2) p01(2) p02(2) p03(2)];
z = [p00(3) p01(3) p02(3) p03(3)];

yinf = -l;
ysup = l;
zinf = -l1;
zsup = l1;

hold off;
#-- Draw the y axis
plot([yinf ysup],[0 0],"cg");

hold on;

#-- Draw the robot
plot(y,z,"-o","linewidth",4,"markersize",10);

#-- Set other atributes
axis([yinf ysup zinf zsup],"equal");
axis("off");







