%%% Hinge Mechanism
%%
close all; % closes all figures
clear all; % clears all variables from memory
clc;       % clears all calculations from the Matlab workspace

% Plot Parameters: these will be used to set the axis limits on the figure
xmin= -100; % leftmost window edge
xmax= 20;  % rightmost window edge
ymin= -10; % bottom window edge
ymax= 50;  % top window edge

%% Link Parameters
increments = 50;
max_rotation_theta2 = 90 *pi/180;
theta2 = linspace(0,max_rotation_theta2,increments);
theta3 = linspace(max_rotation_theta2,0,increments);
link2 = 14;
link3 = 26;
link4 = 63;
link5 = 28;


%% Set Up Figure
figure(1);                        
set(1,'WindowStyle','Docked')

%% Calculate Mechanism Motion and Plot It
for i=1:increments
    hold off;
    
    % Draw Link 2
    Ax(i) = 0;                                   % pivot point of link 2 position
    Ay(i) = 0;                                   % pivot point of link 2 position
    Bx(i) = link2*cos( theta2(i)); 
    By(i) = link2*sin( theta2(i) + pi/32);                  % point B position
    plot( [Ax(i) Bx(i)], [Ay(i) By(i)],'Color','r','LineWidth',3 ); % draw the link from A to B for the current configuration
    hold on;
    grid on;
    
    % Draw Link 3
    Cx(i) = link3*sin( theta2(i) - pi/2) + link2*cos( theta2(i)); 
    Cy(i) = link3*cos( theta2(i) - pi/2) + link2*sin( theta2(i) + pi/32);
    plot( [Bx(i) Cx(i)], [By(i) Cy(i)],'Color','r','LineWidth',3 );
    plot(Cx(i), Cy(i), 'bo','MarkerFaceColor','w');
    text(Cx(i)+1, Cy(i)+1, 'C','color','b');
    
    % Draw Link 4
    Dx(i) = Cx(i) - sqrt(link4^2 - Cy(i)^2);
    Dy(i) = 0;
    plot( [Cx(i) Dx(i)], [Cy(i) Dy(i)],'Color','r','LineWidth',3 );
    plot(Dx(i), Dy(i), 'bo','MarkerFaceColor','w');
    text(Dx(i), Dy(i)+2.5, 'D','color','b');
    
    % Draw Link 5
    Ex(i) = Ax(i) - 13;
    Ey(i) = Ay(i);
    Fx(i) = Cx(i) - ((Cx(i)-Dx(i))/link4)*(26);
    Fy(i) = Cy(i) - ((Cy(i)-Dy(i))/link4)*(26);
    text(Ex(i), Ey(i)+2.5, 'E','color','b');
    text(Fx(i)-1, Fy(i)+2.5, 'F','color','b');
    
    plot( [Ex(i) Fx(i)], [Ey(i) Fy(i)],'Color','r','LineWidth',3 );
    plot(Fx(i), Fy(i), 'bo','MarkerFaceColor','w');
    
    
    %% Draw Base Pivots
    % Base 1
    recsz = 1.5;                                        % size of drawn base pivot
    plot([0,1.5],[0,-1.5],'r');                         % draw base pivot
    plot([0,-1.5],[0,-1.5],'r');                        % draw base pivot
    plot(0,0,'ro','MarkerFaceColor','w');               % draw a small circle at the base pivot point
    plot(Bx(i), By(i), 'bo','MarkerFaceColor','w');     % draw a small circle at B
    text(Bx(i)+0.9, By(i), 'B','color','b');   
    
    % Base 2
    recsz = 1.5;                                        % size of drawn base pivot
    plot([-13,1.5 - 13],[0,-1.5],'r');                         % draw base pivot
    plot([-13,-1.5 - 13],[0,-1.5],'r');                        % draw base pivot
    plot(-13,0,'ro','MarkerFaceColor','w');               % draw a small circle at the base pivot point
    
    %% Draw Slider
    rectangle('Position',[Dx(i)-2, Dy(i)-1, 4 2]);
    
    
    axis( [xmin xmax ymin ymax] );
    axis equal;
    pause(0.01);
end

% For going back down
for i=1:increments
    hold off;
    
    % Draw Link 2
    Ax(i) = 0;                                   % pivot point of link 2 position
    Ay(i) = 0;                                   % pivot point of link 2 position
    Bx(i) = link2*cos( theta3(i)); 
    By(i) = link2*sin( theta3(i) + pi/32);                  % point B position
    plot( [Ax(i) Bx(i)], [Ay(i) By(i)],'Color','r','LineWidth',3 ); % draw the link from A to B for the current configuration
    hold on;
    grid on;
    
    % Draw Link 3
    Cx(i) = link3*sin( theta3(i) - pi/2) + link2*cos( theta3(i)); 
    Cy(i) = link3*cos( theta3(i) - pi/2) + link2*sin( theta3(i) + pi/32);
    plot( [Bx(i) Cx(i)], [By(i) Cy(i)],'Color','r','LineWidth',3 );
    plot(Cx(i), Cy(i), 'bo','MarkerFaceColor','w');
    text(Cx(i)+1, Cy(i)+1, 'C','color','b');
    
    % Draw Link 4
    Dx(i) = Cx(i) - sqrt(link4^2 - Cy(i)^2);
    Dy(i) = 0;
    plot( [Cx(i) Dx(i)], [Cy(i) Dy(i)],'Color','r','LineWidth',3 );
    plot(Dx(i), Dy(i), 'bo','MarkerFaceColor','w');
    text(Dx(i), Dy(i)+2.5, 'D','color','b');
    
    % Draw Link 5
    Ex(i) = Ax(i) - 13;
    Ey(i) = Ay(i);
    Fx(i) = Cx(i) - ((Cx(i)-Dx(i))/link4)*(26);
    Fy(i) = Cy(i) - ((Cy(i)-Dy(i))/link4)*(26);
    text(Ex(i), Ey(i)+2.5, 'E','color','b');
    text(Fx(i)-1, Fy(i)+2.5, 'F','color','b');
    
    plot( [Ex(i) Fx(i)], [Ey(i) Fy(i)],'Color','r','LineWidth',3 );
    plot(Fx(i), Fy(i), 'bo','MarkerFaceColor','w');
    
    
    %% Draw Base Pivots
    % Base 1
    recsz = 1.5;                                        % size of drawn base pivot
    plot([0,1.5],[0,-1.5],'r');                         % draw base pivot
    plot([0,-1.5],[0,-1.5],'r');                        % draw base pivot
    plot(0,0,'ro','MarkerFaceColor','w');               % draw a small circle at the base pivot point
    plot(Bx(i), By(i), 'bo','MarkerFaceColor','w');     % draw a small circle at B
    text(Bx(i)+0.9, By(i), 'B','color','b');   
    
    % Base 2
    recsz = 1.5;                                        % size of drawn base pivot
    plot([-13,1.5 - 13],[0,-1.5],'r');                         % draw base pivot
    plot([-13,-1.5 - 13],[0,-1.5],'r');                        % draw base pivot
    plot(-13,0,'ro','MarkerFaceColor','w');               % draw a small circle at the base pivot point
    
    %% Draw Slider
    rectangle('Position',[Dx(i)-2, Dy(i)-1, 4 2]);
    
    
    axis( [xmin xmax ymin ymax] );
    axis equal;
    pause(0.01);
end