function anim_arm(t, r, p, g, L)
%
% Flexible Arm animation
%
% t: Nx1 : time
% r: Nx1 : reference (rad)
% p: Nx1 : potentiometer (rad)
% g: Nx1 : strain gage (rad)
% L: 1x1 : length of the bar
%
% Usage1: give some data to show (fill the input args)
% Usage2: allow to use the ssx struct in the workspace (give no args)
% Usage3: allow to make a simulation to show (give no args)

% April 2016, Jose Gaspar

if nargin<1
    [okFlag, t, r, p, g, L] = get_data;
    if ~okFlag, return; end
end

% simple plot
figure(202); clf;
plot(t,r*180/pi, 'c', t,p*180/pi, '.-', t,g*180/pi, '.-')
xlabel('time [sec]');
ylabel('angle [deg]');
legend('reference', 'potentiometer', 'strain gauge');
set(gcf, 'position', [6    78   495   367]);

% the animation
t1= min(t); t2= max(t);
ti= (0:1e-3:1)*(t2-t1)+t1;
yi= interp1(t, [r(:) p(:) g(:)], ti);
k= 1;
figure(203); clf; %hold on
set(gcf, 'position', [513    39   493   652]);
anim_arm_main(ti, k*yi(:,1), k*yi(:,2), k*yi(:,3), L)

return


function anim_arm_main(t, r, p, g, L)

maxt= max(t);
for i=1:length(t)
    
    % prepare data
    [ri, x, x0]= draw_arm(r(i), p(i), g(i), L);

    % draw the arm
    %plot(x(1,:), x(2,:), '.-');
    %plot(x0(1,:), x0(2,:), 'o-', x(1,:), x(2,:), '.-');
    %plot(x0(1,:), x0(2,:), '.-', x(1,:), x(2,:), '-', 'linewidth',2);
    plot(ri(1,:), ri(2,:), 'c', x0(1,:), ...
        x0(2,:), '.-', x(1,:), x(2,:), '-', 'linewidth',2);
    axis equal
    axis([0 1 -1 1]*L)
    title(sprintf('Time: %.2f (max t = %.2f), red=pot+strain', t(i), maxt));
    drawnow
    if i>1, pause(t(i)-t(i-1)); end
    
    % abort display
    mousePt= get(0,'PointerLocation');
    if max(mousePt)<100,
        fprintf(1,'MousePointer near origin: aborted animation\n');
        break;
    end
end


function [ri, p, p0]= draw_arm(refAng, alpha, beta, L)
N= 10;
%L= 1;
v= [L/N 0]';

ri= [[0; 0] [cos(refAng); sin(refAng)]*L];

ca= cos(alpha); sa= sin(alpha);
Ra= [ca -sa; sa ca];

p0= [[0; 0] [ca; sa]*L];

cb= cos(beta/N); sb= sin(beta/N);
Rb= [cb -sb; sb cb];

v= Ra*v;
p= [0 0]';
for i=1:N
    v= Rb*v;
    p= [p p(:,end)+v];
end
return


% -------------------------------------------------
function [okFlag, t, r, p, g, L] = get_data
okFlag= 0;

if check_vars_in_base({'t', 'rf', 'p', 'g'})
    % option1: exists data t, r, p, g in the workspace
    t= evalin('base','t');
    r= evalin('base','rf'); r=r(:,2);
    p= evalin('base','p');  p=p(:,2);
    g= evalin('base','g');  g=g(:,2);
elseif check_vars_in_base({'t', 'r', 'ssx'})
    % option2: exists data t, r, ssx in the workspace
    r= evalin('base','r');
    ssx= evalin('base','ssx');
    t= ssx.time;
    p= ssx.signals.values(:,1);
    g= ssx.signals.values(:,2);
else
    % option3: simulate data
    str= questdlg('No data to show. Run demo?','Demo?', 'Yes','No','Yes');
    if strcmp(str,'No'), return; end % quit
    [t,r,p,g,L]= anim_arm_demo;
end

L= 1;
okFlag= 1;
return


function ret= check_vars_in_base( strList )
ret=1;
for i=1:length(strList)
    str= strList{i};
    if ~evalin('base', ['exist(''' str ''', ''var'')'])
        ret=0; break;
    end
end


function [t,r,p,g,L]= anim_arm_demo
%
% This function requires the Control toolbox

% March 2016, J. Gaspar

% prepare animation data
Gcl= def_ss;
%[y,t] = step(Gcl, 1);
[y,t] = step(Gcl, 2);
r= t*0+1;

%figure; plot(t,y,'.-')

angK= 45*pi/180; % set 45deg step
angK= 30*pi/180; % set 30deg step
r= angK*r; y= angK*y;

[t,r,y]= repeat_data(t, r, y);

% ssx= struct('time',t, 'signals',[]);
% ssx.signals.values= y;
% assignin('base', 'ssx',ssx);
% 
% % run animation
% anim_arm

p= y(:,1);
g= y(:,2);
L= 1;

return


function [t2,r2,y2]= repeat_data(t, r, y)
% use a single step to make many steps (assuming it is a SLIT)
t2= t(:);
r2= r(:);
y2= y;
if abs(t(1))<eps
    t= t(2:end);
    r= r(2:end);
    y= y(2:end,:);
end
t2= [t2; t2(end)+t];
r2= [r2; -r];
y2= [y2; repmat(y2(end,:),size(y,1),1)-2*y];
t2= [t2; t2(end)+t];
r2= [r2; +r];
y2= [y2; repmat(y2(end,:),size(y,1),1)+2*y];
t2= [t2; t2(end)+t];
r2= [r2; -r];
y2= [y2; repmat(y2(end,:),size(y,1),1)-2*y];
return


function Gcl= def_ss
% ret= T2Flexsol_my_tst;
% [a,b,c,d]= ssdata(ret.Gsys); c2=[1 0 0 0; 0 1 0 0]; d2= [0; 0];
% Gcl= feedback( ret.Gctrl * ss(a,b,c2,d2), [tf(1,1) tf(1,1)] );
% [A,B,C,D]= ssdata(Gcl)

A= [0.00 0.00 1.00 0.00 0.00 0.00 0.00 0.00
    0.00 0.00 0.00 1.00 0.00 0.00 0.00 0.00
    0.00 566.00 -37.00 0.00 -393.26 1984.74 -66.93 6.07
    0.00 -922.00 37.00 0.00 393.26 -1984.74 66.93 -6.07
    33.46 33.46 0.00 0.00 -33.46 -33.46 1.00 0.00
    89.54 89.54 0.00 0.00 -89.54 -89.54 0.00 1.00
    -1161.21 -1161.21 0.00 0.00 767.95 3711.95 -103.93 6.07
    5088.21 5088.21 0.00 0.00 -4694.95 -7994.95 103.93 -6.07];

B= [0.00 0.00 0.00 -0.00 -33.46 -89.54 1161.21 -5088.21]';

C= [1 0 0 0 0 0 0 0
    0 1 0 0 0 0 0 0];

D= [0 0]';

Gcl= ss(A,B,C,D);