clear

%% Linear model
lin_mod_data %Geting the linear model

% Setting Calibration and Validation Parameters
n_samples = 32;
n_runs = 150;


s.std_dev_d      = [0.05]; %Feed composition
s.std_dev_nx     = 0.5*ones(1,8); %Temperatures 0.2
s.std_dev_u      = [0.08,0.08]; %Flow rates![0.002,0.002]
s.std_dev_ys     = [0.005,0.005]; % Prod. compositions
s.std_dev_zs     = [0.08,2]; %Reflux flow, 25th Temp measurement [0.05,0.5]


lin_mod.size.num_ys = 2;
lin_mod.size.num_zs = 2;


% Model Based Estimators
model_based_estimators %Finds the model based estimators

% Data Based Estimators
%% Calibrating estimators

for h = 1:n_runs;
    % Drawing Random Variation
    rnd_u{h} =  randn(lin_mod.size.num_u,n_samples);
    rnd_ys{h} = randn(lin_mod.size.num_ys,n_samples);
    rnd_zs{h} = randn(lin_mod.size.num_zs,n_samples);
    
    rnd_d{h}  = s.std_dev_d*randn(lin_mod.size.num_d,n_samples); % Generating disturbances
    rnd_nx{h} = diag(s.std_dev_nx)*randn(lin_mod.size.num_x,n_samples); % Generating white noise
    
    % Validating Modelbased Estimatiors
    for j = 1:3  
        calibration_scenario = j;
        % Input vairations
        switch calibration_scenario
            case 1 % Control policy: open-loop
                u{j,h} = diag(s.std_dev_u)*rnd_u{h};
            case 2 % Control policy: y = ys
                ys{h} = diag(s.std_dev_ys)*rnd_ys{h};
                u{j,h} = inv(lin_mod.Gy_u)*(ys{h}-lin_mod.Gy_d*rnd_d{h});
            case 3 % Control policy: z = zs
                zs{h} = diag(s.std_dev_zs)*rnd_zs{h};
                u{j,h} = inv(lin_mod.Gz_u)*(zs{h}-lin_mod.Gz_d*rnd_d{h});
        end
        
        % Model outputs
        y{h} = lin_mod.Gy_u*u{j,h}  + lin_mod.Gy_d*rnd_d{h};
        x{h} = lin_mod.Gx_u*u{j,h}  + lin_mod.Gx_d*rnd_d{h} + rnd_nx{h};
        z{h} = lin_mod.Gz_u*u{j,h}  + lin_mod.Gz_d*rnd_d{h};

        B{1,j,h} = least_squares(x{h},y{h});
        B{2,j,h} = principal_component_regression(x{h},y{h},4);
        B{3,j,h} = partial_least_squares(x{h},y{h},3);
        B{4,j,h} = closed_loop_regression_M_S2(x{h},y{h},diag(s.std_dev_ys));
        B{5,j,h} = truncated_closed_loop_regression(x{h},y{h},4);
        %error(estimator_scenario,scenario,h) = norm(y{h}-H{estimator_scenario}*x{h});
    end
    
end

store_calibration % Stores the calibration data in a struct


%% Validating Estimators
for h = 1:n_runs;
    % Drawing Random Variation
    rnd_u{h} =  randn(lin_mod.size.num_u,n_samples);
    rnd_ys{h} = randn(lin_mod.size.num_ys,n_samples);
    rnd_zs{h} = randn(lin_mod.size.num_zs,n_samples);
    
    rnd_d{h}  = s.std_dev_d*randn(lin_mod.size.num_d,n_samples); % Generating disturbances
    rnd_nx{h} = diag(s.std_dev_nx)*randn(lin_mod.size.num_x,n_samples); % Generating white noise
    
    % Validating Model Based Estimatiors
    
    for i = 1:4
        for j = 1:4
            estimator_scenario = i;
            validation_scenario = j;
            % Input vairations
            switch validation_scenario
                case 1 % Control policy: open-loop
                    u{i,j,h} = diag(s.std_dev_u)*rnd_u{h};
                case 2 % Control policy: y = ys
                    ys{h} = diag(s.std_dev_ys)*rnd_ys{h};
                    u{i,j,h} = inv(lin_mod.Gy_u)*(ys{h}-lin_mod.Gy_d*rnd_d{h});
                case 3 % Control policy: z = zs
                    zs{h} = diag(s.std_dev_zs)*rnd_zs{h};
                    u{i,j,h} = inv(lin_mod.Gz_u)*(zs{h}-lin_mod.Gz_d*rnd_d{h});
                case 4 % Control policy: y_hat =  ys
                    ys{h} = diag(s.std_dev_ys)*rnd_ys{h};
                    u{i,j,h} = -inv(H{estimator_scenario}*lin_mod.Gx_u)...
                        *(H{estimator_scenario}*(lin_mod.Gx_d*rnd_d{h}...
                        + rnd_nx{h}) + ys{h});
            end
             
            
            % Model outputs
            y{h} = lin_mod.Gy_u*u{i,j,h}  + lin_mod.Gy_d*rnd_d{h};
            x{h} = lin_mod.Gx_u*u{i,j,h}  + lin_mod.Gx_d*rnd_d{h} + rnd_nx{h};
            z{h} = lin_mod.Gz_u*u{i,j,h}  + lin_mod.Gz_d*rnd_d{h};

            error_model(estimator_scenario,validation_scenario,h) = norm(y{h}-H{estimator_scenario}*x{h});
        end
    end
    
    % Validating Data Based Estimatiors
    for i = 1:5
        for j = 1:4
            for k = 1:3
                estimator_scenario = i;
                validation_scenario = j;
                calibration_scenario = k;
                % Input vairations
                switch validation_scenario
                    case 1 % Control policy: open-loop
                        u{i,j,h} = diag(s.std_dev_u)*rnd_u{h};
                    case 2 % Control policy: y = ys
                        ys{h} = diag(s.std_dev_ys)*rnd_ys{h};
                        u{i,j,h} = inv(lin_mod.Gy_u)*(ys{h}-lin_mod.Gy_d*rnd_d{h});
                    case 3 % Control policy: z = zs
                        zs{h} = diag(s.std_dev_zs)*rnd_zs{h};
                        u{i,j,h} = inv(lin_mod.Gz_u)*(zs{h}-lin_mod.Gz_d*rnd_d{h});
                    case 4 % Control policy: y_hat =  ys
                        ys{h} = diag(s.std_dev_ys)*rnd_ys{h};
                        u{i,j,h} = inv(B{estimator_scenario,calibration_scenario,h}*lin_mod.Gx_u)...
                            *(B{estimator_scenario,calibration_scenario,h}*(lin_mod.Gx_d*rnd_d{h}...
                            + rnd_nx{h}) + ys{h});
                end
                
                % Model outputs
                y{h} = lin_mod.Gy_u*u{i,j,h}  + lin_mod.Gy_d*rnd_d{h};
                x{h} = lin_mod.Gx_u*u{i,j,h}  + lin_mod.Gx_d*rnd_d{h} + rnd_nx{h};
                z{h} = lin_mod.Gz_u*u{i,j,h}  + lin_mod.Gz_d*rnd_d{h};
                
                error_data{estimator_scenario}(calibration_scenario,validation_scenario,h) = ...
                    norm(y{h}-B{estimator_scenario,calibration_scenario,h}*x{h});
            end
        end
    end
end

store_validation
save('results','res')

%% Print results
fprintf('\n\nResults\nModel Based Estimators\nMedian prediction error:\n')
fprintf('--------------------------------------\n')
fprintf('\t S1 \t S2 \t S3 \t S4 \n')
fprintf('--------------------------------------\n')
for i = 1:4
    fprintf('  H%i \t %.3f \t %.3f \t %.3f \t %.3f \n', i,res.model_based_estimatior.median_pred_error(i,:))
end
fprintf('--------------------------------------\n\n\n\n')


fprintf('\n\nResults\nData Based Estimators\nMedian prediction error:\n')
fprintf('--------------------------------------\n')
fprintf('\t S1 \t S2 \t S3 \t S4 \n')
for j = 1:5
    fprintf('--------------------------------------\n')
    for i = 1:3
        fprintf('B(%i,%i) \t %.3f \t %.3f \t %.3f \t %.3f \n', j,i,res.data_based_estimatior.median_pred_error{j}(i,:))
    end
end
fprintf('--------------------------------------\n')