function [g_msg,g_status]=nma_lab2_eme_121(data)
%
% process function called from GUI driver code to
% plot simulation result
%
% EME 121, spring 2011
%
% by Nasser M. Abbasi, Matlab 2011
% April 2011
%

g_status = true;
g_msg='';

set(0,'CurrentFigure',data.handles.figure1);
cla(data.handles.timeAngularSpeedAxes,'reset');
cla(data.handles.timePositionAxes,'reset');
cla(data.handles.simAxes,'reset');
cla(data.handles.torqueAxes,'reset');
cla(data.handles.springOnlyAxes,'reset');

IC = [data.angle_zero;data.y_zero;data.angle_speed_zero;data.y_speed_zero];
options = odeset('OutputFcn',@output);

RK=1;  %experemental. trying to use RK4.
ODE45=2;
MODE=ODE45;

if MODE==RK  %need more work. Use ode45 for now.
    currentTime=0;
    x=zeros(4);
    t=zeros(1,4);
    x(:,1)=[data.angle_zero;data.y_zero;data.angle_speed_zero;data.y_speed_zero];
    
    data.lastPosition=x(2,1);
    data.lastTime=0;
    data.lastAngularSpeed=x(3,1);
    data.torque = 0.5;
    data.lastTorque= data.torque;
    
    done = false;
    rollCount = 1;
    while not(done)
        [data,sol] = RK4_step(currentTime,x(:,rollCount),data,data.timeStep);
        currentTime = currentTime + data.timeStep;
        rollCount  = rollCount + 1;
        if rollCount == 5
            x(:,1:3)=x(:,2:4);
            x(:,4)=sol;
            t(1:3)=t(2:4);
            t(4)=currentTime;
            rollCount = rollCount - 1;
        else
            x(:,rollCount)=sol;
            t(rollCount)=currentTime;
        end
        
        status = plotSolution(t,x,data,rollCount);
        if not(status) || currentTime > data.maxSimulationTime
            done = true;
        end
    end
else
    %Check if using auto step size of an explicit step size. see
    %GUI for input choice.
    if data.timeStep==0
        timeSpan=[0 data.maxSimulationTime];
    else
        timeSpan=0:data.timeStep:data.maxSimulationTime;
    end
    
    %call  the solver
    [t,x] = feval(data.solver,@rhs,timeSpan,IC,options);
    
    set(0,'CurrentFigure',data.handles.figure1);
    set(data.handles.figure1, 'CurrentAxes',data.handles.timePositionAxes);
    plot(t, x(:,2),'-');       
    xlabel('time (sec)'); ylabel('position (meter)');    
    grid on;            
            
    set(data.handles.figure1, 'CurrentAxes',data.handles.timeAngularSpeedAxes);
    plot(t,x(:,3),'-');
    xlabel('time (sec)'); ylabel('omega (rad/sec)');    
    grid on;                                       
        
      set(data.handles.figure1, 'CurrentAxes',data.handles.torqueAxes);
      grid on;
%     plot(t, ones(1,length(t))*data.torque,'-');
%     title(sprintf('current torque = %3.3f',data.torque));
%     xlabel('time (sec)'); ylabel('torque');
%     set(gca,'FontSize',7);
                  
end

%-----------------------------------
%
%-----------------------------------
    function dx=rhs(t,x)
        %the ode45 function
        
        if not(data.useKP)
            if t<=data.stepTorqueDuration
                data.torque=data.stepTorqueAmount;
            else
                data.torque=0;
            end
        else
            data.torque=data.kp*(data.multiplier*2*pi*data.f - x(3)) ;
            if data.torque>data.maxTorque
                data.maxTorque = data.torque;
            end
        end
        
        am=data.a*data.mass_particle;
        theta_second_derivative = ( data.torque-am*x(2)*x(3)^2-2*data.mass_particle*x(2)*x(3)*x(4)...
            +data.k*data.a*x(2)  )/(data.mass_particle*x(2)^2+data.I);
        
        dx=[x(3);
            x(4);
            theta_second_derivative;
            -(data.k/data.mass_particle)*x(2) - ...
            data.a*theta_second_derivative+x(2)*x(3)^2;
            
            ];
        
        if x(2)>data.maxY
            data.maxY = x(2)+abs(x(2))*0.1;
        end
        
        if x(2)<data.minY
            data.minY = x(2)-abs(x(2))*0.1;
        end
        
        if x(3)>data.maxOmega
            data.maxOmega = x(3)+abs(x(3))*0.1;
        end
        
        if x(3)<data.minOmega
            data.minOmega = x(3)-abs(x(3))*0.1;
        end
        
    end

%-----------------------------------
%
%-----------------------------------
    function status= output(t,x,~)
        %called by ode45 after each step. Plot the current
        %pendulum position for simulation
        
        userData = get(data.handles.figure1, 'UserData');
        if userData.stop
            status=true;
            g_status =true;
            return;
        end
        
        if isempty(t)
            status = true;
        else
            
            y = x(2,1);
            
            if y>0
                if data.lastYsign==-1
                    data.lastYsign=1;
                    data.numberOfYFlips = data.numberOfYFlips + 1;
                end
            elseif y<0
                if data.lastYsign==1
                    data.lastYsign=-1;
                    data.numberOfYFlips = data.numberOfYFlips + 1;
                end
            end
            
            % plot position vs time
            set(0,'CurrentFigure',data.handles.figure1);
            set(data.handles.figure1, 'CurrentAxes',...
                data.handles.timePositionAxes);
            
            [~,nCol]=size(x);
            if nCol>1
                plot([data.lastTime t], [data.lastPosition x(2,:)],'-');
                xlim([0 data.maxSimulationTime]);
                if abs(data.minY-data.maxY)>10*eps
                   ylim([data.minY data.maxY]);
                end
                oscillationFreq = floor(data.numberOfYFlips/2)/t(1);
                
                title(sprintf('Position of particle\noscillation frequency approx. %3.3f Hz',...
                    oscillationFreq));
                
                xlabel('time (sec)'); ylabel('position (meter)');
                hold on;
                set(gca,'FontSize',7);
                drawnow;
                data.lastPosition=x(2,end);
                
                
                % angular speed vs. time plot
                set(data.handles.figure1, 'CurrentAxes',...
                    data.handles.timeAngularSpeedAxes);
                
                plot([data.lastTime t],[data.lastAngularSpeed x(3,:)],'-');
                if data.useKP
                    wDesired = data.multiplier*2*pi*data.f;
                    plot(t,wDesired,'r-.');
                    
                    text(0,wDesired,'<');                    
                    title(sprintf(...
                        'Disk angular speed, proportional controller ON\ndesired angular speed = %3.3f rad/sec',...
                        wDesired));
                else
                    title('Disk angular speed, proportional controller OFF');
                end
                xlim([0 data.maxSimulationTime]);
                if abs(data.minOmega-data.maxOmega) > 2*eps
                   ylim([data.minOmega data.maxOmega+data.maxOmega*.2]);
                end                   
                xlabel('time (sec)'); ylabel('omega (rad/sec)');
                
                hold on;
                set(gca,'FontSize',7);
                drawnow;
                data.lastAngularSpeed=x(3,end);
                
                %torque plot
                set(0,'CurrentFigure',data.handles.figure1);
                set(data.handles.figure1, 'CurrentAxes',data.handles.torqueAxes);
                plot([data.lastTime t], [data.lastTorque ones(1,length(t))*data.torque],'-o');
                data.lastTorque=data.torque;
                xlim([0 data.maxSimulationTime]);
                ylim([-data.maxTorque*.1 data.maxTorque+0.2*data.maxTorque]);
                hold on;
                title(sprintf('current torque = %3.3f N m',data.torque));
                xlabel('time (sec)'); ylabel('torque');
                set(gca,'FontSize',7);
                drawnow;
                
                data.lastTime=t(end);
            else
                data.lastPosition=x(2);
                data.lastTime=t(1);
                data.lastAngularSpeed=x(3);
                data.lastTorque=data.torque;
            end
            
            
            %---------------- central simulation now added
            set(data.handles.figure1, 'CurrentAxes',data.handles.simAxes);
            
            %plot circle
            z=-1:.01:1;
            plot(data.R*sin(2*pi*z),data.R*cos(2*pi*z),'LineWidth',2);
            axis equal;
            hold on;
            theta = x(1,1);                                               
            
            A = [cos(theta) -sin(theta);sin(theta) cos(theta)];            
            %plot torque as arc around center.
            if abs(data.torque)>10*eps
                n = 100; % The number of points in the arc
                x0=0;y0=0; x1=data.R*.2; y1=0; x2=-data.R*.2; y2=.1;
                
                if data.torque>0
                   colorIt='black';
                else
                   colorIt='red';
                end
                    P0 = [x0;y0];
                    P1 = [x1;y1];
                    P2 = [x2;y2];
                    v1 = P1-P0;
                    v2 = P2-P0;
                    c  = det([v1,v2]);
                    a  = linspace(0,atan2(abs(c),dot(v1,v2)),n); % Angle range
                    v3 = [0,-c;c,0]*v1;
                    v  = v1*cos(a)+((norm(v1)/norm(v3))*v3)*sin(a);
                    xy_1=A*[v(1,:)+x0;v(2,:)+y0];
                    line(xy_1(1,:),xy_1(2,:),'LineWidth',3);
                    h = get(gca,'children');
                    set(h(1),'color',colorIt);
                    hold on;
                    %plot(x2,y2,'>', 'MarkerSize',10, 'MarkerFaceColor','k');
                    axis equal                
            end
            
            %plot slot boundaries
            A = [cos(theta) -sin(theta);sin(theta) cos(theta)];            
            xy_1=A*[data.a1;-data.h1];
            xy_2=A*[data.a1;data.h1];
            plot([xy_1(1) xy_2(1)],[xy_1(2) xy_2(2)],'k-');   axis equal;
            
            xy_1=A*[data.a2;-data.h2];
            xy_2=A*[data.a2;data.h2];
            plot([xy_1(1) xy_2(1)],[xy_1(2) xy_2(2)],'k-');   axis equal;
            
            %marker in the middle of disk
            xy_1=A*[0;-data.R];
            xy_2=A*[0;data.R];
            plot([xy_1(1) xy_2(1)],[xy_1(2) xy_2(2)],'m:');   axis equal;
            xy_1=A*[-data.R;0];
            xy_2=A*[data.R;0];
            plot([xy_1(1) xy_2(1)],[xy_1(2) xy_2(2)],'m:');   axis equal;
            
            %plot the box
            [xBox,yBox] = nma_spring.makeBox((data.a2-data.a1),(data.a2-data.a1));
            xBox=xBox+(data.a2+data.a1)/2;
            yBox=yBox+y;
            
            newSpringCoordinates=arrayfun(@(i) A*[xBox(i);yBox(i)],...
                1:length(xBox),'UniformOutput',false);
            newSpringCoordinates= [newSpringCoordinates{:}]';
            
            xBox=newSpringCoordinates(:,1);
            yBox=newSpringCoordinates(:,2);
            
            line(xBox,yBox,'LineWidth',2);
            
            %make spring
            lengthOfSpring = (data.h1+data.h2)/2+y;
            [xSpring,ySpring]=nma_spring.makeV2(lengthOfSpring,10,(data.a2-data.a1)/2);
            
            xSpring=xSpring+(data.a2+data.a1)/2;
            ySpring=ySpring-(data.h1+data.h2)/2;
            
            A = [cos(theta) -sin(theta);sin(theta) cos(theta)];
            newSpringCoordinates=arrayfun(@(i) A*[xSpring(i);ySpring(i)],...
                1:length(xSpring),'UniformOutput',false);
            newSpringCoordinates= [newSpringCoordinates{:}]';
            
            xSpring=newSpringCoordinates(:,1);
            ySpring=newSpringCoordinates(:,2);
            
            line(xSpring,ySpring);
            h = get(gca,'children');
            set(h(1),'color','red');

            if abs(data.torque)>0
               if data.torque<0
                  ht=title(sprintf('angle=%3.3f (degree), time=%3.3f (sec)\n (-) torque',...
                        mod(theta*180/pi,360),t(1)));
               else
                  ht=title(sprintf('angle=%3.3f (degree), time=%3.3f (sec)\n (+) torque',...
                        mod(theta*180/pi,360),t(1)));
               end
            else
                 ht=title(sprintf('angle=%3.3f (degree), time=%3.4f (sec)',...
                        mod(theta*180/pi,360),t(1)));
            end                
                                   
            set(ht,'FontSize',7);                               
                        
            axis equal;
            set(gca,'FontSize',7);
            
            xlim([-data.R-data.R*.2 data.R+data.R*0.2]);
            ylim([-data.R-data.R*.2 data.R+data.R*0.2]);
            drawnow;
            hold off;
            %pause(0.4);
            
            %make spring only plot ------------------------
            %cla(data.handles.springOnlyAxes,'reset');
            set(data.handles.figure1, 'CurrentAxes',data.handles.springOnlyAxes);
            
            %make spring
            lengthOfSpring = (data.h1+data.h2)/2+y;
            [xSpring,ySpring]=nma_spring.makeV2(lengthOfSpring,10,(data.a2-data.a1)/2);
            
            A = [cos(-pi/2) -sin(-pi/2);sin(-pi/2) cos(-pi/2)];
            newSpringCoordinates=arrayfun(@(i) A*[xSpring(i);ySpring(i)],...
                1:length(xSpring),'UniformOutput',false);
            newSpringCoordinates= [newSpringCoordinates{:}]';
            
            xSpring=newSpringCoordinates(:,1);
            ySpring=newSpringCoordinates(:,2);
            
            plot(0,0);
            line(xSpring,ySpring);
            h = get(gca,'children');
            set(h(1),'color','red');
            if y>0
                c='in tension';
            elseif y<0
                c='in compression';
            else
                c='relaxed';
            end
            
            %title(sprintf('spring motion as viewed from its base.\nSpring is %s',c));
            title('spring motion as viewed from its base.');
            set(gca,'FontSize',7);
            xlim([0 2*data.R+data.R*.2]);
            ylim([-(data.a2-data.a1) (data.a2-data.a1)]);
            
            %axis equal;
            hold on;
            %plot the box
            [xBox,yBox] = nma_spring.makeBox((data.a2-data.a1),(data.a2-data.a1));
            xBox=xBox+lengthOfSpring+(data.a2-data.a1)/2;
            yBox=yBox-(data.a2-data.a1)/4;
            line(xBox,yBox,'LineWidth',2);
            
            %draw relaxed line
            line([data.h1 data.h1],[-data.a1  data.a2]);
            h = get(gca,'children');
            set(h(1),'color','black','LineStyle','-.');
            
            hold off;
            
            drawnow;
            
            
            %--------------------------------
            
            if abs(y)>data.arcLength
                uiwait(errordlg('Particle fallen off the edge of the table !',...
                    'Bad Input', 'modal'));
                status = true;
                uicontrol(data.handles.y0Tag);
            else
                status = false;
            end
            
            
        end
    end
end

%--------------------
function v=f1(t,x1,x2,x3,x4,data)
v=x3;
end
%--------------------
function v=f2(t,x1,x2,x3,x4,data)
v=x4;
end
%--------------------
function v=f3(t,x1,x2,x3,x4,data)
am=data.a*data.mass_particle;
v= ( data.torque-am*x2*x3^2-2*data.mass_particle*x2*x3*x4...
    +data.k*data.a*x2  )/(data.mass_particle*x2^2+data.I);
end
%--------------------
function v=f4(t,x1,x2,x3,x4,data)
am=data.a*data.mass_particle;
tmp=( data.torque-am*x2*x3^2-2*data.mass_particle*x2*x3*x4...
    +data.k*data.a*x2  )/(data.mass_particle*x2^2+data.I);
v=-(data.k/data.mass_particle)*x2 - ...
    data.a*tmp+x2*x3^2;

end
%-----------------------
% RK4 only
%-----------------------
function [data,x] = RK4_step(t,x,data,delt)

k1 = f1(t,x(1),x(2),x(3),x(4),data) ;
m1 = f2(t,x(1),x(2),x(3),x(4),data) ;
d1 = f3(t,x(1),x(2),x(3),x(4),data) ;
e1 = f4(t,x(1),x(2),x(3),x(4),data) ;

k2 = f1(t+delt/2,x(1)+delt/2*k1,x(2)+delt/2*m1,x(3)+delt/2*d1,x(4)+delt/2*e1,data) ;
m2 = f2(t+delt/2,x(1)+delt/2*k1,x(2)+delt/2*m1,x(3)+delt/2*d1,x(4)+delt/2*e1,data) ;
d2 = f3(t+delt/2,x(1)+delt/2*k1,x(2)+delt/2*m1,x(3)+delt/2*d1,x(4)+delt/2*e1,data) ;
e2 = f4(t+delt/2,x(1)+delt/2*k1,x(2)+delt/2*m1,x(3)+delt/2*d1,x(4)+delt/2*e1,data) ;

k3 = f1(t+delt/2,x(1)+delt/2*k2,x(2)+delt/2*m2,x(3)+delt/2*d2,x(4)+delt/2*e2,data) ;
m3 = f2(t+delt/2,x(1)+delt/2*k2,x(2)+delt/2*m2,x(3)+delt/2*d2,x(4)+delt/2*e2,data) ;
d3 = f3(t+delt/2,x(1)+delt/2*k2,x(2)+delt/2*m2,x(3)+delt/2*d2,x(4)+delt/2*e2,data) ;
e3 = f4(t+delt/2,x(1)+delt/2*k2,x(2)+delt/2*m2,x(3)+delt/2*d2,x(4)+delt/2*e2,data) ;

k4 = f1(t+delt,x(1)+delt*k3,x(2)+delt*m3,x(3)+delt*d3,x(4)+delt*e3,data) ;
m4 = f2(t+delt,x(1)+delt*k3,x(2)+delt*m3,x(3)+delt*d3,x(4)+delt*e3,data) ;
d4 = f3(t+delt,x(1)+delt*k3,x(2)+delt*m3,x(3)+delt*d3,x(4)+delt*e3,data) ;
e4 = f4(t+delt,x(1)+delt*k3,x(2)+delt*m3,x(3)+delt*d3,x(4)+delt*e3,data) ;

x(1)=x(1) + (k1+2*k2+2*k3+k4)*delt/6;
x(2)=x(2) + (m1+2*m2+2*m3+m4)*delt/6;
x(3)=x(3) + (d1+2*d2+2*d3+d4)*delt/6;
x(4)=x(4) + (e1+2*e2+2*e3+e4)*delt/6;

if x(2)>data.maxY
    data.maxY = x(2)+abs(x(2))*0.1;
end

if x(2)<data.minY
    data.minY = x(2)-abs(x(2))*0.1;
end

if x(3)>data.maxOmega
    data.maxOmega = x(3)+abs(x(3))*0.1;
end

if x(3)<data.minOmega
    data.minOmega = x(3)-abs(x(3))*0.1;
end

end
%------------------------
% RK4 only
%
%-----------------------
function status = plotSolution(t,x,data,rollCount)

if t(rollCount)<=0.5
    data.torque=8;
else
    data.torque=0;
end

set(0,'CurrentFigure',data.handles.figure1);
set(data.handles.figure1, 'CurrentAxes',...
    data.handles.timePositionAxes);

plot(t(1:rollCount), x(2,1:rollCount),'-');
xlim([0 data.maxSimulationTime]);
if abs(data.minY-data.maxY)>10*eps
    ylim([data.minY data.maxY]);
end
title('Position of particle');
xlabel('time (sec)'); ylabel('position (meter)');
hold on;
set(gca,'FontSize',7);
drawnow;

set(data.handles.figure1, 'CurrentAxes',...
    data.handles.timeAngularSpeedAxes);

plot(t(1:rollCount),x(3,1:rollCount),'-');
xlim([0 data.maxSimulationTime]);
if abs(data.minOmega-data.maxOmega)>10*eps
    ylim([data.minOmega data.maxOmega]);
end
title('Disk angular speed');
xlabel('time (sec)'); ylabel('omega (rad/sec)');

hold on;
set(gca,'FontSize',7);
drawnow;

%torque plot
set(0,'CurrentFigure',data.handles.figure1);
set(data.handles.figure1, 'CurrentAxes',data.handles.torqueAxes);
plot(t(1:rollCount), ones(1,rollCount)*data.torque,'-');
xlim([0 data.maxSimulationTime]);
ylim([-1 10]);
hold on;
title('current torque');
xlabel('time (sec)'); ylabel('torque');
set(gca,'FontSize',7);
drawnow;

set(data.handles.figure1, 'CurrentAxes',data.handles.simAxes);

%plot circle
z=-1:.01:1;
plot(data.R*sin(2*pi*z),data.R*cos(2*pi*z));
axis equal;
hold on;
theta = x(1,rollCount);
y = x(2,rollCount);

A = [cos(theta) -sin(theta);sin(theta) cos(theta)];

%plot slot boundaries
xy_1=A*[data.a1;-data.h1];
xy_2=A*[data.a1;data.h1];
plot([xy_1(1) xy_2(1)],[xy_1(2) xy_2(2)],'k-');   axis equal;

xy_1=A*[data.a2;-data.h2];
xy_2=A*[data.a2;data.h2];
plot([xy_1(1) xy_2(1)],[xy_1(2) xy_2(2)],'k-');   axis equal;

%marker in the middle of disk
xy_1=A*[0;-data.R*0.1];
xy_2=A*[0;data.R*0.1];
plot([xy_1(1) xy_2(1)],[xy_1(2) xy_2(2)],'-');   axis equal;
xy_1=A*[-data.R*0.1;0];
xy_2=A*[data.R*0.1;0];
plot([xy_1(1) xy_2(1)],[xy_1(2) xy_2(2)],'-');   axis equal;




%             xy_1=A*[0;0];
%             xy_2=A*[data.a1;0];
%             plot([xy_1(1) xy_2(1)],[xy_1(2) xy_2(2)],'-.');   axis equal;

%             thetaDegree = mod(theta*180/pi,360);
%
%             if thetaDegree>=0 && thetaDegree<=90
%                xy_1= data.a*cos(theta)-y*sin(theta);
%                xy_2=data.a*sin(theta)+y*cos(theta);
%                plot(xy_1,xy_2,'o');
%             elseif thetaDegree>90 && thetaDegree<=180
%                delta=theta-pi/2;
%                xy_1= -data.a*sin(delta)-y*cos(delta);
%                xy_2= data.a*cos(delta)-y*sin(delta);
%                plot(xy_1,xy_2,'o');
%             elseif thetaDegree>180 && thetaDegree<=270
%                delta=theta-pi;
%                xy_1= -data.a*cos(delta)+y*sin(delta);
%                xy_2= -data.a*sin(delta)-y*cos(delta);
%                plot(xy_1,xy_2,'o');
%             else
%                delta=2*pi-theta;
%                xy_1= data.a*cos(delta)+y*sin(delta);
%                xy_2= -data.a*sin(delta)+y*cos(delta);
%                plot(xy_1,xy_2,'o');
%             end

%plot the box
[xBox,yBox] = nma_spring.makeBox((data.a2-data.a1),(data.a2-data.a1));
xBox=xBox+(data.a2+data.a1)/2;
yBox=yBox+y;

newSpringCoordinates=arrayfun(@(i) A*[xBox(i);yBox(i)],...
    1:length(xBox),'UniformOutput',false);
newSpringCoordinates= [newSpringCoordinates{:}]';

xBox=newSpringCoordinates(:,1);
yBox=newSpringCoordinates(:,2);

line(xBox,yBox,'LineWidth',2);

%make spring
lengthOfSpring = (data.h1+data.h2)/2+y;
[xSpring,ySpring]=nma_spring.makeV2(lengthOfSpring,10,(data.a2-data.a1)/2);

xSpring=xSpring+(data.a2+data.a1)/2;
ySpring=ySpring-(data.h1+data.h2)/2;

A = [cos(theta) -sin(theta);sin(theta) cos(theta)];
newSpringCoordinates=arrayfun(@(i) A*[xSpring(i);ySpring(i)],...
    1:length(xSpring),'UniformOutput',false);
newSpringCoordinates= [newSpringCoordinates{:}]';

xSpring=newSpringCoordinates(:,1);
ySpring=newSpringCoordinates(:,2);

line(xSpring,ySpring);
h = get(gca,'children');
set(h(1),'color','red');

axis equal;
ht=title(sprintf('angle=%3.3f (degree), time=%3.4f (sec)',...
    mod(theta*180/pi,360),t(1)));
set(ht,'FontSize',7);
set(gca,'FontSize',7);

xlim([-data.R-data.R*.2 data.R+data.R*0.2]);
ylim([-data.R-data.R*.2 data.R+data.R*0.2]);
drawnow;
hold off;
%pause(0.4);

if abs(y)>data.arcLength
    uiwait(errordlg('Particle fallen off the edge of the table !',...
        'Bad Input', 'modal'));
    status = false;
    uicontrol(data.handles.y0Tag);
else
    status = true;
end

end