% =========================================================================
%   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 [A, B, C, D] = linPendelZR(stPendel, AP)

%% Fehlerabfrage
    if (AP~=0 && AP~=pi)
        fprintf('----------------------------------------------\n');
        fprintf('Es wurde ein falscher Arbeitspunkt gewählt!! \n');
        fprintf('Mögliche Arbeitspunkt 0 bzw pi\n');
        fprintf('----------------------------------------------\n');        
        A='Error';
        B='Error';
        C='Error';
        D='Error';
		
        return;
    end;


    p = stPendel; %% für bessere Lesbarkeit

	
%% Bestimmung der ZR-Darstellung

	% Abkürzungen
    a       = 1/12*p.mPendel*p.lPendel^2+ ... 
               .25*p.mPendel*p.lPendel^2;
    b       = .5*p.mPendel*p.lPendel;
    mges    = p.mSchlitten+p.mPendel; 
    n       = a*mges-b^2;
    
    
    % Systemzustände
    % x = [(Weg Schlitten) (Geschwindigkeit Schlitten)
    %          (Winkel Pendel) (Winkelgeschwindigkeit Pendel)]
    
	% Abfrage um welchen Punkt linearisiert wird.
	switch (AP)    
        case 0
            
            A = 1/n *     [ 0   n   0            0  ;    ...
                            0   0   b^2*p.g      0  ;    ...
                            0   0   0            n  ;    ...
                            0   0   -mges*b*p.g  0] ;
                        
            B = [0 a/n 0 -b/n]';
            C = [1 0 0 0; 0 0 1 0]';
            D = [0 0]';

		case pi
            A = 1/n *     [ 0   n   0            0   ;   ...                      
                            0   0   b^2*p.g      0   ;   ...           
                            0   0   0            n   ;   ...                 
                            0   0   mges*b*p.g   0 ] ;
                        
            B = [0 a/n 0 b/n]';
            C = [1 0 0 0; 0 0 1 0]';
            D = [0 0]';

	end % switch (AP)
	
end % function linPendelZR