%% batch execution of trace conformance for the 6 test-runs given below
%% tighter bounds on em-ax,em-delta; loose bounds on ed

%% parameters: maximum measurement deviation, maximum disturbance
    %0.1°/s error for omega does not result in feasible solutions
    %0.1° error for psi is too restrictive: second testcase saturates constraints 
    em_max = [0.05,0.05,0.5*pi/180,0.1,0.1,0.8*pi/180,0.1,1*pi/180]';
    b_L = 0.57;
    a_L = 0.43;
    mu = 0.005;%maximaler kraftschluss für fehler-kräfte --> emax=mu*Fz
    ed_max = 9.81*mu*[1,b_L,a_L]';

%% evaluation of April-Testdrive/A
    folder = 'trackingTest_20160419/A/';
    date = '2016-4-19-';
    time = {
        '12-2-34'; 
        '12-5-29';
        '12-14-1';
        '12-15-11';
        '12-16-33';
        };
    figure(1);clf;
    figure(2);clf;
    figure(3);clf;
    figure(4);clf;

    addpath(folder);
    coord_trans_set = 0;
    X_offset = 0;
    Y_offset = 0;
    PSI_offset = 0;
    
    cm = lines(numel(time));
    
    for instance = 1:numel(time);
        filename = [ folder '/log_TrackingTest_' date time{instance} '.txt'];
        vdata = readVehicleData(filename);
%         data0 = dlmread(filename);
%         IS_JITTER = findJitter(data0(:,1),data0(:,3),data0(:,4),sqrt(data0(:,39).^2+data0(:,40).^2));
%         IDX = find(~IS_JITTER & sqrt(data0(:,39).^2+data0(:,40).^2)>3.0);
%         data = data0(IDX,:);                                                
        IS_JITTER = findJitter(vdata.T,vdata.X_UTM,vdata.Y_UTM,sqrt(vdata.vx.^2+vdata.vy.^2));
        IDX = find(~IS_JITTER & vdata.vx>3.0);
        
        color = cm(instance,:);

        %% compute coordinate transformation to improve visualization
        if ~coord_trans_set
            X_offset = vdata.X_UTM(IDX(1));
            Y_offset = vdata.Y_UTM(IDX(1));
            dx = vdata.X_UTM(IDX(end))-vdata.X_UTM(IDX(1));
            dy = vdata.Y_UTM(IDX(end))-vdata.Y_UTM(IDX(1));
            PSI_offset = -atan2(dy,dx);
            coord_trans_set = 1;
        end
        
        %% set measured variables
        T = vdata.T(IDX);
        X = cos(PSI_offset) * (vdata.X_UTM(IDX)-X_offset) - sin(PSI_offset) * (vdata.Y_UTM(IDX)-Y_offset);
        Y = sin(PSI_offset) * (vdata.X_UTM(IDX)-X_offset) + cos(PSI_offset) * (vdata.Y_UTM(IDX)-Y_offset);
        psi = vdata.Yaw_UTM(IDX)+PSI_offset;
        psi = atan2(sin(psi),cos(psi));
        vx = vdata.vx(IDX);
        vy = vdata.vy(IDX);
        omega = vdata.Yawrate(IDX);
        ax = vdata.ax(IDX);
        ay = vdata.ay(IDX);
        delta_m = vdata.delta_BLW(IDX);
        Tu = vdata.T(IDX);
        u = [vdata.ax_request(IDX),vdata.delta_request(IDX)];

    %% 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'];
    %     N = 2;
    %     Y_trace = Y_trace(:,500+(1:N));
    %     U_trace0 = U_trace0(:,500+(1:N));
    %     X_trace0 = X_trace0(:,500+(1:N));
    %     T = T(500+(1:N));
    %% parameters: functions
        f = @vmodel_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 
    figure(1);
    hold all;xlabel('X');ylabel('Y');box on;
    plot(Y_trace(1,:),Y_trace(2,:),'LineStyle','none','Marker','.','Color',color);
    plot(XU(1,:),XU(2,:),'Color',color);
    axis tight;

    T2=[];
    for i = 1:numel(T)-1;
        T2(end+1) = T(i);
        T2(end+1) = (T(i+1)+T(i))/2;
    end
    
    figure(2);
    subplot(3,1,1);hold all;box on;
    plot(T2,E(1:3:end),'Color',color);ylabel('f_{xf}[m/s^2]');
    plot(T2([1,end]),ed_max([1,1]),'Color','red','LineWidth',2);
    plot(T2([1,end]),-ed_max([1,1]),'Color','red','LineWidth',2);
    axis tight;grid on;
    subplot(3,1,2);hold all;box on;
    plot(T2,E(2:3:end),'Color',color);ylabel('f_{yf}[m/s^2]');
    plot(T2([1,end]),ed_max([2,2]),'Color','red','LineWidth',2);
    plot(T2([1,end]),-ed_max([2,2]),'Color','red','LineWidth',2);
    axis tight;grid on;
    subplot(3,1,3);hold all;box on;
    plot(T2,E(3:3:end),'Color',color);ylabel('f_{yr}[m/s^2]');
    plot(T2([1,end]),ed_max([3,3]),'Color','red','LineWidth',2);
    plot(T2([1,end]),-ed_max([3,3]),'Color','red','LineWidth',2);
    axis tight;grid on;

    figure(3);
    labels = {'eX','eY','e\psi','ev_x','ev_y','e_\omega','ea_x','e\delta'};
    for i = 1:size(Y_trace,1)
        subplot(size(Y_trace,1),1,i);
        hold all;box on;
        plot(T,XU(i,:)-Y_trace(i,:),'Color',color);
        plot(T([1,end]),em_max([i,i]),'Color','red','LineWidth',2);
        plot(T([1,end]),-em_max([i,i]),'Color','red','LineWidth',2);

        ylabel(labels{i});
        axis tight;
        grid on;
    end
   subplot(8,1,7);plot(Tu,u(:,1)-interp1(T,Y_trace(7,:)',Tu),'Color','black','LineWidth',2);axis tight;
   subplot(8,1,8);plot(Tu,u(:,2)-interp1(T,Y_trace(8,:)',Tu),'Color','black','LineWidth',2);axis tight;
%     subplot(8,1,8);plot(data(:,1),data(:,62)-interp1(T,Y_trace(8,:)',data(:,1)),'Color','black','LineWidth',2);axis tight;


    figure(4);
    labels = {'X','Y','\psi','v_x','v_y','\omega','a_x','\delta'};
    for i = 3:size(Y_trace,1)
        subplot(size(Y_trace,1)-2,1,i-2);
        hold all;box on;
        plot(T,XU(i,:),'LineWidth',2,'Color',color);
%         plot(T,Y_trace(i,:),'Color','magenta');
        plot(T,Y_trace(i,:)+em_max(i),'Color','red','LineWidth',2);
        plot(T,Y_trace(i,:)-em_max(i),'Color','red','LineWidth',2);
        ylabel(labels{i});
        axis tight;
        grid on;
    end

    subplot(6,1,5);plot(Tu,u(:,1),'Color','black','LineWidth',2);axis tight;
    subplot(6,1,6);plot(Tu,u(:,2),'Color','black','LineWidth',2);axis tight;
%     subplot(6,1,6);plot(data(:,1),data(:,62)/180*pi,'Color','black','LineWidth',2);axis tight;

    figure(5);
    labels = {'e_X[m]','e_Y[m]','e_\psi[°]','e_{vx}[m/s]','e_{vy}[m/s]','e_{\omega}[°/s]','e_{ax}[m/s²]','e_{\delta}[°]'};
    scale = [1,1,180/pi,1,1,180/pi,1,180/pi];
    plotsize = size(Y_trace,1) + 3;
    for i = 1:size(Y_trace,1)
        subplot(plotsize,1,i);
        hold all;box on;
        plot(T,(XU(i,:)-Y_trace(i,:)).*scale(i),'Color',color);
        plot(T([1,end]),em_max([i,i]).*scale(i),'Color','red','LineWidth',2);
        plot(T([1,end]),-em_max([i,i]).*scale(i),'Color','red','LineWidth',2);

        ylabel(labels{i});
        axis tight;
        grid on;
    end
    subplot(plotsize,1,plotsize-2);hold all;box on;
    plot(T2,E(1:3:end),'Color',color);ylabel('e_{fxf}[m/s^2]');
    plot(T2([1,end]),ed_max([1,1]),'Color','red','LineWidth',2);
    plot(T2([1,end]),-ed_max([1,1]),'Color','red','LineWidth',2);
    axis tight;grid on;
    subplot(plotsize,1,plotsize-1);hold all;box on;
    plot(T2,E(2:3:end),'Color',color);ylabel('e_{fyf}[m/s^2]');
    plot(T2([1,end]),ed_max([2,2]),'Color','red','LineWidth',2);
    plot(T2([1,end]),-ed_max([2,2]),'Color','red','LineWidth',2);
    axis tight;grid on;
    subplot(plotsize,1,plotsize);hold all;box on;
    plot(T2,E(3:3:end),'Color',color);ylabel('e_{fyr}[m/s^2]');
    plot(T2([1,end]),ed_max([3,3]),'Color','red','LineWidth',2);
    plot(T2([1,end]),-ed_max([3,3]),'Color','red','LineWidth',2);
    xlabel('t[s]');
    axis tight;grid on;
    


drawnow;
end