function [Bx,By,B,Jx,Jy,J,Hx,Hy,H] = MJxy(t0,D,Ax,Ay,t)
% MJxy - evaluate a minimum jerk curve with seperate displacement for x / y
%
% see Flash and Hogan (1985) for details on the minimum jerk equation
%
%    t0 = movement start time
%    D  = movement duration
%    Ax = displacement resulting from the movement (x)
%    Ay = displacement resulting from the movement (y)
%
% The function is evaluated at times t
%
% The function also optionally returns the first-order and second-order
% partial derivatives, for use with optimization routines
%
% Bx, By and B are the x velocity, y velocity and tangential velocities
% Jx, Jy and J are the gradients (partial derivatives) of the same quantities
% Hx, Hy and H are the Hessian (second-order partial derivatives)
%
% [Bx,By,B,Jx,Jy,J,Hx,Hy,H] = MJxy(t0,D,Ax,Ay,t)

% Jason Friedman, 2012
% www.curiousjason.com
% noisyaccumulation.blogspot.com


% nt is normalized time (0 <= nt <= 1)
nt = (t-t0)./D;
Bx = Ax/D * (-60 * nt.^3 + 30 * nt.^4 + 30 * nt.^2);
By = Ay/D * (-60 * nt.^3 + 30 * nt.^4 + 30 * nt.^2);

A_tang = sqrt((Ax/D).^2 + (Ay/D).^2);
B = A_tang * (-60 * nt.^3 + 30 * nt.^4 + 30 * nt.^2);

if nargout > 3
    Jx(1,:) = Ax.*(-(60.*(t-t0))./D.^3+(180.*(t-t0).^2)./D.^4-(120.*(t-t0).^3)./D.^5);
    Jy(1,:) = Ay.*(-(60.*(t-t0))./D.^3+(180.*(t-t0).^2)./D.^4-(120.*(t-t0).^3)./D.^5);
    
    Jx(2,:) = Ax.*(-(90.*(t-t0).^2)./D.^4+(240.*(t-t0).^3)./D.^5-(150.*(t-t0).^4)./D.^6);
    Jy(2,:) = Ay.*(-(90.*(t-t0).^2)./D.^4+(240.*(t-t0).^3)./D.^5-(150.*(t-t0).^4)./D.^6);
    
    Jx(3,:) = (30.*(t-t0).^2)./D.^3-(60.*(t-t0).^3)./D.^4+(30.*(t-t0).^4)./D.^5;
    Jy(3,:) = zeros(size(t));
    
    Jx(4,:) = zeros(size(t));
    Jy(4,:) = (30.*(t-t0).^2)./D.^3-(60.*(t-t0).^3)./D.^4+(30.*(t-t0).^4)./D.^5;
    
    J(1,:) = A_tang.*(-(60.*(t-t0))./D.^2+(180.*(t-t0).^2)./D.^3-(120.*(t-t0).^3)./D.^4);
    J(2,:) = ((-(2.*Ay.^2)./D.^3-(2.*Ax.^2)./D.^3).*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./(2.*sqrt(Ay.^2./D.^2+Ax.^2./D.^2))+(-(60.*(t-t0).^2)./D.^3+(180.*(t-t0).^3)./D.^4-(120.*(t-t0).^4)./D.^5).*A_tang;
    J(3,:) = (Ax.*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./(A_tang.*D.^2);
    J(4,:) = (Ay.*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./(A_tang.*D.^2);
    if nargout >6
        Hx(1,1,:) = Ax.*(60./D.^3-(360.*(t-t0))./D.^4+(360.*(t-t0).^2)./D.^5);
        Hy(1,1,:) = Ay.*(60./D.^3-(360.*(t-t0))./D.^4+(360.*(t-t0).^2)./D.^5);
        H(1,1,:) = (60./D.^2-(360.*(t-t0))./D.^3+(360.*(t-t0).^2)./D.^4).*A_tang;
        
        Hx(2,1,:) = Ax.*((180.*(t-t0))./D.^4-(720.*(t-t0).^2)./D.^5+(600.*(t-t0).^3)./D.^6);
        Hy(2,1,:) = Ay.*((180.*(t-t0))./D.^4-(720.*(t-t0).^2)./D.^5+(600.*(t-t0).^3)./D.^6);
        H(2,1,:) = ((-(2.*Ay.^2)./D.^3-(2.*Ax.^2)./D.^3).*(-(60.*(t-t0))./D.^2+(180.*(t-t0).^2)./D.^3-(120.*(t-t0).^3)./D.^4))./(2.*sqrt(Ay.^2./D.^2+Ax.^2./D.^2))+((120.*(t-t0))./D.^3-(540.*(t-t0).^2)./D.^4+(480.*(t-t0).^3)./D.^5).*A_tang;
        
        Hx(3,1,:) = -(60.*(t-t0))./D.^3+(180.*(t-t0).^2)./D.^4-(120.*(t-t0).^3)./D.^5;
        Hy(3,1,:) = zeros(size(t));
        H(3,1,:) = (Ax.*(-(60.*(t-t0))./D.^2+(180.*(t-t0).^2)./D.^3-(120.*(t-t0).^3)./D.^4))./(A_tang.*D.^2);
        
        Hx(4,1,:) = zeros(size(t));
        Hy(4,1,:) = -(60.*(t-t0))./D.^3+(180.*(t-t0).^2)./D.^4-(120.*(t-t0).^3)./D.^5;
        H(4,1,:) = (Ay.*(-(60.*(t-t0))./D.^2+(180.*(t-t0).^2)./D.^3-(120.*(t-t0).^3)./D.^4))./(A_tang.*D.^2);
        
        Hx(1,2,:) = Ax.*((180.*(t-t0))./D.^4-(720.*(t-t0).^2)./D.^5+(600.*(t-t0).^3)./D.^6);    
        Hy(1,2,:) = Ay.*((180.*(t-t0))./D.^4-(720.*(t-t0).^2)./D.^5+(600.*(t-t0).^3)./D.^6);
        H(1,2,:) = ((-(2.*Ay.^2)./D.^3-(2.*Ax.^2)./D.^3).*(-(60.*(t-t0))./D.^2+(180.*(t-t0).^2)./D.^3-(120.*(t-t0).^3)./D.^4))./(2.*sqrt(Ay.^2./D.^2+Ax.^2./D.^2))+((120.*(t-t0))./D.^3-(540.*(t-t0).^2)./D.^4+(480.*(t-t0).^3)./D.^5).*A_tang;
        
        Hx(2,2,:) = Ax.*((360.*(t-t0).^2)./D.^5-(1200.*(t-t0).^3)./D.^6+(900.*(t-t0).^4)./D.^7);
        Hy(2,2,:) = Ay.*((360.*(t-t0).^2)./D.^5-(1200.*(t-t0).^3)./D.^6+(900.*(t-t0).^4)./D.^7);
        H(2,2,:) = (((6.*Ay.^2)./D.^4+(6.*Ax.^2)./D.^4).*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./(2.*A_tang)-((-(2.*Ay.^2)./D.^3-(2.*Ax.^2)./D.^3).^2.*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./(4.*(Ay.^2./D.^2+Ax.^2./D.^2).^(3./2)) + ...
            ((180.*(t-t0).^2)./D.^4-(720.*(t-t0).^3)./D.^5+(600.*(t-t0).^4)./D.^6).*A_tang+((-(2.*Ay.^2)./D.^3-(2.*Ax.^2)./D.^3).*(-(60.*(t-t0).^2)./D.^3+(180.*(t-t0).^3)./D.^4-(120.*(t-t0).^4)./D.^5))./A_tang;
        
        Hx(3,2,:) = -(90.*(t-t0).^2)./D.^4+(240.*(t-t0).^3)./D.^5-(150.*(t-t0).^4)./D.^6;
        Hy(3,2,:) = zeros(size(t));
        H(3,2,:) = -(Ax.*(-(2.*Ay.^2)./D.^3-(2.*Ax.^2)./D.^3).*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./(2.*(Ay.^2./D.^2+Ax.^2./D.^2).^(3./2).*D.^2)+(Ax.*(-(60.*(t-t0).^2)./D.^3+(180.*(t-t0).^3)./D.^4-(120.*(t-t0).^4)./D.^5))./(A_tang.*D.^2)- ...
            (2.*Ax.*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./(A_tang.*D.^3);
    
        Hx(4,2,:) = zeros(size(t));
        Hy(4,2,:) = -(90.*(t-t0).^2)./D.^4+(240.*(t-t0).^3)./D.^5-(150.*(t-t0).^4)./D.^6;
        H(4,2,:) = -(Ay.*(-(2.*Ay.^2)./D.^3-(2.*Ax.^2)./D.^3).*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./(2.*(Ay.^2./D.^2+Ax.^2./D.^2).^(3./2).*D.^2)+(Ay.*(-(60.*(t-t0).^2)./D.^3+(180.*(t-t0).^3)./D.^4-(120.*(t-t0).^4)./D.^5))./(A_tang.*D.^2)- ...
            (2.*Ay.*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./(A_tang.*D.^3);
        
        Hx(1,3,:) = -(60.*(t-t0))./D.^3+(180.*(t-t0).^2)./D.^4-(120.*(t-t0).^3)./D.^5;
        Hy(1,3,:) = zeros(size(t));
        H(1,3,:) = (Ax.*(-(60.*(t-t0))./D.^2+(180.*(t-t0).^2)./D.^3-(120.*(t-t0).^3)./D.^4))./(A_tang.*D.^2);
        
        Hx(2,3,:) = -(90.*(t-t0).^2)./D.^4+(240.*(t-t0).^3)./D.^5-(150.*(t-t0).^4)./D.^6;
        Hy(2,3,:) = zeros(size(t));
        H(2,3,:) = -(Ax.*(-(2.*Ay.^2)./D.^3-(2.*Ax.^2)./D.^3).*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./(2.*(Ay.^2./D.^2+Ax.^2./D.^2).^(3./2).*D.^2)+(Ax.*(-(60.*(t-t0).^2)./D.^3+(180.*(t-t0).^3)./D.^4-(120.*(t-t0).^4)./D.^5))./(A_tang.*D.^2)- ...
            (2.*Ax.*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./(A_tang.*D.^3);
        
        Hx(3,3,:) = zeros(size(t));
        Hy(3,3,:) = zeros(size(t));
        H(3,3,:) = ((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4)./(A_tang.*D.^2)-(Ax.^2.*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./((Ay.^2./D.^2+Ax.^2./D.^2).^(3./2).*D.^4);
        
        Hx(4,3,:) = zeros(size(t));
        Hy(4,3,:) = zeros(size(t));
        H(4,3,:) = -(Ax.*Ay.*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./((Ay.^2./D.^2+Ax.^2./D.^2).^(3./2).*D.^4);
        
        Hx(1,4,:) = zeros(size(t));
        Hy(1,4,:) = -(60.*(t-t0))./D.^3+(180.*(t-t0).^2)./D.^4-(120.*(t-t0).^3)./D.^5;
        H(1,4,:) = (Ay.*(-(60.*(t-t0))./D.^2+(180.*(t-t0).^2)./D.^3-(120.*(t-t0).^3)./D.^4))./(A_tang.*D.^2);
        
        Hx(2,4,:) = zeros(size(t));
        Hy(2,4,:) = -(90.*(t-t0).^2)./D.^4+(240.*(t-t0).^3)./D.^5-(150.*(t-t0).^4)./D.^6;
        H(2,4,:) = -(Ay.*(-(2.*Ay.^2)./D.^3-(2.*Ax.^2)./D.^3).*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./(2.*(Ay.^2./D.^2+Ax.^2./D.^2).^(3./2).*D.^2)+(Ay.*(-(60.*(t-t0).^2)./D.^3+(180.*(t-t0).^3)./D.^4-(120.*(t-t0).^4)./D.^5))./(A_tang.*D.^2)- ...
            (2.*Ay.*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./(A_tang.*D.^3);
        
        Hx(3,4,:) = zeros(size(t));
        Hx(3,4,:) = zeros(size(t));
        H(3,4,:) = -(Ax.*Ay.*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./((Ay.^2./D.^2+Ax.^2./D.^2).^(3./2).*D.^4);
        
        Hx(4,4,:) = zeros(size(t));
        Hy(4,4,:) = zeros(size(t));
        H(4,4,:) = ((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4)./(A_tang.*D.^2)-(Ay.^2.*((30.*(t-t0).^2)./D.^2-(60.*(t-t0).^3)./D.^3+(30.*(t-t0).^4)./D.^4))./((Ay.^2./D.^2+Ax.^2./D.^2).^(3./2).*D.^4);
    end
end
