% =========================================================================
%   Praktikum MATLAB/Simulink II
%   Technische Universität Darmstadt
%   IAT, FG rtm (Prof. Dr.-Ing. U. Konigorski)
%
%   Copyright (c) 2017, FG Regelungstechnik und Mechatronik,
%                       Technische Universität Darmstadt
%   All rights reserved.
%
%   Redistribution and use in source and binary forms, with or withou
%   modification, are permitted provided that the following conditions are
%   met:
%
%   1. Redistributions of source code must retain the above copyright
%      notice, this list of conditions and the following disclaimer.
%
%   2. Redistributions in binary form must reproduce the above copyright
%      notice, this list of conditions and the following disclaimer in the
%      documentation and/or other materials provided with the distribution.
%
%
%   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
%   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
%   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
%   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
%   HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
%   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
%   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
%   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
%   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
%   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
%   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

% =========================================================================

function vFrames = animierePendel(vT, mX, pendelDaten, hAxes, p, rec)
% t     := Zeitvektor
% x     := Zustandsvektor
%          x1 = Weg des Schlittens
%          x2 = Beschleunigung des Schlittens
%          x3 = Winkel des Stabs
%          x4 = Winkelgeschwindigkeit des Stabs
% hAxes := handle des Achsensystems, in das gezeichnet werden soll
%		   (Leere Matrix ([]), wenn ein neues Figure erstellt
%			werden soll.)
% p     := "Pause" // Zeit zwischen einzelnen gezeichneten Zuständen
%		   Standardwert: p = 0.025
% rec   := Video aus sim?
%          rec == 1  -->  aufnehmen
%          rec == 0  -->  NICHT aufnehmen
%		   Standardwert: rec = 0

	if isempty(hAxes)
		figure();
		hAxes = axes();
	else
		cla(hAxes);
	end

	if (nargin < 5)
		p = 0.025;
		rec = 0;
	elseif (nargin < 6)
		rec = 0;
	end
	
	if (rec == 0)
		vFrames = [];
	end
	
	global STOP;
	STOP = 0;

	% Es werden nur Zustand 1 und 3 benötigt,
	% zur besseren Lesbarkeit werden diese in neue
	% Variablen "kopiert"
	x1roh = mX(1:end,1);
	x3roh = mX(1:end,3);

	% Resampling
	vTroh = vT;
	vT = 0:p:vTroh(end);
	x1 = interp1( vTroh, x1roh, vT );
	x3 = interp1( vTroh, x3roh, vT );	

	% Berechnen der Stabposition für alle Zeitpunkte
	l = pendelDaten.lPendel;
	stab = struct();
	for i=1:length(vT)
		stab.AnfangX(i) = x1(i) + 0;
		stab.AnfangY(i) =   0   + 0;
		stab.EndeX(i)   = x1(i) + sin(x3(i)) * l;
		stab.EndeY(i)   =   0   - cos(x3(i)) * l;
	end

	% für Axenskalierung
	maxX=max(abs(stab.EndeX));
	maxY=max(abs(stab.EndeY));

	maxRange=1.1*max(maxX, maxY);

	% "aufrufen" des übergebenen/erzeugten Axensystems + Skallierung
	axes(hAxes)
	axis(maxRange*[-1 1 -1 1]);  
	% WICHTIG: x- bzw y-Achsen müssen gleich skalliert werde, da sonst
	%          das Pendel beim schwingen verzerrt!

	% Achsenbeschriftung
	xlabel('x [m]');
	ylabel('y [m]');

	% Zeichnen der Anfangslage
	line([stab.AnfangX(1) stab.EndeX(1)],[stab.AnfangY(1) stab.EndeY(1)],'LineWidth',1,'Color',[0 0 0]);
	hold on;

	% Zeichnen der "Tischlinie"
	line(maxRange*[-1 1],[0 0],'LineWidth',1,'Color','b','LineStyle','--');

	% Anfangslage Schlitten zeichnen
	b=pendelDaten.bSchlitten;
	h=pendelDaten.hSchlitten;
	rectangle('Position',[stab.AnfangX(1)-b/2, stab.AnfangY(1)-h/2, b, h]);

	% handle des aktiven figures für vid
	hFig = gcf();

	% Berechnen wann ein Frame gespeichert wird (auf 500 Frames beschränkt)
	% (nur relevant, wenn rec == 1)
	fr = ceil(length(vT)/500);
	n = 1;

	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	% Zeichnen der Zustände des Pendels
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	for i=1:length(vT)
		if i>1
			delete(hLine);
			delete(hRec);
		end
		
		if STOP
			break;
		end

		hRec = plotSchlitten(stab.AnfangX(i), stab.AnfangY(i),b,h);
		hLine = line([stab.AnfangX(i) stab.EndeX(i)],[stab.AnfangY(i) stab.EndeY(i)],'LineWidth',2,'Color',[1 0 0]);

		plot(stab.EndeX(i), stab.EndeY(i), 'r');

		% Zeit im Titel des Figures anzeigen
		% (auf 2Dezimalstellen gerundet)
		title(['t = ' num2str(round(100*vT(i))/100) ' sec']); 

		% Aufnehmen, falls gewünscht
		% Die Anzahl der Frames wird auf 500 beschränkt
		if ( rec && (mod(i-1, fr) == 0) )
			vFrames(n) = getframe(hFig);
			n = n + 1;
			% sollten Frames gespeichert werden ist keine zusätzliche 
			% Pause nötig
		else
			pause(p);
			% Pause, falls keine Frames gespeichert werden.
		end
	end % for i=1:length(vT)
	
	hold off; % nicht unbedingt notwendig, aber sauberer

end % function animiere Pendel


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Subfunction, die ein graues Rechteck zeichnet
% "Soll Schlitten darstellen"
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function hRec = plotSchlitten(x,y,b,h)
	% x/y = Koordinaten des Mittelpunkts
	% b = Breite
	% h = Höhe

	vX = [x-b/2 x-b/2 x+b/2 x+b/2];
	vY = [y-h/2 y+h/2 y+h/2 y-h/2];

	hRec = fill(vX, vY, [0.9 0.9 0.9], 'LineStyle', 'none');

end % subfunction plotSchlitten