function [] = plot_pin0( x, y, scale_factor, theta)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Class:      MECH 6710 - Kinematics and Dynamics of Robots              %
%  Instructor: Dan B. Marghitu                                            %
%  Author:     Jordan Britt                                               %
%  Date:       Aug 21 2012                                                %
%                                                                         %
%  Description:                                                           %
%               This method will plot a grounded pivot.  The pivot is     %
%               drawn so that the center of the drawing is the center of  %
%               the pivot.  It is possible to scale this drawing up or    %
%               down using the 'scale_factor' input, or rotate it so that %
%               it is at a different orientation.                         %
%                                                                         %
%  Input Parameters: [ x, y, scale_factor, theta ]                        %
%  Ouput Paramaters: [none]                                               %
%                                                                         %
%  Explanation of Input Variables:                                        %
%                                                                         %
%       x = Location on X-axis of the center of the pivot                 %
%       y = Location on Y-axis of the center of the pivot                 %
%       scale_factor = scales pivot by this factor to make it appear larger
%                      or smaller                                         %
%       theta = angle (in RADIANS) that pivot should be plotted           %
%                0 radians denotes the base should be down                %
%                                                                         %
%  Key Variables:                                                         % 
%                                                                         %
%                 resolution: How fine the circle is plotted (radians)    %
%                 angle_of_base: angle of the legs of the base (radians)  %
%                 hash_mark_length: how long the hash marks appera (m)    %
%                                                                         %
%  Example Call: plot_grounded_pivot( 1, 3, 3, pi/4)                      %
%                This plot the grounded pivot at (1,3) with a scale factor%
%                of 3 at an angle of pi/4 radians (45 degrees)            %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                        CIRCLE PARAMATERS                                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

rad = 1 * scale_factor;

resolution = 1 / 180 * pi; % resolution of the circle (radians)

psi = 0:resolution:2*pi; % radians

x_circ = rad * cos(psi); % x-coords of circle
y_circ = rad * sin(psi); % y-coords of circle

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                         BASE PARAMATERS                                 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%--- Base Params ---%
base_length = 3*rad;
angle_of_base = 45 / 180 *pi; % angle of base taken from horizontal

%--- Left Leg ---%
L_start_X = cos( pi + angle_of_base   ) * rad;
L_start_Y = sin( pi + angle_of_base   ) * rad;

L_end_X   = cos( pi + angle_of_base   ) * base_length;
L_end_Y   = sin( pi + angle_of_base   ) * base_length;

%--- Right Leg ---%
R_start_X = cos( 2*pi - angle_of_base ) * rad;
R_start_Y = sin( 2*pi - angle_of_base ) * rad;

R_end_X   = cos( 2*pi - angle_of_base ) * base_length;
R_end_Y   = sin( 2*pi - angle_of_base ) * base_length;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                         HASH MARKS                                      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

hash_mark_length = rad;

%--- Top coordinates of Hash Marks ---%
hash_x_top = L_end_X: (R_end_X -L_end_X)/10:  R_end_X;
hash_y_top = L_end_Y * ones(1, length(hash_x_top));

%--- Calculate offset of bottom of hash marks from top ---%
hash_x_offset = hash_mark_length * cos(pi + angle_of_base );
hash_y_offset = hash_mark_length * sin(pi + angle_of_base );

%--- Bottom coordinates of Hash Marks ---%
hash_x_bottom = hash_x_top + hash_x_offset;
hash_y_bottom = hash_y_top + hash_y_offset;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                      ROTATE THE GROUNDED PIVOT                          %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%--- Rotation Matrix ---%
R = [ cos(theta) -sin(theta); sin(theta) cos(theta)];

%--- Rotate Base ---%
base = R * [L_start_X L_end_X R_end_X R_start_X; ...
        L_start_Y L_end_Y R_end_Y R_start_Y];

%--- Rotate Hash Marks ---%   
hash_top    = R * [hash_x_top; hash_y_top];
hash_bottom = R * [hash_x_bottom; hash_y_bottom];
    

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                            Plotting                                     %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%--- Plot Circle ---%
plot(x_circ +x , y_circ +y, 'k')
hold on

%--- Plot Base ---%
plot( base(1,:)+x,...
      base(2,:)+y, 'k');
  
%--- Plot Hash Marks ---%  
for i=2:length( hash_x_top)-1
   plot([hash_top(1,i)+x hash_bottom(1,i)+x],...
        [hash_top(2,i)+y hash_bottom(2,i)+y],'k')
end

% text( hash_bottom(1,end)+x, hash_bottom(2,end)+y, '0','FontSize',10)


