%			EVALUATE SYMBOLIC A MATRIX AND TRANSFORM BY SUBBING IN VALUES

menu3=menu('Evaluate A Matrices and Transform',...
   'Evaluate A matrix',...
   'Evaluate Transforms',...
   'MAIN MENU');

if menu3==1					%EVALUATE A MATRIX
	%reset link number
	link_num=0;
	
	%makes sure link number, n, exists
	while link_num < 1 | link_num > dhrow
		link_num=input('Enter A matrix number (e.g., for A2 enter 2): ');
	end
	
	%create sym matrix A from An
	eval(['A=A' num2str(link_num) ';']);

	
	%check if link is revolute
	if dh(link_num,5)==0
		%ask for joint angle
		eval(['t=input(''Enter Joint Angle, theta' num2str(link_num) ' in degrees: '');'])
		t=t*pi/180;

		%sub joint angle, t, into matrix A
		eval(['A=subs(A,t,''t' num2str(link_num) ''')' ';']);

		%checks for joint offset
		if ~(dh(link_num,4)==0)
			%ask for joint offset
			eval(['d=input(''Enter Joint Offset, d' num2str(link_num) ' in meters: '');'])

			%sub joint offset, d, into matrix A
			eval(['A=subs(A,d,''d' num2str(link_num) ''')' ';']);
		end		
		
	else  %link is prismatic
		%ask for joint offset
		eval(['d=input(''Enter Joint Offset, d' num2str(link_num) ' in meters: '');'])

		%sub joint offset, d, into matrix A
		eval(['A=subs(A,d,''d' num2str(link_num) ''')' ';']);
	end
	
	%check for link length
	if ~(dh(link_num,2)==0)
		%ask for link length
		eval(['a=input(''Enter Link Length, a' num2str(link_num) ' (meters): '');']);

		%sub link length, a, into matrix A
		eval(['A=subs(A,a,''a' num2str(link_num) ''')' ';']);
	end
   disp('Requested A matrix is:')
	A=numeric(A)	
end


if menu3==2 		%EVALUATE TRANSFORMS
   if ~fkindone
         disp('Forward Kinematics to evaluate transforms')
	   	fksym
         fkindone = 1;
   end

	%reset transform number
	trans_num=[0,0];
	
	%makes sure transform number, nm, exists
	while (trans_num(1)<1)|(trans_num(1)>dhrow-1)|(trans_num(2)~=dhrow)
		trans_num=input('Enter Transform number (e.g., for A13=A1*A2*A3 enter [1,3]): ');
	end
	
	%create sym matrix A from Anm
	eval(['A=A' num2str(trans_num(1)) num2str(trans_num(2))  ';']);
	
	for i=trans_num(1):trans_num(2)
		%check if link is revolute
		if dh(i,5)==0
			%ask for joint angle
			eval(['t=input(''Enter Joint Angle, theta' num2str(i) ' in degrees: '');'])
			t=t*pi/180;
			%sub joint angle, t, into matrix Anm
			eval(['A=subs(A,t,''t' num2str(i) ''')' ';']);
			%checks for joint offset
			if ~(dh(i,4)==0)
				%ask for joint offset
				eval(['d=input(''Enter Joint Offset, d' num2str(i) ' in meters: '');'])
				%sub joint offset, d, into matrix A
				eval(['A=subs(A,d,''d' num2str(i) ''')' ';']);
			end		
		

		else		%link is prismatic
			%ask for joint offset
			eval(['d=input(''Enter Joint Offset, d' num2str(i) ' in meters: '');'])
			%sub joint offset, d, into matrix A
			eval(['A=subs(A,d,''d' num2str(i) ''')' ';']);
		end
		
		%check for link length
		if ~(dh(i,2)==0)
			%ask for link length
			eval(['a=input(''Enter Link Length, a' num2str(i) ' (meters): '');'])
			%sub link length, a, into matrix A
			eval(['A=subs(A,a,''a' num2str(i) ''')' ';']);
		end
	end
   disp('Requested Transform is:')
	A=numeric(A)
end
