function [metaboliteList, stoichCoeffList, revFlag] = parseRxnFormula(formula)
% Parses reaction formula into a list of metabolites and a
% list of `S` coefficients
%
% USAGE:
%
%    [metaboliteList, stoichCoeffList, revFlag] = parseRxnFormula(formula)
%
% INPUT:
%    formula:           Reaction formula, may contain symbols '+', '->', '<=>' in
%                       addition to stoichiometric coefficients and metabolite names
%                       examples:
%                       '0.01 cdpdag-SC[m] + 0.01 pg-SC[m]  -> 0.01 clpn-SC[m] + cmp[m] + h[m]' (irreversible reaction)
%                       'cit[c] + icit[x]  <=> cit[x] + icit[c] ' (reversible reaction)
%                       If no stoichiometric coefficient is provided, it is assumed
%                       to be = 1.
%                       Reaction formula should be a string, not a cell array
%
% OUTPUTS:
%    metaboliteList:     Cell array with metabolite names
%    stoichCoeffList:    List of `S` coefficients
%    revFlag:            Indicates whether the reaction is reversible (true) or not (false)
%
% EXAMPLE:
%
%    formula = '0.01 cdpdag-SC[m] + 0.01 pg-SC[m]  -> 0.01 clpn-SC[m] + cmp[m] + h[m]'
%
%    [metaboliteList, stoichCoeffList, revFlag] = parseRxnFormula(formula)
%
%    %metaboliteList = 'cdpdag-SC[m]'    'pg-SC[m]'    'clpn-SC[m]'    'cmp[m]'    'h[m]'
%    %stoichCoeffList = -0.01 -0.01 0.01 1 1
%    %revFlag = false
%
% .. Authors:
%       - Markus Herrgard 6/1/07
%       - Richard Que 1/25/10 Modified to handle '-->' and '<==>' as arrows
%         as well as reactionsformatted as '[compartment] : A --> C'.
%       - IT May 2012 Modified to handle '=>'

tokens = splitString(formula);

stoichCoeffList = [];
metaboliteList = {};
revFlag = true;

% Marks the start of a new stoichiometry + metabolite block
newMetFlag = true;
% Designates products vs reactants
productFlag = false;
compartment = '';
for i = 1:length(tokens)
    t = tokens{i};
    if strcmp(t(1),'[')
        %set compartment
        compartment = t;
    elseif strcmp(t,':')
        %Do nothing
    elseif strcmp(t,'+')
        % Do nothing
        newMetFlag = true;
    elseif strcmp(t,'->') || strcmp(t,'-->') || strcmp(t,'=>')
        % Irreversible
        revFlag = false;
        productFlag = true;
        newMetFlag = true;
    elseif strcmp(t,'<=>') || strcmp(t,'<==>')
        % Reversible
        revFlag = true;
        productFlag = true;
        newMetFlag = true;
    else
        sCoeff = str2double(t);
        if (~isnan(sCoeff))
            % Stoich coefficient
            if ~productFlag
                sCoeff = -sCoeff;
            end
            stoichCoeffList(end+1) = sCoeff;
            newMetFlag = false;
        else
            % Metabolite name
            metaboliteList{end+1} = strcat(t,compartment);
            if newMetFlag
                if ~productFlag
                    stoichCoeffList(end+1) = -1;
                else
                    stoichCoeffList(end+1) = 1;
                end
                newMetFlag = true;
            end
        end
    end
end


function weig = get_w2(sv,method)
% w = get_w(sv)
% High value of w means big decrease in the margin.
% Warning: do not return values of w here. Return a score for each feature
% use the max, change max into sum should not change a lot.
% can be used only if same kernel for all base machines
%% method = 1 ->  take the sum
%% method = 2 ->  used for the l0 update, where the get_w is different from rfe

if nargin < 2,
    method = 1;
end;

if method<2,

    temp = sv.child{1};
    ke=temp.child;
    for i=1:length(sv.child),
        temp = sv.child{i};
            if (temp.child==ke)|strcmp(ke.ker,'weighted_linear'),
              alphaTemp(:,i) = temp.alpha;
            else
                error('Get w for one-vs-rest valid only when the kernel is the same for all base machines.');
            return;
            end;
    end;
    Q = size(alphaTemp,2);
    kertmp = ke.kerparam;
    Ktmp = get_kernel(ke,temp.Xsv,temp.Xsv);
    if strcmp(ke.ker,'linear')|strcmp(ke.ker,'weighted_linear'),
  % compute the multi-class margin for each feature removed.
        if strcmp(ke.ker,'weighted_linear'),
         xTemp = get_x(temp.Xsv);
         xTemp = xTemp.*(ones(size(xTemp,1),1)*ke.kerparam);
         Ktmp = xTemp*xTemp';
        end;
        xTemp = get_x(temp.Xsv);
        temp=xTemp'*alphaTemp;
        tmp2=xTemp'*sum(alphaTemp,2);
        weig = sum(temp.^2,2)' + 1/Q*(tmp2.^2)';
        weig = max(weig)-weig;
        return;
    else
        if strcmp(ke.ker,'rbf'),
      % compute the kernel matrix for all components

         xTemp = get_x(temp.Xsv);

        temp=zeros(1,Q);
        for j=1:Q,
          temp(j)=alphaTemp(:,j)'*(Ktmp)*alphaTemp(:,j);
        end;
        temp = sum(temp) + 1/Q*(sum(alphaTemp,2)'*Ktmp*sum(alphaTemp,2));

      % compute the margin when one component is removed
        for i = 1:size(xTemp,2),
           Ki = xTemp(:,i)*ones(1,size(xTemp,1)) - ones(size(xTemp,1),1)*xTemp(:,i)';
           Ki = Ki.^2;
           Ki = Ki/(2*kertmp^2);
           Ki = exp(Ki);
           for j=1:Q,
            tmpi(j)=alphaTemp(:,j)'*(Ktmp.*Ki)*alphaTemp(:,j);
           end;
            tmpi = sum(tmpi) + 1/Q*(sum(alphaTemp,2)'*(Ktmp.*Ki)*sum(alphaTemp,2));
           weig(i) = temp-tmpi;
        end;
        weig = max(weig)-weig;
       return;
    elseif strcmp(ke.ker,'poly'),
        xTemp = get_x(temp.Xsv);

        temp=zeros(1,Q);
      for j=1:Q,
          temp(j)=alphaTemp(:,j)'*(Ktmp)*alphaTemp(:,j);
      end;
      temp = sum(temp) + 1/Q*(sum(alphaTemp,2)'*Ktmp*sum(alphaTemp,2));
        for i = 1:size(xTemp,2),
           Ki = xTemp(:,i)*xTemp(:,i)';
           K_i = (Ktmp - Ki + 1).^(kertmp);
           for j=1:Q,
            tmpi(j)=alphaTemp(:,j)'*(Ki)*alphaTemp(:,j);
           end;
            tmpi = sum(tmpi) + 1/Q*(sum(alphaTemp,2)'*(Ki)*sum(alphaTemp,2));
           weig(i) = temp-tmpi;
        end;
       weig=max(weig)-weig;
       return;
    end;% if strcmp(...,'rbf')
end;% if strcmp(...,'linear')
%%% Otherwise, all classic kernels have been computed. Now use the get_kernel generically
if strcmp(ke.ker,'custom'),
    error('Get w not implemented for CUSTOM kernels.');
    return;
end;
% compute the multi-class margin for each feature removed.

   xTemp = get_x(temp.Xsv);
   temp=zeros(1,Q);
   for j=1:Q,
          temp(j)=alphaTemp(:,j)'*(Ktmp)*alphaTemp(:,j);
   end;
   temp = sum(temp) + 1/Q*(sum(alphaTemp,2)'*Ktmp*sum(alphaTemp,2));

   for i = 1:size(xTemp,2),
       datTemp = data('temp',[xTemp(:,1:(i-1)),xTemp(:,(i+1):size(xTemp,2))],[]);
       Ki = get_kernel(ke,datTemp,datTemp);
       for j=1:Q,
            tmpi(j)=alphaTemp(:,j)'*(Ki)*alphaTemp(:,j);
       end;
       tmpi = sum(tmpi) + 1/Q*(sum(alphaTemp,2)'*(Ki)*sum(alphaTemp,2));
       weig(i) = temp-tmpi;
   end;
   weig=max(weig)-weig;
return;
else, %% if method < 3
    %% method for the multiplicative update
    for i=1:length(sv.child),
        atmp = sv.child{i};
        W(:,i) = abs(get_w(atmp)');
    end;
    weig = sum(W,2);
    weig=weig';
end;


clc;
clear;
close all;
addpath(genpath('image'));
addpath(genpath('whyte_code'));
addpath(genpath('cho_code'));
opts.prescale = 1; %%downsampling
opts.xk_iter = 5; %% the iterations
opts.gamma_correct = 1.0;
opts.k_thresh = 20;

% modify below line for the input dir for blurry image
inputdir = 'x2/';
% modify below line for the output dir for the kernels
outputdir = 'x2results/';

dirs = dir(inputdir);
L = length(dirs);
for l = 3:L
    picfiles = dir(strcat(inputdir, strcat(dirs(l).name, '/*.jpg')));
    LL = length(picfiles);
    for kk = 1:LL
      try
        opts.kernel_size = 25;  saturation = 0;
        lambda_dark = 4e-3; lambda_grad = 4e-3;opts.gamma_correct = 1.0;
        lambda_tv = 0.003; lambda_l0 = 5e-4; weight_ring = 1;
        img = imread([inputdir dirs(l).name '/' picfiles(kk).name]);
        y = img;
        isselect = 0; %false or true
        if isselect ==1
            figure, imshow(y);
            %tips = msgbox('Please choose the area for deblurring:');
            fprintf('Please choose the area for deblurring:\n');
            h = imrect;
            position = wait(h);
            close;
            B_patch = imcrop(y,position);
            y = (B_patch);
        else
            y = y;
        end
        if size(y,3)==3
            yg = im2double(rgb2gray(y));
        else
            yg = im2double(y);
        end
        [kernel, interim_latent] = blind_deconv(yg, lambda_dark, lambda_grad, opts);

        % for plotting
        k = kernel - min(kernel(:));
        k = k./max(k(:));

        % save kernel
      save(sprintf('%s%s%s%s%s',outputdir,dirs(l).name,'/',string(extractBetween(picfiles(kk).name,1,length(picfiles(kk).name)-4)),'_kernel.mat'), 'kernel');
        % uncomment the following line to generate png files for kernels
        % imwrite(k,sprintf('%s%s%s%s%s',outputdir,dirs(l).name,'/',string(extractBetween(picfiles(kk).name,1,length(picfiles(kk).name)-4)),'_kernel.png'));
      catch
        1
      end

    end
end


function plotDisturbanceComparison(files, laps, labels, max_data_points)
% files: .mat files in a dict. like [controller1 datapoint 1, controller1
% datapoint 2, ...., controller2 datapoint 1,...., controllerN datapoint
% max_data_points]
% max_data_points: number of data points per controller

LineWidth = 1;

k = length(files)/max_data_points;
lap_times_flat = zeros(length(files), 1);
acc_viol_flat = zeros(length(files), 1);
dy_viol_flat = zeros(length(files), 1);
sum_viol_flat = zeros(length(files), 1);
dax_flat = zeros(length(files), 1);
lap_times = zeros(k, max_data_points);
acc_viol = zeros(k, max_data_points);
dy_viol = zeros(k, max_data_points);
sum_viol = zeros(k, max_data_points);
dax = zeros(k,max_data_points);

for i = 1:1:length(files)
    data = load(files{i});
    idx_start = find((data.debug.debug_mvdc_path_matching_debug_ActualTrajPoint_LapCnt.Data == laps(1)), 1, 'first');
    idx_end = find((data.debug.debug_mvdc_path_matching_debug_ActualTrajPoint_LapCnt.Data == laps(2)), 1, 'last');
    if(isempty(idx_end))
        disp(['No lap found for dataset ' files{i}]);
        lap_times_flat(i) = NaN;
        acc_viol_flat(i) = NaN;
        dy_viol_flat(i) = NaN;
        dax_flat(i) = NaN;
    else
        lap_times_flat(i) = data.debug.debug_mvdc_trajectory_driver_perf_LapTime_s.Data(idx_end);
        acc_viol_flat(i) = data.debug.debug_mvdc_trajectory_driver_perf_TireConstViolations.Data(idx_end);
        dy_viol_flat(i) = data.debug.debug_mvdc_trajectory_driver_perf_LatConstViolations.Data(idx_end);
        dax_flat(i) = data.debug_slow.debug_slow_M_tilde.Data(500,1);
    end
end

% resort to generate appropriate plots
for i = 1:1:k
    for j = 1:1:max_data_points
        lap_times(i,j) = lap_times_flat((i-1)*max_data_points+j);
        acc_viol(i,j) = acc_viol_flat((i-1)*max_data_points+j);
        dy_viol(i,j) = dy_viol_flat((i-1)*max_data_points+j);
        dax(i,j) = dax_flat((i-1)*max_data_points+j);
    end
end

figure;
subplot(3, 1, 1); box on, hold on, grid on;
for i = 1:1:k
    plot(dax(i,:), dy_viol(i,:),'LineWidth', LineWidth);
end
xlabel('dax in mps2'); ylabel('violated dy constraints');
legend(labels{1:end});

subplot(3, 1, 2); box on, hold on, grid on;
for i = 1:1:k
    plot(dax(i,:), acc_viol(i,:),'LineWidth', LineWidth);
end
xlabel('dax in mps2'); ylabel('violated a constraints');

subplot(3, 1, 3); box on, hold on, grid on;
for i = 1:1:k
    plot(dax(i,:), lap_times(i,:),'LineWidth', LineWidth);
end
xlabel('dax in mps2'); ylabel('Lap time in s');


classdef SVORLin < Algorithm
    %SVORLin Linear Support Vector for Ordinal Regression (Implicit constraints)
    %   This class derives from the Algorithm Class and implements the
    %   linear SVORIM method. This class uses SVORIM implementation by
    %   W. Chu et al (http://www.gatsby.ucl.ac.uk/~chuwei/svor.htm)
    %
    %   SVORLin methods:
    %      fitpredict               - runs the corresponding algorithm,
    %                                   fitting the model and testing it in a dataset.
    %      fit                        - Fits a model from training data
    %      predict                    - Performs label prediction
    %
    %   References:
    %     [1] W. Chu and S. S. Keerthi, Support Vector Ordinal Regression,
    %         Neural Computation, vol. 19, no. 3, pp. 792–815, 2007.
    %         http://10.1162/neco.2007.19.3.792
    %     [2] P.A. Gutiérrez, M. Pérez-Ortiz, J. Sánchez-Monedero,
    %         F. Fernández-Navarro and C. Hervás-Martínez
    %         Ordinal regression methods: survey and experimental study
    %         IEEE Transactions on Knowledge and Data Engineering, Vol. 28. Issue 1
    %         2016
    %         http://dx.doi.org/10.1109/TKDE.2015.2457911
    %
    %   This file is part of ORCA: https://github.com/ayrna/orca
    %   Original authors: Pedro Antonio Gutiérrez, María Pérez Ortiz, Javier Sánchez Monedero
    %   Citation: If you use this code, please cite the associated paper http://www.uco.es/grupos/ayrna/orreview
    %   Copyright:
    %       This software is released under the The GNU General Public License v3.0 licence
    %       available at http://www.gnu.org/licenses/gpl-3.0.html
    properties
        description = 'Support Vector for Ordinal Regression (Implicit constraints / Linear)';
        parameters = struct('C', 0.1);
    end

    properties (Access = private)
        algorithmMexPath = fullfile(fileparts(which('Algorithm.m')),'SVORIM');
    end

    methods

        function obj = SVORLin(varargin)
            %SVORLin constructs an object of the class SVORLin and sets its default
            %   characteristics
            %   OBJ = SVORLin() builds SVORLin object
        end

        function [projectedTrain,predictedTrain] = privfit(obj,train,parameters)
            %PRIVFIT trains the model for the SVORLin method with TRAIN data and
            %vector of parameters PARAMETERS.
            if isempty(strfind(path,obj.algorithmMexPath))
                addpath(obj.algorithmMexPath);
            end
            [alpha, thresholds, projectedTrain] = svorim([train.patterns train.targets],1,parameters.C,0,0,1);
            predictedTrain = obj.assignLabels(projectedTrain, thresholds);
            model.projection = alpha;
            model.thresholds = thresholds;
            model.parameters = parameters;
            model.train = train.patterns;
            obj.model = model;
            projectedTrain = projectedTrain';
            if ~isempty(strfind(path,obj.algorithmMexPath))
                rmpath(obj.algorithmMexPath);
            end
        end

        function [projected, predicted] = privpredict(obj, test)
            %PREDICT predicts labels of TEST patterns labels. The object needs to be fitted to the data first.
            kernelMatrix = computeKernelMatrix(obj.model.train',test','linear',1);
            projected = obj.model.projection*kernelMatrix;

            predicted = SVORLin.assignLabels(projected, obj.model.thresholds);
            projected = projected';
        end
    end
    methods (Static = true)
        function predicted = assignLabels(projected, thresholds)
            numClasses = size(thresholds,2)+1;
            %TEST assign the labels from projections and thresholds
            project2 = repmat(projected, numClasses-1,1);
            project2 = project2 - thresholds'*ones(1,size(project2,2));

            % Asignation of the class
            % f(x) = max {Wx-bk<0} or Wx - b_(K-1) > 0
            wx=project2;

            % The procedure for that is the following:
            % We assign the values > 0 to NaN
            wx(wx(:,:)>0)=NaN;

            % Then, we choose the biggest one.
            [maximum,predicted]=max(wx,[],1);

            % If a max is equal to NaN is because Wx-bk for all k is >0, so this
            % pattern belongs to the last class.
            predicted(isnan(maximum(:,:)))=numClasses;

            predicted = predicted';
        end

    end

end


%MDL_S4ABB2p8  Create kinematic model of ABB S4 2.8robot
%
% MDL_S4ABB2p8 is a script that creates the workspace variable s4 which
% describes the kinematic characteristics of an ABB S4 2.8 robot using
% standard DH conventions.
%
% Also defines the workspace vector:
%   q0   mastering position.
%
% Author::
%  Wynand Swart,
%  Mega Robots CC, P/O Box 8412, Pretoria, 0001, South Africa,
%  wynand.swart@gmail.com
%
% See also mdl_fanuc10l, mdl_m16, mdl_motormanHP6, mdl_irb140, mdl_puma560, SerialLink.

% MODEL: ABB, S4_2.8, S4 2.8m reach version, 6DOF, standard_DH

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for Matlab (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.

%Cell: 073-1555-430
%30 Sep 2007
%S4 ABB 2.8 robot

%            theta    d      a    alpha
clear L
L(1) = Link([ 0      0.9    0.188  -pi/2   0]);
L(2) = Link([ 0      0      0.95    0      0]);
L(3) = Link([ 0      0      0.225  -pi/2   0]);
L(4) = Link([ 0      1.705  0       pi/2   0]);
L(5) = Link([ 0      0      0      -pi/2   0]);
L(6) = Link([ 0      0.2    0      -pi/2   0]);
%##########################################################
%Pose 0; At SYNCHRONISATION position
%##########################################################
q0 = [0     -pi/2         0       0      0     -pi/2];
s4=SerialLink(L, 'name', 'S4 ABB 2.8');
%##########################################################


% sgwt_check_connected : Check connectedness of graph
%
% function r=sgwt_check_connected(A)
%
% returns 1 if graph is connected, 0 otherwise
% Uses boost graph library breadth first search
%
% Inputs :
% A - adjacency matrix
%
% Outputs :
% r - result
%

% This file is part of the SGWT toolbox (Spectral Graph Wavelet Transform toolbox)
% Copyright (C) 2010, David K. Hammond.
%
% The SGWT toolbox is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% The SGWT toolbox is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with the SGWT toolbox.  If not, see <http://www.gnu.org/licenses/>.

function r=sgwt_check_connected(A)
  d=bfs(A,1);
  r=~any(d==-1);


% This file is part of the following project:
% Oliver Parson, Siddhartha Ghosh, Mark Weal, Alex Rogers.
% Non-intrusive Load Monitoring using Prior Models of General Appliance Types.
% In: 26th AAAI Conference on Artificial Intelligence. Toronto, Canada. 2012.
% Code available for download: https://sites.google.com/site/oliparson/phd-work/research-files/aaai-2012-code.zip?attredirects=0
% Copyright: Oliver Parson et al., University of Southhampton, 2012.

% Modified by Romano Cicchetti, ETH Zurich, in the context of the NILM-Eval project

function [ bnet ] = make_hmm( init, emit_mean, emit_cov, trans )

%generic hmm
intra = [0 1; 0 0];
inter = [1 0; 0 0];
hstates = size(init,2); % discrete states
ostates = 1; % continuous
node_sizes = [hstates ostates];
observed_nodes = [2]; % per slice. all others are assumed hidden
discrete_nodes = [1]; % per slice. all others are assumed continuous
eclass1 = [1 2];
eclass2 = [3 2];
eclass = [eclass1 eclass2];
bnet = mk_dbn(intra, inter, node_sizes, ...
'discrete', discrete_nodes, 'observed', observed_nodes, ...
'eclass1', eclass1, 'eclass2', eclass2);

%model params
bnet.CPD{1} = tabular_CPD(bnet, 1, 'CPT', init);
bnet.CPD{2} = gaussian_CPD(bnet, 2, 'mean', emit_mean, 'cov', emit_cov);
bnet.CPD{3} = tabular_CPD(bnet, 3, 'CPT', trans);

end


%% PRIME_SCRIPT is a script to call PRIME_FUN.
%
%  Discussion:
%
%    The BATCH command runs scripts, not functions.  So we have to write
%    this short script if we want to work with BATCH!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2012
%
%  Author:
%
%    John Burkardt
%
  n = 10000000;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'PRIME_SCRIPT\n' );
  fprintf ( 1, '  Call PRIME_FUN to count prime numbers from 1 to %d\n', n );

  tic
  total = prime_fun ( n );
  toc


%% Example script to visualize the aircraft simulation data
% Add the path of the aircraft_3d_animation function
addpath('../src/');
% path of the *.mat file containing the 3d model information
model_info_file = '../3d_models/saab_gripen_3d_model.mat';
% Load the simulation data
% load('scissors_maneuver.mat')
% load('breakaway_maneuver.mat')
% load('split_s_maneuver.mat')
load('departure.mat')
% define the reproduction speed factor
speedx = 1;
% Do you want to save the animation in a mp4 file? (0.No, 1.Yes)
isave_movie = 0;
% Movie file name
movie_file_name = '';

% -------------------------------------------------------------------------
% The frame sample time shall be higher than 0.02 seconds to be able to
% update the figure (CPU/GPU constraints)
frame_sample_time = max(0.02, tout(2)-tout(1));
% Resample the time vector to modify the reproduction speed
t_new   = tout(1):frame_sample_time*(speedx):tout(end);
% Resample the recorded data
act     = interp1(tout, act, t_new','linear');
stick   = interp1(tout, stick, t_new','linear');
y_new   = interp1(tout, yout, t_new','linear');
% We have to be careful with angles with ranges
y_new(:, 7)  = atan2(interp1(tout, sin(yout(:, 7)), t_new','linear'), interp1(tout, cos(yout(:, 7)), t_new','linear')) * 180 / pi;
y_new(:, 8)  = atan2(interp1(tout, sin(yout(:, 8)), t_new','linear'), interp1(tout, cos(yout(:, 8)), t_new','linear')) * 180 / pi;
y_new(:, 9)  = atan2(interp1(tout, sin(yout(:, 9)), t_new','linear'), interp1(tout, cos(yout(:, 9)), t_new','linear')) * 180 / pi;
% Assign the data
heading_deg           =  y_new(:, 7);
pitch_deg             =  y_new(:, 8);
bank_deg              =  y_new(:, 9);
roll_command          = -stick(:, 2);
pitch_command         = -stick(:, 1);
angle_of_attack_deg   =  y_new(:, 2) * 180 / pi;
angle_of_sideslip_deg =  y_new(:, 3) * 180 / pi;
fligh_path_angle_deg  =  y_new(:, 22) * 180 / pi;
mach                  =  y_new(:, 21);
altitude_ft           = -y_new(:, 12);
nz_g                  =  y_new(:, 19);
% Flight control surfaces
le     = act(:, 9);
dr     = act(:, 8);
df1    = act(:, 6);
df2    = act(:, 5);
df3    = act(:, 4);
df4    = act(:, 3);
dfp    = 0.5 * (act(:, 1) + act(:, 2));
% Control array assignation
% (modify the order according to your particular 3D model)
controls_deflection_deg = [dfp(:), dfp(:), le(:), le(:), dr(:), 0.5*(df1(:)+df2(:)), 0.5*(df3(:)+df4(:))];

%% Run aircraft_3d_animation function
% -------------------------------------------------------------------------
aircraft_3d_animation(model_info_file,...
    heading_deg, ...            Heading angle [deg]
    pitch_deg, ...              Pitch angle [deg]
    bank_deg, ...               Roll angle [deg]
    roll_command, ...           Roll  stick command [-1,+1] [-1 -> left,            +1 -> right]
    pitch_command, ...          Pitch stick command [-1,+1] [-1 -> full-back stick, +1 -> full-fwd stick]
    angle_of_attack_deg, ...    AoA [deg]
    angle_of_sideslip_deg, ...  AoS [deg]
    fligh_path_angle_deg, ...   Flight path angle [deg]
    mach, ...                   Mach number
    altitude_ft, ...            Altitude [ft]
    nz_g,  ...                  Vertical load factor [g]
    controls_deflection_deg, ...Flight control deflection (each column is a control surface)
    frame_sample_time, ...      Sample time [sec]
    speedx, ...                 Reproduction speed
    isave_movie, ...            Save the movie? 0-1
    movie_file_name);           % Movie file name


function varargout = process_ersd_ab( varargin )
% PROCESS_ERSD_AB: Compute event related perturbation (synchrnonization / desynchrnonization)
%
% DESCRIPTION:
%    This function calculates event related perturbation (ERS/ERD) as the percentage of a
%    decrease or increase during a test interval (T), as compared to a reference interval (R).
%    The following formula is used: ERSP = (R-T)/R x 100.

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Nikola Vukovic, University of Cambridge, 2013
%          Francois Tadel, 2013-2015

eval(macro_method);
end

%% ===== GET DESCRIPTION =====
function sProcess = GetDescription() %#ok<DEFNU>
    % Description the process
    sProcess.Comment     = 'Event related perturbation (ERS/ERD) [DEPRECATED]';
    sProcess.FileTag     = 'ersd';
    sProcess.Category    = 'Filter2';
    sProcess.SubGroup    = 'Standardize';
    sProcess.Index       = 205;
    sProcess.Description = 'https://neuroimage.usc.edu/brainstorm/Tutorials/TimeFrequency#Normalized_time-frequency_maps';
    % Definition of the input accepted by this process
    sProcess.InputTypes  = {'data', 'results', 'timefreq', 'matrix'};
    sProcess.OutputTypes = {'data', 'results', 'timefreq', 'matrix'};
    sProcess.nInputs     = 2;
    sProcess.nMinFiles   = 1;
    % Default values for some options
    sProcess.isSourceAbsolute = 1;
    sProcess.processDim       = 1;    % Process channel by channel
    sProcess.isPaired         = 1;

    % Definition of the options
    sProcess.options.description.Comment = ['For each signal in input:
' ...
                                            '1) FilesA: Compute the mean m for the baseline
' ...
                                            '2) FilesB: For each time sample, calculates a percentage of increase/decrease
' ...
                                            'ERSD = (Data-m)/m x 100

'];
    sProcess.options.description.Type    = 'label';
    % === Baseline time window
    sProcess.options.baseline.Comment = 'Baseline (Files A):';
    sProcess.options.baseline.Type    = 'baseline';
    sProcess.options.baseline.Value   = [];
    % === Sensor types
    sProcess.options.sensortypes.Comment = 'Sensor types or names (empty=all): ';
    sProcess.options.sensortypes.Type    = 'text';
    sProcess.options.sensortypes.Value   = 'MEG, EEG';
    sProcess.options.sensortypes.InputTypes = {'data'};
end

%% ===== FORMAT COMMENT =====
function Comment = FormatComment(sProcess) %#ok<DEFNU>
    % Get baseline
    if isfield(sProcess.options, 'baseline') && isfield(sProcess.options.baseline, 'Value') && iscell(sProcess.options.baseline.Value) && ~isempty(sProcess.options.baseline.Value) && ~isempty(sProcess.options.baseline.Value{1})
        Time = sProcess.options.baseline.Value{1};
    else
        Time = [];
    end
    % Format comment
    if isempty(Time)
        Comment = 'Event related perturbation: [All file]';
    elseif any(abs(Time) > 2)
        Comment = sprintf('Event related perturbation: [%1.3fs,%1.3fs]', Time(1), Time(2));
    else
        Comment = sprintf('Event related perturbation: [%dms,%dms]', round(Time(1)*1000), round(Time(2)*1000));
    end
end

%% ===== RUN =====
function sInputB = Run(sProcess, sInputA, sInputB) %#ok<DEFNU>
    % Get options
    if isfield(sProcess.options, 'baseline') && isfield(sProcess.options.baseline, 'Value') && iscell(sProcess.options.baseline.Value) && ~isempty(sProcess.options.baseline.Value) && ~isempty(sProcess.options.baseline.Value{1})
        BaselineBounds = sProcess.options.baseline.Value{1};
    else
        BaselineBounds = [];
    end
    % Get baseline indices
    if ~isempty(BaselineBounds)
        iBaseline = bst_closest(BaselineBounds, sInputA.TimeVector);
        if (iBaseline(1) == iBaseline(2)) && any(iBaseline(1) == sInputA.TimeVector)
            error('Invalid baseline definition.');
        end
        iBaseline = iBaseline(1):iBaseline(2);
    % Get all file
    else
        iBaseline = 1:size(sInputA.A,2);
    end
    % Compute ERS/ERD
    sInputB.A = Compute(sInputA.A(:,iBaseline,:), sInputB.A);
    % Change DataType
    if ~strcmpi(sInputB.FileType, 'timefreq')
        sInputB.DataType = 'zscore';
    end
    % Default colormap
    if strcmpi(sInputB.FileType, 'results')
        sInputB.ColormapType = 'stat1';
    else
        sInputB.ColormapType = 'stat2';
    end
    % Add new tag to comment
    if isfield(sProcess.options, 'source_abs') && sProcess.options.source_abs.Value
        sInputB.Comment = [sInputB.Comment, ' | abs'];
    end
end

%% ===== COMPUTE =====
function B_data = Compute(A_baseline, B_data)
    disp('BST> process_ersd_ab.m is deprecated, use "Standardize > Baseline normalization" instead.');
    % Compute baseline statistics
    meanBaseline = mean(A_baseline, 2);
    % Remove null variance values
    meanBaseline(meanBaseline == 0) = 1e-12;
    % Compute event related perturbation
    B_data = bst_bsxfun(@minus, B_data, meanBaseline);
    B_data = bst_bsxfun(@rdivide, B_data, meanBaseline) .* 100;
end


function f = ddivE(Ekt, varargin)
% function f = ddivE(Ekt, varargin)
% Vxt = varargin{1};  %data
% Wxk = varargin{2};  %estimated H from NMF update
% alpha = varargin{3}; %rate

Vxt = varargin{1};  %data
Wxk = varargin{2};  %estimated values of W
alpha = varargin{3};

Hkt=exp(alpha*Ekt);
f = ddivergence(Vxt,Wxk*Hkt);


s = tf([1 0],1);
Fi = (s-1)/(s^2+3*s+2);
Fo = 1/(s+1);

F = Fi*Fo;
step(F)
bode(F)
% clear all
% t = 0:.001:100;
% thetastar(:,1) = t;
% thetastar(:,2) = .01*exp(.01*t);
% fstar(:,1) = t;
% fstar(:,2) = [(0:.001:10)*0 (10.001:.001:100)*0+.01];


classdef TestPerspectiveTransform
    %TestPerspectiveTransform

    methods (Static)
        function test_2d_numeric
            M = [cos(pi/4) sin(pi/4) 0 ; ...
                -sin(pi/4) cos(pi/4) 0 ; ...
                        0         0  1];
            % Nx2x1
            src = randn(10,2);
            dst = cv.perspectiveTransform(src, M);
            validateattributes(dst, {class(src)}, {'size',size(src)});
            % 1xNx2
            src = shiftdim(randn(10,2), -1);
            dst = cv.perspectiveTransform(src, M);
            validateattributes(dst, {class(src)}, {'size',size(src)});
            % Nx1x2
            src = permute(randn(10,2), [1 3 2]);
            dst = cv.perspectiveTransform(src, M);
            validateattributes(dst, {class(src)}, {'size',size(src)});
        end

        function test_3d_numeric
            M = eye(4);
            % Nx3
            src = randn(10,3);
            dst = cv.perspectiveTransform(src, M);
            validateattributes(dst, {class(src)}, {'size',size(src)});
            % 1xNx3
            src = permute(randn(10,3), [3 1 2]);
            dst = cv.perspectiveTransform(src, M);
            validateattributes(dst, {class(src)}, {'size',size(src)});
            % Nx1x3
            src = permute(randn(10,3), [1 3 2]);
            dst = cv.perspectiveTransform(src, M);
            validateattributes(dst, {class(src)}, {'size',size(src)});
        end

        function test_2d_cell
            M = eye(3);
            pts = num2cell(randn(10,2), 2);
            dst = cv.perspectiveTransform(pts, M);
            validateattributes(dst, {'cell'}, {'vector', 'numel',numel(pts)});
            cellfun(@(pt) validateattributes(pt, {'numeric'}, ...
                {'vector', 'numel',2}), dst);
        end

        function test_3d_cell
            M = eye(4);
            pts = num2cell(randn(10,3), 2);
            dst = cv.perspectiveTransform(pts, M);
            cellfun(@(pt) validateattributes(pt, {'numeric'}, ...
                {'vector', 'numel',3}), dst);
        end

        function test_error_argnum
            try
                cv.perspectiveTransform();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end


function cvx_optval = huber_pos( x, M, t ) %#ok

%HUBER   Internal cvx version.

%
% Check arguments
%

narginchk(1,3);
if ~cvx_isconvex( x ),
    error( 'Disciplined convex programming error:\n    HUBER_POS is convex and nondecreasing in X, so X must be convex.', 1 ); %#ok
end
if nargin < 2,
    M = 1;
elseif isa( M, 'cvx' ),
    error( 'Second argument must be numeric.' );
elseif ~isreal( M ) || any( M( : ) <= 0 ),
    error( 'Second argument must be real and positive.' );
end
if nargin < 3,
    t = 1;
elseif ~isreal( t ),
    error( 'Third argument must be real.' );
elseif cvx_isconstant( t ) && nnz( cvx_constant( t ) <= 0 ),
    error( 'Third argument must be real and positive.' );
elseif ~cvx_isconcave( t ),
    error( 'Disciplined convex programming error:\n    HUBER_POS is convex and nonincreasing in T, so T must be concave.', 1 ); %#ok
end
sz = cvx_size_check( x, M, t );
if isempty( sz ),
    error( 'Sizes are incompatible.' );
end

%
% Compute result
%

v = []; w = [];
cvx_begin separable
    variables v( sz ) w( sz )
    minimize( quad_over_lin( w, t, 0 ) + 2 .* M .* v )
    x <= w + v; %#ok
    w <= M * t; %#ok
    v >= 0; %#ok
cvx_end

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [x,y]=limits(a)
% LIMITS returns min & max values of matrix; else scalar value.
%
%   [lo,hi]=LIMITS(a) returns LOw and HIgh values respectively.
%
%   lim=LIMITS(a) returns 1x2 result, where lim = [lo hi] values

% Copyright 2004-2010 The MathWorks, Inc.

if nargin~=1 | nargout>2 %bogus syntax
  error('usage: [lo,hi]=limits(a)')
end

siz=size(a);

if prod(siz)==1 %scalar
  result=a;                         % value
else %matrix
  result=[min(a(:)) max(a(:))];     % limits
end

if nargout==1 %composite result
  x=result;                         % 1x2 vector
elseif nargout==2 %separate results
  x=result(1);                      % two scalars
  y=result(2);
else %no result
  ans=result                        % display answer
end


function results = vl_test_twister(varargin)
% VL_TEST_TWISTER
vl_test_init ;

function test_illegal_args()
vl_assert_exception(@() vl_twister(-1), 'vl:invalidArgument') ;
vl_assert_exception(@() vl_twister(1, -1), 'vl:invalidArgument') ;
vl_assert_exception(@() vl_twister([1, -1]), 'vl:invalidArgument') ;

function test_seed_by_scalar()
rand('twister',1) ; a = rand ;
vl_twister('state',1) ; b = vl_twister ;
vl_assert_equal(a,b,'seed by scalar + VL_TWISTER()') ;

function test_get_set_state()
rand('twister',1) ; a = rand('twister') ;
vl_twister('state',1) ; b = vl_twister('state') ;
vl_assert_equal(a,b,'read state') ;

a(1) = a(1) + 1 ;
vl_twister('state',a) ; b = vl_twister('state') ;
vl_assert_equal(a,b,'set state') ;

function test_multi_dimensions()
b = rand('twister') ;
rand('twister',b) ;
vl_twister('state',b) ;
a=rand([1 2 3 4 5]) ;
b=vl_twister([1 2 3 4 5]) ;
vl_assert_equal(a,b,'VL_TWISTER([M N P ...])') ;

function test_multi_multi_args()
a=rand(1, 2, 3, 4, 5) ;
b=vl_twister(1, 2, 3, 4, 5) ;
vl_assert_equal(a,b,'VL_TWISTER(M, N, P, ...)') ;

function test_square()
a=rand(10) ;
b=vl_twister(10) ;
vl_assert_equal(a,b,'VL_TWISTER(N)') ;


function [nn, L]  = nntrain(nn, train_x, train_y, opts, val_x, val_y)
%NNTRAIN trains a neural net
% [nn, L] = nnff(nn, x, y, opts) trains the neural network nn with input x and
% output y for opts.numepochs epochs, with minibatches of size
% opts.batchsize. Returns a neural network nn with updated activations,
% errors, weights and biases, (nn.a, nn.e, nn.W, nn.b) and L, the sum
% squared error for each training minibatch.

assert(isfloat(train_x), 'train_x must be a float');
assert(nargin == 4 || nargin == 6,'number ofinput arguments must be 4 or 6')

loss.train.e               = [];
loss.train.e_frac          = [];
loss.val.e                 = [];
loss.val.e_frac            = [];
opts.validation = 0;
if nargin == 6
    opts.validation = 1;
end

fhandle = [];
if isfield(opts,'plot') && opts.plot == 1
    fhandle = figure();
end

m = size(train_x, 1);

batchsize = opts.batchsize;
numepochs = opts.numepochs;

numbatches = m / batchsize;
numbatches

assert(rem(numbatches, 1) == 0, 'numbatches must be a integer');

L = zeros(numepochs*numbatches,1);
n = 1;
for i = 1 : numepochs
    tic;

    kk = randperm(m);
    for l = 1 : numbatches
        batch_x = train_x(kk((l - 1) * batchsize + 1 : l * batchsize), :);

        %Add noise to input (for use in denoising autoencoder)
        if(nn.inputZeroMaskedFraction ~= 0)
            batch_x = batch_x.*(rand(size(batch_x))>nn.inputZeroMaskedFraction);
        end

        batch_y = train_y(kk((l - 1) * batchsize + 1 : l * batchsize), :);

        nn = nnff(nn, batch_x, batch_y);
        nn = nnbp(nn);
        nn = nnapplygrads(nn);

        L(n) = nn.L;

        n = n + 1;
    end

    t = toc;

    if opts.validation == 1
        loss = nneval(nn, loss, train_x, train_y, val_x, val_y);
        str_perf = sprintf('; Full-batch train mse = %f, val mse = %f', loss.train.e(end), loss.val.e(end));
    else
        loss = nneval(nn, loss, train_x, train_y);
        str_perf = sprintf('; Full-batch train err = %f', loss.train.e(end));
    end
    if ishandle(fhandle)
        nnupdatefigures(nn, fhandle, loss, opts, i);
    end

    disp(['epoch ' num2str(i) '/' num2str(opts.numepochs) '. Took ' num2str(t) ' seconds' '. Mini-batch mean squared error on training set is ' num2str(mean(L((n-numbatches):(n-1)))) str_perf]);
    nn.learningRate = nn.learningRate * nn.scaling_learningRate;
end
end


% Test the annotation procedure, the annotation data is backed up so
% feel free to experiment.
%
% Author: Luca Vezzaro (elvezzaro@gmail.com)

clear all;

addpath('..');

training_files = dir('cootes/*.bmp.mat');

for i=1:numel(training_files)
	load(sprintf('cootes/%s', training_files(i).name));

	app = imread(sprintf('cootes/%s', training_files(i).name(1:end-4)));

	% Map RGB colors to [0,1]
	appearances(:,:,:,i) = double(app) ./ 255;

	shapes(:,:,i) = xy2ij(annotations, size(app,1));
end

load('cootes/triangulation.mat');

test = 23;
template = 22;

AAM = build_model_2d(shapes, appearances, 'triangulation', triangulation);
backup_file = sprintf('cootes/%s.bak', training_files(test).name);
copyfile(sprintf('cootes/%s', training_files(test).name), backup_file);

annotate(sprintf('cootes/%s', training_files(test).name(1:end-4)), sprintf('cootes/%s', training_files(template).name), 'AAM', AAM);

copyfile(backup_file, sprintf('cootes/%s', training_files(test).name));
delete(backup_file);


function lengths = grEdgeLength(nodes, edges, varargin)
%GREDGELENGTH  Compute length of edges in a geometric graph.
%
%   Deprecated, use 'grEdgeLengths' instead.
%
%   LENGTHS = grEdgeLength(NODES, EDGES)
%
%   Example
%   grEdgeLength
%
%   See also
%     grEdgeLenghts

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2014-01-13, using Matlab 7.9.0.529 (R2009b)
% Copyright 2014 INRA - Cepia Software Platform

warning('MatGeom:graphs:deprecated', ...
    'function grEdgeLength is obsolete, use grEdgeLengths instead');

nEdges = size(edges, 1);
lengths = zeros(nEdges, 1);

for iEdge = 1:nEdges
    ed = edges(iEdge, :);
    node1 = nodes(ed(1),:);
    node2 = nodes(ed(2),:);
    lengths(iEdge) = sqrt(sum((node1 - node2).^2));
end


function res = adjD(y)

res = zeros(size(y,1),size(y,2));

%y1 = ones(imsize)*y(1)/sqrt(prod(imsize));
%yx = (reshape(y(2:prod(imsize)+1), imsize(1), imsize(2)));
%yy = (reshape(y(prod(imsize)+2:end), imsize(1), imsize(2)));

res = adjDx(y(:,:,1)) + adjDy(y(:,:,2));

return;

function res = adjDy(x)
res = x(:,[1,1:end-1]) - x;
res(:,1) = -x(:,1);
res(:,end) = x(:,end-1);

function res = adjDx(x)
res = x([1,1:end-1],:) - x;
res(1,:) = -x(1,:);
res(end,:) = x(end-1,:);


function z_hat = estimate_states(uu, time, dt, p_drone, p_physics)
% ESTIMATE_STATES: Estimate the Drone states using gyros, accels, pressure
% sensors, and GPS.
% Inputs:
%   uu: input measurements
%   p_drone: drone parameters
%
% Outputs:
%   pnhat    - estimated North position,
%   pehat    - estimated East position,
%   hhat     - estimated altitude,
%   Vahat    - estimated airspeed,
%   alphahat - estimated angle of attack
%   betahat  - estimated sideslip angle
%   phihat   - estimated roll angle,
%   thetahat - estimated pitch angel,
%   chihat   - estimated course,
%   phat     - estimated roll rate,
%   qhat     - estimated pitch rate,
%   rhat     - estimated yaw rate,
%   Vghat    - estimated ground speed,
%   wnhat    - estimate of North wind,
%   wehat    - estimate of East wind
%   psihat   - estimate of heading angle
%

% Rename inputs
y_gyro_x      = uu(1);
y_gyro_y      = uu(2);
y_gyro_z      = uu(3);
y_accel_x     = uu(4);
y_accel_y     = uu(5);
y_accel_z     = uu(6);
y_static_pres = uu(7);
y_diff_pres   = uu(8);

y_gps_n       = uu(9);
y_gps_e       = uu(10);
y_gps_h       = uu(11);
y_gps_Vg      = uu(12);
y_gps_course  = uu(13);

% Persistent variables
persistent  kf1;
%persistent  kf2;
persistent x;
persistent kf_P;

% Initilize estimated states at t=0
if time == 0
    kf1.x_prev = [p_drone.phi_T, p_drone.theta_T]';
    kf1.P_prev = zeros(2,2);
    Vg_T = p_drone.Va_T;
    chi_T = p_drone.psi_T;
    %kf2.x_prev = [P.pn_T; P.pe_T; Vg_T; chi_T; P.wn; P.we; P.psi_T];
    %kf2.P_prev = zeros(7,7);
    phi_hat     = p_drone.phi_T;
    theta_hat   = p_drone.theta_T;
    x = [0; 0];
    kf_P = [0, 0;
        0  , 0];
end

% Not estimating these states
alphahat = 0;
betahat  = 0;
bxhat    = 0;
byhat    = 0;
bzhat    = 0;

pnhat=0;
pehat=0;
hhat=0;
Vahat=0;
alphahat=0;
betahat=0;
phihat=0;
thetahat=0;
chihat=0;
phat=0;
qhat=0;
rhat=0;
Vghat=0;
wnhat=0;
wehat=0;
psihat=0;
bxhat=0;
byhat=0;
bzhat=0;

%% Estimate states by low pass filtering

phat     = lp_filter( y_gyro_x, 0, p_drone.a_gyro, dt);
qhat     = lp_filter( y_gyro_y, 0, p_drone.a_gyro, dt);
rhat     = lp_filter( y_gyro_z, 0, p_drone.a_gyro, dt);

hhat     = (p_drone.pres0 - lp_filter( y_static_pres, 0, p_drone.a_static_pres, dt))/...
    (p_physics.rho*p_physics.gravity);
Vahat    = sqrt(2/p_physics.rho*lp_filter(y_diff_pres, 0, p_drone.a_diff_pres, dt));

%thetahat = asin(lp_filter(y_accel_x, 0, P.a_accel, P.dt)/p_physics.gravity);
%phihat   = atan(lp_filter(y_accel_y, 0, P.a_accel, P.dt)/lp_filter(y_accel_z, 0, P.a_accel, P.dt));

pnhat    = lp_filter(y_gps_n, 0, p_drone.a_gps_pos, dt);
pehat    = lp_filter(y_gps_e, 0, p_drone.a_gps_pos, dt);
chihat   = lp_filter(y_gps_course, 0, p_drone.a_gps_speed, dt);
Vghat    = lp_filter(y_gps_Vg, 0, p_drone.a_gps_speed, dt);

%% Estimated states by kalman filter

% Reconstruct theta and phi measured
% theta_m  = asin((y_accel_x)/p_physics.gravity);
% phi_m    = atan(y_accel_y/y_accel_z);
% last_measure1 = [ t       phi_m;           % not really t, but time of sensor measurement
%                   t       theta_m];        % not really t, ...
%
% input_z_hat = [phihat; thetahat; phat; qhat; rhat; Vahat];
% [F1, G1, H1, u1, Q1, R1] = ekf_phi_theta( input_z_hat, P);
% [kf1] = ext_kalman_filter(F1, G1, H1, u1, Q1, R1, t, last_measure1, kf1, P);

% Extract x to output variables
% phihat = kf1.x(1);
% thetahat = kf1.x(2);
x_prev = x;
phi_hat = x(1);
theta_hat = x(2);
kf_P_prev = kf_P;

z = [y_accel_x ; y_accel_y; y_accel_z];

% Compute f, F, h and H

cp = cos(phi_hat);
sp = sin(phi_hat);
ct = cos(theta_hat);
st = sin(theta_hat);
tt = tan(theta_hat);

p = y_gyro_x;
q = y_gyro_y;
r = y_gyro_z;
Va = Vahat;
g = p_physics.gravity;

f = [  p + q*sp*tt + r*cp*tt; ...
    q*cp - r*sp];

F = [  q*cp*tt - r*sp*tt,   (q*sp-r*cp)/(ct^2); ...
    - q*sp - r*cp,          0                ];

h = [ q*Va*st + g*st; ...
    r*Va*ct - p*Va*st - g*ct*sp; ...
    -q*Va*ct - g*ct*cp];

H = [  0,                 q*Va*ct + p_physics.gravity*ct;...
    - p_physics.gravity*cp*ct,   - r*Va*st - p*Va*ct + p_physics.gravity*sp*st;...
    p_physics.gravity*sp*ct,   (q*Va+p_physics.gravity*cp)*st               ];

%% Compute prediction (a priori)

x = x_prev+dt*f;    % a priori state estimate
kf_P = kf_P_prev + (F*kf_P_prev + kf_P_prev*F' + p_drone.Q);    % a priori estimate covariance

%% Compute update (a posteriori)

if(norm(z-p_physics.gravity) < 0.5)
    z_tilde = z - H*x;              % innovation (a priori residual)
    S = p_drone.R + H*kf_P*(H');    % innovation covariance
    K = kf_P*(H')/S;                % optimal kalman gain
    x = x + K*z_tilde;              % a posteriori state estimate
    kf_P = (eye(2) - K*H)*kf_P;     % a posteriori estimate covariance
end

z_res = z - H*x;                    % a posteriori residual

phihat      = x(1);
thetahat    = x(2);

%% Output data

z_hat = [
    pnhat;...
    pehat;...
    hhat;...
    0
    0
    0
    Vahat;...
    alphahat;...
    betahat;...
    phihat;...
    thetahat;...
    chihat;...
    phat;...
    qhat;...
    rhat;...
    Vghat;...
    wnhat;...
    wehat;...
    psihat;...
    bxhat;...
    byhat;...
    bzhat
    ];

end


function [sf,out2,out3] = symmetric_dirichlet_sym(in1,in2)
%SYMMETRIC_DIRICHLET_SYM
%    [SF,OUT2,OUT3] = SYMMETRIC_DIRICHLET_SYM(IN1,IN2)

%    This function was generated by the Symbolic Math Toolbox version 8.6.
%    24-Apr-2021 15:46:34

P1 = in2(:,1);
P2 = in2(:,2);
P3 = in2(:,3);
P4 = in2(:,4);
P5 = in2(:,5);
P6 = in2(:,6);
P7 = in2(:,7);
P8 = in2(:,8);
P9 = in2(:,9);
U1 = in1(:,1);
U2 = in1(:,2);
U3 = in1(:,3);
U4 = in1(:,4);
U5 = in1(:,5);
U6 = in1(:,6);
t2 = U1.*2.0;
t3 = U2.*2.0;
t4 = U3.*2.0;
t5 = U4.*2.0;
t6 = U5.*2.0;
t7 = U6.*2.0;
t8 = -P2;
t9 = -P3;
t10 = -P5;
t11 = -P6;
t12 = -P8;
t13 = -P9;
t15 = -U2;
t17 = -U3;
t20 = -U5;
t22 = -U6;
t24 = U1./2.0;
t25 = U2./2.0;
t26 = U3./2.0;
t27 = U4./2.0;
t28 = U5./2.0;
t29 = U6./2.0;
t14 = -t2;
t16 = -t3;
t18 = -t4;
t19 = -t5;
t21 = -t6;
t23 = -t7;
t30 = P1+t8;
t31 = P1+t9;
t32 = P4+t10;
t33 = P4+t11;
t34 = P7+t12;
t35 = P7+t13;
t36 = U1+t15;
t37 = U1+t17;
t38 = U4+t20;
t39 = U4+t22;
t40 = -t25;
t41 = -t26;
t42 = -t28;
t43 = -t29;
t44 = U2+U3+t14;
t45 = U5+U6+t19;
t46 = t30.^2;
t47 = t31.^2;
t48 = t32.^2;
t49 = t33.^2;
t50 = t34.^2;
t51 = t35.^2;
t52 = t2+t16;
t53 = t2+t18;
t54 = t5+t21;
t55 = t5+t23;
t56 = t36.^2;
t57 = t37.^2;
t58 = t38.^2;
t59 = t39.^2;
t66 = t24+t40;
t67 = t24+t41;
t68 = t25+t41;
t69 = t27+t42;
t70 = t27+t43;
t71 = t28+t43;
t72 = t30.*t31;
t73 = t32.*t33;
t74 = t34.*t35;
t75 = t36.*t37;
t76 = t38.*t39;
t77 = (t30.*t33)./2.0;
t78 = (t31.*t32)./2.0;
t79 = (t30.*t35)./2.0;
t80 = (t31.*t34)./2.0;
t81 = (t32.*t35)./2.0;
t82 = (t33.*t34)./2.0;
t83 = (t36.*t39)./2.0;
t84 = (t37.*t38)./2.0;
t60 = t46.*2.0;
t61 = t47.*2.0;
t62 = t48.*2.0;
t63 = t49.*2.0;
t64 = t50.*2.0;
t65 = t51.*2.0;
t85 = -t78;
t86 = -t80;
t87 = -t82;
t88 = -t84;
t89 = t56+t58;
t90 = t57+t59;
t91 = t46+t48+t50;
t92 = t47+t49+t51;
t93 = t75+t76;
t110 = t72+t73+t74;
t94 = t60+t62+t64;
t95 = t61+t63+t65;
t96 = t77+t85;
t97 = t79+t86;
t98 = t81+t87;
t99 = t83+t88;
t100 = t53.*t91;
t101 = t52.*t92;
t102 = t55.*t91;
t103 = t54.*t92;
t111 = t90.*t91;
t112 = t89.*t92;
t104 = t96.^2;
t105 = t97.^2;
t106 = t98.^2;
t107 = 1.0./t99.^2;
t108 = 1.0./t99.^3;
t113 = t94+t95;
t114 = t100+t101;
t115 = t102+t103;
t116 = t111+t112;
t109 = t107.^2;
t117 = t104+t105+t106;
t118 = 1.0./sqrt(t117);
t121 = t107.*t117;
t119 = (t91.*t118)./2.0;
t120 = (t92.*t118)./2.0;
t122 = t121+1.0;
t123 = (t94.*t118)./4.0;
t124 = (t95.*t118)./4.0;
t127 = (t100.*t118)./4.0;
t128 = (t101.*t118)./4.0;
t129 = (t102.*t118)./4.0;
t130 = (t103.*t118)./4.0;
t135 = t110.*t118;
t145 = (t113.*t118)./4.0;
t148 = (t114.*t118)./4.0;
t149 = (t115.*t118)./4.0;
t150 = (t116.*t118)./4.0;
t125 = -t123;
t126 = -t124;
t131 = -t127;
t132 = -t128;
t133 = -t129;
t134 = -t130;
t136 = -t135;
t137 = t135./2.0;
t147 = t93.*t135.*(-1.0./2.0);
t151 = t119.*t122;
t152 = t120.*t122;
t156 = t122.*t135.*(-1.0./2.0);
t173 = t66.*t108.*t117.*(t127-(t36.*t135)./2.0).*-2.0;
t174 = t67.*t108.*t117.*(t127-(t36.*t135)./2.0).*-2.0;
t175 = t68.*t108.*t117.*(t127-(t36.*t135)./2.0).*-2.0;
t176 = t66.*t108.*t117.*(t128-(t37.*t135)./2.0).*-2.0;
t177 = t67.*t108.*t117.*(t128-(t37.*t135)./2.0).*-2.0;
t178 = t68.*t108.*t117.*(t128-(t37.*t135)./2.0).*-2.0;
t179 = t70.*t108.*t117.*(t127-(t36.*t135)./2.0).*-2.0;
t180 = t71.*t108.*t117.*(t127-(t36.*t135)./2.0).*-2.0;
t181 = t69.*t108.*t117.*(t128-(t37.*t135)./2.0).*-2.0;
t182 = t71.*t108.*t117.*(t128-(t37.*t135)./2.0).*-2.0;
t183 = t67.*t108.*t117.*(t129-(t38.*t135)./2.0).*-2.0;
t184 = t68.*t108.*t117.*(t129-(t38.*t135)./2.0).*-2.0;
t185 = t66.*t108.*t117.*(t130-(t39.*t135)./2.0).*-2.0;
t186 = t68.*t108.*t117.*(t130-(t39.*t135)./2.0).*-2.0;
t187 = t69.*t108.*t117.*(t129-(t38.*t135)./2.0).*-2.0;
t188 = t70.*t108.*t117.*(t129-(t38.*t135)./2.0).*-2.0;
t189 = t71.*t108.*t117.*(t129-(t38.*t135)./2.0).*-2.0;
t190 = t69.*t108.*t117.*(t130-(t39.*t135)./2.0).*-2.0;
t191 = t70.*t108.*t117.*(t130-(t39.*t135)./2.0).*-2.0;
t192 = t71.*t108.*t117.*(t130-(t39.*t135)./2.0).*-2.0;
t193 = t67.*t108.*t117.*(t127-(t36.*t135)./2.0).*2.0;
t194 = t67.*t108.*t117.*(t128-(t37.*t135)./2.0).*2.0;
t195 = t71.*t108.*t117.*(t127-(t36.*t135)./2.0).*2.0;
t196 = t69.*t108.*t117.*(t128-(t37.*t135)./2.0).*2.0;
t197 = t71.*t108.*t117.*(t128-(t37.*t135)./2.0).*2.0;
t198 = t67.*t108.*t117.*(t129-(t38.*t135)./2.0).*2.0;
t199 = t69.*t108.*t117.*(t129-(t38.*t135)./2.0).*2.0;
t200 = t71.*t108.*t117.*(t129-(t38.*t135)./2.0).*2.0;
t201 = t69.*t108.*t117.*(t130-(t39.*t135)./2.0).*2.0;
t202 = t71.*t108.*t117.*(t130-(t39.*t135)./2.0).*2.0;
t203 = -t122.*(t135-t145);
t138 = -t137;
t139 = t36.*t137;
t140 = t37.*t137;
t141 = t38.*t137;
t142 = t39.*t137;
t143 = t44.*t137;
t144 = t45.*t137;
t146 = t93.*t137;
sf = -t122.*(t146-t150);
if nargout > 1
    t155 = t122.*t137;
    t157 = t125+t137;
    t158 = t126+t137;
    t163 = t136+t145;
    t166 = t147+t150;
    t153 = t119+t138;
    t154 = t120+t138;
    t159 = t131+t139;
    t160 = t132+t140;
    t161 = t133+t141;
    t162 = t134+t142;
    t164 = t143+t148;
    t165 = t144+t149;
    t171 = -t122.*(t123+t138);
    t172 = -t122.*(t124+t138);
    t219 = -t108.*t117.*(t146-t150);
    t220 = t108.*t117.*(t146-t150);
    out2 = [t122.*t164+t71.*t220.*2.0;t70.*t220.*-2.0-t122.*(t128-(t37.*t135)./2.0);t69.*t220.*2.0-t122.*(t127-(t36.*t135)./2.0);t122.*t165-t68.*t220.*2.0;t67.*t220.*2.0-t122.*(t130-(t39.*t135)./2.0);t66.*t220.*-2.0-t122.*(t129-(t38.*t135)./2.0)];
end
if nargout > 2
    t221 = t66.*t67.*t109.*t117.*(t146-t150).*-6.0;
    t222 = t66.*t68.*t109.*t117.*(t146-t150).*-6.0;
    t223 = t67.*t68.*t109.*t117.*(t146-t150).*-6.0;
    t224 = t66.*t69.*t109.*t117.*(t146-t150).*-6.0;
    t225 = t66.*t70.*t109.*t117.*(t146-t150).*-6.0;
    t226 = t67.*t69.*t109.*t117.*(t146-t150).*-6.0;
    t227 = t66.*t71.*t109.*t117.*(t146-t150).*-6.0;
    t228 = t67.*t70.*t109.*t117.*(t146-t150).*-6.0;
    t229 = t68.*t69.*t109.*t117.*(t146-t150).*-6.0;
    t230 = t67.*t71.*t109.*t117.*(t146-t150).*-6.0;
    t231 = t68.*t70.*t109.*t117.*(t146-t150).*-6.0;
    t232 = t68.*t71.*t109.*t117.*(t146-t150).*-6.0;
    t233 = t69.*t70.*t109.*t117.*(t146-t150).*-6.0;
    t234 = t69.*t71.*t109.*t117.*(t146-t150).*-6.0;
    t235 = t70.*t71.*t109.*t117.*(t146-t150).*-6.0;
    t236 = t66.*t67.*t109.*t117.*(t146-t150).*6.0;
    t237 = t67.*t68.*t109.*t117.*(t146-t150).*6.0;
    t238 = t66.*t69.*t109.*t117.*(t146-t150).*6.0;
    t239 = t66.*t71.*t109.*t117.*(t146-t150).*6.0;
    t240 = t67.*t70.*t109.*t117.*(t146-t150).*6.0;
    t241 = t68.*t69.*t109.*t117.*(t146-t150).*6.0;
    t242 = t68.*t71.*t109.*t117.*(t146-t150).*6.0;
    t243 = t69.*t70.*t109.*t117.*(t146-t150).*6.0;
    t244 = t70.*t71.*t109.*t117.*(t146-t150).*6.0;
    t167 = t122.*t153;
    t168 = t122.*t154;
    t204 = t66.*t108.*t117.*t164.*2.0;
    t205 = t67.*t108.*t117.*t164.*2.0;
    t206 = t68.*t108.*t117.*t164.*2.0;
    t207 = t69.*t108.*t117.*t164.*2.0;
    t208 = t70.*t108.*t117.*t164.*2.0;
    t209 = t66.*t108.*t117.*t165.*2.0;
    t210 = t67.*t108.*t117.*t165.*2.0;
    t211 = t69.*t108.*t117.*t165.*2.0;
    t212 = t70.*t108.*t117.*t165.*2.0;
    t213 = t71.*t108.*t117.*t165.*2.0;
    t245 = t173+t199+t238;
    t246 = t191+t194+t240;
    t248 = t156+t179+t196+t243;
    t249 = t156+t185+t198+t236;
    t250 = t193+t201+t219+t226;
    t251 = t176+t188+t220+t225;
    t169 = -t167;
    t170 = -t168;
    t214 = -t205;
    t215 = -t207;
    t216 = -t210;
    t217 = -t211;
    t218 = -t213;
    t252 = t178+t212+t219+t231;
    t253 = t200+t204+t219+t239;
    t247 = t206+t218+t242;
    t254 = t202+t214+t220+t230;
    t255 = t175+t217+t220+t241;
    out3 = [t203-t71.*t108.*t117.*t164.*4.0-t71.^2.*t109.*t117.*(t146-t150).*6.0;t172+t197+t208+t244;t171+t195+t215+t234;t247;t254;t253;t170+t197+t208+t244;t152-t70.^2.*t109.*t117.*(t146-t150).*6.0-t70.*t108.*t117.*(t128-(t37.*t135)./2.0).*4.0;t248;t252;t246;t251;t169+t195+t215+t234;t248;t151-t69.^2.*t109.*t117.*(t146-t150).*6.0+t69.*t108.*t117.*(t127-(t36.*t135)./2.0).*4.0;t255;t250;t245;t247;t252;t255;t203+t68.*t108.*t117.*t165.*4.0-t68.^2.*t109.*t117.*(t146-t150).*6.0;t172+t186+t216+t237;t171+t184+t209+t222;t254;t246;t250;t170+t186+t216+t237;t152-t67.^2.*t109.*t117.*(t146-t150).*6.0+t67.*t108.*t117.*(t130-(t39.*t135)./2.0).*4.0;t249;t253;t251;t245;t169+t184+t209+t222;t249;t151-t66.^2.*t109.*t117.*(t146-t150).*6.0-t66.*t108.*t117.*(t129-(t38.*t135)./2.0).*4.0];
end


function test_bug1481

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_componentanalysis ft_rejectcomponent ft_apply_montage

load(dccnpath('/home/common/matlab/fieldtrip/data/test/latest/raw/eeg/preproc_brainvision.mat'));

elec = ft_read_sens('standard_1020.elc');
data.elec = elec;

cfg = [];
cfg.reref = 'yes';
cfg.refchannel = 'all';
data_reref = ft_preprocessing(cfg,data);

cfg = [];
cfg.method = 'fastica';
cfg.numcomponent = 10; % to make it go fast
cfg.randomseed = 13; % so we get the same output each time
comp = ft_componentanalysis(cfg,data_reref);

% this step does not add a balancing matrix to the elec-description
cfg = [];
cfg.component = 2; % chosen randomly
rej1 = ft_rejectcomponent(cfg, comp, data_reref);

% create a montage rereference and call cfg.montage, then assess if
% ft_componentanalysis/ft_rejectcomponent still works correctly.
montage = [];
montage.tra = eye(numel(data.label))-ones(numel(data.label))./numel(data.label);
montage.labelold = data.label;
montage.labelnew = data.label;

cfg = [];
cfg.montage = montage;
data_reref2 = ft_preprocessing(cfg,data);


function [Population,Range] = EnvironmentalSelection(Population,RefPoint,Range,N)
% The environmental selection of AR-MOEA

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    CV = sum(max(0,Population.cons),2);
    if sum(CV==0) > N
        %% Selection among feasible solutions
        Population = Population(CV==0);
        % Non-dominated sorting
        [FrontNo,MaxFNo] = NDSort(Population.objs,N);
        Next = FrontNo < MaxFNo;
        % Select the solutions in the last front
        Last   = find(FrontNo==MaxFNo);
        Choose = LastSelection(Population(Last).objs,RefPoint,Range,N-sum(Next));
        Next(Last(Choose)) = true;
        Population = Population(Next);
        % Update the range for normalization
        Range(2,:) = max(Population.objs,[],1);
        Range(2,Range(2,:)-Range(1,:)<1e-6) = 1;
    else
        %% Selection including infeasible solutions
        [~,rank]   = sort(CV);
        Population = Population(rank(1:N));
    end
end

function Remain = LastSelection(PopObj,RefPoint,Range,K)
% Select part of the solutions in the last front

    N  = size(PopObj,1);
    NR = size(RefPoint,1);

    %% Calculate the distance between each solution and point
    Distance    = CalDistance(PopObj-repmat(Range(1,:),N,1),RefPoint);
    Convergence = min(Distance,[],2);

    %% Delete the solution which has the smallest metric contribution one by one
    [dis,rank] = sort(Distance,1);
    Remain     = true(1,N);
    while sum(Remain) > K
        % Calculate the fitness of noncontributing solutions
        Noncontributing = Remain;
        Noncontributing(rank(1,:)) = false;
        METRIC = sum(dis(1,:)) + sum(Convergence(Noncontributing));
        Metric = inf(1,N);
        Metric(Noncontributing) = METRIC - Convergence(Noncontributing);
        % Calculate the fitness of contributing solutions
        for p = find(Remain & ~Noncontributing)
            temp = rank(1,:) == p;
            noncontributing = false(1,N);
            noncontributing(rank(2,temp)) = true;
            noncontributing = noncontributing & Noncontributing;
            Metric(p) = METRIC - sum(dis(1,temp)) + sum(dis(2,temp)) - sum(Convergence(noncontributing));
        end
        % Delete the worst solution and update the variables
        [~,del] = min(Metric);
        temp    = rank ~= del;
        dis     = reshape(dis(temp),sum(Remain)-1,NR);
        rank    = reshape(rank(temp),sum(Remain)-1,NR);
        Remain(del) = false;
    end
end


% =========================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% Setup NIREP problem (data needs to be obtained
% separately)
%
% =========================================================================

dataset='na06';
example = ['3D-nirep-',dataset];
checkSetupDataFile; if OK, return; end;

% set view options and interpolation options
[viewer,viewOptn] = viewImage('reset','viewImage','imgmontage','colormap','gray(256)','direction','-zyx');

% setup interpolation scheme
imgOptn  = {'imgModel','splineInterMex','regularizer','moments','theta',.01};

% setup transformation used in the parametric part
traOptn  = {'trafo','affine3D'};

% setup distance measure
disOptn  = {'distance','SSD'};

% initialize the regularizer for the non-parametric part
regOptn = {'regularizer','mbHyperElastic','alpha',1,'alphaLength',1,'alphaArea',.1,'alphaVolume',2};

FAIRmessage(mfilename)
load('na01-128x150x128');
load('na01-128x150x128-labels');
load([dataset,'-128x150x128']);
load([dataset,'-128x150x128-labels']);
dataT = double(dataT);
dataR = double(dataR);
dataTl = double(dataTl);
dataRl = double(dataRl);
m = size(dataT);

dataT = 256.*dataT;
dataR = 256.*dataR;
max(dataT(:))

%omega = [0,m(1),0,m(2),0,m(3)];
omega = [0,20,0,23.4375,0,20];
ML = getMultilevel({dataT,dataR},omega,m,'fig',2);
save(outfile,'dataT','dataR','dataTl','dataRl','omega','m','ML');
save(outfile,'-append','viewOptn','imgOptn','traOptn','disOptn','regOptn');
checkSetupDataFile;

% xc       = getCellCenteredGrid(omega,m);
% viewData = @(I) viewImage(imgModel(I,omega,xc),omega,m);

% FAIRfigure(1,'figname',mfilename); clf;
% subplot(1,2,1); viewData(dataT); title('template');
% subplot(1,2,2); viewData(dataR); title('reference');


% Digital Video Stabilization and Rolling Shutter Correction using Gyroscopes
% Copyright (C) 2011 Alexandre Karpenko
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

function [ff td ts wd err] = solve_projective(gyro, frame_time, W, video_file)

gyro(:,[2 1]) = gyro(:,1:2);    % x & y are swapped
gyro(:,3) = -gyro(:,3);         % rotations about z
dft = diff(frame_time);
dT = diff(gyro(:,4));
theta = ((gyro(1:end-1,1:3) + gyro(2:end,1:3)) / 2) .* dT(:,[1 1 1]);
theta = [0 0 0; cumsum(theta, 1)];

ff = -1572;
td = -0.1564;
ts = 0;

alpha_f = 1e-12;
alpha_td = 1e-12;
alpha_ts = 1e-12;
alpha_wd = 1e-10;
wd = -mean(gyro(:,1:3),1);
%d = [0 0 0];        % gyro drift

% read corresponding movie
xyloObj = mmreader(['data/' video_file '.mov']);
num_frames = xyloObj.NumberOfFrames;
vh = xyloObj.Height;
vw = xyloObj.Width;

frame1 = read(xyloObj, 200);
frame2 = read(xyloObj, 201);

for iter = 1:5000
    for f=100:num_frames-1
        dE_df = 0;
        dE_dtd = 0;
        dE_dts = 0;

        err = 0;
        denom = 0;

        x = W(f).x1;
        y = W(f).x2;
        for i = 1:size(x,1)
            K = [1 0 -vw/2; 0 1 -vh/2; 0 0 ff];

            X = K*[x(i,:)'; 1];
            Y = K*[y(i,:)'; 1];

            tx = frame_time(f) - td - ts * X(2) / vh;
            ty = frame_time(f+1) - td - ts * Y(2) / vh;

            w = interp1(gyro(:,4), gyro(:,1:3), [tx ty], 'linear', 'extrap');
            dw = diff(w);
            dws = (w(2,:) * Y(2) - w(1,:) * X(2)) / vh;
            dth = diff(interp1(gyro(:,4), theta(:,1:3), [tx ty], 'linear', 'extrap'));

            Rx = [1            0           0;
                  0 cos(dth(1)) -sin(dth(1));
                  0 sin(dth(1))  cos(dth(1))];

            Ry = [ cos(dth(2)) 0 sin(dth(2));
                   0           1           0;
                  -sin(dth(2)) 0 cos(dth(2))];

            Rz = [cos(dth(3)) -sin(dth(3)) 0;
                  sin(dth(3))  cos(dth(3)) 0;
                  0           0            1];

            dRx =[0             0           0;
                  0 -sin(dth(1)) -cos(dth(1));
                  0 cos(dth(1))  -sin(dth(1))];

            dRy =[-sin(dth(2)) 0  cos(dth(2));
                   0           0            0;
                  -cos(dth(2)) 0 -sin(dth(2))];

            dRz =[-sin(dth(3)) -cos(dth(3)) 0;
                   cos(dth(3)) -sin(dth(3)) 0;
                   0           0            0];

            R = Rz * Ry * Rx;
            RY = R * Y;

            XXY_XYX = X'*X*RY' - X'*RY*X';
            dE_dts = dE_dts + 2 * (XXY_XYX) * (Rz * Ry * dRx * dws(1) + Rz * dRy * Rx * dws(2) + dRz * Ry * Rx * dws(3)) * Y;
            dE_dtd = dE_dtd + 2 * (XXY_XYX) * (Rz * Ry * dRx * dw(1) + Rz * dRy * Rx * dw(2) + dRz * Ry * Rx * dw(3)) * Y;
            dE_df = dE_df + 2*(RY'*RY)*X(3)*ff + 2*(X'*X)*(RY'*(R*[0;0;f])) - 2*(X'*RY)*(ff*RY(3) + X'*(R*[0;0;f]));
            %{
            if f == 200 && mod(iter-1,20) == 0
                figure(1); clf;
                h1 = imshow(frame2); hold on;
                h2 = imshow(imtransform(frame1, maketform('affine', M'), 'XData', [1 vid_width], 'YData', [1 vid_height]));
                set(h2, 'AlphaData', 0.6);
                hold off;
                pause(0.01);
            end
            %}
            err = err + (X'*X)*(Y'*Y) - (X'*RY)^2;
            denom = denom + 1;
        end

        err = err / denom;

        ff = ff + alpha_f * dE_df;
        %td = td - alpha_td * dE_dtd;
        %ts = ts - alpha_ts * dE_dts;

        fprintf('f: %f, td: %f, ts: %f, d: (%f, %f, %f), error: %f\n', ff, td, ts, 0, 0, 0, err);
    end
end


clear;
DIRNAME='C:\Benisil';

%Define the System and Generate Stationary Sensor Outputs
[SenErrDef, M, pva, imu_th, dt]=sys_def(DIRNAME);

%load Imu outputs
imu_data=readbin_v000([DIRNAME '\imu.bin'],size(M,1)+2);

%Imu model
[Aimu Bimu Cimu Dimu sPimu]=imu_modTI_v000(SenErrDef);

%compute u and self calibrate
%[u, ximu, Pimu, Pu, Pux]=selfcalib1_v000(imu_data(3:end,:), Aimu, Bimu*Bimu',Cimu, Dimu*Dimu', sPimu*sPimu', M, []);
sensor_ref=zeros(size(M,2),4);
sensor_ref(4:6,:)=ones(3,1)*[1 0 0.001 0.0001];
sensor_ref=[];
[u, ximu, Pimu, Pu, Pux]=selfcalib1_v001(imu_data(3:end,:), Aimu, Bimu*Bimu',Cimu, Dimu*Dimu', inv(sPimu*sPimu'), M, sensor_ref);

%[u1, ximu1, Pimu1, Pu1, Pux1]=selfcalib2_v000(imu_data(3:end,:), Aimu, Bimu*Bimu',Cimu, Dimu*Dimu', sPimu*sPimu', M, sensor_ref);


function [u,p] = StokesDGSRT0(A,B,u,p,f,g,itStep,auxMat)
%% STOKESDGS
%  Created by Ming Wang. Discussed with Long Chen.
% Reference:

Su = auxMat.trilA;
Bt = auxMat.Bt;
Sp = auxMat.Sp;
invMp = auxMat.invMp;

%% DGS relaxation step
for k = 1: itStep
    % Step 1: relax Momentum eqns twice
    for i = 1:2
        u = u + Su\(f-Bt*p-A*u);
    end
    % Step 2: relax Continuity eqns
    rp = g - B*u;
%     rp = rp - mean(rp);  Whether need this step ?????
    dq = Sp\rp;
    % Step 3: update u and p
    u = u + Bt*dq;
    p = p - invMp*rp;
end


function assert_grid(theta_start,theta_delta,theta_end,rho_start,rho_delta,rho_end,varargin)
% check for valid grid parameters

if check_option(varargin,'degree')
  stop = 180;
else
  stop = pi;
end

assert(theta_start>=0 && theta_start <= theta_end && theta_end<=stop &&...
  theta_delta > 0 && theta_delta <= theta_end - theta_start && ...
  rho_start>=0 && rho_start < rho_end && rho_end<=2*stop &&...
  rho_delta > 0 && rho_delta <= rho_end - rho_start);


function [p,q] = bandwidth(A)
%BANDWIDTH    Upper and lower bandwidth of matrix A
%
%   A   = 0  for  i-j > p  or j-i > q
%    ij
%
%    [p,q] = bandwidth(A)
%

% written  05/21/09     S.M. Rump
%

  [p,q] = bandwidth(reshape(A.t(1,:),A.size));


function F = triangles_from_strip(S)
  % TRIANGLES_FROM_STRIP Create a list of triangles from a stream of indices
  % along a strip.
  %
  % Inputs:
  %   S  #S list of indices
  % Outputs:
  %   F  #S-2 by 3 list of triangle indices
  %

  %F = zeros(numel(S)-2,3);
  %for s = 3:numel(S)
  %  if mod(s,2) == 0
  %    F(s-2,:) = fliplr([S(s-2) S(s-1) S(s)]);
  %  else
  %    F(s-2,:) = [S(s-2) S(s-1) S(s)];
  %  end
  %end

  S = S(:);
  % Maintain order and orientations
  F = [ ...
    S(1:2:end-2) S(2:2:end-1) S(3:2:end) ...
    S(3:2:end-1) S(2:2:end-2) S(4:2:end)];
  F = reshape(F',3,numel(S)-2)';

end