GNU Octave

function fileLength = get_file_length(fd)
% extracts file length in bytes from a file opened by fopen
% seek must be positioned at beginning on entry

seek = ftell(fd);
% move to end
fseek(fd, 0, 1);
% read end position
fileLength = ftell(fd);
% move to previous position
fseek(fd, seek, -1);
end

function [lev_vec, pos_vec] = md_idx_to_lev_pos (num_dims, max_lev, idx)

% disp(['md_idx_to_lev_pos max_lev : ', num2str(max_lev)]);

stride = 2^max_lev;

sd_idx = zeros(num_dims,1);

for d=1:num_dims
    
    sd_idx(d) = fix(mod((idx-1)/stride^(d-1),stride));
    
    [lev,pos] = sd_idx_to_lev_pos(sd_idx(d)+1);
    lev_vec(d) = lev;
    pos_vec(d) = pos;
    
end

assert(min(lev_vec)>=0);
assert(min(pos_vec)>=0);

assert(max(lev_vec)<max_lev,'not sure why this doesnt work, but just increase max_lev');

end


% function [lev_vec, pos_vec] = md_idx_to_lev_pos (num_dims, max_lev, idx)
% 
% if num_dims == 1
%     
%     [lev,pos] = sd_idx_to_lev_pos(idx);
%     
%     lev_vec(1) = lev;
%     pos_vec(1) = pos;
%     
% elseif num_dims == 2
%     
%     stride = prod(ones(num_dims-1,1)*(2^max_lev))
%     
%     sd_idx_1 = mod(idx-1,stride);
%     sd_idx_2 = (idx-1)/stride;
%     
%     [lev1,pos1] = sd_idx_to_lev_pos(sd_idx_1+1);
%     [lev2,pos2] = sd_idx_to_lev_pos(sd_idx_2+1);
%     
%     lev_vec(1) = lev1;
%     lev_vec(2) = lev2;
%     
%     pos_vec(1) = pos1;
%     pos_vec(2) = pos2;
%     
% elseif num_dims == 3
%     
%     stride = 2^max_lev;
%     
%     sd_idx = zeros(num_dims);
%     for d=1:num_dims
%         
%         sd_idx(1) = mod(idx-1,stride);
%         sd_idx(2) = mod((idx-1)/stride,stride);
%         sd_idx(3) = mod((idx-1)/stride^2,stride);
%         
%     end
%     
%     [lev1,pos1] = sd_idx_to_lev_pos(sd_idx_1+1);
%     [lev2,pos2] = sd_idx_to_lev_pos(sd_idx_2+1);
%     [lev3,pos3] = sd_idx_to_lev_pos(sd_idx_3+1);
%     
%     lev_vec(1) = lev1;
%     lev_vec(2) = lev2;
%     lev_vec(3) = lev3;
%     
%     pos_vec(1) = pos1;
%     pos_vec(2) = pos2;
%     pos_vec(3) = pos3;
%     
% end
% 
% end

:- import member/2 from lists.
hasEffect(privEscalation).

vulExists(Host, CVE, Prog):- 
  vuln_exists(Host, CVE, Prog, _Range, _Losstype, _Severity, _AC ).

vulExists(Host, CVE, Prog):- 
  vuln_exists(Host, CVE, Prog, _Range, _Losstype, _Severity, _AC, Port, Prot ).

vulProperty(CVE, localExploit, Effect):- 
  vuln_exists(_Host, CVE, _Prog, RangeList,Losstype, _Severity, _AC ),
  member(local,RangeList),
  lossTypetoEffect(Losstype, Effect).

vulProperty(CVE, remoteExploit, Effect, Port, Prot):- 
  vuln_exists(_Host, CVE, _Prog, RangeList,Losstype, _Severity, _AC, Port, Prot ),
  member(remoteExploit,RangeList),
  not  member(user_action_req,RangeList),
  lossTypetoEffect(Losstype, Effect).

vulProperty(CVE, remoteExploit, Effect):- 
  vuln_exists(_Host, CVE, _Prog, RangeList,Losstype, _Severity, _AC, _Port, _Prot ),
  member(remoteExploit,RangeList),
  not  member(user_action_req,RangeList),
  lossTypetoEffect(Losstype, Effect).


vulProperty(CVE, remoteClient, Effect):- 
  vuln_exists(_Host, CVE, _Prog, RangeList,Losstype, _Severity, _AC ),
  member(remoteExploit,RangeList),
  member(user_action_req,RangeList),
  lossTypetoEffect(Losstype, Effect).


lossTypetoEffect(Losstype, Effect):-
            member('data_modification', Losstype),
            hasEffect(Effect).

lossTypetoEffect(Losstype, Effect):-
            member('data_loss', Losstype),
            hasEffect(Effect).

cvss(CVE, AC):-
	vuln_exists(Host, CVE, Prog, _RangeList, _Losstype, _Severity, AC ).

cvss(CVE, AC):-
        vuln_exists(_Host, CVE, _Prog, RangeList,Losstype, _Severity, AC, Port, Prot ).

networkServiceInfo(Host, Program, Prot, Port, someUser) :-
	vulExists(Host, CVE, Program),
        vulProperty(CVE, remoteExploit, _Effect, Port, Prot).

%hacl(Host, internet, Prot, Port) :-
 %       vuln_exists(Host, CVE, _Prog, RangeList,Losstype, _Severity, _AC, Port, Prot ).

%hacl(internet, Host, Prot, Port) :-
 %      vuln_exists(Host, CVE, _Prog, RangeList,Losstype, _Severity, _AC, Port, Prot ).

%this is not corrent, because it binds the CVE Port and Prot together which couldn't be true.
%hacl(Host, Host2, Prot, Port) :-
 %      vuln_exists(Host, CVE, _Prog, RangeList,Losstype, _Severity, _AC, Port, Prot ),
  %     vuln_exists(Host2, CVE, _Prog, RangeList,Losstype, _Severity, _AC, Port, Prot ).

%% stroke max

figure
subplot(3,2,1)
title('stroke min')
hold on
subplot(3,2,2)
title('strokemin mod coefficient')
hold on
subplot(3,2,3)
hold on
subplot(3,2,4)
hold on
subplot(3,2,5)
hold on
subplot(3,2,6)
hold on

n_loc = round(Rds_steady_meanCIstd(1)*size(pitch_wb_L_bins,1));

subplot(3,2,1)
% plot((M_axis1_mean_wb),stroke_wb_L_bins(n_loc,:),'.k')
plot((M_axis1_mean_wb),stroke_wb_R_bins(n_loc,:),'.k')
plot(M_axis1_TorqueAxis1UP,stroke_wb_R_TorqueAxis1UP_bins(n_loc,:),'.b')

% plot trend
RC = strokeMOD_wb_R_TorqueAxis1UP_bins_meanCIstd(n_loc,1)/M_axis1_norm;
var0 = stroke_wb_steady_bins_meanCIstd(n_loc,1);
x0 = 0;
x1 = max(M_axis1_TorqueAxis1UP);
var1 = var0 + RC * (x1-x0);
xmin1 = min(M_axis1_TorqueAxis1UP);
varmin1 = var0 + RC * (xmin1-x0);
plot([xmin1 x1],[varmin1 var1],'-r','linewidth',2)
% xlabel('rotR Torque','fontsize',10)
ylabel('right wing','fontsize',10)

ang_min = -90;
ang_max = 0;
axis([xmin xmax ang_min ang_max])
set(gca,'YTick',ang_min:(ang_max-ang_min)/2:ang_max) 

subplot(3,2,3)
plot((M_axis1_mean_wb),stroke_wb_L_bins(n_loc,:),'.k')
% plot((M_axis1_mean_wb),stroke_wb_R_bins(n_loc,:),'.k')
plot((M_axis1_TorqueAxis1UP),stroke_wb_L_TorqueAxis1UP_bins(n_loc,:),'.b')

% plot trend
RC = strokeMOD_wb_L_TorqueAxis1UP_bins_meanCIstd(n_loc,1)/M_axis1_norm;
var0 = stroke_wb_steady_bins_meanCIstd(n_loc,1);
x0 = 0;
x1 = max(M_axis1_TorqueAxis1UP);
var1 = var0 + RC * (x1-x0);
xmin1 = min(M_axis1_TorqueAxis1UP);
varmin1 = var0 + RC * (xmin1-x0);
plot([xmin1 x1],[varmin1 var1],'-r','linewidth',2)
% xlabel('rotR Torque','fontsize',10)
ylabel('left wing','fontsize',10)

ang_min = -90;
ang_max = 0;
axis([xmin xmax ang_min ang_max])
set(gca,'YTick',ang_min:(ang_max-ang_min)/2:ang_max) 

subplot(3,2,5)
plot((M_axis1_mean_wb),Dstroke_wb_bins(n_loc,:),'.k')
% plot((M_axis1_mean_wb),stroke_wb_R_bins(n_loc,:),'.k')
plot((M_axis1_TorqueAxis1UP),Dstroke_wb_TorqueAxis1UP_bins(n_loc,:),'.b')

% plot trend
RC = DstrokeMOD_wb_TorqueAxis1UP_bins_meanCIstd(n_loc,1)/M_axis1_norm;
var0 = 0;
x0 = 0;
x1 = max(M_axis1_TorqueAxis1UP);
var1 = var0 + RC * (x1-x0);
xmin1 = min(M_axis1_TorqueAxis1UP);
varmin1 = var0 + RC * (xmin1-x0);
plot([xmin1 x1],[varmin1 var1],'-r','linewidth',2)
xlabel('rotR Torque','fontsize',10)
ylabel('R - L','fontsize',10)

ang_min = -45;
ang_max = 45;
axis([xmin xmax ang_min ang_max])
set(gca,'YTick',ang_min:(ang_max-ang_min)/2:ang_max) 

%% coeff
% right
subplot(3,2,2)
plot((M_axis1_TorqueAxis1UP),strokeMOD_wb_R_TorqueAxis1UP_bins(n_loc,:),'.b')

RC = strokeMOD_wb_R_TorqueAxis1UP_bins_meanCIstd(n_loc,1);
var0 = 0;
x0 = 0; 
x1 = max(M_axis1_TorqueAxis1UP); 
var1 = RC
xmin1 = min(M_axis1_TorqueAxis1UP); 
varmin1 = RC
plot([xmin1 x1],[varmin1 var1],'-r','linewidth',2)
% xlabel('rotR Torque','fontsize',10)
% ylabel('strokemax mod coefficient','fontsize',10)

ang_min = -45;
ang_max = 45;
axis([xmin xmax ang_min ang_max])
set(gca,'YTick',ang_min:(ang_max-ang_min)/2:ang_max) 

% Left
subplot(3,2,4)
plot((M_axis1_TorqueAxis1UP),strokeMOD_wb_L_TorqueAxis1UP_bins(n_loc,:),'.b')

RC = strokeMOD_wb_L_TorqueAxis1UP_bins_meanCIstd(n_loc,1);
var0 = 0;
x0 = 0; 
x1 = max(M_axis1_TorqueAxis1UP); 
var1 = RC
xmin1 = min(M_axis1_TorqueAxis1UP); 
varmin1 = RC
plot([xmin1 x1],[varmin1 var1],'-r','linewidth',2)
% xlabel('rotR Torque','fontsize',10)
% ylabel('strokemax mod coefficient','fontsize',10)

ang_min = -45;
ang_max = 45;
axis([xmin xmax ang_min ang_max])
set(gca,'YTick',ang_min:(ang_max-ang_min)/2:ang_max) 

% right-Left
subplot(3,2,6)
plot((M_axis1_TorqueAxis1UP),DstrokeMOD_wb_TorqueAxis1UP_bins(n_loc,:),'.b')

RC = DstrokeMOD_wb_TorqueAxis1UP_bins_meanCIstd(n_loc,1);
var0 = 0;
x0 = 0; 
x1 = max(M_axis1_TorqueAxis1UP); 
var1 = RC
xmin1 = min(M_axis1_TorqueAxis1UP); 
varmin1 = RC
plot([xmin1 x1],[varmin1 var1],'-r','linewidth',2)
xlabel('rotR Torque','fontsize',10)
% ylabel('strokemax mod coefficient','fontsize',10)

ang_min = -45;
ang_max = 45;
axis([xmin xmax ang_min ang_max])
set(gca,'YTick',ang_min:(ang_max-ang_min)/2:ang_max) 

%CE_BIG Parameter script for the Centauro Big Motor with 2300 Nm/rad torsion bar
%
%
% Notes::
%  All inertiae/damping is reflected to link side using n^2
%
% Author::
%  Joern Malzahn
%  Wesley Roozing
%
% See also genericJoint, full_dyn, WMBig10k.

% Copyright (C) 2016, by Joern Malzahn, Wesley Roozing
%
% This file is part of the Compliant Joint Toolbox (CJT).
%
% CJT 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.
%
% CJT 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 CJT. If not, see <http://www.gnu.org/licenses/>.
%
% For more information on the toolbox and contact to the authors visit
% <https://github.com/geez0x1/CompliantJointToolbox>

n = 100;
% Inertiae
params.('I_m')      = 3.04E-005 * n^2;              %% Motor rotor inertia [kg m^2]
params.('I_g')      = 1.33 * 6.6944E-005 * n^2;     %% Gear inertia [kg m^2] % TO BE UPDATED!
params.('I_b')      = 8.6719E-006 * n^2;            %% Torsion bar inertia [kg m^2] % TO BE UPDATED!
% Stiffnesses
params.('k_g')      = 31e3;             	%% Gearbox stiffness [Nm/rad]
params.('k_b')      = 31e3;                 %% Torsion bar stiffness [Nm/rad] % TO BE UPDATED! The strain based torque sensor is super stiff.
% Linear viscous friction
params.('d_m')      = 0.0064;               %% Motor Damping [Nms/rad]
params.('d_g')      = 6.0;                  %% Gearbox damping [Nms/rad]   % TO BE UPDATED!
params.('d_b')      = 7.0;                  %% Torsion bar damping [Nms/rad] % TO BE UPDATED!
% Asymmetric viscous friction
params.('d_m_n')    = 0.0064;               %% Motor Damping - negative direction [Nms/rad]
params.('d_g_n')    = 5.0;                  %% Gearbox Damping - negative direction [Nms/rad] % TO BE UPDATED!
params.('d_b_n')    = 5.0;                  %% Torsion bar damping - negative direction [Nms/rad] % TO BE UPDATED!
% Linear internal viscous friction
params.('d_mg')     = 144.0;                %% Gearbox internal damping [Nms/rad]     % TO BE UPDATED!
params.('d_gb')     = 2.0;                  %% Torsion bar internal damping [Nms/rad] % TO BE UPDATED!
% Coulomb friction
params.('d_cm')     = 3.2;                 	%% Motor Coulomb damping [Nm]  
params.('d_cg')     = 3.5;            	    %% Gearbox Coulomb damping [Nm] % TO BE UPDATED!
params.('d_cb')     = 0.0;                  %% Torsion bar Coulomb damping [Nm] % TO BE UPDATED!
% Asymmetric Coulomb friction
params.('d_cm_n')   = 0.0;                  %% Motor Coulomb damping - negative direction [Nm]
params.('d_cg_n')   = 0.0;                  %% Gearbox Coulomb damping - negative direction [Nm]
params.('d_cb_n')   = 0.0;                  %% Torsion bar Coulomb damping - negative direction [Nm]
% Cogging
params.('cog_a1')   = 1e-2 * n;             %% Cosine amplitude [Nm]
params.('cog_a2')   = 0 * n;                %% Sine amplitude [Nm]
params.('cog_f')    = 6;                    %% Spatial frequency [periods/revolution]
% Harmonic drive torque ripple
params.('rip_a1')   = 2.0;                  %% Cosine amplitude [Nm]
params.('rip_a2')   = 0.0;                  %% Sine amplitude [Nm]
params.('rip_f')    = 2;                    %% Spatial frequency [periods/revolution]
% Misc
params.('n')        = n;                    %% Gear ratio []
params.('k_t')      = 0.0455;               %% Torque constant [Nm/A]
params.('r')        = 0.099;                %% Armature resistance [Ohm]
params.('x')        = 0;                    %% Armature inductance [H]
params.('Ts')       = 1E-3;                 %% Sampling time [s]
% Operating/max conditions
params.('v_0')     	= 24;					%% Operating voltage [V]
params.('i_c')      = 20;					%% Max. continuous current [A]
params.('i_p')		= 20; 					%% Peak current [A]
params.('dq_c')     = 5.86;					%% Max. continuous speed (output) [rad/s]
params.('dq_p')		= 5.86; 		     	%% Max. peak speed (output) [rad/s]

%% Initialize
% clc; clear all; 
close all;

%% World
inc = 0.01;     % cn resolution
xDim = 3/inc; 	%2m
yDim = 3/inc;   %2m
world = zeros(xDim,yDim);

numObs = 3;
obsSize = [20 40];

armBase = [ round(xDim/2) round(yDim/2)];
armLen = [70 50];
armAng = [10 30];

armPos = updateArmPos(armBase, armLen, armAng(:));
obstacles = zeros(numObs,4);

% for i=1:numObs
%     rad = round(obsSize(1) + (obsSize(2)-obsSize(1))*rand(1,1));
%     origX = round(10 + (xDim-20)*rand(1,1));
%     origY = round(10 + (yDim-20)*rand(1,1));
%     
%     obstacles(i,:) = [ origX-rad origX+rad origY-rad origY+rad ];
% end

obstacles(1,:) = [220 250 170 200];
obstacles(2,:) = [150 180 225 240];
obstacles(3,:) = [150 180  50  75];
obstacles(4,:) = [ 60  80 125 175];


for obs=1:length(obstacles(:,1))
    for i=1:xDim
        for j=1:yDim
            if(i > obstacles(obs,1) && i < obstacles(obs,2) && j > obstacles(obs,3) && j < obstacles(obs,4))
                world(i,j) = 1;
            end
        end
    end
end



% plotWorld(world, armPos);

%% Calcular Configuration Space
% if(~exist('cSpace','var'))
    cSpaceLimits = [1 360 1 360];
    [cSpace F] = buildCspace(armBase, armLen, world, cSpaceLimits);
% end

%% Plots
% plotWorld(world, armPos);
% plotCspace( cSpace );
plotAll( world, armPos, cSpace );

close all
clear

photo{1} = imread('lena_std.tif');
photo{2} = imread(['ImageDB-432parts-MIT',filesep,'8.png']);
photo{3} = imread(['ImageDB-540parts-McGill',filesep,'16.jpg']);
photo{4} = imread(['ImageDB-540parts-McGill',filesep,'14.jpg']);
photo{5} = imread(['ImageDB-540parts-McGill',filesep,'2.jpg']);

for casenum = 1:numel(photo)
    
    image = photo{casenum};
    blocksize = 64;
    image = im2double(image);
    [imageblocks,M,N] = disorg(image,blocksize);
    n = M * N;
    [x1,y1] = meshgrid(1:N,1:M);
    puzzleimage = jointblocks(imageblocks,x1(:),y1(:));
    hold on
    stop = uicontrol('style','toggle','string','Stop','background','white');
    
    D = MGC_compute_cvx3(imageblocks);
    A = MGC2A(D,M,N);
    p = ones(n^2,1)/n;
    Active = true(n^2,1);
    
    Aeq1 = double(bsxfun(@eq,(1:n).',reshape(repmat(1:n,n,1),1,[])));
    Aeq2 = repmat(eye(n),1,n);
    Aeq = [Aeq1; Aeq2];
    Aeq(end,:) = [];
    Aineq = sparse(eye(n^2));
    
    max_iter = 500;
    iternum = 0;
    
    while (~get(stop,'value') && iternum < max_iter)
        
        iternum = iternum + 1;
        
        d = A * p;
        MT = [Aineq(p<1e-5,:); Aeq];
        if det(MT*MT.') == 0
            break
        end
        K = eye(n^2) - MT.'*inv(MT*MT.')*MT;%#ok
        s = K * d;
        
        t = (1-p)./s.*(s>=0) - p./s.*(s<0);
        step = min(t(Active));
        p = p + step .* s .* Active;
        Active(t==step) = false;
        
        P = reshape(p,n,n);
        [x,y] = P2xy(P,M,N);
        jointimage = jointblocks(imageblocks,x,y,'hold');
        title([num2str(iternum) ' Iterations']);
        drawnow;
        
    end
    
    stop = uicontrol('style','pushbutton','string','Continue','callback','close all');
    drawnow;
    uiwait(gcf);
end

function [bloom_number, bloom, base, left_peak,right_peak, bloom_ownership, ...
    odd_level, even_level, candidates, bridges] = ...
    CREATE_BLOOM(create_bloom_struct)

graph = create_bloom_struct.graph;
search_level = create_bloom_struct.search_level;
bloom_number = create_bloom_struct.bloom_number;
bloom = create_bloom_struct.bloom;
bloom_ownership = create_bloom_struct.bloom_ownership;
left_peak = create_bloom_struct.left_peak;
right_peak = create_bloom_struct.right_peak;
base = create_bloom_struct.base;
bottleneck = create_bloom_struct.bottleneck;
init_right = create_bloom_struct.init_right;
init_left = create_bloom_struct.init_left;
ownership = create_bloom_struct.ownership;
even_level = create_bloom_struct.even_level;
odd_level = create_bloom_struct.odd_level;
candidates = create_bloom_struct.candidates;
anomalies = create_bloom_struct.anomalies;
bridges = create_bloom_struct.bridges;
% define bloom: give a number, assign it left_peak,
% right_peak, and base.
level = min(even_level,odd_level);
bloom_number = bloom_number + 1; % add new bloom.
bloom_vertices = find(ownership); % all things in trees are bloom.



base(bloom_number) = bottleneck;
left_peak(bloom_number) = init_left;
right_peak(bloom_number) = init_right;

index = @ (search_level) search_level+1;
1;
for y = bloom_vertices % assign other levels and find extra bridges
    bloom(y) = bloom_number;
    bloom_ownership(y) = ownership(y);
    if mod(level(y),2)==0 % y is outer
        odd_level(y) = 2*search_level + 1 - ...
            even_level(y);
    else % y inner
        even_level(y) = 2 * search_level + 1 - ...
            odd_level(y);
        candidates{index(even_level(y))} = ...
            [candidates{index(even_level(y))}, y];
        for z = anomalies{y}
            j = (even_level(y) + even_level(z)) / 2;
            bridges{index(j)} = [bridges{index(j)}, graph.get_e_from_vs(y,z)];
            % I don't understand why the algorithm requires us to mark y,z
            % used for future DDFS. I am ignoring for now, hoping it is not
            % necessary! TODO.
        end
    end
end

end

function [match,total_val]=Hiuristic_staff(agent2conf,all_conf)

    global targetsData;
    global allConf;
    allConf = all_conf;
    VAL_COL=3;
        
    %update each conf with its value
    % where c is index of conf and t index of target
    for c=1:size(agent2conf,2)
        conf_val=0;
        for t=1:size(allConf,1)
            if allConf(t,c)>0
                conf_val=conf_val+(8-targetsData(t,VAL_COL));
            end
        end
        for a=1:size(agent2conf,1)
            if agent2conf(a,c)>0
                agent2conf(a,c)=conf_val;
            end
        end
    end
    
    % here we assign for each drone his best conf
    % can think of another starting point
    match=zeros(size(agent2conf,1),1);
    for a=1:size(agent2conf,1)
        maximum=find(agent2conf(a,:)==max(agent2conf(a,:)));
        match(a)= maximum(1); %first maximum
    end
    %match=b_match;
    total_val=calculate_assign_value(match);

    %another starting point
    hiuristic_match=new_start(agent2conf);
    val_hiuristic_first_match=calculate_assign_value(hiuristic_match);
  
    % check which is better starting point
    if val_hiuristic_first_match>    total_val
        total_val=val_hiuristic_first_match;
        match=hiuristic_match;
    end
        
    %the improvment
    continue_param=1;
    counter=0;
    new_match=match;
    while (continue_param && counter<5000)
        continue_param=0;
        for a=1:size(agent2conf,1)
        %for a=size(agent2conf,1):-1:1
            agent_conf=find(agent2conf(a,:)>0);
            for i=1:size(agent_conf,2) %run over all new possible confs
                new_match(a)=agent_conf(i);
                new_val= calculate_assign_value(new_match);
                if new_val>total_val
                    total_val=new_val;
                    match=new_match;
                    continue_param=1;
                end
            end
        end
        counter=counter+1;
        total_val;
    end
end  

 function [val]=calculate_assign_value(match)
    VAL_COL=3;
    global targetsData;
    global allConf;
    choosen_conf= allConf(:,match');
    targets=sum(choosen_conf,2)>0;
    val=(8-targetsData(:,VAL_COL)') * targets;     
 end
        
        
 function [match]=new_start(agent2conf)
     global allConf;
     match=zeros(size(agent2conf,1),1);
    for a=1:size(agent2conf,1)
        maximum=find(agent2conf(a,:)==max(agent2conf(a,:)));
        maximum=maximum(1);
        targets_of_best_conf=allConf(:,maximum);
        match(a)= maximum; %first maximum
        for c=1:size(allConf,2)
            if allConf(:,c)'*targets_of_best_conf>0
                agent2conf(:,c)=0;
            end
        end
    end
 end
 
%  function [best_match]=new_start_point_to_hiuristic_2(agent2conf,allConf)
%     num=1:size(agent2conf,1);
%     all_permutation=perms(num);
%     match=zeros(size(agent2conf,1),1);
%     best_match=match;
%     val_best=0;
%     for p=1:size(all_permutation,1)
%         permutation=all_permutation(p,:);
%         match=zeros(size(agent2conf,1),1);
%         agent2conf_temp=agent2conf;
%         for a=permutation
%             maximum=find(agent2conf_temp(a,:)==max(agent2conf_temp(a,:)));
%             maximum=maximum(1);
%             targets_of_best_conf=allConf(:,maximum);
%             match(a)= maximum;%first maximum
%             for c=1:size(allConf,2)
%                 if allConf(:,c)'*targets_of_best_conf>0
%                     agent2conf_temp(:,c)=0;
%                 end
%             end
%         end
%         new_val=calculate_assign_value(match)
%         if val_best<new_val
%             best_match=match;
%             val_best=new_val
%         end
%     end
%  end
%  

a = 1;
b = 3;
f = @(x) (100./(x.^2)) .* sin(10./x);

eps = 1e-4;
exact = -1.4260247818;

n1 = 50;
n2 = 100;

aq1 = adquad(a,b, f, eps,n1)
aq2 = adquad(a,b, f, eps,n2)

s1 = simpson(f, a, b, n1)
s2 = simpson(f,a,b,n2)

set(0,'DefaultFigureWindowStyle','docked')


lapsim.plotpooper=false;

%% 1. Chassis Initialization

% Structural
chassis.wheelbase= 1.524; % [m]
chassis.fronttrack = 1.2192; % [m]
chassis.reartrack = 1.1684; % [m]


% Mass Distribution
chassis.mass.totalmass = 275; % [kg]
chassis.mass.totalmassdistribution = 50; % [%] weight on front tires
chassis.mass.frontnonsusmass = 12; % [kg] front non suspended mass per wheel
chassis.mass.rearnonsusmass = 13; % [kg] rear non suspended mass per wheel
chassis.mass.susmass = chassis.mass.totalmass - 2*chassis.mass.frontnonsusmass - 2*chassis.mass.rearnonsusmass;


chassis.mass.a = chassis.wheelbase*chassis.mass.totalmassdistribution/100;
chassis.mass.b = chassis.wheelbase*(100-chassis.mass.totalmassdistribution)/100;

chassis.mass.cg = 294; % [mm] total mass cg height
chassis.mass.frontnonsusmassheight = 260; % [mm] front non suspended mass height
chassis.mass.rearnonsusmassheight = 260; % [mm] rear non suspended mass height

chassis.mass.susmassrollinertia = 200; % [kg.m^2] suspended mass roll inertia (ref SM CG - Ixx)

% Spring & ARB stiffnesses
chassis.spring.fspringstiff = 26.3; % [N/mm] front spring stiffness
chassis.spring.rspringstiff = 35.03; % [N/mm] rear spring stiffness
chassis.arb.fARBstiff = 19.27; % [N/mm]
chassis.arb.rARBstiff = 19.91; % [N/mm]


% Motion Ratios
chassis.spring.fspringMR = 1.25; % [mm/mm] front spring motion ratio
chassis.spring.rspringMR = 1.3; % [mm/mm] rear spring motion ratio
chassis.arb.fARBMR = 2.250; % [mm/mm]
chassis.arb.rARBMR = 2.250; % [mm/mm]
chassis.spring.FWR = chassis.spring.fspringstiff/chassis.spring.fspringMR^2; %Front Wheel Rate (N/mm)
chassis.spring.RWR = chassis.spring.rspringstiff/chassis.spring.rspringMR^2; %Rear Wheel Rate (N/mm)
% Roll
chassis.roll.fRCheight = 15; % [mm] front roll center height
chassis.roll.rRCheight = 40; % [mm] rear roll center height
chassis.roll.SMCGheight = (chassis.mass.totalmass*chassis.mass.cg-(2*chassis.mass.frontnonsusmass*chassis.mass.frontnonsusmassheight+2*chassis.mass.rearnonsusmass*chassis.mass.rearnonsusmassheight))/chassis.mass.susmass;

chassis.roll.AR_FrontSpring = ((chassis.fronttrack.^2)*tan(deg2rad(1)) * chassis.spring.FWR * 1000)/2; %Anti-roll stiffness from front spring (Nm/deg)
chassis.roll.AR_RearSpring = ((chassis.reartrack.^2)*tan(deg2rad(1)) * chassis.spring.RWR * 1000)/2; %Anti-roll stiffness from rear spring (Nm/deg)

chassis.roll.AR_FrontARB = ((chassis.arb.fARBstiff*1000)*(chassis.fronttrack.^2)*tan(deg2rad(1)))/(chassis.arb.fARBMR.^2); %Anti-roll stiffness from front ARB (Nm/deg)
chassis.roll.AR_RearARB = (chassis.arb.rARBstiff*1000* (chassis.reartrack.^2)*tan(deg2rad(1)))/(chassis.arb.rARBMR.^2); %Anti-roll stiffness from rear ARB (Nm/deg)
chassis.roll.AR_Total = chassis.roll.AR_FrontSpring + chassis.roll.AR_RearSpring + chassis.roll.AR_FrontARB + chassis.roll.AR_RearARB; %Total Anti-roll stiffness
chassis.roll.dz = chassis.roll.SMCGheight -(((chassis.roll.rRCheight-chassis.roll.fRCheight)/(chassis.wheelbase*1000) * ((1-(chassis.mass.totalmassdistribution/100))*(chassis.wheelbase*1000))) + chassis.roll.fRCheight); % Z distance from SM CG to roll axis at the SM CG

% Aerodynamics
chassis.aero.DFcoeff = 0; % [] down force coefficient
chassis.aero.DFdist = 45; % [%] front down force distribution
chassis.aero.airdensity = 1.2255; % [kg/m^3] 
chassis.aero.frontalarea = 1.75; % [m^2]
chassis.aero.aerospeed = 187.2; % [km/h] vehicle speed for aero calcs
chassis.aero.Cd = 0.3;  %  [] drag coefficient

% Tire
chassis.tire.tireR=0.2286; %[m] tire radius
chassis.tire.rollingresistance=0.015; %[] coefficient of rolling resistance
chassis.tire.ftirestiff = 120; % [N/mm]
chassis.tire.rtirestiff = 120; % [N/mm]
% load(strcat(pwd,'\mat files\','tiredata.mat'));
% chassis.tire.Fypos=tiredata.Fypos;
% chassis.tire.Fxpos=tiredata.Fxpos;
% chassis.tire.Fyneg=tiredata.Fyneg;
% chassis.tire.Fxneg=tiredata.Fxneg;
% chassis.tire.Fzdata=tiredata.Fzdata;
lapsim.vehicle.chassis=chassis;
clear chassis %tiredata

classdef Tone < Signal
    properties (GetAccess = public, SetAccess = private)
        amplitude_dB_SPL@double
        frequency@double
        nSamples
        duration
    end
    methods
        function obj = Tone(duration, amplitude_dB_SPL, frequency)
            obj.duration = duration;
            obj.amplitude_dB_SPL = amplitude_dB_SPL;
            obj.frequency = frequency;            
            obj.nSamples = getappdata(0, 'samplingFrequency') * obj.duration;
        end
        function data = getData(obj)
            dataWithoutWindow = obj.amplitude .* sin(2*pi*obj.frequency .* (1:obj.nSamples)/getappdata(0, 'samplingFrequency'))';
            data = dataWithoutWindow .* window(@tukeywin, obj.nSamples);
        end
        str = struct(obj)
    end
    methods (Static = true)
        obj = importStruct(struct)
    end
end

% ����Yan. ʹ���Լ�ʵ�ֵ��������, ȥ��ϲ�ַ�, �������
clear
clc

step = 0.01;  % ��ϲ���Ϊstep, ��ʵ�켣�IJ���Ϊstep/2  (��Ϊ:RungeKutta���õ�1/2�IJ���ȥ����)
real_b = 4000;  % ��ʵ�켣�Ľ���ʱ�� 
real_a = 0;  % ��ʵ�켣����ʼʱ�� 
y0 = [-8,7,27];  % ��ʵ�켣�ij�ʼ�� 
interp_step = step  % step �� step/2: #ע: ��������step/2, ���к����ص�����
interp_a = real_a + interp_step;  % ��ֵF�Ŀ�ʼʱ��  ȡstep/2�����IJ�ֺ�, ǰ�����1��ʱ�̵�����;
interp_b = real_b - interp_step;  % ��ֵF�Ľ���ʱ��

% ����׼��: ��ȡ��֪�ɹ۲�켣
[t,y] = fun_8_RungeKutta(@fun_11_lorenz_noise, real_a, interp_step, real_b, y0); 
real_trajectory = y';
known_X = real_trajectory(:, 1);  % ��ʵ����֪�Ĺ켣
% ��ֵ: ֱ�Ӳ�X��ֵ
FX = griddedInterpolant(t, known_X);

% ����׼��: ��ȡ��֪�ɹ۲�켣�����IJ��, �Ӷ�
diff_before_X = known_X(2:end-1) - known_X(1:end-2);  % ǰ����: #��֤����
diff_after_X = (known_X(3:end) - known_X(2:end-1));  % ������  #��֤����
diff_center_X = (diff_before_X+diff_after_X)/2;  % ���IJ��
dot_X = diff_center_X/(interp_step);  % ��ֵ����
save('mat_9_dot_X', 'dot_X');  % ��������

% �����ֵ���� fun_9_lorenz_solver: ʹ���ܽ����ⲿ����dot_X����, ���ܸ���stepѡȡ�ض���dot_X������
time_range = [interp_a: interp_step: interp_b];
F = griddedInterpolant(time_range, dot_X);

% ��ʼ���
n=floor((real_b-real_a)/step);       % ����: ֻҪ�����ܹ���dot_X����, �Ͳ��������
time(1)=0;             % ʱ���б�
y=[-8; 7; 27; ones(26,1)];              % ����ֵ������������������Ҫע��ά��

% n=1000
for i=1:n          % �����������������ֵ���
    time(i+1)=time(i)+step;
    k1=fun_9_lorenz_solver(time(i),y(:,i),F, FX);
    k2=fun_9_lorenz_solver(time(i)+step/2,y(:,i)+step*k1/2,F, FX);
    k3=fun_9_lorenz_solver(time(i)+step/2,y(:,i)+step*k2/2,F, FX);
    k4=fun_9_lorenz_solver(time(i)+step,y(:,i)+step*k3,F, FX);
    y(:,i+1)=y(:,i)+step*(k1+2*k2+2*k3+k4)/6;
end
result = y';
result(end,[25,26,27])  % ���ǽ��


% �����������˶�ͼ
figure
plot(result(:,25), 'LineWidth',1.5) % a
hold on
plot(result(:,26), 'LineWidth',1.5) % b
hold on 
plot(result(:,27), 'LineWidth',1.5) % r
hold on
legend('a','b','r') ;
xlabel('\it Step \rm', 'fontsize',17);
ylabel('\it Value \rm', 'fontsize',17);
% �Զ���̶�
xtickformat('%.1f');
ytickformat('%.1f');
ax = gca;
ax.XAxis.Exponent = 5;
ylim([0 30])
% ���������С
set(gca,'FontSize',14)  %�����ÿ̶������С
% % �����������˶�ͼ



% figure
% plot(result(:,25)) % a
% hold on 
% plot(result(:,26)) % b
% hold on 
% plot(result(:,27)) % r
% hold on 
% legend('a','b','r') 

stop  % ���涼�ǵ���

% ��ͼ(1): ��Ҫ�ڵ���Ч����
% ��ͼ(2): �������
 

classdef Utils
    %UTILS �˴���ʾ�йش����ժҪ
    % ���߼�
    %   �˴���ʾ��ϸ˵��
    
    properties
    end
    
    methods (Static)
        function [ Y ] = K( Kernel, U, V, p1, p2, p3 )
        %KERNEL �˴���ʾ�йش˺�����ժҪ
        % �˺���
        %   �˴���ʾ��ϸ˵��
        % ������
        %     Kernel    -�˺���
        %     U         -����U
        %     V         -����V
        %     p1~p3     -�˺�������

            switch Kernel
                case 'linear'
                    % Linear Kernel
                    % disp('You are using a Linear Kernal')
                    Y = U*V.';
                case 'poly'
                    % Polynomial Kernel
                    % disp('You are using a Polynomial Kernal')
                    Y = (p1*U*V'+p2).^p3;
                case 'rbf'
                    % Gaussian Kernel
                    % disp('You are using a RBF Kernal')
                    D = XYDist(U, V);
                    Y = exp(-D.^2/(2*p1.^2));
                case 'sigmoid'
                    % Sigmoid Kernel
                    Y = tanh(p1*U*V'+p2);
                case 'log'
                    % Log Kernel
                    D = XYDist(U, V);
                    Y = -log(1+D.^p1);
                otherwise
                    disp('Please enter a Valid kernel choice ')
                    Y = [ ];
            end
        end
        function [ A ] = Cond( H )
        %COND �˴���ʾ�йش˺�����ժҪ
        % �����˾���
        %   �˴���ʾ��ϸ˵��
        % ������
        %     H     -�˾���
        %     A     -���������

            if (abs(cond(H)) > 1e+10)
                A = H + 0.00000001*eye(size(H));
            else
                A = H;
            end            
        end
        function [ idx, dist, M ] = KnnGraph( X, k )
        %MKNN �˴���ʾ�йش˺�����ժҪ
        % Mutual K Nearest Neighbours
        %   �˴���ʾ��ϸ˵��
        % ������
        %     X    -���ݼ�
        %     k    -������
        % �����
        %   idx    -��������k���ھ���
        %  dist    -��������k���ھ������
        %     M    -Mutual KNN Graph

            [m, ~] = size(X);
            % ����໥�����
            [idx, dist] = knnsearch(X, X, 'K', k + 1,'Distance','euclidean');
            idx = idx(:, 2:k + 1);
            dist = dist(:, 2:k + 1);
            % ����Mutual KNN Graph
            M = zeros(m, m); % zeros(m, m); % spalloc(m, m, 2*k*m);
            for i = 1 : m
                M(i, idx(i, :)) = 1;%dist(i, :);
            end
            M = (M + M')/2;
        end
    end
end

clear all;close all;   warning off;clc;

load project_paths projectroot src_path;
%% Prepare output directories
% allow overwriting existing results if true
overwrite=false;
%overwrite=true;
test_case=[1:2]; % select file numbers for processing
%% Processing parameters
Nx = 500;   % number of points after interpolation in X direction
Ny = 500;   % number of points after interpolation in Y direction
Nmed = 3;   % median filtering window size e.g. Nmed = 2 gives 2 by 2 points window size
m = 2.5;    % weight scale for wieghted RMS

trs = 0.95; % 
thrs = 20;  % if energy drops below x% stop processing ERMS
%%
% create path to the experimental raw data folder
%raw_data_path = fullfile( projectroot, 'data','raw','exp', filesep );
specimen_folder = 'L3_S1_B';
raw_data_path = ['/pkudela_odroid_laser/aidd/data/raw/exp/',specimen_folder,'/'];
%raw_data_path = ['\\odroid-laser\laser\aidd\data\raw\exp\',specimen_folder,'\'];
% create path to the experimental interim data folder
interim_data_path = fullfile( projectroot, 'data','interim','exp', filesep); % local
%interim_data_path = ['/pkudela_odroid_sensors/aidd/data/interim/exp/']; % NAS
%interim_data_path = ['\\odroid-sensors\sensors\aidd\data\interim\exp\']; % NAS
% check if folder exist, if not create it
if ~exist([interim_data_path,specimen_folder], 'dir')
    mkdir([interim_data_path,specimen_folder]);
end
% full field measurements
list = {'333x333p_50kHz_5HC_9Vpp_x20_20avg_no_filter', ...          % 1  Length = ?;Width = ?;           
    '333x333p_50kHz_5HC_9Vpp_x20_20avg_pzt'};% 2
                 

disp('Interpolation and full wavefield to image calculation');
folder  = raw_data_path;
nFile   = length(test_case);
success = false(1, nFile);
for k = test_case
    filename = list{k};
    processed_filename = filename; % filename of processed .mat data
    % check if folder exist, if not create it
    if ~exist([interim_data_path,specimen_folder,filesep,processed_filename], 'dir')
        mkdir([interim_data_path,specimen_folder,filesep,processed_filename]);
    end
    frame_filename=[interim_data_path,specimen_folder,filesep,processed_filename,filesep,'frame_1_',processed_filename];
              
    % check if already exist
    if(overwrite||(~overwrite && ~exist([frame_filename,'.png'], 'file')))
        try 
            % load raw experimental data file
            disp('loading data');
            load([raw_data_path,filename]); % Data, (time XI YI ZI)
            
            %% PROCESS DATA
            fprintf('Processing:\n%s\n',filename);
            % exclude points at the boundary
            Data=Data(2:end-2,2:end-2,:);
            [nx,ny,nft]=size(Data);
            [X,Y] = meshgrid(1:ny,1:nx);                                        % original value grid
            [XI,YI] = meshgrid(1:(ny-1)/(Ny-1):ny,1:(nx-1)/(Nx-1):nx);          % new value grid
            %% Median filtering
             if Nmed > 1      
                 for frame = 1:nft/2
%                      Data(:,:,frame) = medfilt2(Data(:,:,frame),[Nmed Nmed],'symmetric');  
                       Data(:,:,frame) = mymedian3x3(Data(:,:,frame)); % 3x3 median filtering
                 end
             end
            %% make interpolation of full wavefield     
            for frame = 1:nft/2
                Data_frame_interp = squeeze(interp2(X,Y,Data(:,:,frame),XI,YI,'spline'));
                % convert to image    
                frame_filename=[interim_data_path,specimen_folder,filesep,processed_filename,filesep,'frame_',num2str(frame),'_',processed_filename];
                F=frame2image(Data_frame_interp, frame_filename);           
            end
            %% END OF PROCESSING
            [filepath,name,ext] = fileparts(filename);
            fprintf('Successfully processed:\n%s\n', name);% successfully processed
        catch
            fprintf('Failed: %s\n', filename);
        end
    else
        fprintf('Filename: \n%s \nalready exist\n', processed_filename);
    end
end

%********************consumption only
%********************no shares
% ===============================================================
% load 'Xshort','Xlong','u_country','u_cont'
% Xshort has
% country_all1 country_id2 years3 IC4 urban5 rural6 mean_dollar7 
% agPG8 gdpPG9 rain10 AgBorder11 NonBorder12 deciles(10c) 
% AgBorder & NonBorder are already in difference;
% Xlong has DCL13 and decile14 as last 2 columns
% ==============================================================

caption=['\\caption{'...
   'Second stage regression of the growth rate of decile expenditures '...
   'on the aggregate growth rates of agricultural and non-agricultural '...
   'income.  The annotations *, **, and *** indicate significance of '...
   'the corresponding coefficient estimate with levels of confidence '...
   'corresponding to 90\\%, 95\\%, and 99\\%.  In addition to the reported '...
   'variables, the (second stage) estimating equation includes fixed '...
   'effects for each country-decile, while the first stage includes '...
   'year effects.}'];

load ../Data/WBdata;

DR=sortrows(Xlong,[1 13 3]);
varlist={'country_all','country_id','years','IC','urban',...
    'rural','mean_dollar','ag','gdp','rain',...
    'AgBorder','NonBorder','DCL','dshares'};
for i=1:length(varlist);
    eval([varlist{i} '=DR(:,i);']);
end
deciles=mean_dollar.*dshares*0.1;
X=[country_all DCL years];
[D,I1,I2]=sdifference(X);
temp1=zeros(length(X),1); temp2=temp1;
temp1(I2)=AgBorder(I2); AgBorder=temp1;
temp2(I2)=NonBorder(I2); NonBorder=temp2;

includelist={'country_all','DCL','years','deciles',...
    'ag','gdp','AgBorder', 'NonBorder',...
    'country_id','IC','urban','rural'};
X=[];
for i=1:length(includelist);
    eval(['X=[X ' includelist{i} '];']);
end
idx=find(IC==0); %use only consumptions
X=X(idx,:);
sumX=sum(X')';
idx=find(~isnan(sumX));
X=X(idx,:); %delete obs with missing values;

% % get rid of the negative shares
index=find(X(:,4)<0);
this_obs=X(index,1); this_year=X(index,3);
for i=1:length(this_obs);
    index=find(X(:,1)==this_obs(i) & X(:,3)==this_year(i));
    X(index,:)=[];
end

% % get rid of data with leass than 3 years
u_X1=unique(X(:,1));
INDEX=[];
for i=1:length(u_X1);
    index=find(X(:,1)==u_X1(i));
    if length(index)>=30;
        INDEX=[INDEX;index];
    end
end
X=X(INDEX,:);
X=sortrows(X,[1 2 3]);

for i=1:length(includelist);
    eval([includelist{i} '=X(:,i);']);
end
%********************* end of data cleaning*******************
% =================================================================
% ***************start of data preparation*********************

[DD,I1,I2]=sdifference([country_all DCL years]); % only to get I & I2

% % % % get change rate
% % dAg=(ag(I2)./ag(I1)-1)./diff(DD')';
% % non=gdp-ag;
% % dNon=(non(I2)./non(I1)-1)./diff(DD')';
% % dWef=(deciles(I2)./deciles(I1)-1)./diff(DD')'; y=dWef;

% % get log change
dAg=(log(ag(I2))-log(ag(I1)))./diff(DD')';
non=gdp-ag;
dNon=(log(non(I2))-log(non(I1)))./diff(DD')';
dWef=(log(deciles(I2))-log(deciles(I1)))./diff(DD')'; y=dWef;

dInstrAg=AgBorder(I2)./diff(DD')';
dInstrNon=NonBorder(I2)./diff(DD')';

cinstr=size(dInstrAg,2);
rr=length(I1);

% % dummies
Qd=dummies(DCL(I1));
Cd=dummies(country_all(I1));
CQd=dummies([DCL(I1) country_all(I1)]);

u_year=unique(years);
yearmin=min(u_year);
yearmax=max(u_year);
Yd=zeros(rr,length(u_year)-1);
for i=1:length(u_year)-1;
    index=find(DD(:,1)<= u_year(i) & DD(:,2)>u_year(i));
    Yd(index,i)=1;
end
Yd=Yd./repmat(diff(DD')',1,size(Yd,2));
idx=find(sum(Yd)>0);
Yd=Yd(:,idx);

% generate interaction with DCL;
% generate 'dAgQ','dNonQ','dInstrAgQ','dInstrNonQ'
interlist={'dAg','dNon','dInstrAg','dInstrNon'};
for i=1:length(interlist);
    eval([interlist{i} 'Q=repmat(' interlist{i} ',1,size(Qd,2)).*Qd;']);
end

% ***************end of data preparations*********************
% ===============================================================
% *************start of Stage 1 analysis *********************
% 1st stage for Ag
putin1={[dInstrAg Yd]};
[B,E,T,R,P,Yhat,se1]=simplereg(dAg,putin1);
Aghat=Yhat{1}; % for use in deciles2.m
AghatQ=repmat(Aghat,1,size(Qd,2)).*Qd;

% 1st stage for Non
putin1={[dInstrNon Yd]};
[B,E,T,R,P,Yhat,se1]=simplereg(dNon,putin1);
Nonhat=Yhat{1};
NonhatQ=repmat(Nonhat,1,size(Qd,2)).*Qd;

% =================================================================
% ***************start of Stage 2 analysis**********************
y=dWef;
putin2={[AghatQ NonhatQ CQd]};
putin2_={[dAgQ dNonQ CQd]};
[B,E,T,R,P,Yhat,se1]=simplereg(y,putin2);
agB=B{1}(1:10);nonB=B{1}(11:20);

yhat=putin2_{1}*B{1};
E2sls=y-yhat;
T=B{1}./sqrt(diag(inv(putin2_{1}'*putin2_{1})*cov(E2sls)));
se2=full(sqrt(diag(inv(putin2_{1}'*putin2_{1})*cov(E2sls))));
agT=T(1:10);nonT=T(11:20);
agE=se2(1:10);nonE=se2(11:20);

% over-identification
putin3={[dInstrAgQ dInstrNonQ CQd]};
[B,E,T,R,P,Yhat]=simplereg(E2sls,putin3);
R2=sum(Yhat{1}.*Yhat{1})./sum(E2sls.*E2sls);

if ismatlab;
  p=1-chi2cdf(rr*R2,(size(putin1{1},2)+10-20));
else;
  p=1-chisquare_cdf(rr*R2,(size(putin1{1},2)+10-20));
end;
ovid=p;

% ==============================================================
% *********************start of table*********************
sAg=ag(I1)./gdp(I1);
shares=[mean(sAg) 1-mean(sAg)];
BB=[];BB(:,1)=agB; BB(:,2)=nonB;
TT=[];TT(:,1)=agT; TT(:,2)=nonT;
SSE=[]; SSE(:,1)=agE;SSE(:,2)=nonE;

BB,TT,SSE

tab=BB; 
title=['deciles2.tex'];

h=fopen(title,'w');
titlestr=['How Income Growth Affects Expenditures'];

cl=size(tab,2);
cname={'Deciles','Agriculture','Non-Agriculture'};
cname2={'','Income Growth','Income Growth'};
rname={'10','20','30','40','50','60','70','80','90','100'};
fmtstr=['l|' repmat('c',1,cl)];

fprintf(h,'\\begin{table}[h]\n\\centering\n');
fprintf(h,'\\begin{tabular}{%s}',fmtstr);
fprintf(h,['\\hline\\hline\n']);
% fprintf(h,['& \\multicolumn{' num2str(cl) '}{l}{\\textsl{%s}}\\\\\\hline\n'],titlestr);
fprintf(h,'%s',cname{1});
fprintf(h,'&\t%s',cname{2:end}); fprintf(h,'\\\\\n');
fprintf(h,'&\t%s',cname2{2:end}); fprintf(h,'\\\\\\hline\n');

fprintf(h,'Shares');
fprintf(h,'&\t $%5.3f$',shares);fprintf(h,'\\\\\\hline\n');

fprintf(h,'Std. Errors');
fprintf(h,'&\t $%5.3f$',SSE(1,:));fprintf(h,'\\\\\n');

for i=1:size(rname,2);
    fprintf(h,'%s\\%%',rname{i});
    plug=[BB(i,:);TT(i,:)];
    bplug
end

fprintf(h,'\\hline\\hline\n\\end{tabular}\n');

fprintf(h,'%s',caption);
fprintf(h,'\\label{tab:deciles2}\n');
fprintf(h,'\\end{table}\n');

function [comparedPerformance] = comparator(originalVideo, reconstructedVideo, reconstructedFilteredVideo)
% this function compares the performance of algorithm 2 with the SVR-LMS + Kalman filter
% algorithm

%the name 'filteredReconstructedVideo' is temporary and depends on the
%final name of the output of the reconstructed and filtered video

% INPUT
% todo

% OUTPUT
% todo

%%
%initializing zero vectors for the error
numFrames=length(originalVideo);
comparedPerformance = zeros(numFrames,2); % Can 'length(originalVideo)' be replaced by 'numFrames' as well?
relativeErrorRGB = zeros(3,2); % this matrix will contain the errors per color and per reconstructed video (filtered and unfiltered)
% the first column corresponds to the unfiltered video, the second one to the filtered video  


%loop through every time frame
for k = 1 : numFrames
    
    %convert uint-8 to double for calculation
    originalVideo(k).frame = im2double(originalVideo(k).frame);
    reconstructedVideo(k).frame = im2double(reconstructedVideo(k).frame);
    reconstructedFilteredVideo(k).frame = im2double(reconstructedFilteredVideo(k).frame);
    
    %loop throug all colours, and caclulate the relative error of the Frobinian norm
    for rgb = 1:3 
        relativeErrorRGB(rgb,1) = norm((reconstructedVideo(k).frame(:,:,rgb) - originalVideo(k).frame(:,:,rgb)),'fro')/norm(originalVideo(k).frame(:,:,rgb),'fro');
        relativeErrorRGB(rgb,2) = norm((reconstructedFilteredVideo(k).frame(:,:,rgb) - originalVideo(k).frame(:,:,rgb)),'fro')/norm(originalVideo(k).frame(:,:,rgb),'fro');
    end
    
 %take the mean of the relative error of the colours
 comparedPerformance(k,1) = mean(relativeErrorRGB(:,1)); 
 comparedPerformance(k,2) = mean(relativeErrorRGB(:,2)); 
 
end

%setup the x-axis for the plot
frames = linspace(1, numFrames, numFrames); 

%plot the relative error for each frame
figure
hold on
plot(frames(1,:), comparedPerformance(:,1),'bx'); % We have to make sure that the same color is used for the same algorithm, so comparing graphs is easy
plot(frames(1,:), comparedPerformance(:,2),'gx');
title('Performance of the algorithms')
xlabel('Timestep')
ylabel('Relative Frobenius error')
legend('Reconstructed','Reconstructed & filtered')

end

% The function returns the first to third derivative of the y coordinate 
% of the path y(x) performed by the by the axle of the last trailer . 


function [yx, yxx, yxxx] = ComputeDerivatives(xs,d0,d1)
% Input State vector xs:
% With elements
%                x,            x coord of center of last trailer rear axle
%                y,            y coord of center of last trailer rear axle
%                \theta0,      orientation of truck
%                \theta1,      orientation of trailer
%                \phi          steering angle

% parameters d0 and d1 - % d0: distance front/rear axle truck

% outputs yx := dy/dx   yxx := d^2y/dx^2   yxxx := d^3y/dy^3
  
    kappa1 = 1./d1 .* tan(xs(3)-xs(4));
    kappa0 = 1./d0 .* tan(xs(5));
  
    yx = tan( xs(4) ); % slope of  trailer %tan(theta1)

    gamma1 = 1 + yx.^2;
    
    eta1   =  1 + d1.^2.*kappa1.^2 ;
     
   dkappa1_ds1 = (kappa0.*sqrt(eta1) - kappa1 ) .* eta1/d1; 

   dkappa1_dx  =  dkappa1_ds1 .* sqrt(eta1) .* sqrt(gamma1);      %ds1 = sqrt(eta2) ds2 

   yxx = kappa1 .* gamma1.^(3/2);
    

   yxxx = gamma1.^(3/2).*(dkappa1_dx + 3.*yx.*yxx.^2./gamma1^(5/2));
 


    
end

clc; clear; close;

t = csvread('data.txt');

input = t;
out = t(:,end);
input(:,end) = ones(size(t,1),1);  
weight = [0.3 0.5 1.4]; 
w = [];
errorplot = [];
z= [5 10 50 100];      
array = [];
alpha = 0.01;
damp = 0.8;

             for epoc = 1:100
                     
                 [weight,error] = Delta_Rule1(weight, input, out ,alpha, damp, epoc);
                 errorplot(end+1) = error;
                 if (epoc == 5 || epoc == 10 || epoc == 50 || epoc == 100)
                    array(end+1) = error;
                    w = [w;weight];
                 end                   
             end
     
     
      figure;
      plot(errorplot);
      xlabel('Iterations');
      ylabel('Error');
      title('Dampening effect of error');
      

% STK_OPTIONS_SET sets the value of one or all STK options.

% Copyright Notice
%
%    Copyright (C) 2013 SUPELEC
%
%    Author:  Julien Bect  <julien.bect@supelec.fr>

% Copying Permission Statement
%
%    This file is part of
%
%            STK: a Small (Matlab/Octave) Toolbox for Kriging
%               (http://sourceforge.net/projects/kriging)
%
%    STK 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.
%
%    STK 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 STK.  If not, see <http://www.gnu.org/licenses/>.

function opts = stk_options_set(varargin)

persistent options

if isempty (options)
    options = init_options ();
    mlock;
end

switch nargin
    
    case 0, % nothing to do, just return the output
        
    case 2,
        options.(varargin{1}) = varargin{2};
        
    case 3,
        options.(varargin{1}).(varargin{2}) = varargin{3};
        
    otherwise
        stk_error('Incorrect number of input arguments.', 'SyntaxError');
        
end

opts = options;

end % function stk_options_set


function opts = init_options()

opts = struct();

opts.stk_sf_matern.min_size_for_parallelization = 1e5;
opts.stk_sf_matern.min_block_size = 1e3;

opts.stk_dataframe.disp_format = 'basic'; % 'basic' or 'verbose'
opts.stk_dataframe.disp_spstr = '    ';

opts.stk_figure.properties = {'InvertHardcopy', 'off', 'Color', [1 1 1]};
opts.stk_xlabel.properties = {'FontSize', 10, 'Color', [0.2 0 1]};
opts.stk_ylabel.properties = opts.stk_xlabel.properties;
opts.stk_title.properties = {'FontSize', 10, 'FontWeight', 'bold'};
opts.stk_axes.properties = {'FontSize', 8};

end % function init_options

function [soilCommonStateParams, soilInnerStateParams]...
                           = mergeInnerOuter(fractSoil, fractPlant,...
                                             simulationSettings,...
                                             soilConsParams,...
                                             soilCommonStateParams,...
                                             soilInnerStateParams,...
                                             soilOuterStateParams)
                                             
t = simulationSettings.t;

%% Determine field situation and set corresponding values
if simulationSettings.flag_double_sim == 1
    
    % Soil organic matter
    soilCommonStateParams.soil_om =  soilOuterStateParams.soil_om*fractSoil +...
        soilInnerStateParams.soil_om*fractPlant;
    
    % Water content
    soilCommonStateParams.wcmo =  soilOuterStateParams.wcmo*fractSoil + ...
        soilInnerStateParams.wcmo*fractPlant;
    
    % Solute content
    soilCommonStateParams.tcsolo =  soilOuterStateParams.tcsolo*fractSoil +....
        soilInnerStateParams.tcsolo*fractPlant;
    
    %Store temperature data
    soilCommonStateParams.temp =  soilOuterStateParams.temp*fractSoil +...
        soilInnerStateParams.temp*fractPlant;
    
    % Mineralised organic matter
    soilCommonStateParams.minerm =  soilOuterStateParams.minerm(:,2)*fractSoil +...
        soilInnerStateParams.minerm(:,2)*fractPlant;
    
    % water balance
    if soilCommonStateParams.water_balance
        soilCommonStateParams.water_balance(1,end+1) = soilOuterStateParams.water_balance(1, end);
        soilCommonStateParams.water_balance(2:4,end) = soilCommonStateParams.water_balance(2:4, end-1) +...
                                                          (soilInnerStateParams.water_balance(2:4, end) -...
                                                           soilInnerStateParams.water_balance(2:4, end-1)) * fractPlant +...
                                                          (soilOuterStateParams.water_balance(2:4, end) -...
                                                           soilOuterStateParams.water_balance(2:4, end-1)) * fractSoil;
    else
        soilCommonStateParams.water_balance(1,end+1) = soilOuterStateParams.water_balance(1, end);
        soilCommonStateParams.water_balance(2:4,end) =  soilInnerStateParams.water_balance(2:4, end) * fractPlant +...
                                                          soilOuterStateParams.water_balance(2:4, end) * fractSoil;

    end
    
elseif simulationSettings.flag_double_sim == 0
    % Soil organic matter
    soilCommonStateParams.soil_om =  soilOuterStateParams.soil_om*fractSoil;
    
    % Water content
    soilCommonStateParams.wcmo =  soilOuterStateParams.wcmo*fractSoil;
    
    % Solute content
    soilCommonStateParams.tcsolo =  soilOuterStateParams.tcsolo*fractSoil;
    
    %Store temperature data
    soilCommonStateParams.temp =  soilOuterStateParams.temp*fractSoil;
    
    % Mineralised organic matter
    soilCommonStateParams.minerm =  soilOuterStateParams.minerm(:,2)*fractSoil;
    
    % water balance
    if soilCommonStateParams.water_balance % if soilCommonState.water_balance is not empty
        soilCommonStateParams.water_balance(1,end+1) = soilOuterStateParams.water_balance(1, end);
        soilCommonStateParams.water_balance(2:4,end) = soilCommonStateParams.water_balance(2:4, end-1) +...
                                                          (soilOuterStateParams.water_balance(2:4, end) -...
                                                           soilOuterStateParams.water_balance(2:4, end-1)) * fractSoil;
        
    else % if soilCommonState.water_balance is empty
        soilCommonStateParams.water_balance(1,end+1) = soilOuterStateParams.water_balance(1, end);
        soilCommonStateParams.water_balance(2:4,end) = soilOuterStateParams.water_balance(2:4, end) * fractSoil;                                                       
    end
    
    % Zeros should be added to balance variables such that after planting
    % the balance is calculated correctly 
    SoluteFields = {"UreumBalance_log", "NH4Balance_log", "NO3Balance_log"};
    
    for i = 1:numel(SoluteFields)
       [nRow, ~] = size(soilOuterStateParams.(SoluteFields{1}(1)));
       soilInnerStateParams.(SoluteFields{i}(1))(:,end+1) = [t; soilInnerStateParams.(SoluteFields{i}(1))(2:end,end)];
    end
   
    [nRow, ~] = size(soilOuterStateParams.N_reaction_balance);
    soilInnerStateParams.N_reaction_balance(:,end+1) = [t; zeros(nRow-1, 1)];

end
%% merge inner state parameters & outer state parameters in common state parameters


soilCommonStateParams.water_storage(end+1) = sum(soilCommonStateParams.wcmo.*simulationSettings.dx); %Wcmo calculated at end of a day dependent on fractions
soilCommonStateParams.WCSoil_log(:,end+1) = soilCommonStateParams.wcmo';

soilCommonStateParams.UreumSoil_log(:,end+1) = soilCommonStateParams.tcsolo(:,1);
soilCommonStateParams.NH4Soil_log(:,end+1) = soilCommonStateParams.tcsolo(:,2);
soilCommonStateParams.NO3Soil_log(:,end+1) = soilCommonStateParams.tcsolo(:,3);

% save solute balance
SoluteFields = {["UreumBalance_log", "UreumSoil_log"],...
                ["NH4Balance_log", "NH4Soil_log"],...
                ["NO3Balance_log", "NO3Soil_log"]};

% loop over cell array
for i = 1:numel(SoluteFields)
    
    % time
    soilCommonStateParams.(SoluteFields{i}(1))(1,end+1)= soilOuterStateParams.(SoluteFields{i}(1))(1,end);
    if simulationSettings.t == datenum(2009, 1, 1) + 60
        disp('')
    end
    %Inflow (2) & outflow (3)
    soilCommonStateParams.(SoluteFields{i}(1))(2:3,end)= soilCommonStateParams.(SoluteFields{i}(1))(2:3,end-1)+...
        (soilOuterStateParams.(SoluteFields{i}(1))(2:3,end) -...
        soilOuterStateParams.(SoluteFields{i}(1))(2:3,end-1))*fractSoil + ...
        (soilInnerStateParams.(SoluteFields{i}(1))(2:3,end) -...
        soilInnerStateParams.(SoluteFields{i}(1))(2:3,end-1))*fractPlant; 
    
    %Solute change in profiile
    soilCommonStateParams.(SoluteFields{i}(1))(4,end) = sum(soilCommonStateParams.(SoluteFields{i}(2))(:,end)) -...
        sum(soilConsParams.tcsolo_ini(:,i)); 
    
    %Sink term
    soilCommonStateParams.(SoluteFields{i}(1))(6,end)= soilCommonStateParams.(SoluteFields{i}(1))(6,end-1)+...
        (soilOuterStateParams.(SoluteFields{i}(1))(6,end) -...
        soilOuterStateParams.(SoluteFields{i}(1))(6,end-1))*fractSoil + ...
        (soilInnerStateParams.(SoluteFields{i}(1))(6,end) -...
        soilInnerStateParams.(SoluteFields{i}(1))(6,end-1))*fractPlant;
    
    %Balance error
    soilCommonStateParams.(SoluteFields{i}(1))(5,end) = soilCommonStateParams.(SoluteFields{i}(1))(4,end)-....
        (soilCommonStateParams.(SoluteFields{i}(1))(2,end)+....
        soilCommonStateParams.(SoluteFields{i}(1))(3,end)+...
        soilCommonStateParams.(SoluteFields{i}(1))(6,end)); 
    
end

% Nitrogen reaction balance
soilCommonStateParams.N_reaction_balance(:,end+1) =...
                        soilCommonStateParams.N_reaction_balance(:,end) +...
                        (soilOuterStateParams.N_reaction_balance(:,end) -...
                        soilOuterStateParams.N_reaction_balance(:,end-1)) * fractSoil +...
                        (soilInnerStateParams.N_reaction_balance(:,end) -...
                        soilInnerStateParams.N_reaction_balance(:,end-1)) * fractPlant;


soilCommonStateParams.daily_mineral(:,end+1) = soilCommonStateParams.minerm;


soilCommonStateParams.TSoil = soilCommonStateParams.temp';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Clayton Kramp and Katrina Steinman
% CSCI 507 Final Project
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all
close all

directory = 'images/DtoY.png';

I=rgb2gray(imread(directory));
%I = imresize(I, 0.5);
imshow(I)
I =~imbinarize(I,0.4);
I = bwareaopen(I,150);
numSymbols = 1;
symbolsExtracted = {};

[Labels, numConnectedComponents]=bwlabel(I);
props = regionprops(Labels,'BoundingBox');

minSize = inf; minIndex = 1;
hold on
for n=1:size(props,1)
    rectangle('Position',props(n).BoundingBox,'EdgeColor','r','LineWidth',2)
    if props(n).BoundingBox(3) * props(n).BoundingBox(4) < minSize
        minSize = props(n).BoundingBox(3) * props(n).BoundingBox(4);
        minIndex = n;
    end 
end
hold off

pause

path = 'Data/';

tempTrainImages = loadMNISTImages(strcat(path,'Train/train-images-idx3-ubyte'));
TrainImages = zeros(28, 28, 1, size(tempTrainImages, 2), 'single');
for i = 1:size(tempTrainImages, 2)
    TrainImages(:,:,1,i) = reshape(tempTrainImages(1:784, i), [28 28]);
end
TrainLabels = loadMNISTLabels(strcat(path,'Train/train-labels-idx1-ubyte'));

tempTestImages = loadMNISTImages(strcat(path,'Test/t10k-images-idx3-ubyte'));
TestImages = zeros(28, 28, 1, size(tempTestImages, 2), 'single');
for i = 1:size(tempTestImages, 2)
    TestImages(:,:,1,i) = reshape(tempTestImages(1:784, i), [28 28]);
end
TestLabels = loadMNISTLabels(strcat(path, 'Test/t10k-labels-idx1-ubyte'));

[trainSymbols, testSymbols, trainSymLabels, testSymLabels] = ...
    processSymbols('Data/Symbols/');

symbolLayers = [ imageInputLayer([28 28 1])
            convolution2dLayer(5,20)
            reluLayer
            maxPooling2dLayer(2, 'Stride', 2)
            fullyConnectedLayer(3)
            softmaxLayer
            classificationLayer() ];
        
batch = 15;
trainOptions = trainingOptions( 'sgdm',...
    'MiniBatchSize', batch,...
    'Plots', 'training-progress',...
    'MaxEpochs',25);
nnSymbol = trainNetwork(trainSymbols, trainSymLabels, symbolLayers, trainOptions);
testing = nnSymbol.classify(testSymbols);
testAccuracy = sum(testing == testSymLabels) / numel(testSymLabels);
fprintf("Test Accuracy for Symbols is %f\n", testAccuracy);

layers = [  imageInputLayer([28 28 1])
            convolution2dLayer(5,20)
            reluLayer
            maxPooling2dLayer(2, 'Stride', 2)
            fullyConnectedLayer(10)
            softmaxLayer
            classificationLayer()   ];
        
batch = 50;
trainOptions = trainingOptions( 'sgdm',...
    'MiniBatchSize', batch,...
    'Plots', 'training-progress',...
    'MaxEpochs',1);
nn = trainNetwork(TrainImages, categorical(TrainLabels), layers, trainOptions);
testing = nn.classify(TestImages);
testAccuracy = sum(testing == categorical(TestLabels)) / numel(TestLabels);
fprintf("Test Accuracy is for numerics is %f\n", testAccuracy);

%%
for i=1:numConnectedComponents
    if i == minIndex
        symbolsExtracted(numSymbols) = {'.'};
    else
        [row,col] = find(Labels == i);
        symbol = zeros(28, 28, 1, 1);
        newImg = imgaussfilt(single(I(min(row):max(row),min(col):max(col))),1);
        symbol(:,:,1,1) = padarray(imresize(newImg, [18 18]),[5 5],0,'both');
        if i == 1 || i == 2
            [prediction, score] = nnSymbol.classify(symbol);
            symbolsExtracted(numSymbols) = {char(prediction)};
        else
            [prediction, score] = nn.classify(symbol);
           symbolsExtracted(numSymbols) = {str2num(char(prediction))};
        end
    end
    numSymbols = numSymbols + 1;
end
symbolsExtracted

%% Search for the four quadrants

IProject=rgb2gray(imread(directory));
%IProject = imresize(IProject, 0.5);
figure, imshow(IProject);
[row,col] = find(Labels == 2);
x = max(col);
y = min(row);

% Gathering info for the text
text_str = '';
if symbolsExtracted{1} == 'D', text_str = [text_str '$'];
elseif symbolsExtracted{1} == 'E', text_str = [text_str 'E'];
else text_str = [text_str 'Y'];
end
for i = 2:size(symbolsExtracted, 2)
    text_str = [text_str num2str(symbolsExtracted{i})];
end

remainingSpace = size(IProject, 2) - x;
if remainingSpace > 200
    spacePerChar = 70;
else
    spacePerChar = remainingSpace / numConnectedComponents * 1.5;
end

IProject = insertText(IProject, [x y], text_str, 'FontSize', floor(spacePerChar), 'BoxColor', 'white');
figure, imshow(IProject);

rng(42)
addpath('utils/');
addpath('NNLS-0/solver');
addpath('NNLS-0/PROPACKmod/');
close all;
clc;clear;clear global;

funcSimulation=@runSMCSimulation;
outfile='resultSMC';

niter=3;
dim_iter={{100,100,5},{250,250,5}};

c=2:2:10;
giter = cell(0);
giter{1}=@(a) a;
for ci=1:length(c)   
    giter{ci+1}=@(a) 1.0./(1+exp(-c(ci)*a));
end
f={'spearman_rho', 'kendall_tau', 'NDCG'};

result=cell(niter*length(dim_iter),1);

parfor ix=1:niter*length(dim_iter)
    [n,i]=ind2sub([niter,length(dim_iter)],ix);
    [d1,d2,r]=dim_iter{i}{:};
    result{ix}=funcSimulation(d1,d2,r,giter,f);
end
save(outfile,'result')


fprintf('DONE COMPUTING\n')

1;

if ( ~exist('stn','var') || isempty(stn) )
    stn = optimize_station_heat_budget('mlrf1','erai','avhrr_weekly','ndbc',[],'ww3');
end;

fmg; plot_ts(stn.ww3_avhrr_advected_heat); ylim([-.2,.2]); titlename('Advected Heat');
xlim(datenum([2001,2002],10,1)); datetick3('x',2,'keeplimits');
fmg; plot_ts(stn.ww3_ndbc_stokes_speed); titlename('Stokes current');
xlim(datenum([2001,2002],10,1)); datetick3('x',2,'keeplimits');
fmg; plot_ts(stn.hourly_avhrr_weekly_sst_xshore,stn.hourly_avhrr_weekly_sst_lshore); legend('Cross-shore','Longshore'); titlename('\nablaT_A_V_H_H_R');
xlim(datenum([2001,2002],10,1)); datetick3('x',2,'keeplimits');

fmg; plot_ts(stn.avhrr_weekly_diffused_heat); ylim([-.2,.2]); titlename('Diffused Heat');
xlim(datenum([2001,2002],10,1)); datetick3('x',2,'keeplimits');
fmg; plot_ts(stn.hourly_avhrr_weekly_sst_l,stn.hourly_avhrr_weekly_sst_xx,stn.hourly_avhrr_weekly_sst_yy,ts_op(stn.hourly_avhrr_weekly_sst_xx,stn.hourly_avhrr_weekly_sst_yy,'+')); legend('DEL2','XX','YY','XX+YY'); titlename('\nabla^2T_A_V_H_H_R');
xlim(datenum([2001,2002],10,1)); datetick3('x',2,'keeplimits');

station_plot_fields(stn,'avhrr_weekly_sst_field',datenum(2002,6,10):datenum(2002,6,25));
titlename(['AVHRR SST around ',upper(stn.station_name)]);

function generate_forces_pro_ws_solver(params) 
    global n_x n_obs n_ineq d_ineq m_ineq N_ineq M_ineq G g 
    
    n_x = params.n_x;
    n_obs = params.n_obs;
    n_ineq = params.n_ineq; % Number of constraints for each obstacle
    d_ineq = params.d_ineq; % Dimension of constraints for all obstacles
    G = params.G;
    g = params.g;
    m_ineq = size(G,1); % Number of constraints for controlled object
    
    N_ineq = sum(n_ineq);
    M_ineq = n_obs*m_ineq;
    
    ws_model = {};
    
    ws_model.N = params.N + 1;
    
    ws_model.objective = @eval_ws_obj;
    ws_model.objectiveN = @(z) 0;
    
    % [lambda, mu, d]
    ws_model.nvar = N_ineq + M_ineq + n_obs;
    ws_model.ub = [inf*ones(N_ineq+M_ineq,1); inf*ones(n_obs,1)];
    ws_model.lb = [zeros(N_ineq+M_ineq,1); -inf*ones(n_obs,1)];
    
    % [obca dist, obca]
    ws_model.neq = n_obs + n_obs*d_ineq;
    ws_model.eq = @eval_ws_eq;
    ws_model.E = zeros(ws_model.neq, ws_model.nvar);
    
    % [obca norm]
    ws_model.nh = n_obs;
    ws_model.ineq = @eval_ws_ineq;
    ws_model.hu = ones(n_obs,1);
    ws_model.hl = -inf*ones(n_obs,1);
    
    % [x_ref, obs_A, obs_b]
    ws_model.npar = n_x + N_ineq*d_ineq + N_ineq;

    ws_codeopts = getOptions(params.name);
%     ws_codeopts.maxit = 1000;
    ws_codeopts.overwrite = 1;
    ws_codeopts.printlevel = 1;
    ws_codeopts.optlevel = params.optlevel;
    ws_codeopts.BuildSimulinkBlock = 0;
    ws_codeopts.cleanup = 0;
    ws_codeopts.platform = 'Generic';
    % ws_codeopts.gnu = 1;
    ws_codeopts.sse = 1;
    
    ws_codeopts.nlp.linear_solver = 'symm_indefinite';
    ws_codeopts.nlp.ad_tool = 'casadi-351';

    FORCES_NLP(ws_model, ws_codeopts);
end

function ws_obj = eval_ws_obj(z)
    global N_ineq M_ineq
    
    d = z(N_ineq+M_ineq+1:end);
    ws_obj = -sum(d);
end
    
function ws_eq = eval_ws_eq(z, p)
    global n_x n_obs n_ineq d_ineq m_ineq N_ineq M_ineq G g 
    
    t_ws = p(1:2);
    R_ws = [cos(p(3)), -sin(p(3)); sin(p(3)), cos(p(3))];

    obca_d = [];
    obca = [];
    j = 0;
    for i = 1:n_obs
        A = reshape(p(n_x+j*d_ineq+1:n_x+(j+n_ineq(i))*d_ineq), n_ineq(i), d_ineq);
        b = p(n_x+N_ineq*d_ineq+j+1:n_x+N_ineq*d_ineq+j+n_ineq(i));
        lambda = z(j+1:j+n_ineq(i));
        mu = z(N_ineq+(i-1)*m_ineq+1:N_ineq+i*m_ineq);
        d = z(N_ineq+M_ineq+i);
        
        obca_d = vertcat(obca_d, -dot(g, mu)+dot(mtimes(A, t_ws)-b, lambda)-d);
        obca = vertcat(obca, mtimes(G', mu)+mtimes(transpose(mtimes(A, R_ws)), lambda));
        
        j = j + n_ineq(i);
    end
    
    ws_eq = [];
    ws_eq = vertcat(ws_eq, obca_d);
    ws_eq = vertcat(ws_eq, obca);
end

function ws_ineq = eval_ws_ineq(z, p)
    global n_x n_obs n_ineq d_ineq
    
    ws_ineq = [];
    j = 0;
    for i = 1:n_obs
        A = reshape(p(n_x+j*d_ineq+1:n_x+(j+n_ineq(i))*d_ineq), n_ineq(i), d_ineq);
        lambda = z(j+1:j+n_ineq(i));
        ws_ineq = vertcat(ws_ineq, dot(mtimes(transpose(A),lambda), mtimes(transpose(A),lambda)));
        
        j = j + n_ineq(i);
    end
end

%   function [PV] = chainimages(matches)
%   Construct the point-view matrix with the matches found between
%   consecutive frames. This matrix has tracked points as columns and
%   views/frames as rows, and contains the indices of the descriptor for
%   each frame. Therefore, if a certain descriptors can be seen in all
%   frames, their columns are completely filled. Similarly, if it can be 
%   matched only between frame 1 and 2, only the first 2 rows of the columns 
%   will be non-zero.
%
% Inputs:
% - matches: cell array containing matches, with descriptor indices for the
%   1st image in the 1st row, indices for the 2nd image in the 2nd row.
%   Each cell contains one frame pair (1-2, 2-3, 3-4, ... , 11-1).
%
% Outputs:
% - PV: matrix containing matches between consecutive frames

function [PV] = chainimages(matches)
    tic;
    % number of views
    frames = size(matches,2);

    % Initialize PV
    % We add an extra row to process the match between frame_last and frame_1.
    % This extra row will be deleted at the end.
    PV = zeros(frames+1, 0);

    %  Starting from the first frame
    for i=1:frames
        newmatches = matches{i};
        
        % For the first pair, simply add the indices of matched points to the same
        % column of the first two rows of the point-view matrix.
        if i==1
            PV(1:2,:) = newmatches;
        else
            % Find already found points using intersection on PV(i,:) and newmatches 
            [~, IA, IB] = intersect(PV(i, :), PV(i+1, :));
            PV(i+1, :) = intersection(IA, newmatches);
            
            % Find new matching points that are not in the previous match set using setdiff.
            [diff, IA] = setdiff(newmatches , PV(i+1, :));
            
            % Grow the size of the point view matrix each time you find a new match.
            start = size(PV,2)+1;
            PV    = [PV zeros(frames+1, size(diff,2))]; 
            PV(i, start:end)   = diff;
            PV(i+1, start:end) = diff;
        end
    end

    % Process the last frame-pair. This part is already completed by TAs.
    % The last frame-pair, consisting of the last and first frames, requires special treatment.
    % Move matches between last & 1st frame to their corresponding columns in
    % the 1st frame, to prevent multiple columns for the same point.
    [~, IA, IB]      = intersect(PV(1, :), PV(end, :));
    PV(:, IA(2:end)) = PV(:, IA(2:end)) + PV(:, IB(2:end));  % skip 1st index (contains zeros)
    PV(:, IB(2:end)) = [];  % delete moved points in last frame

    % Copy the non zero elements from the last row which are not in the first row to the first row. 
    nonzero_last  = find(PV(end, :));
    nonzero_first = find(PV(1, :));
    no_member     = ~ismember(nonzero_last, nonzero_first);
    nonzero_last  = nonzero_last(no_member);
    tocopy        = PV(:, nonzero_last);

    % Place these points at the very beginning of PV
    PV(:, nonzero_last) = [];
    PV                  = [tocopy PV];

    % Copy extra row elements from last row to 1st row and delete the last row
    PV(1 ,1:size(tocopy, 2)) = PV(end, 1:size(tocopy, 2));
    PV                       = PV(1:frames,:); 
       
    disp(strcat(int2str(size(PV,2)), ' points in pointview matrix so far'));
    toc;
end

Copyright (c) 2005 X.Org Foundation L.L.C.

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
of the Software, and to permit persons to whom the Software is furnished to do
so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
$Header: /cvs/xtest/xtest/xts5/tset/Xlib13/XSetModifierMapping/XSetModifierMapping.m,v 1.2 2005-11-03 08:42:41 jmichael Exp $

Copyright (c) Applied Testing and Technology, Inc. 1995
All Rights Reserved.

>># Project: VSW5
>># 
>># File: xts5/tset/Xlib13/XSetModifierMapping/XSetModifierMapping.m
>># 
>># Description:
>># 	Tests for XSetModifierMapping()
>># 
>># Modifications:
>># $Log: stmdfrmppn.m,v $
>># Revision 1.2  2005-11-03 08:42:41  jmichael
>># clean up all vsw5 paths to use xts5 instead.
>>#
>># Revision 1.1.1.2  2005/04/15 14:05:19  anderson
>># Reimport of the base with the legal name in the copyright fixed.
>>#
>># Revision 8.0  1998/12/23 23:33:43  mar
>># Branch point for Release 5.0.2
>>#
>># Revision 7.0  1998/10/30 22:55:28  mar
>># Branch point for Release 5.0.2b1
>>#
>># Revision 6.0  1998/03/02 05:25:07  tbr
>># Branch point for Release 5.0.1
>>#
>># Revision 5.0  1998/01/26 03:21:39  tbr
>># Branch point for Release 5.0.1b1
>>#
>># Revision 4.0  1995/12/15 09:08:39  tbr
>># Branch point for Release 5.0.0
>>#
>># Revision 3.1  1995/12/15  01:10:04  andy
>># Prepare for GA Release
>>#
/*
Portions of this software are based on Xlib and X Protocol Test Suite.
We have used this material under the terms of its copyright, which grants
free use, subject to the conditions below.  Note however that those
portions of this software that are based on the original Test Suite have
been significantly revised and that all such revisions are copyright (c)
1995 Applied Testing and Technology, Inc.  Insomuch as the proprietary
revisions cannot be separated from the freely copyable material, the net
result is that use of this software is governed by the ApTest copyright.

Copyright (c) 1990, 1991  X Consortium

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
X CONSORTIUM BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN
AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

Except as contained in this notice, the name of the X Consortium shall not be
used in advertising or otherwise to promote the sale, use or other dealings
in this Software without prior written authorization from the X Consortium.

Permission to use, copy, modify, distribute, and sell this software and
its documentation for any purpose is hereby granted without fee,
provided that the above copyright notice appear in all copies and that
both that copyright notice and this permission notice appear in
supporting documentation, and that the name of UniSoft not be
used in advertising or publicity pertaining to distribution of the
software without specific, written prior permission.  UniSoft
makes no representations about the suitability of this software for any
purpose.  It is provided "as is" without express or implied warranty.
*/
>>TITLE XSetModifierMapping Xlib13
int
xname
Display	*display = Dsp;
XModifierKeymap	*modmap;
>>SET return-value MappingSuccess
>>EXTERN

static	XModifierKeymap	*origmap;

static	int	Minkc, Maxkc;

>>SET startup savemap
static void
savemap()
{
	startup();
	if(Dsp) {
		origmap = XGetModifierMapping(Dsp);
		XDisplayKeycodes(Dsp, &Minkc, &Maxkc);
	}
}

>>SET cleanup cleanmap
static void
cleanmap()
{
	if(Dsp)
		XSetModifierMapping(Dsp, origmap);
	cleanup();
}

>>ASSERTION Good A
A succesful call to xname
specifies the KeyCodes of the keys that are to be used
as modifiers and returns
.S MappingSuccess .
>>STRATEGY
Set up a modifier map.
Call xname to set servers map.
Verify that MappingSuccess is returned.
Get current map with XGetModifierMapping.
Verify that the mapping has been set correctly.
>>CODE
int 	i;
int 	kpm;
XModifierKeymap	*newmap;

	/*
	 * Because some keycodes may not be usable as modifiers in a server
	 * dependent fashion, then we must take steps to avoid this.
	 * Therefore: get current modifiers and rearrange them.
	 */
	kpm = origmap->max_keypermod;
	modmap = XNewModifiermap(kpm);
	if (modmap == 0) {
		delete("Could not create new map");
		return;
	}

	for (i = 0; i < kpm*8; i++)
		modmap->modifiermap[i] = origmap->modifiermap[kpm*8-1 - i];

	XCALL;

	newmap = XGetModifierMapping(display);

	if (newmap->max_keypermod == modmap->max_keypermod)
		CHECK;
	else {
		report("max_keypermod was %d, expecting %d", newmap->max_keypermod,
			modmap->max_keypermod);
		FAIL;
	}
	for (i = 0; i < kpm*8; i++) {
		if (modmap->modifiermap[i] == newmap->modifiermap[i])
			CHECK;
		else {
			report("Modifier map was not set correctly");
			FAIL;
			break;
		}
	}
	CHECKPASS(1+kpm*8);

	XFreeModifiermap(newmap);
>>ASSERTION Good A
When a call to xname succeeds, then a
.S MappingNotify
event is generated.
>>STRATEGY
Call xname to set mapping.
Verify that a MappingNotify event is generated.
>>CODE
int 	n;
XEvent	ev;
XMappingEvent	good;

	modmap = origmap;

	XCALL;

	defsetevent(good, display, MappingNotify);
	good.window = None;	/* unused */
	good.request = MappingModifier;
	/* rest not used */

	n = getevent(display, &ev);
	if (n == 0 || ev.type != MappingNotify) {
		report("Expecting a MappingNotify event");
		FAIL;
		return;
	} else
		CHECK;

	if (checkevent((XEvent*)&good, &ev))
		FAIL;
	else
		CHECK;

	CHECKPASS(2);
>>ASSERTION def
The
.M modifiermap
member of the
.S XModifierKeymap
structure contains eight sets of
.M max_keypermod
KeyCodes, one for each modifier in the order
.S Shift ,
.S Lock ,
.S Control ,
.S Mod1 ,
.S Mod2 ,
.S Mod3 ,
.S Mod4 ,
and
.S Mod5 .
>>ASSERTION Good A
When a zero KeyCode occurs in a set, then it is ignored.
>>STRATEGY
>># This is not really true in any sense that we can test.
>># Check that 0 does not generate BadValue when used multiple times.
Set up a mapping with all keycodes zero.
Set mapping with xname.
Verify no BadValue error.
>># Verify that mapping did not change.
>>CODE
XModifierKeymap	*oldmap;
int 	i;

	oldmap = XGetModifierMapping(display);
	if (oldmap == 0) {
		delete("Could not get the old map");
		return;
	}

	modmap = XNewModifiermap(1);
	for (i = 0; i < 8; i++)
		modmap->modifiermap[i] = 0;

	XCALL;

	if (geterr() == Success)
		CHECK;

	CHECKPASS(1);

>>ASSERTION Bad C
When an implementation restriction on which keys can be used
as modifiers is violated,
then a call to xname returns
.S MappingFailed
and none of the modifiers are changed.
>>STRATEGY
Try in turn all possible keycodes.
If all return MappingSuccess:
  Report unsupported.
else
  Verify that MappingFailed is returned.
  Verify that modifier has not been set to this keycode.
>>CODE
int 	i;
int 	ret;
int 	found;
XModifierKeymap *newmap;
unsigned int	kc;

	if ((modmap = XNewModifiermap(1)) == 0) {
		delete("Failed to create new modifier map");
		return;
	}

	for (i = 0; i < 8; i++)
		modmap->modifiermap[i] = 0;

	found = 0;
	for (kc = Minkc; kc <= Maxkc; kc++) {
		modmap->modifiermap[0] = (KeyCode)kc;

		ret = XCALL;

		if (ret != MappingSuccess) {

			found = True;

			if (ret != MappingFailed) {
				report("Return value was %d, expecting MappingFailure", ret);
				FAIL;
				break;
			}
			newmap = XGetModifierMapping(display);

			if (newmap->modifiermap[0] == (KeyCode)kc) {
				report("An invalid keycode (%u) was set into the map", kc);
				FAIL;
				break;
			} else
				CHECK;
		} else
			CHECK;
	}

	if (!found)
		unsupported("All keycodes are acceptable as modifiers for this server");
	else
		CHECKPASS(Maxkc-Minkc+1);

>>ASSERTION Bad B 3
When the new KeyCodes specified for a modifier differ from those
currently defined and any of the
current or new keys for that modifier are
in the logically down state, then a call to xname returns
.S MappingBusy
and none of the modifiers are changed.
>>STRATEGY
If extension available:
  Get current modifier mapping (other tests may have changed it from origmap).
  Make a new modifier map by permuting the old.
  Check there is at least one non-zero keycode in it.
  Simulate pressing this key using extension.
  Call xname to set the new mod. map.
  Expect MappingBusy
  Release all keys.
  Get current mapping with XGetModifierMapping.
  Compare with that got at first.
  Remove key from map.
  Simulate pressing key not in the map.
  Call xname.
  Expect MappingBusy as one of current (at time of xname call) mod keys pressed.
  Release all keys.
  Call xname to set up map to not include this key.
  Expect MappingSuccess.
  Simulate pressing key not in the map.
  Call xname.
  Expect !MappingBusy as key not in current or new maps (at time of xname call).
  Release all keys.
else
  Report untested.
>>CODE
int 	i;
int 	kpm;
int	modkc;
XModifierKeymap	*newmap;
XModifierKeymap	*oldmap;
int	ret;
KeyCode *kcp;

	if (noext(0))
		return;
	else
		CHECK;

	oldmap = XGetModifierMapping(display);

	/*
	 * Because some keycodes may not be usable as modifiers in a server
	 * dependent fashion, then we must take steps to avoid this.
	 * Therefore: get current modifiers and rearrange them.
	 */
	kpm = oldmap->max_keypermod;
	modmap = XNewModifiermap(kpm);
	if (modmap == 0) {
		delete("Could not create new map");
		XFreeModifiermap(oldmap);
		return;
	}

	modkc = 0;
	for (i = 0; i < kpm*8; i++) {
		KeyCode kc = modmap->modifiermap[i] = oldmap->modifiermap[kpm*8-1 - i];

		if (!modkc && kc) {
			modkc = kc;
			kcp = &(modmap->modifiermap[i]);
		}
	}

	if (!modkc) {
		delete("Can't find a usable modifier key code.");
		XFreeModifiermap(oldmap);
		XFreeModifiermap(modmap);
		return;
	} else
		CHECK;

	keypress(display, modkc);

	ret = XCALL;

	relalldev();

	if (ret == MappingBusy)
		CHECK;
	else {
		report("Key %d did not cause MappingBusy when pressed.", modkc);
		FAIL;
	}
	/* now check no change occurred */
	newmap = XGetModifierMapping(display);

	if (newmap->max_keypermod == oldmap->max_keypermod)
		CHECK;
	else {
		report("max_keypermod was %d, expecting %d", newmap->max_keypermod,
			oldmap->max_keypermod);
		FAIL;
	}
	for (i = 0; i < kpm*8; i++) {
		if (oldmap->modifiermap[i] == newmap->modifiermap[i])
			CHECK;
		else {
			report("Modifier map was not set correctly");
			FAIL;
			break;
		}
	}

	/* use a key not in new map, but in current, and expect MappingBusy */
	*kcp = 0; /* remove modkc from map */
	keypress(display, modkc);
	ret = XCALL;
	relalldev();

	if (ret == MappingBusy)
		CHECK;
	else {
		report("Pressing key %d caused %s, not MappingBusy, though in old map.",
			modkc, (ret==MappingSuccess)?"MappingSuccess":"MappingFailed");
		FAIL;
	}
	/* make sure that key is not in map, now */
	ret = XCALL;
	if (ret != MappingSuccess) {
		delete("Can't install new mod. map not including key %d.", modkc);
		XFreeModifiermap(modmap);
		XFreeModifiermap(oldmap);
		XFreeModifiermap(newmap);
		return;
	} else
		CHECK;
	/* use a key not in the new or current map and expect other than MappingBusy */
	keypress(display, modkc);
	ret = XCALL;
	relalldev();

	if (ret != MappingBusy)
		CHECK;
	else {
		report("Pressing key %d caused MappingBusy though not in new or old map.", modkc);
		FAIL;
	}

	XFreeModifiermap(modmap);
	XFreeModifiermap(oldmap);
	XFreeModifiermap(newmap);

	CHECKPASS(7 + kpm*8);
>>SET return-value MappingSuccess
>>ASSERTION Bad B 3
.ER BadAlloc
>>ASSERTION Bad A
When a KeyCode is not in the range returned by
>># This was an error not spotted in the assertion reviews.
>># .F XDisplayCodes ,
.F XDisplayKeycodes ,
then a
.S BadValue
error occurs.
>>STRATEGY
Call XDisplayKeycodes to get range of valid keycodes.
Set up map with keycode less than the minimum value.
Call xname.
Verify that a BadValue error occurs.

Set up map with keycode greater than the maximum value (if possible).
Call xname.
Verify that a BadValue error occurs.
>>CODE BadValue
>>SET return-value MappingFailed
int 	i;

	modmap = XNewModifiermap(1);
	for (i = 0; i < 8*modmap->max_keypermod; i++)
		modmap->modifiermap[i] = 0;

	modmap->modifiermap[0] = Minkc-1;
	XCALL;

	if (geterr() == BadValue)
		CHECK;
	else
		FAIL;

	if (Maxkc+1 < 0xff) {
		modmap->modifiermap[0] = Maxkc+1;
		XCALL;

		if (geterr() == BadValue)
			CHECK;
		else
			FAIL;
	} else
		CHECK;

	CHECKPASS(2);