% Name:         TECScriptConformance.m
% Description:  Trace conformance
% Date:         27 / 04 / 2017    V1.0
% Autor:        rayalejandro.lattarulo@tecnalia.com
% Observations: 1) Matlab must have installed the following modules:
%                  - Optimization Toolbox
%               2) The program is running with the "RUN" command.
%               3) The functions used by this script are:
%                  - 
%clear all; close all; clc; % Cleaning the environment
% Modify this line>
SIM_Type = 3; % 1 = LC     - Open   Loop
              % 2 = LC     - Closed Loop
              % 3 = DLC    - Open   Loop
              % 4 = DLC    - Closed Loop
              % 5 = Slalom - Open   Loop
              % 6 = Slalom - Closed Loop

em_max = TECFunc_MaxErrorsInputs(); % Max. error inputs [x,y,yaw,vx,vy,w,ax,delta]
ed_max = TECFunc_MaxErrorsForces(); % Max. error forces [Fxf, Fyf, Fyr]

% set measured variables
Xj = TECFunc_ReadDataCSV(SIM_Type);
T       = Xj.t  ;
X       = Xj.x  ;
Y       = Xj.y  ;
psi     = Xj.psi;
vx      = Xj.vx ;
vy      = Xj.vy ;
omega   = Xj.w  ;
ax      = Xj.ax ;
ay      = Xj.ay ;
delta_m = Xj.delta;
Tu      = Xj.t;
u       = [Xj.ax , Xj.delta];


% set up traces
N = size(X,1);
Y_trace = [X';Y';psi';vx';vy';omega';ax';delta_m'];
U_trace0 = [ax';delta_m'];
X_trace0 = [X';Y';psi';vx';vy';omega'];

% parameters: functions
f = @TECFunc_VehicleModel_Bicycle_Openloop_COR;

% trace_conformance_optctrl
[XU,E] = trace_conformance_optctrl__version03(T,Y_trace,X_trace0,U_trace0,f,ed_max,em_max);
  
% plotting 
TECScript_Plotting;
TECScript_PlottingDocument;