Cam Gear shapes for parametric curves

function [M] = erraticSpin(R,n,xo,yo,f,a,b)
%erraticSpin - Calculates the shape of cams required to form a parametric
%curve.
%%
%   R = Radius of center disk
%   n = # line divisions
%   xo/yo = origin of function
%   f = function
%   a/b vectors which describe positions of cams
%%
figure('rend','painters','pos',[10 10 900 900])
myVideo = VideoWriter('Para7.avi');
myVideo.Quality = 100;
myVideo.FrameRate = 60;
open(myVideo);
hold on;
grid on;
axis equal;

circle=linspace(0,8*pi,n); %change this range to do more loops
T=reshape(circle,[n,1]);
numcams=size(a);
vals= f(T);
X = xo+vals(:,1);
Y = yo+vals(:,2);
%Now we have X and Y coords of the center of the disk.

for i=1:n
    for j=1:numcams(2)
        THETA(i,j) = atan((Y(i)-b(j))/(X(i)-a(j)));
        %
        if THETA(i,j)<0
            THETA(i,j) = pi + atan((Y(i)-b(j))/(X(i)-a(j)));
        end
        x(i,j) = (X(i)-R*cos(THETA(i,j)));
        y(i,j) = (Y(i)-R*sin(THETA(i,j)));
        r(i,j) = sqrt((x(i,j)-a(j))^2+(y(i,j)-b(j))^2);
    end
end

for i=1:n
    hold on;grid on;axis square;
    xlim([-20,20]);
    ylim([-15,25]);
    plot(X,Y,'r');%plot of the function F(T)
    plot(a,b,'ro');%cam centers
    plot(X(i)+R*cos(T),Y(i)+R*sin(T),'b'); %plot the disk
    for j=1:numcams(2)
        plot([a(j),x(i,j)],[b(j),y(i,j)],'g');
        %Plots a line going from the cam to the closest point on the disk
        plot( a(j)+r(:,j).*cos( -T(i) + T + THETA(i,j) ) , b(j)+r(:,j).*sin(-T(i)+T+THETA(i,j)),'k');
        %Angle is an increment of 2pi/n + initial angle THETA + the element
        %by element addition of T
    end
    M(i) = getframe;
    clf;
end
%writeVideo(myVideo,M);
close(myVideo);
%movie(M,30,60);

end