clear;

%Sensors
%%Acc set1
A1=diag([0.999987092709754 1]);
B1=diag([3.5596949329112e-006 1e-7]);
C1=[1 1];
sR1=(8.55478077790315e-007)^0.5;
sP1=diag([(4.9087e-007)^0.5 1e-3]);
[A1, Q1, R1]=dc2dc_v000(A1,B1*B1',sR1*sR1',1/100,2,1);
B1=Q1^0.5;
sR1=R1^0.5;

A2=A1;A3=A1;A4=A1;A5=A1;
B2=B1;B3=B1;B4=B1;B5=B1;
C2=C1;C3=C1;C4=C1;C5=C1;
sR2=sR1;sR3=sR1;sR4=sR1;sR5=sR1;
sP2=sP1;sP3=sP1;sP4=sP1;sP5=sP1;

%%Acc set2
A11=1;
B11=1e-7;%5.35244311625171e-008;
C11=1;
sR11=(8.55478077790315e-007)^0.5;
sP11=(6.8307e-007*20)^0.5;
[A11, Q11, R11]=dc2dc_v000(A11,B11*B11',sR11*sR11',1/100,2,1);
B11=Q11^0.5;
sR11=R11^0.5;

A12=A11;A13=A11;A14=A11;A15=A11;
B12=B11;B13=B11;B14=B11;B15=B11;
C12=C11;C13=C11;C14=C11;C15=C11;
sR12=sR11;sR13=sR11;sR14=sR11;sR15=sR11;
sP12=sP11;sP13=sP11;sP14=sP11;sP15=sP11;

%gyros
A6=0.999954464990211;
B6=7.50002159071526e-007*10;
C6=1;
sR6=(4.40958675010914e-008)^0.5;
vr_a=markov1st_v000(A6 ,B6,[], 2,100);
sP6=(vr_a(3))^0.5;
[A6, Q6, R6]=dc2dc_v000(A6,B6*B6',sR6*sR6',1/100,2,1);
B6=Q6^0.5;
sR6=R6^0.5;

A7=A6;B7=B6;C7=C6;sR7=sR6;sP7=sP6;

% A7=0.999977286299473;
% B7=2.06907835784507e-006;
% C7=1;
% sR7=(3.74167215805145e-007)^0.5;
% sP7=(9.4241e-008)^0.5;
% [A7, Q7, R7]=dc2dc_v000(A7,B7*B7',sR7*sR7',1/100,2,1);
% B7=Q7^0.5;
% sR7=R7^0.5;

MA1=[cos(0) sin(0) 0;
    cos(0) sin(0) 0;
    cos(-pi/5) sin(-pi/5) 0;
    cos(-pi/2) sin(-pi/2) 0;
    cos(-pi/2) sin(-pi/2) 0];

MG1=[0 0 1;0 0 1];

MA2=[cos(pi/10) sin(pi/10) 0;
    cos(pi/8) sin(pi/8) 0;
    cos(pi/6) sin(pi/6) 0;
    cos(pi/4) sin(pi/4) 0;
    cos(pi/2) sin(pi/2) 0];

%%% Main model
%M=[MA1;MG];
M=[MA1;MG1;MA2];

%imu observation structure
Rimu=sR1*sR1';
Rimu=diagmat_v000(sR2*sR2',Rimu,1);
Rimu=diagmat_v000(sR3*sR3',Rimu,1);
Rimu=diagmat_v000(sR4*sR4',Rimu,1);
Rimu=diagmat_v000(sR5*sR5',Rimu,1);
Rimu=diagmat_v000(sR6*sR6',Rimu,1);
Rimu=diagmat_v000(sR7*sR7',Rimu,1);
Rimu=diagmat_v000(sR11*sR11',Rimu,1);
Rimu=diagmat_v000(sR12*sR12',Rimu,1);
Rimu=diagmat_v000(sR13*sR13',Rimu,1);
Rimu=diagmat_v000(sR14*sR14',Rimu,1);
Rimu=diagmat_v000(sR15*sR15',Rimu,1);

Cimu=C1;
Cimu=diagmat_v000(C2,Cimu,1);
Cimu=diagmat_v000(C3,Cimu,1);
Cimu=diagmat_v000(C4,Cimu,1);
Cimu=diagmat_v000(C5,Cimu,1);
Cimu=diagmat_v000(C6,Cimu,1);
Cimu=diagmat_v000(C7,Cimu,1);
Cimu=diagmat_v000(C11,Cimu,1);
Cimu=diagmat_v000(C12,Cimu,1);
Cimu=diagmat_v000(C13,Cimu,1);
Cimu=diagmat_v000(C14,Cimu,1);
Cimu=diagmat_v000(C15,Cimu,1);

[T Mls]=cp_Tparam_v000(M,Rimu);
Cimu_obs=[T*Cimu];
Rimu_obs=T*Rimu*T';
Rimu_inp=Mls*Rimu*Mls';

A=A1;
A=diagmat_v000(A2,A,1);
A=diagmat_v000(A3,A,1);
A=diagmat_v000(A4,A,1);
A=diagmat_v000(A5,A,1);
A=diagmat_v000(A6,A,1);
A=diagmat_v000(A7,A,1);
A=diagmat_v000(A11,A,1);
A=diagmat_v000(A12,A,1);
A=diagmat_v000(A13,A,1);
A=diagmat_v000(A14,A,1);
A=diagmat_v000(A15,A,1);

Q=B1*B1';
Q=diagmat_v000(B2*B2',Q,1);
Q=diagmat_v000(B3*B3',Q,1);
Q=diagmat_v000(B4*B4',Q,1);
Q=diagmat_v000(B5*B5',Q,1);
Q=diagmat_v000(B6*B6',Q,1);
Q=diagmat_v000(B7*B7',Q,1);
Q=diagmat_v000(B11*B11',Q,1);
Q=diagmat_v000(B12*B12',Q,1);
Q=diagmat_v000(B13*B13',Q,1);
Q=diagmat_v000(B14*B14',Q,1);
Q=diagmat_v000(B15*B15',Q,1);

P=sP1*sP1';
P=diagmat_v000(sP2*sP2',P,1);
P=diagmat_v000(sP3*sP3',P,1);
P=diagmat_v000(sP4*sP4',P,1);
P=diagmat_v000(sP5*sP5',P,1);
P=diagmat_v000(sP6*sP6',P,1);
P=diagmat_v000(sP7*sP7',P,1);
P=diagmat_v000(sP11*sP11',P,1);
P=diagmat_v000(sP12*sP12',P,1);
P=diagmat_v000(sP13*sP13',P,1);
P=diagmat_v000(sP14*sP14',P,1);
P=diagmat_v000(sP15*sP15',P,1);

Pwls=P;

% %%%model for the equivalent systems
%Equivalent models for identical systems
[Mls_e1, Ae1, Be1, Ce1, sRe1, sPe1]=cp_redsys_v000(MA1(:,1:2), A1, B1, C1, sR1, sP1);
[Mls_e2, Ae2, Be2, Ce2, sRe2, sPe2]=cp_redsys_v000(MA2(:,1:2), A11, B11, C11, sR11, sP11);
[Mls_e3, Ae3, Be3, Ce3, sRe3, sPe3]=cp_redsys_v000(MG1(:,3), A6, B6, C6, sR6, sP6);

%imu observation structure
Rimue=sRe1*sRe1';
Rimue=diagmat_v000(sR6*sR6',Rimue,1);
Rimue=diagmat_v000(sR7*sR7',Rimue,1);
Rimue=diagmat_v000(sRe2*sRe2',Rimue,1);

Cimue=Ce1;
Cimue=diagmat_v000(C6,Cimue,1);
Cimue=diagmat_v000(C7,Cimue,1);
Cimue=diagmat_v000(Ce2,Cimue,1);

Me=[1 0 0;0 1 0;MG1;1 0 0;0 1 0];
[Te Mlse]=cp_Tparam_v000(Me,Rimue);
Cimu_obse=[Te*Cimue];
Rimu_obse=Te*Rimue*Te';
Rimu_inpe=Mlse*Rimue*Mlse';

%single system model
Ae=Ae1;
Ae=diagmat_v000(A6,Ae,1);
Ae=diagmat_v000(A7,Ae,1);
Ae=diagmat_v000(Ae2,Ae,1);

Qe=Be1*Be1';
Qe=diagmat_v000(B6*B6',Qe,1);
Qe=diagmat_v000(B7*B7',Qe,1);
Qe=diagmat_v000(Be2*Be2',Qe,1);

Pe=sPe1*sPe1';
Pe=diagmat_v000(sP6*sP6',Pe,1);
Pe=diagmat_v000(sP7*sP7',Pe,1);
Pe=diagmat_v000(sPe2*sPe2',Pe,1);

%%%%%%%%%%%%%%%%%%
nit=60*100;
nst=size(A,1);
nste=size(Ae,1);

x=(P^0.5)*randn(nst,1);
xest=zeros(size(A,1),1);
xeste=zeros(size(Ae,1),1);

sRimu=Rimu^0.5;
nsen=size(sRimu,1);
sQ=Q^0.5;

%debug variables
debctr=0;
debx=zeros(nst,nit);
debp=zeros(nst,nit);
debxe=zeros(nste,nit);
debpe=zeros(nste,nit);
debuopt=zeros(3,nit);
debuopte=zeros(3,nit);
debuwls=zeros(3,nit);
debpopt=zeros(3,nit);
debpopte=zeros(3,nit);
debpwls=zeros(3,nit);
debeq=zeros(5,nit);

% debctr=debctr+1;
% debx(:,debctr)=xest;
% debp(:,debctr)=diag(P).^0.5;
% debxe(:,debctr)=xeste;
% debpe(:,debctr)=diag(Pe).^0.5;

%Start main loop
for in=1:nit
    %observations
    u=[0;0;0];

    y=M*u+Cimu*x+sRimu*randn(nsen,1);
    imu_inp=Mls*y;
    imu_obs=T*y;

    ye=[Mls_e1*y(1:5);y(6:7);Mls_e2*y(8:end)];
    imu_inpe=Mlse*ye;
    imu_obse=Te*ye;

    %Kalman
    K=P*Cimu_obs'*inv(Cimu_obs*P*Cimu_obs'+Rimu_obs);
    P=P-K*Cimu_obs*P;
    dz=K*(imu_obs-Cimu_obs*xest);
    xest=xest+dz;

    Ke=Pe*Cimu_obse'*inv(Cimu_obse*Pe*Cimu_obse'+Rimu_obse);
    Pe=Pe-Ke*Cimu_obse*Pe;
    dze=Ke*(imu_obse-Cimu_obse*xeste);
    xeste=xeste+dze;

    mx_a=Mls*Cimu*P*Cimu'*Mls';
    mx_b=Mlse*Cimue*Pe*Cimue'*Mlse';
    mx_c=Mls*Cimu*Pwls*Cimu'*Mls';

    debctr=debctr+1;
    debx(:,debctr)=xest;
    debp(:,debctr)=diag(P).^0.5;
    debxe(:,debctr)=xeste;
    debpe(:,debctr)=diag(Pe).^0.5;
    debuopt(:,debctr)=Mls*y-Mls*Cimu*xest;
    debuopte(:,debctr)=Mlse*ye-Mlse*Cimue*xeste;
    debuwls(:,debctr)=Mls*(y);
    debpopt(:,debctr)=diag(mx_a+Rimu_inp).^0.5;
    debpopte(:,debctr)=diag(mx_b+Rimu_inpe).^0.5;
    debpwls(:,debctr)=diag(mx_c+Rimu_inp).^0.5;
    debeq(1:2,debctr)=Mls_e1*y(1:5);
    debeq(3,debctr)=Mls_e3*y(6:7);
    debeq(4:5,debctr)=Mls_e2*y(8:end);

    %Compute the states of next cycle
    x=A*x+sQ*randn(nst,1);

    %prediction for the next states
    xest=A*xest;
    P=A*P*A'+Q;
    Pwls=A*Pwls*A'+Q;

    xeste=Ae*xeste;
    Pe=Ae*Pe*Ae'+Qe;

end

figure(10);
yy=1;
tt=[1:10:debctr]/60;
plot(tt,debuopte(yy,1:10:debctr));
hold on;
plot(tt,debuopt(yy,1:10:debctr));
plot(tt,debuwls(yy,1:10:debctr),'k');
plot(tt,debeq(0+yy,1:10:debctr),'r');
plot(tt,debeq(3+yy,1:10:debctr),'m');

plot(tt,debpopte(yy,1:10:debctr),'--');
plot(tt,debpopt(yy,1:10:debctr),'--');
plot(tt,debpwls(yy,1:10:debctr),'--k');
grid;
xlabel('Time (min)');
ylabel('X-Acceleration Error(m/sec^2)');
%legend('Opt. S. (u^{opt})', 'Red. Opt. S. (u^{red})','WLS S. (u^{wls})','Set1 Opt. (u^{A1})','Set2 Opt. (u^{A2})','Opt. SD.', 'Red. Opt. SD.','WLS SD');
legend('Opt. S. (u^{opt})', 'Red. Opt. S. (u^{red})','WLS S. (u^{wls})','Set1 Opt. (u^{A1})','Set2 Opt. (u^{A2})');

figure(11);
yy=2;
tt=[1:10:debctr]/60;
plot(tt,debuopte(yy,1:10:debctr));
hold on;
plot(tt,debuopt(yy,1:10:debctr));
plot(tt,debuwls(yy,1:10:debctr),'k');
plot(tt,debeq(0+yy,1:10:debctr),'r');
plot(tt,debeq(3+yy,1:10:debctr),'m');

plot(tt,debpopte(yy,1:10:debctr),'--');
plot(tt,debpopt(yy,1:10:debctr),'--');
plot(tt,debpwls(yy,1:10:debctr),'--k');
grid;
xlabel('Time (min)');
ylabel('Y-Acceleration Error(m/sec^2)');
%legend('Opt. S. (u^{opt})', 'Red. Opt. S. (u^{red})','WLS S. (u^{wls})','Set1 Opt. (u^{A1})','Set2 Opt. (u^{A2})','Opt. SD.', 'Red. Opt. SD.','WLS SD');
legend('Opt. S. (u^{opt})', 'Red. Opt. S. (u^{red})','WLS S. (u^{wls})','Set1 Opt. (u^{A1})','Set2 Opt. (u^{A2})');

return;


% [LEV,IND] = spyrBand(PYR,INDICES,LEVEL,BAND)
%
% Access a band from a steerable pyramid.
%
%   LEVEL indicates the scale (finest = 1, coarsest = spyrHt(INDICES)).
%
%   BAND (optional, default=1) indicates which subband
%     (1 = vertical, rest proceeding anti-clockwise).

% Eero Simoncelli, 6/96.

function res =  spyrBand(pyr,pind,level,band)

if (exist('level') ~= 1)
  level = 1;
end

if (exist('band') ~= 1)
  band = 1;
end

nbands = spyrNumBands(pind);
if ((band > nbands) | (band < 1))
  error(sprintf('Bad band number (%d) should be in range [1,%d].', band, nbands));
end

maxLev = spyrHt(pind);
if ((level > maxLev) | (level < 1))
  error(sprintf('Bad level number (%d), should be in range [1,%d].', level, maxLev));
end

firstband = 1 + band + nbands*(level-1);
res  = pyrBand(pyr, pind, firstband);


function varargout=minDist(varargin)

% function [D1,minIND]=minDist(V1,V2,maxVarSize,selfAvoid,numFreeBytes)

%% Parse input
if nargin<2
    error('Insufficient input arguments');
end

V1=varargin{1};
V2=varargin{2};
switch nargin
    case 2
        maxVarSize=[]; %Empty will force calcucation below
        selfAvoid=0;
        numFreeBytes=[];
    case 3
        maxVarSize=varargin{3};
        selfAvoid=0;
        numFreeBytes=[];
    case 4
        maxVarSize=varargin{3};
        selfAvoid=varargin{4};
        numFreeBytes=[];
    case 5
        maxVarSize=varargin{3};
        selfAvoid=varargin{4};
        numFreeBytes=varargin{5};
end

%Get free memory
if isempty(numFreeBytes)
    [numFreeBytes]=freeMemory;
end

%Get max variable size available
if isempty(maxVarSize)
    maxVarSize=numFreeBytes/2;
end

if isnan(maxVarSize)
    numSteps=1;
else
    %Derive class dependent variable size
    [~,b1]=maxnumel(V1(1),numFreeBytes);
    [~,b2]=maxnumel(V2(1),numFreeBytes);
    b=max([b1 b2]);
    numelVar=numel(V1)*numel(V2);
    varSize=numelVar*b;

    numSteps=ceil(varSize/maxVarSize);
    indSteps=round(linspace(0,size(V1,1),numSteps));
    indSteps=sort(unique(indSteps));
    numSteps=numel(indSteps);
end

if numSteps>1 %In steps
    D1=zeros(size(V1,1),1);
    minIND=zeros(size(V1,1),1);
    for q=1:1:numSteps-1
        v1=V1(indSteps(q)+1:indSteps(q+1),:);
        try
            d=dist(v1,V2'); %dist from Neural network toolbox
        catch
            d=distND(v1,V2); %GIBBON's dist function
        end
        if selfAvoid
            %Set "diagonal" to something too large so self is avoided in
            %minimum (could use NaN and nanmin but the latter is a toolbox
            %function)
            I=1:size(v1,1);
            J=indSteps(q)+1:indSteps(q+1);
            ind=sub2ind(size(d),I,J); %Indices of selfies
            d(ind)=1+max(d(:)); %Overide selfies
        end

        [min_d,min_ind]=min(d,[],2);
        D1(indSteps(q)+1:indSteps(q+1))=min_d;
        minIND(indSteps(q)+1:indSteps(q+1))=min_ind;
    end
else %In one go
    try
        D=dist(V1,V2'); %dist from Neural network toolbox
    catch
        D=distND(V1,V2); %GIBBON's dist function
    end
    if selfAvoid
        %Set "diagonal" to something too large so self is avoided in
        %minimum (could use NaN and nanmin but the latter is a toolbox
        %function)
        L=eye(size(D))>0;
        D(L)=1+max(D(:));
    end
    [D1,minIND]=min(D,[],2);
    D1=D1(:);
    minIND=minIND(:);
end

switch nargout
    case 1
        varargout{1}=D1;
    case 2
        varargout{1}=D1;
        varargout{2}=minIND;
    otherwise
        error('wrong number of output arguments');
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2018  Kevin Mattheus Moerman
%
% 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
% (at your option) 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 [ha hb hc] = shadedplot(x, y1, y2, varargin)

% SHADEDPLOT draws two lines on a plot and shades the area between those
% lines.
%
% SHADEDPLOT(x, y1, y2)
%   All of the arguments are vectors of the same length, and each y-vector is
%   horizontal (i.e. size(y1) = [1  N]). Vector x contains the x-axis values,
%   and y1:y2 contain the y-axis values.
%
%   Plot y1 and y2 vs x, then shade the area between those two
%   lines. Highlight the edges of that band with lines.
%
%   SHADEDPLOT(x, y1, y2, areacolor, linecolor)
%   The arguments areacolor and linecolor allow the user to set the color
%   of the shaded area and the boundary lines. These arguments must be
%   either text values (see the help for the PLOT function) or a
%   3-element vector with the color values in RGB (see the help for
%   COLORMAP).
%
%   [HA HB HC = SHADEDPLOT(x, y1, y2) returns three handles to the calling
%   function. HA is a vector of handles to areaseries objects (HA(2) is the
%   shaded area), HB is the handle to the first line (x vs y1), and HC is
%   the handle to the second line (x vs y2).
%
%   Example:
%
%     x1 = [1 2 3 4 5 6];
%     y1 = x1;
%     y2 = x1+1;
%     x3 = [1.5 2 2.5 3 3.5 4];
%     y3 = 2*x3;
%     y4 = 4*ones(size(x3));
%     ha = shadedplot(x1, y1, y2, [1 0.7 0.7], 'r'); %first area is red
%     hold on
%     hb = shadedplot(x3, y3, y4, [0.7 0.7 1]); %second area is blue
%     hold off

% plot the shaded area
y = [y1; (y2-y1)]';
ha = area(x, y);
set(ha(1), 'FaceColor', 'none') % this makes the bottom area invisible
set(ha, 'LineStyle', 'none')

% plot the line edges
hold on
hb = plot(x, y1, 'LineWidth', 1);
hc = plot(x, y2, 'LineWidth', 1);
hold off

% set the line and area colors if they are specified
switch length(varargin)
        case 0
        case 1
                set(ha(2), 'FaceColor', varargin{1})
        case 2
                set(ha(2), 'FaceColor', varargin{1})
                set(hb, 'Color', varargin{2})
                set(hc, 'Color', varargin{2})
        otherwise
end

% put the grid on top of the colored area
set(gca, 'Layer', 'top')
grid off


% -   Minus.
%    X - Y subtracts matrix Y from X. X and Y must have compatible sizes. In
%    the simplest cases, they can be the same size or one can be a scalar.
%    Two inputs have compatible sizes if, for every dimension, the dimension
%    sizes of the inputs are either the same or one of them is 1.
%
%    C = MINUS(A,B) is called for the syntax 'A - B' when A or B is an
%    object.
%
%    Reference page in Doc Center
%       doc minus
%
%    Other functions named minus
%
%       calendarDuration/minus    fints/minus       sym/minus
%       cell/minus                gpuArray/minus    tall/minus
%       codistributed/minus       LagOp/minus       timeseries/minus
%       datetime/minus            mtree/minus       ts/minus
%       duration/minus
%


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtRayCube.m                                  |
%|    #    |   VERSION    : 0.41                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.04.2018                                    |
%| ( === ) |   SYNOPSIS   : Ray tracing with absorbing cube               |
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Parameters
L     = [5 4 3]
% Xsrc  = [3.9 2.8 1.7]
% Xmes  = [2.1 2.2 1.3]
Xsrc  = [4 2 1.7];
Xmes  = [2 2 1.7];
Nray  = 1e5
rad   = 0.2

% Read mesh
mesh = msh('cube1e1.mesh');

% Mesh reshape to feat dimensions in L
mesh.vtx = 0.5 .* (1 + mesh.vtx);
mesh.vtx = (ones(size(mesh.vtx,1),1)*L) .* mesh.vtx;

% Initialize ray
ray = ray(mesh,Xsrc,Nray);

% Graphical sphere
[X,Y,Z] = sphere(50);
X = Xmes(1) + rad*X; Y = Xmes(2) + rad*Y; Z = Xmes(3) + rad*Z;

% Graphical representation
figure
plot(mesh)
hold on
% plot(ray)
surf(X,Y,Z,ones(size(X)))
axis equal;
xlabel('X');   ylabel('Y');   zlabel('Z');
alpha(0.3)
colorbar

% Material properties (http://www.odeon.dk/material-manufactures)
ray.msh.col(mesh.col==1) = 10;    % X = 1
ray.msh.col(mesh.col==2) = 20;    % Y = 1
ray.msh.col(mesh.col==3) = 30;    % X = 0
ray.msh.col(mesh.col==4) = 40;    % Y = 0
ray.msh.col(mesh.col==5) = 50;    % Z = 0
ray.msh.col(mesh.col==6) = 60;    % Z = 1
% ray.msh.col(:) = 60;

% Maximum distances
r1000 = rad/2 * sqrt(Nray/1000);
r100  = rad/2 * sqrt(Nray/100);
r10   = rad/2 * sqrt(Nray/10);
rMax  = rad/2 * sqrt(Nray/2);

% Ray-tracing
tic
ray = ray.tracer(100,rMax);
toc
% plot(ray)

% Images sources
tic
[img,nrg] = ray.image(Xmes,rad,rMax);
toc

% Graphical representation for images
n   = size(img,1);
tmp = ones(n,1)*Xmes + img;
tmp = msh(tmp,(1:n)');
plot(tmp,10*log10(nrg(:,1)));
axis equal
colorbar

% Analytical solution
load('odeon.mat')
num = [30 ; % X = 0
    10 ;    % X = 1
    40 ;    % Y = 0
    20 ;    % Y = 1
    50 ;    % Z = 0
    60];    % Z = 1
% num = 60 * ones(6,1);
mat = odeon.mat(num,:);
rhm = 30;
air = 5.5 * (50/rhm) .* (odeon.frq/1000).^1.7 * 1e-4;
tic
[imgRef,nrgRef] = rayCubeAnalytic(L,mat,air,Xsrc,Xmes,rMax);
toc

% Graphical representation
plot3(imgRef(:,1)+Xmes(1),imgRef(:,2)+Xmes(2),imgRef(:,3)+Xmes(3),'ok')

% Data
r    = sqrt(sum(img.^2,2));
rRef = sqrt(sum(imgRef.^2,2));
sol  = mean(nrg,2);
ref  = mean(nrgRef,2);

% Energy error in dB
figure
plot(rRef,10*log10(ref),'or',r,10*log10(sol),'+b')
hold on
plot([r1000 r1000],[-60,0],'k--')
text(r1000,-55,' n = 1000')
plot([r100 r100],[-60,0],'k--')
text(r100,-57,' n = 100')
plot([r10 r10],[-60,0],'k--')
text(r10,-55,' n = 10')
plot([rMax rMax],[-60,0],'k--')
text(rMax,-55,' n = 1')
xlabel('Distance source mesure')
ylabel('Energie mesuree (dB)')
title('Energie mesuree selon la distance de mesure')
legend({'Analytique','Statistique'})
grid on

% Audio file
[audio,fs] = audioread('anechoicVoice.wav');

% Fir from images
T = floor(r/340*fs) + 1;
for i = 1:size(nrg,2)
    fir8(:,i) = accumarray(T,nrg(:,i),[max(T) 1]);
end

% Bank filtering
dir8 = ray.bank(256,fs);
rir  = 0;
abs(sum(sum(dir8,2)) - 1)
for i = 1:size(dir8,2)
    rir = rir + fftfilt(dir8(:,i),fir8(:,i));
%     figure
%     freqz(dir8(:,i),1,2*256)
end

% Graphical representation
figure
plot(rir)

% Audio rendering (5 seconds)
out = fftfilt(rir,audio(1:5*fs));
% sound(out,fs)

disp('~~> Michto gypsilab !')


function soln = directCollocation(problem)
% soln = directCollocation(problem)
%
% OptimTraj utility function
%
% This function is designed to be called by either "trapezoid" or
% "hermiteSimpson". It actually calls FMINCON to solve the trajectory
% optimization problem.
%
% Analytic gradients are supported.
%
% NOTES:
%
%   If analytic gradients are used, then the sparsity pattern is returned
%   in the struct: soln.info.sparsityPattern. View it using spy().
%

%To make code more readable
G = problem.guess;
B = problem.bounds;
F = problem.func;
Opt = problem.options;

nGrid = length(F.weights);

flagGradObj = strcmp(Opt.nlpOpt.GradObj,'on');
flagGradCst = strcmp(Opt.nlpOpt.GradConstr,'on');

% Print out notice about analytic gradients
if Opt.verbose > 0
    if flagGradObj
        fprintf('      - using analytic gradients of objective function\n');
    end
    if flagGradCst
        fprintf('      - using analytic gradients of constraint function\n');
    end
    fprintf('\n');
end

% Interpolate the guess at the grid-points for transcription:
guess.tSpan = G.time([1,end]);
guess.time = linspace(guess.tSpan(1), guess.tSpan(2), nGrid);
guess.state = interp1(G.time', G.state', guess.time')';
guess.control = interp1(G.time', G.control', guess.time')';

[zGuess, pack] = packDecVar(guess.time, guess.state, guess.control);

if flagGradCst || flagGradObj
    gradInfo = grad_computeInfo(pack);
end

% Unpack all bounds:
tLow = linspace(B.initialTime.low, B.finalTime.low, nGrid);
xLow = [B.initialState.low, B.state.low*ones(1,nGrid-2), B.finalState.low];
uLow = B.control.low*ones(1,nGrid);
zLow = packDecVar(tLow,xLow,uLow);

tUpp = linspace(B.initialTime.upp, B.finalTime.upp, nGrid);
xUpp = [B.initialState.upp, B.state.upp*ones(1,nGrid-2), B.finalState.upp];
uUpp = B.control.upp*ones(1,nGrid);
zUpp = packDecVar(tUpp,xUpp,uUpp);

%%%% Set up problem for fmincon:
if flagGradObj
    P.objective = @(z)( ...
        myObjGrad(z, pack, F.pathObj, F.bndObj, F.weights, gradInfo) );   %Analytic gradients
    [~, objGradInit] = P.objective(zGuess);
    sparsityPattern.objective = (objGradInit~=0)';  % Only used for visualization!
else
    P.objective = @(z)( ...
        myObjective(z, pack, F.pathObj, F.bndObj, F.weights) );   %Numerical gradients
end
if flagGradCst
    P.nonlcon = @(z)( ...
        myCstGrad(z, pack, F.dynamics, F.pathCst, F.bndCst, F.defectCst, gradInfo) ); %Analytic gradients
    [~,~,cstIneqInit,cstEqInit] = P.nonlcon(zGuess);
    sparsityPattern.equalityConstraint = (cstEqInit~=0)';  % Only used for visualization!
    sparsityPattern.inequalityConstraint = (cstIneqInit~=0)';  % Only used for visualization!
else
    P.nonlcon = @(z)( ...
        myConstraint(z, pack, F.dynamics, F.pathCst, F.bndCst, F.defectCst) ); %Numerical gradients
end

P.x0 = zGuess;
P.lb = zLow;
P.ub = zUpp;
P.Aineq = []; P.bineq = [];
P.Aeq = []; P.beq = [];
P.options = Opt.nlpOpt;
P.solver = 'fmincon';

%%%% Call fmincon to solve the non-linear program (NLP)
tic;
[zSoln, objVal,exitFlag,output] = fmincon(P);
[tSoln,xSoln,uSoln] = unPackDecVar(zSoln,pack);
nlpTime = toc;

%%%% Store the results:

soln.grid.time = tSoln;
soln.grid.state = xSoln;
soln.grid.control = uSoln;

soln.interp.state = @(t)( interp1(tSoln',xSoln',t','linear',nan)' );
soln.interp.control = @(t)( interp1(tSoln',uSoln',t','linear',nan)' );

soln.info = output;
soln.info.nlpTime = nlpTime;
soln.info.exitFlag = exitFlag;
soln.info.objVal = objVal;

if flagGradCst || flagGradObj  % Then return sparsity pattern for visualization
    if flagGradObj
        [~, objGradInit] = P.objective(zSoln);
        sparsityPattern.objective = (objGradInit~=0)';
    end
    if flagGradCst
        [~,~,cstIneqInit,cstEqInit] = P.nonlcon(zSoln);
        sparsityPattern.equalityConstraint = (cstEqInit~=0)';
        sparsityPattern.inequalityConstraint = (cstIneqInit~=0)';
    end
    soln.info.sparsityPattern = sparsityPattern;
end

soln.problem = problem;  % Return the fully detailed problem struct

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%
%%%%                          SUB FUNCTIONS                            %%%%
%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [z,pack] = packDecVar(t,x,u)
%
% This function collapses the time (t), state (x)
% and control (u) matricies into a single vector
%
% INPUTS:
%   t = [1, nTime] = time vector (grid points)
%   x = [nState, nTime] = state vector at each grid point
%   u = [nControl, nTime] = control vector at each grid point
%
% OUTPUTS:
%   z = column vector of 2 + nTime*(nState+nControl) decision variables
%   pack = details about how to convert z back into t,x, and u
%       .nTime
%       .nState
%       .nControl
%

nTime = length(t);
nState = size(x,1);
nControl = size(u,1);

tSpan = [t(1); t(end)];
xCol = reshape(x, nState*nTime, 1);
uCol = reshape(u, nControl*nTime, 1);

indz = reshape(2+(1:numel(u)+numel(x)),nState+nControl,nTime);

% index of time, state, control variables in the decVar vector
tIdx = 1:2;
xIdx = indz(1:nState,:);
uIdx = indz(nState+(1:nControl),:);

% decision variables
% variables are indexed so that the defects gradients appear as a banded
% matrix
z = zeros(2+numel(indz),1);
z(tIdx(:),1) = tSpan;
z(xIdx(:),1) = xCol;
z(uIdx(:),1) = uCol;

pack.nTime = nTime;
pack.nState = nState;
pack.nControl = nControl;
pack.tIdx = tIdx;
pack.xIdx = xIdx;
pack.uIdx = uIdx;

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [t,x,u] = unPackDecVar(z,pack)
%
% This function unpacks the decision variables for
% trajectory optimization into the time (t),
% state (x), and control (u) matricies
%
% INPUTS:
%   z = column vector of 2 + nTime*(nState+nControl) decision variables
%   pack = details about how to convert z back into t,x, and u
%       .nTime
%       .nState
%       .nControl
%
% OUTPUTS:
%   t = [1, nTime] = time vector (grid points)
%   x = [nState, nTime] = state vector at each grid point
%   u = [nControl, nTime] = control vector at each grid point
%

nTime = pack.nTime;
nState = pack.nState;
nControl = pack.nControl;

t = linspace(z(1),z(2),nTime);

x = z(pack.xIdx);
u = z(pack.uIdx);

% make sure x and u are returned as vectors, [nState,nTime] and
% [nControl,nTime]
x = reshape(x,nState,nTime);
u = reshape(u,nControl,nTime);

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function cost = myObjective(z,pack,pathObj,bndObj,weights)
%
% This function unpacks the decision variables, sends them to the
% user-defined objective functions, and then returns the final cost
%
% INPUTS:
%   z = column vector of decision variables
%   pack = details about how to convert decision variables into t,x, and u
%   pathObj = user-defined integral objective function
%   endObj = user-defined end-point objective function
%
% OUTPUTS:
%   cost = scale cost for this set of decision variables
%

[t,x,u] = unPackDecVar(z,pack);

% Compute the cost integral along trajectory
if isempty(pathObj)
    integralCost = 0;
else
    dt = (t(end)-t(1))/(pack.nTime-1);
    integrand = pathObj(t,x,u);  %Calculate the integrand of the cost function
    integralCost = dt*integrand*weights;  %Trapazoidal integration
end

% Compute the cost at the boundaries of the trajectory
if isempty(bndObj)
    bndCost = 0;
else
    t0 = t(1);
    tF = t(end);
    x0 = x(:,1);
    xF = x(:,end);
    bndCost = bndObj(t0,x0,tF,xF);
end

cost = bndCost + integralCost;

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [c, ceq] = myConstraint(z,pack,dynFun, pathCst, bndCst, defectCst)
%
% This function unpacks the decision variables, computes the defects along
% the trajectory, and then evaluates the user-defined constraint functions.
%
% INPUTS:
%   z = column vector of decision variables
%   pack = details about how to convert decision variables into t,x, and u
%   dynFun = user-defined dynamics function
%   pathCst = user-defined constraints along the path
%   endCst = user-defined constraints at the boundaries
%
% OUTPUTS:
%   c = inequality constraints to be passed to fmincon
%   ceq = equality constraints to be passed to fmincon
%

[t,x,u] = unPackDecVar(z,pack);

%%%% Compute defects along the trajectory:
dt = (t(end)-t(1))/(length(t)-1);
f = dynFun(t,x,u);
defects = defectCst(dt,x,f);

%%%% Call user-defined constraints and pack up:
[c, ceq] = collectConstraints(t,x,u,defects, pathCst, bndCst);

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%
%%%%            Additional Sub-Functions for Gradients                 %%%%
%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

%%%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %%%%

function gradInfo = grad_computeInfo(pack)
%
% This function computes the matrix dimensions and indicies that are used
% to map the gradients from the user functions to the gradients needed by
% fmincon. The key difference is that the gradients in the user functions
% are with respect to their input (t,x,u) or (t0,x0,tF,xF), while the
% gradients for fmincon are with respect to all decision variables.
%
% INPUTS:
%   nDeVar = number of decision variables
%   pack = details about packing and unpacking the decision variables
%       .nTime
%       .nState
%       .nControl
%
% OUTPUTS:
%   gradInfo = details about how to transform gradients
%

nTime = pack.nTime;
nState = pack.nState;
nControl = pack.nControl;
nDecVar = 2 + nState*nTime + nControl*nTime;

zIdx = 1:nDecVar;
gradInfo.nDecVar = nDecVar;
[tIdx, xIdx, uIdx] = unPackDecVar(zIdx,pack);
gradInfo.tIdx = tIdx([1,end]);
gradInfo.xuIdx = [xIdx;uIdx];

%%%% Compute gradients of time:
% alpha = (0..N-1)/(N-1)
% t = alpha*tUpp + (1-alpha)*tLow
alpha = (0:(nTime-1))/(nTime-1);
gradInfo.alpha = [1-alpha; alpha];

if (gradInfo.tIdx(1)~=1 || gradInfo.tIdx(end)~=2)
    error('The first two decision variables must be the initial and final time')
end
gradInfo.dtGrad = [-1; 1]/(nTime-1);

%%%% Compute gradients of state
gradInfo.xGrad = zeros(nState,nTime,nDecVar);
for iTime=1:nTime
    for iState=1:nState
        gradInfo.xGrad(iState,iTime,xIdx(iState,iTime)) = 1;
    end
end

%%%% For unpacking the boundary constraints and objective:
gradInfo.bndIdxMap = [tIdx(1); xIdx(:,1); tIdx(end); xIdx(:,end)];

end

%%%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %%%%

function [c, ceq, cGrad, ceqGrad] = grad_collectConstraints(t,x,u,defects, defectsGrad, pathCst, bndCst, gradInfo)
% [c, ceq, cGrad, ceqGrad] = grad_collectConstraints(t,x,u,defects, defectsGrad, pathCst, bndCst, gradInfo)
%
% OptimTraj utility function.
%
% Collects the defects, calls user-defined constraints, and then packs
% everything up into a form that is good for fmincon. Additionally, it
% reshapes and packs up the gradients of these constraints.
%
% INPUTS:
%   t = time vector
%   x = state matrix
%   u = control matrix
%   defects = defects matrix
%   pathCst = user-defined path constraint function
%   bndCst = user-defined boundary constraint function
%
% OUTPUTS:
%   c = inequality constraint for fmincon
%   ceq = equality constraint for fmincon
%

ceq_dyn = reshape(defects,numel(defects),1);
ceq_dynGrad = grad_flattenPathCst(defectsGrad);

%%%% Compute the user-defined constraints:
if isempty(pathCst)
    c_path = [];
    ceq_path = [];
    c_pathGrad = [];
    ceq_pathGrad = [];
else
    [c_pathRaw, ceq_pathRaw, c_pathGradRaw, ceq_pathGradRaw] = pathCst(t,x,u);
    c_path = reshape(c_pathRaw,numel(c_pathRaw),1);
    ceq_path = reshape(ceq_pathRaw,numel(ceq_pathRaw),1);
    c_pathGrad = grad_flattenPathCst(grad_reshapeContinuous(c_pathGradRaw,gradInfo));
    ceq_pathGrad = grad_flattenPathCst(grad_reshapeContinuous(ceq_pathGradRaw,gradInfo));
end
if isempty(bndCst)
    c_bnd = [];
    ceq_bnd = [];
    c_bndGrad = [];
    ceq_bndGrad = [];
else
    t0 = t(1);
    tF = t(end);
    x0 = x(:,1);
    xF = x(:,end);
    [c_bnd, ceq_bnd, c_bndGradRaw, ceq_bndGradRaw] = bndCst(t0,x0,tF,xF);
    c_bndGrad = grad_reshapeBoundary(c_bndGradRaw,gradInfo);
    ceq_bndGrad = grad_reshapeBoundary(ceq_bndGradRaw,gradInfo);
end

%%%% Pack everything up:
c = [c_path;c_bnd];
ceq = [ceq_dyn; ceq_path; ceq_bnd];

cGrad = [c_pathGrad;c_bndGrad]';
ceqGrad = [ceq_dynGrad; ceq_pathGrad; ceq_bndGrad]';

end

%%%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %%%%

function C = grad_flattenPathCst(CC)
%
% This function takes a path constraint and reshapes the first two
% dimensions so that it can be passed to fmincon
%
if isempty(CC)
    C = [];
else
    [n1,n2,n3] = size(CC);
    C = reshape(CC,n1*n2,n3);
end

end

%%%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %%%%

function CC = grad_reshapeBoundary(C,gradInfo)
%
% This function takes a boundary constraint or objective from the user
% and expands it to match the full set of decision variables
%

CC = zeros(size(C,1),gradInfo.nDecVar);
CC(:,gradInfo.bndIdxMap) = C;

end

%%%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %%%%

function grad = grad_reshapeContinuous(gradRaw,gradInfo)
% grad = grad_reshapeContinuous(gradRaw,gradInfo)
%
% OptimTraj utility function.
%
% This function converts the raw gradients from the user function into
% gradients with respect to the decision variables.
%
% INPUTS:
%   stateRaw = [nOutput,nInput,nTime]
%
% OUTPUTS:
%   grad = [nOutput,nTime,nDecVar]
%

if isempty(gradRaw)
    grad = [];
else
    [nOutput, ~, nTime] = size(gradRaw);

    grad = zeros(nOutput,nTime,gradInfo.nDecVar);

    % First, loop through and deal with time.
    timeGrad = gradRaw(:,1,:); timeGrad = permute(timeGrad,[1,3,2]);
    for iOutput=1:nOutput
        A = ([1;1]*timeGrad(iOutput,:)).*gradInfo.alpha;
        grad(iOutput,:,gradInfo.tIdx) = permute(A,[3,2,1]);
    end

    % Now deal with state and control:
    for iOutput=1:nOutput
        for iTime=1:nTime
            B = gradRaw(iOutput,2:end,iTime);
            grad(iOutput,iTime,gradInfo.xuIdx(:,iTime)) = permute(B,[3,1,2]);
        end
    end
end

end

%%%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %%%%

function [cost, costGrad] = myObjGrad(z,pack,pathObj,bndObj,weights,gradInfo)
%
% This function unpacks the decision variables, sends them to the
% user-defined objective functions, and then returns the final cost
%
% INPUTS:
%   z = column vector of decision variables
%   pack = details about how to convert decision variables into t,x, and u
%   pathObj = user-defined integral objective function
%   endObj = user-defined end-point objective function
%
% OUTPUTS:
%   cost = scale cost for this set of decision variables
%

%Unpack the decision variables:
[t,x,u] = unPackDecVar(z,pack);

% Time step for integration:
dt = (t(end)-t(1))/(length(t)-1);
dtGrad = gradInfo.dtGrad;
nTime = length(t);
nState = size(x,1);
nControl = size(u,1);
nDecVar = length(z);

% Compute the cost integral along the trajectory
if isempty(pathObj)
    integralCost = 0;
    integralCostGrad = zeros(nState+nControl,1);
else

    % Objective function integrand and gradients:
    [obj, objGradRaw] = pathObj(t,x,u);
    nInput = size(objGradRaw,1);
    objGradRaw = reshape(objGradRaw,1,nInput,nTime);
    objGrad = grad_reshapeContinuous(objGradRaw,gradInfo);

    % integral objective function
    unScaledIntegral = obj*weights;
    integralCost = dt*unScaledIntegral;

    % Gradient of integral objective function
    dtGradTerm = zeros(1,nDecVar);
    dtGradTerm(1) = dtGrad(1)*unScaledIntegral;
    dtGradTerm(2) = dtGrad(2)*unScaledIntegral;
    objGrad = reshape(objGrad,nTime,nDecVar);
    integralCostGrad = ...
        dtGradTerm + ...
        dt*sum(objGrad.*(weights*ones(1,nDecVar)),1);
end

% Compute the cost at the boundaries of the trajectory
if isempty(bndObj)
    bndCost = 0;
    bndCostGrad = zeros(1,nDecVar);
else
    t0 = t(1);
    tF = t(end);
    x0 = x(:,1);
    xF = x(:,end);
    [bndCost, bndCostGradRaw] = bndObj(t0,x0,tF,xF);
    bndCostGrad = grad_reshapeBoundary(bndCostGradRaw,gradInfo);
end

% Cost function
cost = bndCost + integralCost;

% Gradients
costGrad = bndCostGrad + integralCostGrad;

end

%%%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%

function [c, ceq, cGrad, ceqGrad] = myCstGrad(z,pack,dynFun, pathCst, bndCst, defectCst, gradInfo)
%
% This function unpacks the decision variables, computes the defects along
% the trajectory, and then evaluates the user-defined constraint functions.
%
% INPUTS:
%   z = column vector of decision variables
%   pack = details about how to convert decision variables into t,x, and u
%   dynFun = user-defined dynamics function
%   pathCst = user-defined constraints along the path
%   endCst = user-defined constraints at the boundaries
%
% OUTPUTS:
%   c = inequality constraints to be passed to fmincon
%   ceq = equality constraints to be passed to fmincon
%

%Unpack the decision variables:
[t,x,u] = unPackDecVar(z,pack);

% Time step for integration:
dt = (t(end)-t(1))/(length(t)-1);
dtGrad = gradInfo.dtGrad;

% Gradient of the state with respect to decision variables
xGrad = gradInfo.xGrad;

%%%% Compute defects along the trajectory:
[f, fGradRaw] = dynFun(t,x,u);
fGrad = grad_reshapeContinuous(fGradRaw,gradInfo);

[defects, defectsGrad] = defectCst(dt,x,f,...
    dtGrad, xGrad, fGrad);

% Compute gradients of the user-defined constraints and then pack up:
[c, ceq, cGrad, ceqGrad] = grad_collectConstraints(t,x,u,...
    defects, defectsGrad, pathCst, bndCst, gradInfo);

end


function J = pidtest(G,dt,parms)

s = tf('s');
K = parms(1) + parms(2)/s + parms(3)*s/(1+.001*s);
Loop = series(K,G);
ClosedLoop = feedback(Loop,1);
t = 0:dt:20;
[y,t] = step(ClosedLoop,t);

CTRLtf = K/(1+K*G);
u = lsim(K,1-y,t);

Q = 1;
R = .001;
J = dt*sum(Q*(1-y(:)).^2 + R*u(:).^2)

step(ClosedLoop,t)
h = findobj(gcf,'type','line');
set(h,'linewidth',2);
drawnow


%
% Perform an iteration of MCMC
% for re-sampling all of the type-level
% variables, excluding relation types
%
function mcmc_iter_type(MH,M,lib)

    % shape types
    for sid=1:M.ns
        for bid=1:M.S{sid}.nsub
            MH.mh_shape_type(sid,bid,M,lib);
        end
    end

    % scale types
    for sid=1:M.ns
        for bid=1:M.S{sid}.nsub
            MH.mh_scale_type(sid,bid,M,lib);
        end
    end

    % specific relation parameters
    for sid=1:M.ns
       if strcmp(M.S{sid}.R.type,'unihist')
          MH.mh_gobal_position(sid,M,lib);
       elseif strcmp(M.S{sid}.R.type,'mid')
          MH.mh_eval_spot_type(sid,M,lib);
       end
    end

    % sub-stroke ids
    for sid=1:M.ns
        for bid=1:M.S{sid}.nsub
             MH.gibbs_substroke_id(sid,bid,M,lib);
        end
    end

end


function test_failed = test_libltfat_ifft(varargin)
test_failed = 0;

fprintf(' ===============  %s ================ \n',upper(mfilename));

definput.flags.complexity={'double','single'};
[flags]=ltfatarghelper({},definput,varargin);
dataPtr = [flags.complexity, 'Ptr'];

[~,~,enuminfo]=libltfatprotofile;
phaseconv = enuminfo.ltfat_phaseconvention;

fftwflags = struct('FFTW_MEASURE',0,'FFTW_ESTIMATE',64,'FFTW_PATIENT',32,'FFTW_DESTROY_INPUT',1,...
    'FFTW_UNALIGNED',2,'FFTW_EXHAUSTIVE',8,'FFTW_PRESERVE_INPUT',16);

Larr  = [350 350   9   1];
Warr  = [  1   3   3   1];

    for idx = 1:numel(Larr)
        L = Larr(idx);
        W = Warr(idx);

        f = randn(L,W,flags.complexity) + 1i*randn(L,W,flags.complexity);
        fin = complex2interleaved(f);
        fPtr = libpointer(dataPtr,fin);

        c = cast(randn(L,W)+1i*randn(L,W),flags.complexity);
        cout = complex2interleaved(c);
        coutPtr = libpointer(dataPtr,cout);

        truec = ifft(f)*L;

        funname = makelibraryname('ifft',flags.complexity,0);
        status = calllib('libltfat',funname,fPtr,L,W,coutPtr);

        res = norm(truec - interleaved2complex(coutPtr.Value),'fro');
        [test_failed,fail]=ltfatdiditfail(res+status,test_failed);
        fprintf(['ifft   L:%3i, W:%3i, %s %s %s\n'],L,W,flags.complexity,ltfatstatusstring(status),fail);

        % With plan
        c = cast(randn(L,W)+1i*randn(L,W),flags.complexity);
        cout = complex2interleaved(c);
        coutPtr = libpointer(dataPtr,cout);

        plan = libpointer();
        funname = makelibraryname('ifft_init',flags.complexity,0);
        statusInit = calllib('libltfat',funname,L,W,fPtr,coutPtr, fftwflags.FFTW_MEASURE, plan);

        funname = makelibraryname('ifft_execute',flags.complexity,0);
        statusExecute = calllib('libltfat',funname,plan);

        res = norm(truec - interleaved2complex(coutPtr.Value),'fro');
        [test_failed,fail]=ltfatdiditfail(res+statusInit,test_failed);
        fprintf(['ifft L:%3i, W:%3i, %s %s %s\n'],L,W,flags.complexity,ltfatstatusstring(status),fail);

        c = cast(randn(L,W)+1i*randn(L,W),flags.complexity);
        cout = complex2interleaved(c);
        coutPtr = libpointer(dataPtr,cout);

        funname = makelibraryname('ifft_execute_newarray',flags.complexity,0);
        statusExecute = calllib('libltfat',funname,plan,fPtr,coutPtr);

        res = norm(truec - interleaved2complex(coutPtr.Value),'fro');
        [test_failed,fail]=ltfatdiditfail(res+statusInit,test_failed);
        fprintf(['FFT L:%3i, W:%3i, %s %s %s\n'],L,W,flags.complexity,ltfatstatusstring(status),fail);

        funname = makelibraryname('ifft_done',flags.complexity,0);
        statusDone = calllib('libltfat',funname,plan);

        %%%%%% Inplace
        c = f;
        cout = complex2interleaved(c);
        coutPtr = libpointer(dataPtr,cout);

        plan = libpointer();
        funname = makelibraryname('ifft_init',flags.complexity,0);
        statusInit = calllib('libltfat',funname,L,W, coutPtr, coutPtr, fftwflags.FFTW_MEASURE, plan);

        funname = makelibraryname('ifft_execute',flags.complexity,0);
        statusExecute = calllib('libltfat',funname,plan);

        res = norm(truec - interleaved2complex(coutPtr.Value),'fro');
        [test_failed,fail]=ltfatdiditfail(res+statusInit,test_failed);
        fprintf(['FFT L:%3i, W:%3i, %s %s %s\n'],L,W,flags.complexity,ltfatstatusstring(status),fail);

        c = f;
        cout = complex2interleaved(c);
        coutPtr = libpointer(dataPtr,cout);

        funname = makelibraryname('ifft_execute_newarray',flags.complexity,0);
        statusExecute = calllib('libltfat',funname,plan,coutPtr,coutPtr);

        res = norm(truec - interleaved2complex(coutPtr.Value),'fro');
        [test_failed,fail]=ltfatdiditfail(res+statusInit,test_failed);
        fprintf(['FFT L:%3i, W:%3i, %s %s %s\n'],L,W,flags.complexity,ltfatstatusstring(status),fail);

        funname = makelibraryname('ifft_done',flags.complexity,0);
        statusDone = calllib('libltfat',funname,plan);

    end


function [params] = setAttnParams(params)
  if params.attnFunc>0
    params.numSrcHidVecs = params.srcMaxLen-1;

    if params.attnGlobal % global
      params.numAttnPositions = params.numSrcHidVecs;
    else % local
      params.numAttnPositions = 2*params.posWin + 1;
    end
  else
    params.numSrcHidVecs = 0;
  end
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
% MRI data of a human knee, image courtesy by
% Thomas Netsch, Philips Medical Solutions, Hamburg
%==============================================================================

checkDataFile
if expfileExists, return; end;

infile = fullfile(FAIRpath,'kernel','data','knee3D.mat');
try
  load(infile);
catch
  fprintf('sorry, this data is not available');
  expfile = [];
  return;
end;

dataT = double(dataT);
dataR = double(dataR);
viewK = @(T) volView(T,omega,m,'isovalue',15,'view',[-20,-5],'colormap','bone(256)');

FAIRfigure(1,'color','w','figname',sprintf('%s/template',mfilename)); clf;
viewK(dataT); hold on; axis off; colormap(gray(128));
FAIRfigure(2,'color','w','figname',sprintf('%s/reference',mfilename)); clf;
viewK(dataR); hold on; axis off; colormap(gray(128));

[viewer,viewPara] = viewImage('reset','viewImage','imgmontage',...
  'direction','-zyx','colormap','bone(256)');
imgPara     = {'inter','linearInterMex'};
traPara     = {'trafo','affine3Dsparse'};
disPara     = {'distance','SSD'};
regPara     = {'regularizer','mfElastic','alpha',500,'mu',1,'lambda',0};

ML = getMultilevel({dataT,dataR},omega,m,'fig',2);

% save to outfile
save(expfile,'dataT','dataR','omega','m','ML',...
  'viewPara','imgPara','traPara','disPara','regPara');

checkDataFile
%==============================================================================


function omega = angle(f,ori,varargin)
% angle fibre to orientation or fibre to fibre
%
% Syntax
%
%   omega = angle(f,ori) % angle orientation to fibre
%   omega = angle(f1,f2) % angle fibre to fibre
%
% Input
%  f, f1, f2 - @fibre
%  ori - @orientation
%
% Output
%  omega - double
%
% See also
% orientation/angle

if isa(ori,'orientation')
  omega = angle(ori .\ f.r,f.h,varargin{:});

else
  omega = max(angle(f,orientation(ori),varargin{:}));

  % in the non symmetric case we have also
  %omega = min(angle(f.h,ori.h) + angle(f.r,ori.r), angle(f.h,-ori.h) + angle(f.r,-ori.r));

end

end


% DEMROBOTWIRELESSVARGPLVMDYN1 Run variational GPLVM on robot wireless
% data.
%
% DESC: Run variational GPLVM on robot wireless data with the option to
% also add dynamics to the model.
%
% COPYRIGHT : Michalis K. Titsias, 2009-2011
% COPYRIGHT : Neil D. Lawrence, 2009-2011
% COPYRIGHT : Andreas C. Damianou, 2010-2011
%
% VARGPLVM

%clear;

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'robotWireless';
clear timeStamps; % in case it's left from a previous experiment

%-- Constants
if ~exist('trainModel'), trainModel=1; end
if ~exist('indPoints'), indPoints = 70; end % Default: 50
if ~exist('latentDim'), latentDim = 10; end
if ~exist('dynUsed'),  dynUsed = 1; end
if ~exist('printDiagram'), printDiagram = 1; end
if ~exist('experimentNo'), experimentNo = 404; end
if ~exist('doPredictions'), doPredictions = 0; end
if ~exist('itNo'), itNo = 5000; end
if ~exist('initVardistIters'), initVardistIters = 400; end
if ~exist('dynamicKern')   ,     dynamicKern = {'rbf', 'white', 'bias'}; end
if ~exist('vardistCovarsMult'), vardistCovarsMult = 2.5; end

% load data
%[Y, lbls] = lvmLoadData(dataSetName);
%%% Like lvmLoadData but also parse times.
fprintf(1,'# Preparing the dataset...\n');
baseDir = datasetsDirectory;
dirSep = filesep;
[Ydat, timeStampsdat, wireless_x, wireless_y, storedMacs] = parseWirelessData([baseDir 'uw-floor.txt']);

Ydat = (Ydat + 85)/15;
lbls = 'connect';

if ~exist('dataToKeep')
    dataToKeep = size(Ydat,1); % Default should be: 215
end

Y = Ydat(1:dataToKeep, :);
timeStampsTraining = timeStampsdat(1:dataToKeep);
Ytest = Ydat((dataToKeep+1):end, :);
timeStampsTest = timeStampsdat((dataToKeep+1):end);
%%%

% Set up model
options = vargplvmOptions('dtcvar');
options.kern = 'rbfardjit';%{'rbfard2', 'bias', 'white'};
options.numActive = indPoints; % Default: 50

options.optimiser = 'scg2';
d = size(Y, 2);

capName = dataSetName;
capName(1) = upper(capName(1));
modelType = 'Vargplvm';
saveName = ['dem' capName modelType num2str(experimentNo) '.mat'];

if trainModel
    % demo using the variational inference method for the gplvm model
    fprintf(1,'# Creating the model...\n');
    model = vargplvmCreate(latentDim, d, Y, options);
    %
    model = vargplvmParamInit(model, model.m, model.X);
    model.vardist.covars = 0.5*ones(size(model.vardist.covars)) + 0.001*randn(size(model.vardist.covars));
    if dynUsed
        optionsDyn.type = 'vargpTime';
        optionsDyn.t=timeStampsTraining;
        optionsDyn.inverseWidth=20;
        optionsDyn.kern = dynamicKern;
        optionsDyn.vardistCovars = vardistCovarsMult;

        % Fill in with default values whatever is not already set
        optionsDyn = vargplvmOptionsDyn(optionsDyn);
        model = vargplvmAddDynamics(model, 'vargpTime', optionsDyn, optionsDyn.t, 0, 0,optionsDyn.seq);

        fprintf(1,'# Further calibration of the initial parameters...\n');
        model = vargplvmInitDynamics(model,optionsDyn);
    end
    modelInit = model;

    %     for q=1:model.q, plot(X(:,q)); hold on; plot(model.vardist.means(:,q),'r');
    %         plot(model.X_u(:,q),'+r');  pause(1); hold off;
    %     end
    %

    %model.dynamics.vardist.covars = ones(size(model.dynamics.vardist.covars)); % Good initialization

    % Optimise the model.
    model.initVardist = 1; model.learnSigmaf = 0;
    display = 1;
    fprintf(1,'# Optimising the model (initialising var.distr) for %d iters...\n',initVardistIters);
    model = vargplvmOptimise(model, display, initVardistIters);

    model.initVardist = 0; model.learnSigmaf = 1;

    fprintf(1,'# Optimising the model for %d iters...\n',itNo);
    model = vargplvmOptimise(model, display, itNo);

    fprintf(1,'# Saving the model...(%s)\n',saveName);

    prunedModel = vargplvmPruneModel(model);
    save(saveName, 'prunedModel');

    % order wrt to the inputScales
    mm = vargplvmReduceModel(model,2);
    % plot the two largest twe latent dimensions
    if exist('printDiagram') & printDiagram
        lvmPrintPlot(mm, lbls, capName, experimentNo);
    end

else
    load(saveName);
    model = vargplvmRestorePrunedModel(prunedModel, Y);
end

% fprintf(1,'Plotting inferred latent GPs (intial with blue and final  with red)...\n');
% for q=1:model.q, plot(X(:,q)); hold on; plot(model.vardist.means(:,q),'r');
%     plot(model.X_u(:,q),'+r');  pause(1); hold off;
% end

%%%% Reconstruct training data %%%%%%%%
% mu  = vargplvmPosteriorMeanVar(model, model.vardist.means, model.vardist.covars);
% Varmutr=mu;
% % TODO

if ~(doPredictions && dynUsed)
    return
end

%%%% predictions
Yts = Ytest;
Varmu = [];
Varsigma = [];

fprintf(1, '# Prediction...\n');
t_star = timeStampsTest;
[x varx] = vargplvmPredictPoint(model.dynamics, t_star);

% find the largest dimensions
[max, imax] = sort(model.kern.comp{1}.inputScales,'descend');

N = size(model.X,1);
Nstar = size(Yts,1);
figure;
fprintf(1,'The two larger latent dims after re-training with partial test data. Red are the test points\n');
plot(model.X(1:model.N,imax(1)), model.X(1:model.N,imax(2)),'--rs', 'Color','b');
hold on
plot(x(:,imax(1)),x(:,imax(2)),'--rs', 'Color','r');
title('Visualization of the latent space after re-training with partial test data');
hold off

figure;

% for i=1:size(t_star,1)
%     [mu, sigma] = vargplvmPosteriorMeanVar(model, x(i,:), varx(i,:));
%     Varmu(i,:) = mu;
%     Varsigma(i,:) = sigma;
%     %
% end

[mu, sigma] = vargplvmPosteriorMeanVar(model, x, varx);
Varmu = mu;
Varsigma = sigma;

% Find the square error
errsum = sum((Varmu - Yts).^2);
% Devide by the total number of test points to find a mean
error = errsum / size(Varmu,1);

% Find the mean sq. error for each datapoint, to see how it increases as
% we give points further to the future. This time sum accros dimensions
errSq = sum((Varmu - Yts).^2 ,2);
errPt = errSq ./ size(Varmu,2);

fprintf(1,'*** Mean error: %d\n', mean(error));

% See how the latent space looks like
N = size(model.X,1);
Nstar = size(t_star,1);
newX = zeros( N+Nstar, latentDim );
newX(1:N,:) = model.X(:,:);
newX(N+1:N+Nstar, :) = x(:,:);
figure;plot(errPt);
xlabel('time test datapoints','fontsize',18);
ylabel('Mean error','fontsize',18);
%---

% Now we will plot the latent space INCLUDING the newly predicted points.
model.X = newX;
modelTEMP = vargplvmCreate(latentDim, d, Ydat, options);
model.m = modelTEMP.m;
model.y = modelTEMP.y;
model.N = modelTEMP.N;
model.vardist.means = [model.vardist.means; x];
model.vardist.covars = [model.vardist.covars; varx];
%%%%%%%%% end: predictions

% order wrt to the inputScales
mm = vargplvmReduceModel(model,2);
% plot the two largest twe latent dimensions
if exist('printDiagram') & printDiagram
    lvmPrintPlot(mm, lbls, capName, experimentNo);
    %lvmResultsDynamic(model.type, dataSetName, experimentNo, 'robotWireless','vector')
end


function test_suite=equation_test
try % assignment of 'localfunctions' is necessary in Matlab >= 2016
    test_functions=localfunctions();
catch % no problem; early Matlab versions can use initTestSuite fine
end
initTestSuite;

function TestSetup
setenv('ISDISPLAY','0') % go faster! Fit only 2 voxels in FitData.m

function test_equation
disp('testing equations...')
MethodList = list_models;
for im = 1:length(MethodList)
    Model = str2func(MethodList{im}); Model = Model();
    if ~Model.voxelwise, continue; end
    disp(class(Model))
    ModelOpt = button2opts(Model.buttons,1);
    clear st Smodel
    for iopt=1:length(ModelOpt) % try all model options
        Model.options = ModelOpt(iopt);
        disp(['Testing ' class(Model) ' option:'])
        disp(Model.options)
        try Model = Model.UpdateFields; end
        try st{iopt} = Model.st; catch, try st{iopt} = mean([Model.lb(:),Model.ub(:)],2); catch, st{iopt} = ones(length(Model.xnames),1); end; end
        Smodel{iopt} = Model.equation(st{iopt});
        % CHECK CONSITENSY WITH PREVIOUS VERSIONS:
        if exist(['value_' class(Model) '.mat'],'file')
            % compare with Ground Truth
            GT = load(['value_' class(Model) '.mat']);
            [~,ModelOpttest,GTModelOpttest]=comp_struct(ModelOpt,GT.ModelOpt);
            if ~isempty(ModelOpttest) || ~isempty(GTModelOpttest)
                msg = [MethodList{im} ' buttons/options has changed' evalc('ModelOpttest, GTModelOpttest')];
            else
                msg = '';
            end
            assertVectorsAlmostEqual(st{iopt},GT.st{iopt},'relative',1e-4,[MethodList{im} ' starting point (st) changed... ' msg ' value_' MethodList{im} '.mat has to be regenerated.'])
            assertVectorsAlmostEqual(Smodel{iopt},GT.Smodel{iopt},'relative',1e-2,['Synthetic signal obtained from ' MethodList{im} ' equation is not consistent with previous versions... ' msg])
        elseif iopt == length(ModelOpt)
            save(['value_' class(Model) '.mat'],'Smodel','st','ModelOpt')
        end
    end
end

function TestTeardown
setenv('ISDISPLAY','') % go faster! Fit only 2 voxels in FitData.m


function val = essential_costE2cost(X, costE)
% Cost evaluation at X given function handle in the Essential matrix E.
%
% function val = essential_costE2cost(X, costE)
%
% costE is the function handle for the cost function in E.
%
% See also: essential_egradE2egrad essential_ehessE2ehess

% This file is part of Manopt: www.manopt.org.
% Original author: Roberto Tron, Aug. 8, 2014
% Contributors: Bamdev Mishra, May 22, 2015.

    e3hat = [0 -1 0; 1 0 0; 0 0 0];

    RA = X(:,1:3,:);
    RB = X(:,4:6,:);
    E = multiprod(multiprod(multitransp(RA), e3hat), RB);

    val = costE(E);
end


close all;
%%
load('ExperimentInformation.mat');
load('NAT.mat');
%% paremater settings
SpeedBinning=1.5; %cm/s
MinSpeed=2.5;
MaxSpeed=16;
SpeedRange=[MinSpeed:SpeedBinning:MaxSpeed]; %cm0
MinSpanTime=10;%second
ShuffleNum=200;
Shuffling_mininterval=30;
SpeedSmooth=round(0.25*ExperimentInformation.Trackingframerate);
MinEventCount=100;

%%
SpeedTunning=cell(ExperimentInformation.Session,ExperimentInformation.TotalCell); % tuning curve
Speed_KStest=zeros(ExperimentInformation.Session,ExperimentInformation.TotalCell,ShuffleNum+3); % K-S value for shuffles (1 to ShuffleNum, 95th, 5th, and observed data)
Speed_Spearman=zeros(ExperimentInformation.Session,ExperimentInformation.TotalCell,ShuffleNum+3); % Spearman for shuffles (1 to ShuffleNum, 95th, 5th, and observed data)

%% K-S analysis and Spearmean test
% generate speed tuning curve with finner binning

for j=1:1:ExperimentInformation.Session
    k=1;
    for i=1:1:ExperimentInformation.TotalCell

        Event_filtered=intersect(find(NAT{1,j}(:,4*i+12)>0),find(NAT{1,j}(:,6)==1));% filter out the frames with speed valid
        Event_filtered=intersect(Event_filtered,find(NAT{1,j}(:,5)>MinSpeed));% filter out the frames with speed threadholds
       if length(Event_filtered)>MinEventCount && ExperimentInformation.CellSNR(i)>3 && ~ismember(i,ExperimentInformation.RepeatCell)
       SelectedFrame_raw=find(~isnan(NAT{1,j}(:,12+4*i))); %all the position are select
       SpeedTrain=NAT{1,1}(SelectedFrame_raw,5);
       SpeedTrain_smooth=general.smoothGauss(SpeedTrain,SpeedSmooth);
       SelectedFrame_filtered=logical((SpeedTrain_smooth>=MinSpeed).*(SpeedTrain_smooth<=MaxSpeed+SpeedBinning));
       SpeedTrain_filtered=SpeedTrain_smooth(SelectedFrame_filtered);
       EventTrain_original=NAT{1,j}(SelectedFrame_raw,4*i+12);
       EventTrain_smooth=general.smoothGauss(EventTrain_original,SpeedSmooth);
       EventTrain_filtered=EventTrain_smooth(SelectedFrame_filtered);
       SpeedTunning{j,i}=SpatialTuning_BNT.SpeedTuningCalcultation(SpeedTrain_filtered,EventTrain_filtered,...
                         ExperimentInformation.Trackingframerate./double(ExperimentInformation.ImagingPlane),...
                         SpeedRange,MinSpanTime,...
                         MaxSpeed+SpeedBinning);
       SpeedTunning{j,i}.Rate=smoothdata(SpeedTunning{k,1}.Rate,'lowess',3);
       SpeedCount=round(SpeedTunning{j,i}.Rate./max(SpeedTunning{j,i}.Rate)*100);
       KS_SpeedBinVector{k,1}=zeros(sum(SpeedCount(~isnan(SpeedCount))),1);
       m=1;
              for p=1:1:length(SpeedCount)
                  if ~isnan(SpeedCount(p))
                    for q=1:1:SpeedCount(p)
                            KS_SpeedBinVector{k,1}(m,1)=SpeedTunning{k,1}.SpeedRange(p);
                            m=m+1;
                    end
                  else
                  end
              end
      for n=1:1:ShuffleNum
          xmin=Shuffling_mininterval*ExperimentInformation.Trackingframerate;
          xmax=size(SelectedFrame_filtered,1)-xmin;
          ShiftFrame=round(xmin+rand(1,1)*(xmax-xmin));
          SpikeTrain_raw_shuffled=circshift(EventTrain_filtered,ShiftFrame);
          Speedtuning_shuffled=SpatialTuning_BNT.SpeedTuningCalcultation(SpeedTrain_filtered,SpikeTrain_raw_shuffled,...
                                ExperimentInformation.Trackingframerate./double(ExperimentInformation.ImagingPlane),...
                                SpeedTunning{k,1}.SpeedRange,MinSpanTime,17.5);
          RateSmoothed=smoothdata(Speedtuning_shuffled.Rate,'lowess',3);
          SpeedCount=round(RateSmoothed./max(SpeedTunning{k,1}.Rate)*100);

          % calculate the KS value for shuffles
          KS_SpeedBinVector_shuffle=zeros(sum(SpeedCount(~isnan(SpeedCount))),1);
          m=1;
          for p=1:1:length(SpeedCount)
              if ~isnan(SpeedCount(p))
                  for q=1:1:SpeedCount(p)
                      KS_SpeedBinVector_shuffle(m,1)=Speedtuning_shuffled.SpeedRange(p);
                      m=m+1;
                  end
              else
              end
          end
          [~,~,Speed_KStest(j,i,n)]=kstest2(KS_SpeedBinVector_shuffle,SpeedTunning{k,1}.SpeedRange);
          % calculate the spearman correlation
          speedScore_tem=SpatialTuning_BNT.speedScore(SpeedTrain_filtered,SpikeTrain_raw_shuffled(:,2),...
                         ExperimentInformation.Trackingframerate./double(ExperimentInformation.ImagingPlane));
          Speed_Spearman(j,i,n)=speedScore_tem(1);

      end
      Speed_KStest(j,i,ShuffleNum+1)=prctile(Speed_KStest(j,i,1:ShuffleNum),95);
      Speed_KStest(j,i,ShuffleNum+2)=prctile(Speed_KStest(j,i,1:ShuffleNum),5);
      [~,~,Speed_KStest(k,ShuffleNum+3)]=kstest2(KS_SpeedBinVector{k,1},SpeedTunning{k,1}.SpeedRange);

      Speed_Spearman(j,i,ShuffleNum+1)=prctile(Speed_Spearman(j,i,1:ShuffleNum),95);
      Speed_Spearman(j,i,ShuffleNum+2)=prctile(Speed_Spearman(j,i,1:ShuffleNum),5);
      speedScore_tem=SpatialTuning_BNT.speedScore(SpeedTrain_filtered,EventTrain_filtered(:,2),...
                                                  ExperimentInformation.Trackingframerate./double(ExperimentInformation.ImagingPlane));
      Speed_Spearman(j,i,ShuffleNum+3)=speedScore_tem(1);
       else
       end
    end
end

%% identify speed cells
IsPosSpeedCell=cell(ExperimentInformation.Session,1);
IsNagSpeedCell=cell(ExperimentInformation.Session,1);
IsNonlinearSpeedCell=cell(ExperimentInformation.Session,1);
for j=1:1:ExperimentInformation.Session
    g=1;
    k=1;
    h=1;
    for i=1:1:ExperimentInformation.TotalCell
            if Speed_KStest(j,i,Shuffling+3,j)>Speed_KStest(j,i,Shuffling+1,j)
                if Speed_Spearman(j,i,ShuffleNum+3)>Speed_Spearman(j,i,ShuffleNum+1)
                    IsPosSpeedCell{j,1}(g)=i;
                    g=g+1;
                elseif Speed_Spearman(j,i,ShuffleNum+3)<Speed_Spearman(j,i,ShuffleNum+2)
                    IsNagSpeedCell{j,1}(k)=i;
                    k=k+1;
                else
                    IsNonlinearSpeedCell{j,1}(h)=i;
                    h=h+1;
                end
            else
            end
    end
end


function [xVals, yVals, zVals] = getScanXYZVals(scanStruct, slice)
%"getScanXYZVals"
%   Returns the x, y, and z values of the cols, rows, and slices of the
%   passed scanStruct.  If specified, slice determines which element of
%   scanInfo is used to determine the x and y vals.  If slice is invalid or
%   not specified, 1 is used.
%
%   REMINDER: These x,y,z values are the coordinates of the MIDDLE of the
%   voxels of the scan.  They are not the coordinates of the dividers
%   between the voxels.
%
%   By JRA 12/26/03
%
%   scanStruct      : ONE planC{indexS.scan} struct.
%   slice           : slice to base x,y vals on
%
% xVals yVals zVals : x,y,z Values for scan.
%
% Usage:
%   function [xVals, yVals, zVals] = getScanXYZVals(scanStruct, slice)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

if exist('slice','var')
    scanInfo = scanStruct.scanInfo(slice);
else
    scanInfo = scanStruct.scanInfo(1);
end

sizeDim1 = scanInfo.sizeOfDimension1-1;
sizeDim2 = scanInfo.sizeOfDimension2-1;

MATLABVer = version;
if MATLABVer(1) ~= '6'
    sizeDim1 = double(sizeDim1);
    sizeDim2 = double(sizeDim2);
end

xVals = scanInfo.xOffset - (sizeDim2*scanInfo.grid2Units)/2 : scanInfo.grid2Units : scanInfo.xOffset + (sizeDim2*scanInfo.grid2Units)/2;
yVals = fliplr(scanInfo.yOffset - (sizeDim1*scanInfo.grid1Units)/2 : scanInfo.grid1Units : scanInfo.yOffset + (sizeDim1*scanInfo.grid1Units)/2);
sI = scanStruct.scanInfo;
zVals = [sI.zValue];


function [train_sample1, train_sample2, label1, label2] = gen_train_sample_xqda(label, cam, train_feat)

uni_label = unique(label);
ind1 = [];
ind2 = [];
label1 = [];
label2 = [];
for n = 1:length(uni_label)
    pos = find(label == uni_label(n));
    perm = randperm(length(pos));
    tmp1 = pos(perm(1:floor(length(pos)/2)));
    tmp2 = pos(perm(floor(length(pos)/2)+1:floor(length(pos)/2)*2));
    cam1 = cam(tmp1);
    cam2 = cam(tmp2);
    pos2 = find(cam1~=cam2);
    tmp1 = tmp1(pos2);
    tmp2 = tmp2(pos2);
    ind1 = [ind1; tmp1];
    ind2 = [ind2; tmp2];
    label1 = [label1; repmat(uni_label(n), [length(tmp1), 1])];
    label2 = [label2; repmat(uni_label(n), [length(tmp2), 1])];
end
train_sample1 = train_feat(:, ind1)';
train_sample2 = train_feat(:, ind2)';


clear;close all;
setenv('LC_ALL','C');

%% settings
folder = './Train_291/';
size_input = 31;
size_label = 31;
stride = 21;
savepath = ['./train_291_' num2str(size_input) '_x234.h5'];

% scale augmentation
scale_2 = 2;
scale_3 = 3;
scale_4 = 4;

%% initialization
data = zeros(size_input, size_input, 1, 1);
label = zeros(size_label, size_label,1, 1);
padding = abs(size_input - size_label)/2;
count = 0;

%% generate data
filepaths = dir(fullfile(folder,'*.bmp'));

for i = 1 : length(filepaths)
    disp(['i = ' num2str(i)]);
    image = imread(fullfile(folder,filepaths(i).name));
    image = rgb2ycbcr(image);
    image_luminance = image(:,:,1); % lumination
    image = im2double(image_luminance);

    % scale 2
    im_label_2 = modcrop(image, scale_2);
    [hei_2,wid_2] = size(im_label_2);
    im_input_2 = imresize(imresize(im_label_2,1/scale_2,'bicubic'),[hei_2,wid_2],'bicubic');
%     imshow([im_label_2 im_input_2],[]);

    for x = 1 : stride : hei_2-size_input+1
        for y = 1 :stride : wid_2-size_input+1

            subim_input = im_input_2(x : x+size_input-1, y : y+size_input-1);
            subim_label = im_label_2(x+padding : x+padding+size_label-1, y+padding : y+padding+size_label-1);

            count=count+1;
            data_temp = subim_input;
            label_temp = subim_label;

            data(:, :, :, count) = single(data_temp);
            label(:, :, :, count) = single(label_temp);
        end
    end

    % scale 3
    im_label_3 = modcrop(image, scale_3);
    [hei_3,wid_3] = size(im_label_3);
    im_input_3 = imresize(imresize(im_label_3,1/scale_3,'bicubic'),[hei_3,wid_3],'bicubic');
%     imshow([im_label_3 im_input_3],[]);

    for x = 1 : stride : hei_3-size_input+1
        for y = 1 :stride : wid_3-size_input+1

            subim_input = im_input_3(x : x+size_input-1, y : y+size_input-1);
            subim_label = im_label_3(x+padding : x+padding+size_label-1, y+padding : y+padding+size_label-1);

            count=count+1;
            data_temp = subim_input;
            label_temp = subim_label;

            data(:, :, :, count) = single(data_temp);
            label(:, :, :, count) = single(label_temp);
        end
    end

    % scale 4
    im_label_4 = modcrop(image, scale_4);
    [hei_4,wid_4] = size(im_label_4);
    im_input_4 = imresize(imresize(im_label_4,1/scale_4,'bicubic'),[hei_4,wid_4],'bicubic');
%     imshow([im_label_4 im_input_4],[]);

    for x = 1 : stride : hei_4-size_input+1
        for y = 1 :stride : wid_4-size_input+1

            subim_input = im_input_4(x : x+size_input-1, y : y+size_input-1);
            subim_label = im_label_4(x+padding : x+padding+size_label-1, y+padding : y+padding+size_label-1);

            count=count+1;
            data_temp = subim_input;
            label_temp = subim_label;

            data(:, :, :, count) = single(data_temp);
            label(:, :, :, count) = single(label_temp);
        end
    end

end

order = randperm(count);
data = data(:, :, :, order);
label = label(:, :, :, order);

%% writing to HDF5
chunksz = 64;
created_flag = false;
totalct = 0;

for batchno = 1:floor(count/chunksz)
    last_read=(batchno-1)*chunksz;
    batchdata = data(:,:,:,last_read+1:last_read+chunksz);
    batchlabs = label(:,:,:,last_read+1:last_read+chunksz);

    startloc = struct('dat',[1,1,1,totalct+1], 'lab', [1,1,1,totalct+1]);
    curr_dat_sz = store2hdf5(savepath, batchdata, batchlabs, ~created_flag, startloc, chunksz);
    created_flag = true;
    totalct = curr_dat_sz(end);
end
h5disp(savepath);


function FRR = CreditAssignment(SW,D)
% Credit assignment

%------------------------------- 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".
%--------------------------------------------------------------------------

    K = 4;  % Number of operators
    Reward = zeros(1,K);
    for i = 1 : K
        Reward(i) = sum(SW(2,SW(1,:)==i));
    end
    [~,Rank] = sort(Reward,'descend');
    [~,Rank] = sort(Rank);
    Decay    = D.^Rank.*Reward;
    FRR      = Decay./sum(Decay);
end


function [lb, ub] = getMultiFlybyLBUB(lOpenUT, lDuration, bodiesInfo, celBodyData)
%getMultiFlybyLBUB Summary of this function goes here
%   Detailed explanation goes here

    nvars = length(bodiesInfo)+(length(bodiesInfo)-1);
    IntCon = 1:nvars;
    IntCon = IntCon(end-(length(bodiesInfo)-1)+1:end);
    lb = zeros(1,nvars);
    ub = zeros(1,nvars);

    bodyInfo = bodiesInfo{1};
    parentBodyInfo = bodyInfo.getParBodyInfo(celBodyData);
    xferGmu = parentBodyInfo.gm;

    lb(1) = lOpenUT;
    ub(1) = lOpenUT + lDuration;

    for(i=1:length(bodiesInfo)-1) %#ok<*NO4LP>
        b1 = bodiesInfo{i};
        b2 = bodiesInfo{i+1};

        mSMA = mean([b1.sma, b2.sma]);
        xP = computePeriod(mSMA, xferGmu);

        lb(i+1) = xP/25;
        ub(i+1) = xP*5;
    end

    lb(IntCon) = 1;
    ub(IntCon) = 2;
end


function self = correct(self)
     ref = 0.707; % note that median, rms and std all give same value on x=sin(0:pi/1000:2*pi)
     for c=1:numel(self)
        if strcmp(self(c).measure, 'max')
            self(c).data = self(c).data * ref;
        end
        if strcmp(self(c).measure, '68')
            self(c).data = self(c).data/0.8761 * ref;
        end
        if strcmp(self(c).measure, 'mean')
            self(c).data = self(c).data/0.6363 * ref;
        end
     end
end


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nickabattista@gmail.com
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs, non-invariant beams*)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "Hill+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (nickabattista@gmail.com) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the spring attributes!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function springs_info = update_Springs(dt,current_time,xLag,yLag,springs_info)

%springs_info: col 1: starting spring pt (by lag. discretization)
%              col 2: ending spring pt. (by lag. discretization)
%              col 3: spring stiffness
%              col 4: spring resting lengths

%RL = springs_info(:,4); % resting-length vector

% Contraction Frequency
freq = 1.0;

% CHANGE RESTING LENGTH BTWN SIDES OF JELLYFISH BELL
springs_info(119:end,4) = abs( cos(freq*current_time*pi) );

%NOTE: not 2*pi*ft b/c of abs()