function [pOpt, Info] = parameterEstimationSdoBattery_run(dataFilename,vin)
%PARAMETERESTIMATIONSDOBATTERY_RUN
%
% Solve a parameter estimation problem for the sdoBattery model.
%
% The function returns estimated parameter values, pOpt, and estimation
% termination information, Info.
%
% Input arguments:
%    dataFilename -- Name of file with experiment data
%    vin          -- Vehicle identification number, to look up vehicle's
%                    parameters in database
%
% Modify the function to change the estimation options.
%

% Copyright 2020 The MathWorks, Inc.


%% Ensure model is compiled
%#function sdoBattery.slx


%% Load configured experiment and simulator objects
load sdoBatteryObjectsToDeploy Exper Simulator p vinDatabase

%% Load the experiment data and update the experiment object
if nargin < 1
    dataFilename = 'sdoBattery_Data1.csv';
end
data = readtable(dataFilename);
%Update experiment with input current data
current = timeseries(data.Current,data.Time);
Exper = updateIOData(Exper,'sdoBattery/Current',current);
%Update experiment with output voltage data
voltage = timeseries(data.Voltage,data.Time);
Exper = updateIOData(Exper,'sdoBattery/SOC -> Voltage',voltage);


%% Update Parameters for this Vehicle from the Database
if nargin < 2
    vin = "4DEF";
end
carData =  vinDatabase(vin);
for ct = 1:width(carData)
    variableName = carData.Properties.VariableNames{ct};
    bIdx = strcmp(variableName,{p.Name});
    p(bIdx).Value = carData{'CurrentValue',variableName};
end


%% Create Estimation Objective Function
%
% Create a function that is called at each optimization iteration
% to compute the estimation cost.
%
% Use an anonymous function with one argument that calls sdoBattery_optFcn.
optimfcn = @(P) sdoBattery_optFcn(P,Simulator,Exper);


%% Optimization Options
%
% Specify optimization options.
Options = sdo.OptimizeOptions;
Options.Method = 'lsqnonlin';
Options.OptimizedModel = Simulator;


%% Estimate the Parameters
%
% Call sdo.optimize with the estimation objective function handle,
% parameters to estimate, and options.
[pOpt,Info] = sdo.optimize(optimfcn,p,Options);

%%
% Update Database with New Estimates of Parameters
for ct = 1:numel(pOpt)
    variableName = pOpt(ct).Name;
    carData{'PreviousValue',variableName} = carData{'CurrentValue',variableName};
    carData{'CurrentValue',variableName} = pOpt(ct).Value;
end
vinDatabase(vin) = carData;
save sdoBatteryObjectsToDeploy vinDatabase -append

end

function Vals = sdoBattery_optFcn(P,Simulator,Exper)
%SDOBATTERY_OPTFCN
%
% Function called at each iteration of the estimation problem.
%
% The function is called with a set of parameter values, P, and returns
% the estimation cost, Vals, to the optimization solver.
%
% See the sdoExampleCostFunction function and sdo.optimize for a more
% detailed description of the function signature.
%

%%
% Define a signal tracking requirement to compute how well the model
% output matches the experiment data.
r = sdo.requirements.SignalTracking(...
    'Method', 'Residuals');
%%
% Update the experiment(s) with the estimated parameter values.
Exper = setEstimatedValues(Exper,P);

%%
% Simulate the model and compare model outputs with measured experiment
% data.

F_r = [];
Simulator = createSimulator(Exper,Simulator);
strOT = mat2str(Exper.OutputData(1).Values.Time);
Simulator = sim(Simulator, 'OutputOption', 'AdditionalOutputTimes', 'OutputTimes', strOT);

SimLog = find(Simulator.LoggedData,'logsout');
Sig = find(SimLog,Exper.OutputData.Name);

Error = evalRequirement(r,Sig.Values,Exper.OutputData.Values);
F_r = [F_r; Error(:)];

%% Return Values.
%
% Return the evaluated estimation cost in a structure to the
% optimization solver.
Vals.F = F_r;
end