% =========================================================================
%   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 stTraj = berechneTrajektorie(stTraj, T)
	% Gibt eine Struktur mit Inhalt Uebergangszeit T, Zeitvektor t,
	% Zustandsvektor x und Steuerfolge u zurück


	% Initialisierung von Randwertproblem. Die persistent Variablen werden gesetzt
	RandwertproblemDGL(0, 0, 0, stTraj, T);

	% Erstellung von solinit, benötigt von bvp4c, siehe help bvp4c
	% x:            Zeitschritte von Null bis T
	% y:            linearer Verlauf für die Zustände vom Anfangszustand zum Endzustand
	%               Randwerte, hier Winkel von Null auf Pi, Winkelgeschwindigkeit von Null auf Null
	% parameters:   Annahme irgendwelcher Werte für die zwei freien Parameter p1 und p2 im Steuergesetz
	solinit.x = linspace( 0, T, 1000 );
	solinit.y = [(linspace( 0, pi, length(solinit.x))); zeros(1, length(solinit.x))];
	solinit.parameters = [0, 0];


	% sol:          Berechnung des Randwertproblems. Rückgabewerte sind Verlauf
	%               der Zeit und der zugehörigen Zuständen und die berechneten freien
	%               Parameter, die zur gewünschenten Lösung des
	%               Randwertproblems führen
	% bvp4c:        Es bekommt drei Argumente übergeben. Die drei müssen die
	%               gleiche Reinfolgen in ihren Eingangsargumenten besitzen.
	%               Somit muss auch in Randwerte ein Parameter für die freien
	%               Parameter dastehen. Hier: Zeit, Zustände, freie Parameter
	sol = bvp4c(@RandwertproblemDGL, @RandwertproblemRB, solinit);



	% Zeitvektor der Lösung
	t = sol.x;
	
	% Zustände 3 und 4 (des Gesamtsystems!) können direkt aus Lösung
	% RWP entnommen werden.
	x3 = sol.y(1, :);
	x4 = sol.y(2, :);
	
	% Auslesen der berechnet freien Parameter für Randwertproblem
	p1 = sol.parameters(1);
	p2 = sol.parameters(2);

	% Das Steuerfolge wird für Zeitverlauf t berechnet
	u = ( 6*(-p1 - 3*p2).*(t./T) + ...
				12*(3*p1 + 8*p2).*(t./T).^2 + ...
				20*(-3*p1 - 6*p2).*(t./T).^3 + ...
				30*p1.*(t./T).^4 + 42*p2.*(t./T).^5 ) / (T^2);

	% Zustände 1 und 2 werden für Zeitverlauf t berechnet
	x1 = (-p1 - 3*p2).*(t./T).^3 + ...
				(3*p1 + 8*p2).*(t./T).^4 + ...
				(-3*p1 - 6*p2).*(t./T).^5 + ...
				p1.*(t./T).^6 + p2.*(t./T).^7;

	x2 = ( 3*(-p1 - 3*p2).*(t./T).^2 + ...
				4*(3*p1 + 8*p2).*(t./T).^3 + ...
				5*(-3*p1 - 6*p2).*(t./T).^4 + ...
				6*p1.*(t./T).^5 + 7*p2.*(t./T).^6 ) / T;
	
	% Rückgabestruktur mit allen benötigten Werten
	stTraj.T = T;
	stTraj.vT = t;
	stTraj.mX = [x1; x2; x3; x4];
	stTraj.vU = u;
	stTraj.P = sol.parameters;

end % function berechneTrajektorie
