clear all
clc


%% Init
%Endzeit
global tend;
tend=1;
%disrSchrittanzahl
global n;
n=tend*100+1;
%Wichtungsfaktoren
global z;
global a;
global b;
a=0;
b=1e-7;
z=1;

global x0;
x10=0.04;
x20=-0.8;
x0(1,1)=x10;
x0(2,1)=x20;

%Startwerte der Optimierungsparameter
ustart = 1*ones(1,n);

%Diskretes Zustandsmodell
A=[0 1; -40 0];
B=[0; -1];
C=[-5 0; -200 0];
D=[-1; 0];
global dt
dt=tend/(n-1);
sys=ss(A,B,C,D);
sysd=c2d(sys,dt,'foh');

global Ad;
global Bd;
global Cd;
global Dd;
Ad=sysd.a;
Bd=sysd.b;
Cd=sysd.c;
Dd=sysd.d;


%% fmincon
options = optimset('Display','iter','GradObj','off','GradConstr','off','Algorithm','interior-point','MaxFunEvals',200000,'MaxIter',5000);
tic
[u,fval,extraflag,output] = fmincon(@functional,ustart,[],[],[],[],[],[],@constraints,options);
toc
t=linspace(0,tend,n);


%% Plots
%Berechnung der Zustände
x1(1)=x0(1);
x2(1)=x0(2);
for i=1:n-1
    x_temp1=[x1(i);x2(i)];
    x_temp2=Ad*x_temp1+Bd*u(i);
    x1(i+1)=x_temp2(1);
    x2(i+1)=x_temp2(2);
end

% Zustände
figure
plot(t,x1,'b')
hold on
grid on
plot(t,x2,'r')
legend('x1','x2')

% Eingang
figure
x2temp = [-2:0.001:2];

umax = 20*x2temp;
umin = 10*x2temp;

plot(x2temp,umin,'m')
hold on
plot(x2temp,umax,'r')
plot(x2,u,'x')
grid on