%---------------------------------------------------------
%ConversionS
inch2cm=2.54;
rad2degree=57.296;

%----------------------------------------------------------
%PUP dimensions; in cm unless otherwise noted
d6=12*inch2cm;
a6=4*inch2cm;
range=15;
rmin=21.23+a6; 
rmax=35.93+a6;
zmin=round(d6);
zmax=range*2+zmin;
R=[rmin rmax];
Z=[zmin zmax];
P0=[0 0];

%----------------------------------------------------------
%THETARANGE=[-1*pi/4:pi/10:pi/4];
THETARANGE=[-13.8/rad2degree 13.8/rad2degree -9.37/rad2degree 9.37/rad2degree];
%THETARANGE=[-13.8/rad2degree 13.8/rad2degree -13.8/rad2degree 13.8/rad2degree];

%----------------------------------------------------------
%return cylinder points of PUP focus point range of motion
[X0,Y0,Z0]=rthetaz(P0, R, THETARANGE, Z);

colormap winter
a=colormap;
s=size(a,1);
for i=1:s
b(i,1)=0;
b(i,2)=a(s+1-i,2);
b(i,3)=a(s+1-i,3);
end
colormap(b)
 
subplot(2,3,1);
surf(X0,Y0,Z0)
xlabel({'x (cm) HORIZONTAL', '           STROKE'}) 
ylabel('y (cm) SWEEP'); 
zlabel('z(cm) VERTICAL STROKE'); 
zlim([0 zmax])
set(gca,'Zdir','reverse')
xlim([0 R(2)])
title('PUP END-POINT RANGE OF MOTION','FontSize',12);
text(0,0,-5,'BASE B0=(0,0,0)')
text(0,0,-10,'TRIPOD TOP PLATE');  
xlim([0 R(2)]);ylim([-10 10]);

subplot(2,3,2);
surf(X0,Y0,Z0)
xlabel('x (cm)');ylabel('y (cm)');zlabel('z (cm)');
view([0 90]) %xy view
%axis square
xlim([0 R(2)])
ylim([-10 10])
text(0,range/2+1,['Ri=' num2str(R(1)) 'cm' '   \thetai=',num2str(rad2degree*THETARANGE(1)) ':' num2str(rad2degree*THETARANGE(2)) ' deg'])
text(0,range/2+2,['Ro=' num2str(R(2))  'cm'  '   \thetao=',num2str(rad2degree*THETARANGE(3)) ':' num2str(rad2degree*THETARANGE(4)) ' deg'])
axis square
text(1,1,'B0')

subplot(2,3,3)
surf(X0,Y0,Z0)
axis square
xlabel('x (cm)');ylabel('y (cm)');zlabel('z (cm)');
view([0 0]) %xz view
zlim([0 zmax])
set(gca,'Zdir','revers')
xlim([0 R(2)])
text(R(1)/2,0,zmin,['zmin= ' num2str(zmin)]);
text(R(1)/2,0,zmax-2,['zmax= ' num2str(zmax)]);
text(1,1,2,'B0')
text(rmax,1,Z(2)+4,{'OCEAN', 'BOTTOM'})

%----------------------------------------------------------
%return cylinder points of PUP joint range of motion
R=R-a6;
[X0,Y0,Z0]=rthetaz(P0, R, THETARANGE, Z);

subplot(2,3,4);
surf(X0,Y0,Z0);
ylabel('y (cm) SWEEP'); 
xlabel({'x (cm) HORIZONTAL', '           STROKE'});
ylabel('y (cm) SWEEP'); 
zlabel('z(cm) VERTICAL STROKE');
zlim([0 zmax])
set(gca,'Zdir','revers')
xlim([0 (R(2)+a6)])
set(gca,'Zdir','revers')
title('PUP JOINT RANGE OF MOTION','FontSize',12);
text(0,0,-5,'BASE B0=(0,0,0)')
text(0,0,-10,'TRIPOD TOP PLATE') 

xlim([0 R(2)]);ylim([-10 10]);
subplot(2,3,5);
surf(X0,Y0,Z0)
xlabel('x (cm)');ylabel('y (cm)');zlabel('z (cm)');
view([0 90]) %xy view
%axis squarE
xlim([0 (R(2)+a6)])
ylim([-10 10])
text(0,range/2+1,['Ri=' num2str(R(1)) 'cm' '   \thetai=',num2str(rad2degree*THETARANGE(1)) ':' num2str(rad2degree*THETARANGE(2)) ' deg'])
text(0,range/2+2,['Ro=' num2str(R(2))  'cm'  '   \thetao=',num2str(rad2degree*THETARANGE(3)) ':' num2str(rad2degree*THETARANGE(4)) ' deg'])
axis square
text(1,1,'B0')

subplot(2,3,6)
surf(X0,Y0,Z0)
axis square
xlabel('x (cm)');ylabel('y (cm)');zlabel('z (cm)');
view([0 0]) %xz view
zlim([0 zmax])
set(gca,'Zdir','revers')
xlim([0 R(2)+a6])
text(1,1,2,'B0')
text(rmax,1,Z(2)+4,{'OCEAN ','BOTTOM'})
text(R(1)/4,0,zmin,['zmin= ' num2str(zmin)]);
text(R(1)/4,0,zmax-2,['zmax= ' num2str(zmax)]);
