delt = 0.1;      % Sample time of the generated sequence
num_frames= 64;  % Number of frames to generate
framesize = 64;  % Square frame size in pixels

out = zeros(framesize,framesize,num_frames);    % Initialize movie storage as a 3D Array

target = [zeros(3,11)
          zeros(1,5) 6 zeros(1,5)
          zeros(1,5) 6 zeros(1,5)
          zeros(1,3) 6 6 6 6 6 zeros(1,3) % Target is a plus sign 5 by 5 pixels across
          zeros(1,5) 6 zeros(1,5)         %  with an intensity of 6 (S/N ratio is ~4).
          zeros(1,5) 6 zeros(1,5)         % The total target image is made on an 11x11 grid to
          zeros(3,11)];                   %  allow the image to be interpolated without error.

target_velx = 1;                 % target velocity in x direction in pixels per second
target_vely = 1;                 % target velocity in y direction in pixels per second
target_x_initially = framesize/2; % the target is initially in the center of the frame in x
target_y_initially = framesize/2; % and in y

figure(1);
colormap('gray');
image(target*32);
title('Target Image')

backsize = framesize+36;  % Make the background bigger than the frame so when it
                          % drifts there are new pixels available to drift into.
xygrid = (1:backsize)/backsize;
B=2*sin(2*pi*xygrid).^2'*cos(2*pi*xygrid).^2;

psd = fft2(B);
psd = real(psd.*conj(psd));

background = B + 0.5*randn(backsize);    % Add a specular Gaussian white
					 % sequence to the structure with
					 % variance of 0.25  (sigma of 0.5).

xoff = 10;
yoff = 10;     % Sensor location is offset from the 0,0 of the background
driftx = 1;
drifty = 1;    % drift rate of the background in a and y directions pix/sec.

minout = min(min(min(background)));
maxout = max(max(max(background)));
colormap('gray');
image((background-minout)*64/(maxout-minout))
title('Background image with additive white specular noise')

omega = 2*pi*5;       % The structural frequencies are 5, 10 and 15 Hz in the model.
zeta  = 0.01;         % Damping ratio for all modes

open_system('aero_vibrati')
simout = sim('aero_vibrati','SrcWorkspace','current');

vibdat = simout.get('vibdat');          % The Simulink model "aero_vibrati"
                                        % generates the vibration data at
                                        % a sample time of 0.01 sec.
vibx = vibdat(1:10:1000);               % The output of simulation is
                                        % returned as the variable simout
                                        % The variable simout contains
viby = vibdat(1001:10:2000);            % the in array vibdat that contains
                                        % the vibration data

levarmx = 10;   % Rotational lever arm for vibration noise in x
levarmy = 10;   %  and in y.

subplot(211);
plot(0.01*(1:10:1000),vibx);grid;
title('Time history of the random Tracker rotations')
xlabel('Time');ylabel('x direction')

subplot(212);
plot(0.01*(1:10:1000),viby);grid;
xlabel('Time');ylabel('y direction')

clf; drawnow;

for t = 1:num_frames

  % Drift the Background at the rate driftx and drifty
  % (in pixels/second) and add in the vibration:
  xshift = driftx*delt*t+levarmx*vibx(t,1);
  yshift = drifty*delt*t+levarmy*viby(t,1);

  % Interpolate the 2D image using the MATLAB(R) function interp2:
  [xgrid, ygrid]   = meshgrid(1:backsize);
  [xindex, yindex] = meshgrid(xshift:1:xshift+backsize,yshift:1:yshift+backsize);
  outtemp = interp2(xgrid,ygrid,background,xindex,yindex);

  % Truncate the drifted image down from backsize to framesize:
  out(:,:,t) = outtemp(xoff:xoff+framesize-1,xoff:xoff+framesize-1);

  % Now let the target move also:
  tpixinx = floor(target_velx*delt*t);
  tpixiny = floor(target_vely*delt*t);  % Before interpolating extract the number of pixels moved
  txi = target_velx*delt*t - tpixinx;
  tyi = target_vely*delt*t - tpixiny;   % Interpolate on sub-pixels around the origin only
  [txgrid tygrid] = meshgrid(1:11);     % meshgrid here generates a matrix of grid elements
  [txi tyi] = meshgrid(txi+1:txi+11,tyi+1:tyi+11); % meshgrid generates 2 matrices with the x and y grids

  % Interpolate the intensity values first using interp2 -- a built in MATLAB command
  temp = interp2(txgrid,tygrid,target,txi,tyi);

  % Insert the target at the location determined by the initial offset, and the number of whole pixels moved
  tx = tpixinx + target_x_initially-1;
  ty = tpixiny + target_y_initially-1;
  out(tx:tx+6,ty:ty+6,t) = temp(9:-1:3,9:-1:3) + out(tx:tx+6,ty:ty+6,t);

end

minout = min(min(min(out)));
maxout = max(max(max(out)));
colormap('gray');
image((out(:,:,1)-minout) * 64/(maxout-minout));
title('First frame of combined target and background image.')

x = 1:framesize;
y = 1:framesize;
sigma      = 120;
apfunction = exp(-(x-framesize/2).^2/(2*sigma))' * exp(-(y-framesize/2).^2/(2*sigma));
apfunction = fftshift(apfunction);      % Rotate so it conforms with FFT convention

for j = 1:num_frames
  out(:,:,j) = real(ifft2(apfunction.*fft2(out(:,:,j))));
end

minout = min(min(min(out)));
maxout = max(max(max(out)));
colormap('gray');
image((out(:,:,1)-minout)*64/(maxout-minout));
title('First frame of blurred image.')

minout = min(min(min(out)));
maxout = max(max(max(out)));

M = moviein(num_frames);
for j = 1:num_frames
  image((out(:,:,j)-minout)*64/(maxout-minout))
  drawnow
  M(:,j) = getframe;
end

% colormap('gray')
% movie(M);

bdclose('aero_vibrati');