function completed = example_nonlinearDA_reach_05_powerSystem_14bus_compositional()
% example_nonlinearDA_reach_05_powerSystem_14bus_compositional - example of 
%    nonlinear-differential-algebraic reachability analysis: 14-bus power 
%    system from [1], where the abstraction error is computed 
%    compositionally.
%
% Syntax:
%    completed = example_nonlinearDA_reach_05_powerSystem_14bus_compositional()
%
% Inputs:
%    -
%
% Outputs:
%    completed - true/false 
%
% References:
%    [1] M. Althoff, "Formal and Compositional Analysis of Power Systems 
%        using Reachable Sets", IEEE Transactions on Power Systems 29 (5), 
%        2014, 2270-2280

% Authors:       Matthias Althoff
% Written:       08-June-2022
% Last update:   ---
% Last revision: ---

% ------------------------------ BEGIN CODE -------------------------------

% Bus Parameters ----------------------------------------------------------

nrOfGenerators = 5;
nrOfBuses = 14;
nrOfInputs = nrOfGenerators + nrOfBuses;


% Reachability Settings ---------------------------------------------------

options.taylorTerms = 6;
options.zonotopeOrder = 400;
options.tensorOrder = 2;
options.errorOrder = 3;
options.intermediateOrder = 2;
options.lagrangeRem.simplify = 'optimize';

options.maxError_x = [zeros(5,1); 0.6*ones(5,1); zeros(5,1)];
options.maxError_y = 0.01*ones(27,1);


% System Dynamics ---------------------------------------------------------

% set path
path = [CORAROOT filesep 'models' filesep 'powerSystemsConverted'];

% create models (normal and faulty operation)
if ~isfile([path filesep 'IEEE14_model.mat'])
    powerSystem2cora('IEEE14');
    load([path filesep 'IEEE14_model'], 'IEEE14_model');
    derivatives(IEEE14_model, options); % derivatives are not automatically created
end
if ~isfile([path filesep 'IEEE14_sub1_model.mat'])
    powerSystem2cora('IEEE14_sub1');
    load([path filesep 'IEEE14_sub1_model'], 'IEEE14_sub1_model');
    derivatives(IEEE14_sub1_model, options); % derivatives are not automatically created for subsystems
end
if ~isfile([path filesep 'IEEE14_sub2_model.mat'])
    powerSystem2cora('IEEE14_sub2');
    load([path filesep 'IEEE14_sub2_model'], 'IEEE14_sub2_model');
    derivatives(IEEE14_sub2_model, options); % derivatives are not automatically created for subsystems
end
if ~isfile([path filesep 'IEEE14_fault_model.mat'])
    powerSystem2cora('IEEE14_fault');
    load([path filesep 'IEEE14_fault_model'], 'IEEE14_fault_model');
    derivatives(IEEE14_model, options); % derivatives are not automatically created
end
if ~isfile([path filesep 'IEEE14_fault_sub1_model.mat'])
    powerSystem2cora('IEEE14_fault_sub1');
    load([path filesep 'IEEE14_fault_sub1_model'], 'IEEE14_fault_sub1_model');
    derivatives(IEEE14_fault_sub1_model, options); % derivatives are not automatically created for subsystems
end

% load models (normal and faulty operation)
load([path filesep 'IEEE14_model'], 'IEEE14_model');
load([path filesep 'IEEE14_sub1_model'], 'IEEE14_sub1_model');
load([path filesep 'IEEE14_sub2_model'], 'IEEE14_sub2_model');
load([path filesep 'IEEE14_fault_model'], 'IEEE14_fault_model');
load([path filesep 'IEEE14_fault_sub1_model'], 'IEEE14_fault_sub1_model');

% rewrite systems as cells
sys{1} = IEEE14_model;
sys{2} = IEEE14_sub1_model;
sys{3} = IEEE14_sub2_model;
sys_fault{1} = IEEE14_fault_model;
sys_fault{2} = IEEE14_fault_sub1_model;
sys_fault{3} = IEEE14_sub2_model; % this subsystem has no fault

% build indices for the input, state, and constraints relating the full 
% full system
load IEEE14.mat bus
busFull = bus;
% subsystem 1
load IEEE14_sub1.mat bus
bus_subsystem{1} = bus;
% subsystem 2
load IEEE14_sub2.mat bus
bus_subsystem{2} = bus;
% create cell of subsystems
subsystem{1} = sys{2};
subsystem{2} = sys{3};
% compute indices for subsystems
index = indexForSubsystems(sys{1}, subsystem, busFull, bus_subsystem, 3);

% faulty system: build indices for the input, state, and constraints 
% relating the full 
% full system
load IEEE14_fault.mat bus
busFull = bus;
% subsystem 1
load IEEE14_fault_sub1.mat bus
bus_subsystem{1} = bus;
% subsystem 2; this subsystem has no fault
load IEEE14_sub2.mat bus
bus_subsystem{2} = bus;
% create cell of subsystems
subsystem{1} = sys_fault{2};
subsystem{2} = sys_fault{3};
% compute indices for subsystems
index_fault = indexForSubsystems(sys_fault{1}, subsystem, busFull, bus_subsystem, 3);

% Reachability Analysis ---------------------------------------------------

%% normal operation
% time horizon and time step
params.tFinal = 0.1; 
options.timeStep = 0.005;
timeSteps = params.tFinal/options.timeStep;
% provide index for compositional computation
options.index = index;
% load model parameters to obtain Vgen
load IEEE14.mat VM bus
options.Vgen = VM(bus.generator);
% store Jacobian and Hessians of subsystems
sys{2}.setHessian('int');
sys{3}.setHessian('int');
options.subsystemHessian{1} = sys{2}.hessian;
options.subsystemHessian{2} = sys{3}.hessian;
options.subsystemJacobian{1} = sys{2}.jacobian;
options.subsystemJacobian{2} = sys{3}.jacobian;
% input
u0_load = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0];
u0_gen = [2; 0.4; 0; 0; 0];
params.u = [u0_gen; u0_load]*ones(1,timeSteps);
% parameters
x0guess = [0*ones(nrOfGenerators,1); 377*ones(nrOfGenerators,1); 1*ones(nrOfGenerators,1)];
y0guess = [ones(nrOfBuses, 1); zeros(nrOfBuses-1, 1)];

[x0,params.y0guess] = steadyState(IEEE14_model, x0guess, y0guess, params.u);

R0 = zonotope([x0, diag([1e-2*ones(nrOfGenerators,1); ...
    1e-1*ones(nrOfGenerators,1); 1e-3*ones(nrOfGenerators,1)])]);
params.R0 = R0;

params.U = zonotope(zeros(nrOfInputs,1));


% computation of reachable set
timerVal = tic;
R_1 = reach(IEEE14_model, params, options);
tComp = toc(timerVal);
disp(['computation time of reachable set: ',num2str(tComp)]);

%% operation under fault
% time horizon and time step
params.tStart = 0.1; 
params.tFinal = 0.13; 
options.timeStep = 0.001; 
timeSteps = (params.tFinal - params.tStart)/options.timeStep;
% provide index for compositional computation
options.index = index_fault;
% load model parameters to obtain Vgen
load IEEE14_fault.mat VM bus
options.Vgen = VM(bus.generator);
% store Jacobian and Hessians of subsystems
sys_fault{2}.setHessian('int');
sys_fault{3}.setHessian('int');
options.subsystemHessian{1} = sys_fault{2}.hessian;
options.subsystemHessian{2} = sys_fault{3}.hessian;
options.subsystemJacobian{1} = sys_fault{2}.jacobian;
options.subsystemJacobian{2} = sys_fault{3}.jacobian;
% input
u0_gen = [0; 0.4; 0; 0; 0];
params.u = [u0_gen; u0_load]*ones(1,timeSteps);
% initial set is final set of normal operation
params.R0 = R_1.timePoint.set{end}; %initial state for reachability analysis
% computation of reachable set
timerVal = tic;
R_2 = reach(IEEE14_fault_model, params, options);
tComp = toc(timerVal);
disp(['computation time of reachable set: ',num2str(tComp)]);

%% operation after fixing the fault
% time horizon and time step
params.tStart = 0.13; 
params.tFinal = 2; 
options.timeStep = 0.005; 
timeSteps = round((params.tFinal - params.tStart)/options.timeStep);
% provide index for compositional computation
options.index = index;
% load model parameters to obtain Vgen
load IEEE14.mat VM bus
options.Vgen = VM(bus.generator);
% store Jacobian and Hessians of subsystems
options.subsystemHessian{1} = sys{2}.hessian;
options.subsystemHessian{2} = sys{3}.hessian;
options.subsystemJacobian{1} = sys{2}.jacobian;
options.subsystemJacobian{2} = sys{3}.jacobian;
% input
u0_gen = [2; 0.4; 0; 0; 0];
params.u = [u0_gen; u0_load]*ones(1,timeSteps);
% initial set is final set of normal operation
params.R0 = R_2.timePoint.set{end}; %initial state for reachability analysis
% computation of reachable set
timerVal = tic;
R_3 = reach(IEEE14_model, params, options);
tComp = toc(timerVal);
disp(['computation time of reachable set: ',num2str(tComp)]);

%% operation after fixing the fault (larger time step size)
% time horizon and time step
params.tStart = 2; 
params.tFinal = 5; 
options.timeStep = 0.02; 
timeSteps = round((params.tFinal - params.tStart)/options.timeStep);
% input
u0_gen = [2; 0.4; 0; 0; 0];
params.u = [u0_gen; u0_load]*ones(1,timeSteps);
% initial set is final set of normal operation
params.R0 = R_3.timePoint.set{end}; %initial state for reachability analysis
% computation of reachable set
timerVal = tic;
R_4 = reach(IEEE14_model, params, options);
tComp = toc(timerVal);
disp(['computation time of reachable set: ',num2str(tComp)]);

% Simulation --------------------------------------------------------------

nrRuns = 10;
traj_1 = [];
traj_2 = [];
traj_3 = [];
for iRun = 1:nrRuns
    simOpt.points = 1;
    
    % normal operation
    params.R0 = R0;
    params.tStart = 0;
    params.tFinal = 0.1; 
    u0_load = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0];
    u0_gen = [2; 0.4; 0; 0; 0];
    params.u = [u0_gen; u0_load];
    traj_1 = [traj_1; simulateRandom(IEEE14_model, params, simOpt)];

    % operation under fault
    params.tStart = 0.1; 
    params.tFinal = 0.13; 
    u0_gen = [0; 0.4; 0; 0; 0];
    params.u = [u0_gen; u0_load];
    % initial set is final solution of normal operation
    params.R0 = zonotope(traj_1(iRun).x(:,end));
    traj_2 = [traj_2; simulateRandom(IEEE14_fault_model, params, simOpt)];
    
    % operation after fixing the fault
    params.tStart = 0.13; 
    params.tFinal = 5; 
    u0_gen = [2; 0.4; 0; 0; 0];
    params.u = [u0_gen; u0_load];
    % initial set is final solution of normal operation
    params.R0 = zonotope(traj_2(iRun).x(:,end));
    traj_3 = [traj_3; simulateRandom(IEEE14_model, params, simOpt)];
end
    
% Visualization -----------------------------------------------------------

%plot dynamic variables
projDims = {[1 6],[2 7],[3 8],[4 9],[5 10],[11 12],[13 14],[6 11],[7 12], ...
    [8 13],[9 14],[1 11],[2 12],[3 13],[4 14],[1 2],[3 4],[5 6],[7 8]};
for plotRun=1:numel(projDims)
    projDim = projDims{plotRun};

    figure; hold on; box on;

    % plot reachable sets
    plot(R_4, projDim, 'Order', 10, 'FaceColor',[.6 .6 .6]);
    plot(R_3, projDim, 'Order', 10, 'FaceColor',[.6 .6 .6]);
    plot(R_2, projDim, 'Order', 10, 'FaceColor',[.8 .8 .8]);
    plot(R_1, projDim, 'Order', 10, 'FaceColor',[.6 .6 .6]);
    
    
    % plot initial set
    plot(params.R0,projDim,'EdgeColor','k','FaceColor','w');


    % plot simulation results
    for j=1:nrRuns
        plot(traj_1(j),projDim,'k');
        plot(traj_2(j),projDim,'k');
        plot(traj_3(j),projDim,'k');
    end


    % label plot
    xlabel(['x_{',num2str(projDim(1)),'}']);
    ylabel(['x_{',num2str(projDim(2)),'}']);
end

%plot constraint variables
projDims = {[1 15],[2 16],[3 17],[4 18],[5 19],[6 20],[7 21],[8 22],[9 23], ...
    [10 24],[11 25],[12 26],[13 27],[14 27],[1 2],[3 4],[5 6], [7 8]};
for plotRun=1:numel(projDims)
    projDim = projDims{plotRun};
    
    figure; hold on; box on;
    
    plotOrder = 10;
    
    %plot reachable sets of zonotope; mode 4
    for i=1:length(R_4.timeInterval.algebraic)
        Zproj = project(R_4.timeInterval.algebraic{i},projDim);
        Zproj = reduce(Zproj,'girard',plotOrder);
        plot(Zproj,[1 2],'FaceColor',[.6 .6 .6]);
    end
    
    %plot reachable sets of zonotope; mode 3
    for i=1:length(R_3.timeInterval.algebraic)
        Zproj = project(R_3.timeInterval.algebraic{i},projDim);
        Zproj = reduce(Zproj,'girard',plotOrder);
        plot(Zproj,[1 2],'FaceColor',[.6 .6 .6]);
    end 

    %plot reachable sets of zonotope; mode 1
    for i=1:length(R_1.timeInterval.algebraic)
        Zproj = project(R_1.timeInterval.algebraic{i},projDim);
        Zproj = reduce(Zproj,'girard',plotOrder);
        plot(Zproj,[1 2],'FaceColor',[.6 .6 .6]);
    end
    
    %plot reachable sets of zonotope; mode 2
    for i=1:length(R_2.timeInterval.algebraic)
        Zproj = project(R_2.timeInterval.algebraic{i},projDim);
        Zproj = reduce(Zproj,'girard',plotOrder);
        plot(Zproj,[1 2],'FaceColor',[.8 .8 .8]);
    end   

    % plot simulation results
    for j=1:nrRuns
        % mode 1
        plot(traj_1(j).a(projDim(1),:), traj_1(j).a(projDim(2),:),'Color',0*[1 1 1]);
        % mode 2
        plot(traj_2(j).a(projDim(1),:), traj_2(j).a(projDim(2),:),'Color',0*[1 1 1]);
        % mode 3
        plot(traj_3(j).a(projDim(1),:), traj_3(j).a(projDim(2),:),'Color',0*[1 1 1]);
    end

    xlabel(['y_{',num2str(projDim(1)),'}']);
    ylabel(['y_{',num2str(projDim(2)),'}']);
    
    % label plot
    xlabel(['y_{',num2str(projDim(1)),'}']);
    ylabel(['y_{',num2str(projDim(2)),'}']);

end

% example completed
completed = true;


% ------------------------------ END OF CODE ------------------------------
