function [data, comp_data, bnet_miss] = gener_NMAR_dataset(bnet_orig, m, bnet_miss, upd)
% [NMAR_data] = gener_NMAR_data(bnet_miss, length_of_dataset)
%
% this function takes in input a bnet that can be used to generate NMAR data.
% this bnet bnet_miss can be creating by the function gener_NMAR_bnet.
%
% - comp_data (array) is a dataset that was generate by the bnet_orig that you enter in gener_NMAR_bnet
% - NMAR_data (cell array) is the dataset compdata that was emptyied by the NMAR process encodes in bnet_miss
%
% optional :
% - bnet_miss : an old bnet_miss built by this function
% - upd==1 if you want to update the bnet_miss
%
%  [data, comp_data, bnet_miss] = gener_NMAR_dataset(bnet_orig, m, bnet_miss, upd);
%
% version 0.5 : june 8th 2005, olivier.francois@insa-rouen.fr
%
% TO DO :
%   - allow the combinaison of a missing state of one variable and another state of another variable to have influence
%   - allow the introduction of new nodes and specify which nodes it influence and which nodes has influence on it (it will also satisfy the first task then)
%

% INIT
N = size(bnet_orig.dag,2);
if mod(N,2)~=0, error('The number of nodes must be even'); end
if nargin<4, upd =0; end

% fisrt rules
if nargin<3,
 l1=[]; l2=[]; lp=[]; b=-1;
 while ~(b==0 | b==1), b = input('Would you like to make a node missing when another one is missing (1 for yes, 0 for no) ?   '); end
else
 b=-1;
 if upd, while ~(b==0 | b==1), b = input('Would you like to add rules (1 for yes, 0 for no) ?   '); end
   l1=bnet_miss.list{1};
   l2=bnet_miss.list{2};
   lp=bnet_miss.list{3};
 else
   l1=[]; l2=[]; lp=[]; b=-1;
 end
end

while b==1,
  n = input('The firts node ?   ');
  s = input('The node that have to be missing when this one is missing ?   ');
  p = input('The probability of the second node to be missing ?   ');
  l1 = [l1, n]; l2 = [l2, s]; lp=[lp, p];
  b=-1;
  while ~(b==0 | b==1), b = input('Another one (1 for yes, 0 for no) ?   '); end
end
bb = length(l1);

if nargin>=3,
  bnet_miss = gener_NMAR_bnet(upd, bnet_orig, bnet_miss);
else
  bnet_miss = gener_NMAR_bnet(1, bnet_orig);
  bnet_miss.list={l1; l2; lp};
%%%%%%%%%%% SAVING FILE
 ss = 1;
 if nargin == 2 | upd==1, ss = input('Would you like to save the bnet of the NMAR process you have made (1 for yes) ?   '); end
 if ss == 1,
  ddd = datestr(now);
  ddd([12 15 18])='-' ;
  fnout=['NMAR-bnet-' ddd '.mat'];
  eval(['save ' fnout ' bnet_miss']);
  fprintf(' The bnet for NMAR process was saved as : %s\n',fnout);
 end
end %if nargin

% Generation of a complete dataset
if N>9 & m>2000, disp('  ! It could take a long time...'); end
data = cell(N,m);
for l = 1:m, data(:,l) = sample_bnet(bnet_orig); end
disp('Complete data have been creating.');

% Generation of a NMAR dataset
miss_array = cell(2*N,m);
vide = cell(1,N); l= 1;
while l <= m,
  ev(1:N) = data(:,l); ev(N+1:2*N) = vide;
  miss_array(:,l) = sample_bnet(bnet_miss, 'evidence', ev);
  % apply simple rule of missingness
  ev2 = cell2mat(miss_array(N+1:2*N, l));
  if bb,
   missl1 = myintersect(find(ev2==2), l1);
   if ~isempty(missl1),
    for i=1:length(l1),
     if ev2(l1(i))==2, if rand<lp(i), ev2(l2(i))=2; miss_array{N+l2(i),l}=[2]; end, end
  end, end, end
  % verification that we have not a completly missing sample
  ev2 = 3-ev2;
  if prod(ev2)==1, fprintf(' - %d, one completly missing sample removed', l); else l=l+1; end
  if mod(l,100)==0, fprintf('\n - %d',l); end
end
fprintf('\n');
data = bnt_to_mat(data); comp_data = data;
miss_array = bnt_to_mat(miss_array(N+1:2*N, :));
miss_array = 2-miss_array;
data = data.*miss_array;
data = mat_to_bnt(data, 0);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function bnet_miss = gener_NMAR_bnet(upd, bnet_orig, bnet_miss)

%%%%%%%%%%%% INIT
dag = bnet_orig.dag;
N = size(dag,2);
ns = bnet_orig.node_sizes;
NN = 0;
ns_miss = zeros(1,2*N+NN);
ns_miss(1:N) = ns;
ns_miss(N+1:2*N) = 2*ones(1,N); % 1= node i-N present, 2= node i-N missing
  dagm = zeros(2*N,2*N);
  dagm(1:N,1:N) = dag;
  dagm(N+1:2*N, N+1:2*N) = dag;
  dagm(1:N, N+1:2*N) = dag;
  for i=1:N, dagm(i, i+N)=1; end

%%%%%%%%%%%% NEW NODES
%  b=-1;
%  while ~(b==0 | b==1), b = input('Would you like to add new nodes ?   '); end
%
%  if b==1,
%  NN = -1;
%  while (NN<0 | round(NN)~=NN), NN = input('How Many ?   '); end
%  if NN==1, fprintf('New node will be called %d\n',(2*N+1));
%  else fprintf('New node will be called %d and following numbers\n',(2*N+1)); end
%
%  disp(' !!! Make sur that the dependence you will create will not create cycle in the Bnet used to create NMAR data !!! here is the current DAG');
%  dagm2 = zeros(2*N+NN, 2*N+NN);
  dagm2(1:2*N, 1:2*N) = dagm;
%  draw_graph(dagm2); drawnow;
%  clear dagm
%
%  L1={};L2={};
%  for i=1:NN
%    fprintf('For the node %d, ',(2*N+i));
%    L1{i} = input('FROM which nodes will it have influence (sample [3 2]) ?   ');
%    L2{i} = input('ON which nodes will it have influence (sample [1 4 3]) ?   ');
%    ns_miss(2*N+i) = input('What is its size ?   ');
%  end
%
%  end

%%%%%%%%%%%% BNET CREATION
if nargin==2,
  dag_miss = dagm2;
  for i=1:NN, dag_miss(L1{i},2*N+i)=1; dag_miss(2*N+i, L2{i})=1; end

  bnet_miss = mk_bnet(dag_miss, ns_miss);
  CPT = CPT_from_bnet(bnet_orig);
  for i=1:N
    bnet_miss.CPD{i} = tabular_CPD (bnet_miss, i, CPT{i}); % error with new nodes
  end
elseif nargin==3,
  ns_miss = bnet_miss.node_sizes;
  dag_miss = bnet_miss.dag;
end
clear dagm2
order = 1:(2*N+NN);

%%%%%%%%%%%% Base probability of missing value
if nargin==2, b=1; else b=0; end
if b==1,
disp('Probability MUST be between 0 and 1.');

if nargin==2,
 p=-1;
 while p<0 | p>1, p = input('Base probability of a value to be missing ?   '); end
 for i=1:N
  fam = find(dag_miss(:,N+i)==1)';
  semisize = prod(ns_miss(fam)); % as node N+i is binary to say i is present or missing
  CPT = zeros(1,2*semisize);
  CPT(1:semisize) = 1-p;
  CPT(semisize+1:2*semisize) = p;
  bnet_miss.CPD{N+i} = tabular_CPD (bnet_miss, N+i, CPT);
 end
end
end

b=-1;
while ~(b==0 | b==1), b = input('Would you like to change a probability of a node to be missing (1 for yes, 0 for no) ?   '); end
if b, disp(' BE CAREFULL !! New rules can overwrite old ones partialy or fully !! So the order of entries is important'); end

%%%%%%%%%%%% Update CPT with NMAR process
while b
  fprintf('Nodes are from 1 to %d. ',N);
  i=0;
  while i<1 | i>N | round(i)~=i, i = input('Which node ?   '); end
  fam = find(dag(:,i)==1)';
  fam_miss = find(dag_miss(:,N+i)==1)';
  cas = -ones(1, length(fam_miss)+1);
  familly = [fam_miss, N+i];
  fprintf('States are from 1 to %d (-1 for any states, -2 to cancel). For which state of the variable %d ?', ns(i), i);
  state=-3;
  while state<-2 | state>ns(i) | round(state)~=state | state==0, state  = input('   ');end
  if isempty(fam),
    if state==-1,
      p=-1;
      while p<0 | p>1, p = input(' - A priori probability for this node to be missing ?   ');end
      semisize = prod(ns_miss(fam_miss));
      CPT = zeros(1,2*semisize);
      CPT(1:semisize) = 1-p;
      CPT(semisize+1:2*semisize) = p;
      bnet_miss.CPD{N+i} = tabular_CPD (bnet_miss, N+i, CPT);
    elseif state~=-2
      cas = state;
      CPT = CPT_from_bnet(bnet_miss);
      CPT = CPT{N+i};
      p=-1;
      while p<0 | p>1, p = input(' - A priori probability for this node to be missing in this state ?   ');end
      ind = subv2ind(ns_miss(familly),[cas, 1]);
      CPT(ind)=1-p;
      ind = subv2ind(ns_miss(familly),[cas, 2]);
      CPT(ind)=p;
      bnet_miss.CPD{N+i} = tabular_CPD (bnet_miss, N+i, CPT);
    end
  else
   if state>-2,
    siz=length(cas);
    place = find(fam_miss==i);
    cas(place) = state;

    for k = fam,
        state=-3;
        fprintf(' - For the parent named %d, states are from 1 to %d (-1 for any states of this parent). ',k, ns(k));
        while state<0 | state>ns(k) | round(state)~=state, state  = input('Which state ?   ');end
        %if state==0,
        %  place = find(fam_miss==(fam_miss(k)+N));
        %  cas(place) = 2;                   % Missing
        %elseif state==-2,                   % a changer ???
        %  disp(' This case is buggy, taking missing state instand to minimise influence.');
        %  place = find(fam_miss==(fam_miss(k)+N));
        %  cas(place) = 2;
        %elseif state~=0 & state~=-2,
          place = find(fam_miss==(fam_miss(k)));
          cas(place) = state;               % Present
          if state~=-1; place = find(fam_miss==(fam_miss(k)+N)); cas(place) = 1; end
        %end
      end

    p=-1;
    while p<0 | p>1, p = input('Probability in this case of the value to be missing ?   ');end
    CPT = CPT_from_bnet(bnet_miss);
    CPT = CPT{N+i};

    cas(end) = 1; % i is present
    subcas_names = find(cas==-1);
    if isempty(subcas_names),
      ind = subv2ind(ns_miss(familly),cas);
      CPT(ind) = 1-p;
    else
      subcas = ones(1, length(subcas_names));
      continu = 1;
      while continu
        cas(subcas_names) = subcas;
        ind = subv2ind(ns_miss(familly),cas);
        CPT(ind) = 1-p;
        [subcas, continu] = next_case(subcas, ns_miss(familly(subcas_names)));
      end
    end

    cas(end)=2; % i is missing
    if isempty(subcas_names),
      ind = subv2ind(ns_miss(familly),cas);
      CPT(ind) = p;
    else
      subcas = ones(1, length(subcas_names));
      continu = 1;
      while continu
        cas(subcas_names) = subcas;
        ind = subv2ind(ns_miss(familly),cas);
        CPT(ind) = p;
        [subcas, continu] = next_case(subcas, ns_miss(familly(subcas_names)));
      end
    end
    mass = sum(CPT, length(size(CPT)));
    while length(size(mass))>2, mass = prod(mass, length(size(mass))); end
    mass = prod(prod(mass));
    if mass~=1, disp('not a proba...'); end
    bnet_miss.CPD{N+i} = tabular_CPD (bnet_miss, N+i, CPT);
   end %if state~=-2 for the node
  end %if isempty(fam),
  b=-1;
  while ~(b==0 | b==1), b = input('Would you like to change a probability of a node to be missing (1 for yes, 0 for no) ?   ');end
end


%% Tilt and Twist Boundaries
%
%%
% If a material deforms through the movement of dislocations, rearrangement
% of dislocations to a low-energy configuration may happen during
% deformation (i.e. in slow, geologic deformation) or or afterwards (in
% many metals). In any case, the arrangement of dislocation walls can lead
% to so-called subgrains boundaries. If such a boundary is composed of edge
% dislocations, it is called a tilt boundary and the rotation axis relating
% both parts of the grain at each side can be expected to be within the
% boundary plane (ideally parallel to the edge dislocation line). If the
% boundary is composed of screw dislocations, the rotation axis should be
% normal to the boundary. Between those end-members, there are general
% boundaries where the rotation axis is not easily related to the type of
% dislocations unless further information is available.
%
% In this chapter we discuss the computation of the misorientation axes at
% subgrain boundaries and discuss whether they vote for twist or tilt
% boundaries. We start by importing an sample EBSD data set and computing
% all subgrain boundaries as it is described in more detail in the chapter
% <subGrainBoundaries.html Subgrain Boundaries>.

% load some test data
mtexdata forsterite silent

% remove one pixel grains
[grains,ebsd.grainId] = calcGrains(ebsd('indexed'));
ebsd(grains(grains.grainSize<5)) = [];

% compute subgrain boundaries with 1.5 degree threshold angle
[grains,ebsd.grainId] = calcGrains(ebsd('indexed'),'threshold',[1*degree, 15*degree]);

% lets smooth the grain boundaries a bit
grains = smooth(grains,5);

% set up the ipf coloring
cKey = ipfColorKey(ebsd('fo').CS.properGroup);
cKey.inversePoleFigureDirection = xvector;
color = cKey.orientation2color(ebsd('fo').orientations);

% plot the forsterite phase
plot(ebsd('fo'),color,'faceAlpha',0.5,'figSize','large')

% init override mode
hold on

% plot grain boundares
plot(grains.boundary,'linewidth',2)

% compute transparency from misorientation angle
alpha = grains('fo').innerBoundary.misorientation.angle / (5*degree);

% plot the subgrain boundaries
plot(grains('fo').innerBoundary,'linewidth',1.5,'edgeAlpha',alpha,'linecolor','b');

% stop override mode
hold off

%%
% In the above plot we have marked all subgrain boundaries in blue and
% adjusted the transparency value according to the misorientation angle.
%
%% Misorientation Axes
%
% When analysing the misorientation axes of the subgrain boundary
% misorientations we need to distinguish whether we look at the
% misorientation axes in crystal coordinates or in specimen coordinates.
% Lets start with the misorientation axes in crystal coordinates which can
% directly be computed by the command <orientation.axis.html |axis|>.

% extract the Forsterite subgrain boundaries
subGB = grains('fo').innerBoundary;

% plot the misorientation axes in the fundamental sector
plot(subGB.misorientation.axis,'fundamentalRegion','figSize','small')

%%
% Obviously from the above plot it is not easy to judge about prefered
% misorientation axes. We get more insight if we <DensityEstimation.html
% compute the density distribution> of the misorientation axes and look for
% <S2FunOperations.html#4 local extrema>.

% compute the density distribution of misorientation axes
density = calcDensity(subGB.misorientation.axis,'halfwidth',3*degree);

% plot them
plot(density,'figSize','small')
mtexColorbar

% find the two prefered misorientation axes
[~,hkl] = max(density,'numLocal',2); round(hkl)

%%
% We find two preferred misorientation axes - (001) and (071). *TODO*: can
% this be interpreted?
%
%% The misorientation axis in specimen coordinates
%
% The computation of the misorientation axis in specimen coordinates is a
% little bit more complicated as it is impossible using only the
% misoriention. In fact we require the adjacent orientations on both sides
% of the subgrain boundaries. We can find those by making use of the
% |ebsdId| stored in the grain boundaries. The command

oriGB = ebsd('id',subGB.ebsdId).orientations

%%
% results in a $N \times 2$ matrix of orientations with rows corresponding
% to the boundary segments and two columns for both sides of the boundary.
% The misorientation axis in specimen coordinates is again computed by the
% command <orientation.axis.html |axis|>

axS = axis(oriGB(:,1),oriGB(:,2),'antipodal')

% plot the misorientation axes
plot(axS,'MarkerAlpha',0.2,'MarkerSize',2,'figSize','small')

%%
% We have used here the option |antipodal| as we have no fixed ordering of
% the grains at the two sides of the grain boundaries. For a more
% quantitative analysis we again compute the corresponding density
% distribution and find the preferred misorientation axes in specimen
% coordinates

density = calcDensity(axS,'halfwidth',5*degree);
plot(density,'figSize','small')
mtexColorbar

[~,pos] = max(density)
annotate(pos)

%% Tilt and Twist Boundaries
%
% Subgrain boundaries are often assumed to form during deformation by the
% accumulation of edge or screw dislocations. In the first extremal case of
% exclusive edge dislocations the misorientation axis is parallel to the
% deformation line and within the boundary plane. Such boundaries are
% called *tilt boundaries*. In the second extremal case of exclusive screw
% dislocations the misorientation axis is the screw axis and is parallel to
% the boundary normal. Such boundaries are called *twist boundaries*.
%
% In the case of 2d EBSD data one usually has not the full boundary
% information, but only the trace of the boundary with the measurement
% surface. Hence, it is impossible to distinguish tilt and twist
% boundaries. However, for twist boundaries misorientation axis must be normal
% to the boundary trace. This means, if the misorientation axis lays in the
% measurement plane and normal to the boundary trace, the boundary is quite
% likely to be a twist boundary. At the other hand, if the misorientation axis
% is parallel to the trace of a boundary, the boundary is quite likely to be a
% tilt boundary.
% We can be easily check the latter situation from our EBSD data, which allows us to
% exclude certain boundaries to be twist boundaries and to be most likely tilt
% boundaries To do so, we colorize in the following plot all subgrain boundaries
% according to the angle between the boundary trace and the misorientation axis.
% Blue subgrain boundaries are very likely tilt boundaries, while red subgrain
% boundaries are can be either tilt or twist boundaries.

plot(ebsd('fo'),color,'faceAlpha',0.5,'figSize','large')

% init override mode
hold on

% plot grain boundares
plot(grains.boundary,'linewidth',2)

% colorize the subgrain boundaries according the angle between boundary
% trace and misorientation axis
plot(subGB,angle(subGB.direction,axS)./degree,'linewidth',2)
mtexColorMap blue2red
mtexColorbar

hold off


function FinalEx
%{
FinalEx.m

WASDQE for steering. V to switch to 1st person view. More below.

Combine stuff from all the previous examples to make:
1. Import a plane as a patch object.
2. Translate/rotate this through space as per user input. User changes
heading, constant velocity dynamics. Use Ws,AD, QE for pitch,roll,yaw
3. Put in horizon and ground with textures. If the plane COM tries to
penetrate, simply project it back up.
4. Have camera follow the plane in 2 ways, press v to switch between.

NOTE: I wouldn't start here if you want to understand each step. Start with
the other examples and see how this is built.

Matt Sheen, mws262@cornell.edu
%}

close all

fig = figure;

fig.UserData.firstPerson = false; %do we start in 1st person view, or not?
resetPositionOnBounds = true; %keeps the plane in the region with the matlab logo ground.

hold on
fig.Position = [600 600 1500 1200];
%Disable axis viewing, don't allow axes to clip the plane, turn on
%perspective mode.
fig.Children.Visible = 'off';
fig.Children.Clipping = 'off';
fig.Children.Projection = 'perspective';

%% Add the sky - Fancy environment folder ex
skymap = [linspace(1,0.4,100)',linspace(1,0.4,100)',linspace(1,0.99,100)'];

[skyX,skyY,skyZ] = sphere(50);
sky = surf(500000*skyX,500000*skyY,500000*skyZ,'LineStyle','none','FaceColor','interp');
colormap(skymap);

%% Add the plane - Fancy planes folder
fv = stlread('test.stl');
p1 = patch(fv,'FaceColor',       'red', ...
         'EdgeColor',       'none',        ...
         'FaceLighting',    'gouraud',     ...
         'AmbientStrength', 0.15);
vert = p1.Vertices;

material('metal')

%% Add the ground + textures - Fancy environments folder Ex
texture = imread('texture.jpg');

ground = 10000*membrane(1,40)-10000;
groundSurf = surf(linspace(-10000,10000,size(ground,1)),linspace(-10000,10000,size(ground,2)),ground,'LineStyle','none','AmbientStrength',0.3,'DiffuseStrength',0.8,'SpecularStrength',0,'SpecularExponent',10,'SpecularColorReflectance',1);
groundSurf.FaceColor = 'texturemap';
groundSurf.CData = texture;

%add some extra flat ground going off to (basically) infinity.
flatground = surf(linspace(-500000,500000,size(ground,1)),linspace(-500000,500000,size(ground,2)),-10001*ones(size(ground)));
flatground.FaceColor = 'texturemap';
flatground.CData = texture;
flatground.AlphaData = 0.8;

camlight('headlight');

camva(40); %view angle

%% Set keyboard callbacks and flags for movement.
set(fig,'WindowKeyPressFcn',@KeyPress,'WindowKeyReleaseFcn', @KeyRelease);
        fig.UserData.e = false;
         fig.UserData.q = false;
         fig.UserData.a = false;
         fig.UserData.d = false;
         fig.UserData.w = false;
         fig.UserData.s = false;

forwardVec = [1 0 0]'; %Vector of the plane's forward direction in plane frame
rot = eye(3,3); %Initial plane rotation
pos = [-8000,8000,-2000]; %Initial plane position
vel = 1000; %Velocity

hold off
axis([-10000 10000 -10000 10000 -10000 10000])

%% Animation loop:
tic
told = 0;
while(ishandle(fig))
  tnew = toc;

  %Check for user inputs:
  if fig.UserData.e
      rot = rot*angle2dcm(0.05,0,0);
  end
  if fig.UserData.q
      rot = rot*angle2dcm(-0.05,0,0);
  end
  if fig.UserData.s
      rot = rot*angle2dcm(0,-0.05,0);
  end
  if fig.UserData.w
      rot = rot*angle2dcm(0,0.05,0);
  end
  if fig.UserData.a
      rot = rot*angle2dcm(0,0,-0.05);
  end
  if fig.UserData.d
      rot = rot*angle2dcm(0,0,0.05);
  end

  %Update plane's center position.
  pos = vel*(rot*forwardVec*(tnew-told))' + pos;

  %If the plane wants to go under the ground, then bring it back up to the
  %ground surface.
  nearestGroundZ = interp2(groundSurf.XData,groundSurf.YData,groundSurf.ZData,pos(1),pos(2));
  if pos(3)<nearestGroundZ
      pos(3) = nearestGroundZ;
  end

  if resetPositionOnBounds
      % If we leave the ground area in the X direction, then snap the plane
      % back to the other side.
      if pos(1)>max(groundSurf.XData)
          pos(1) = min(groundSurf.XData);
      elseif pos(1)<min(groundSurf.XData)
          pos(1) = max(groundSurf.XData);
      end

      % If we leave the ground area in the y direction, then snap the plane
      % back to the other side.
      if pos(2)>max(groundSurf.YData)
          pos(2) = min(groundSurf.YData);
      elseif pos(2)<min(groundSurf.YData)
          pos(2) = max(groundSurf.YData);
      end
  end

  %Update the plane's vertices using the new position and rotation
  p1.Vertices = (rot*vert')' + repmat(pos,[size(vert,1),1]);

  %Camera updates:
  if fig.UserData.firstPerson %First person view -- follow the plane from slightly behind.
      camupvec = rot*[0 0 1]';
      camup(camupvec);
      campos(pos' - 1000*rot*[1 0 -0.25]');
      camtarget(pos' + 100*rot*[1 0 0]');
  else %Follow the plane from a fixed angle
    campos(pos + [-3000,3000,1000]);%3000*abs(pos-campos)/norm(pos-campos));
    camtarget(pos);

  end

     cam = campos;
    %Also keep the camera from going into the ground (could be done a
    %smarter way to also not look through the ground).
     nearestGroundZ = interp2(groundSurf.XData,groundSurf.YData,groundSurf.ZData,cam(1),cam(2));
     if cam(3)<nearestGroundZ
      	campos([cam(1),cam(2),nearestGroundZ]);
     end

    told = tnew;
    pause(0.01);

end
end

function KeyPress(varargin)
     fig = varargin{1};
     key = varargin{2}.Key;
     if strcmp(key,'e')
         fig.UserData.e = true;
     elseif strcmp(key,'q')
         fig.UserData.q = true;
     elseif strcmp(key,'a')
         fig.UserData.a = true;
     elseif strcmp(key,'d')
         fig.UserData.d = true;
     elseif strcmp(key,'w')
         fig.UserData.w = true;
     elseif strcmp(key,'s')
         fig.UserData.s = true;
     elseif strcmp(key,'v')
         fig.UserData.firstPerson = ~fig.UserData.firstPerson;
     end
end

function KeyRelease(varargin)
     fig = varargin{1};
     key = varargin{2}.Key;
     if strcmp(key,'e')
         fig.UserData.e = false;
     elseif strcmp(key,'q')
         fig.UserData.q = false;
     elseif strcmp(key,'a')
         fig.UserData.a = false;
     elseif strcmp(key,'d')
         fig.UserData.d = false;
     elseif strcmp(key,'w')
         fig.UserData.w = false;
     elseif strcmp(key,'s')
         fig.UserData.s = false;
     end
end


classdef MultinomialResamplerX < ResamplerX & matlabshared.tracking.internal.MultinomialResampler
% MULTINOMIALRESAMPLERX Class
%
% Summary of MultinomialResamplerX:
% This is a class implementation of a multinomial resampler.
%
% MultinomialResamplerX Properties:
%   None
%
% MultinomialResamplerX Methods:
%    MultinomialResamplerX  - Constructor method
%    resample - Perform multinomial resampling
%
% See also SystematicResamplerX
    properties
    end

    methods
        function this = MultinomialResamplerX(varargin)
        % MULTINOMIALRESAMPLERX Constructor method
        %
        % Usage:
        % * MultinomialResamplerX() returns a "MultinomialResamplerX" object
        %   handle
        %
        % See also MultinomialResamplerX/resample

        end

        function [newSamples, newWeights,idx] = resample(this,samples,weights,Nnew)
        % RESAMPLE Perform multinomial resampling
        %
        % Parameters
        % ----------
        % samples: (NumDims x NumSamples) matrix
        %   A matrix, whose columns correspond to individual samples
        % weights: (1 x NumSamples) row vector
        %   A row vector, whose columns correspond to the weights of the
        %   respective columns/samples of the samples matrix
        % Nnew: scalar, optional
        %   The number of samples to be resampled.
        %   (default = NumSamples, which implies that the same ammount of
        %   samples will be generated, as the number of samples provided)
        %
        % Returns
        % -------
        % newSamples: (NumDims x Nnew) matrix
        %   A matrix, whose columns correspond to resampled samples.
        % newWeights: (1 x Nnew) row vector
        %   A row vector, whose columns correspond to the weights of the
        %   respective columns/samples of the newSamples matrix
        % idx: (1 x Nnew) row vector
        %   A row vector of indices, mapping each sample in newSamples to the
        %   corresponding sample in samples.
        %
        % Usage
        % -----
        % * idx = resample(this,weights) returns a row vector idx containing
        %   the indexes of samples generated through by performing multinomial
        %   resampling based on the vector weights.
        %
        % See also SystematicResamplerX/resample

            N = numel(weights);
            if(nargin<4)
                Nnew = N;
            end
            % idx = randsample(1:N, Nnew, true, weights);
            idx = resample@matlabshared.tracking.internal.MultinomialResampler(this,weights,Nnew);
            newSamples = samples(:,idx);
            newWeights = repmat(1/Nnew, 1, Nnew);
        end
    end
end


% SYNTAX:
% yAvgStd = hmrE_RunAvgStd2_oldversion_nirs(yAvgRuns, ySum2Runs, mlActRuns, nTrialsRuns)
%
% UI NAME:
% Run_Average_Standard_Deviation
%
% DESCRIPTION:
% Calculate avearge HRF standard deviation of all runs for one subject.
%
% INPUTS:
% yAvgRuns:
% ySum2Runs:
% mlActRuns:
% nTrialsRuns:
%
% OUTPUTS:
% yAvgStdOut: the standard deviation across runs
%
% USAGE OPTIONS:
% Run_Average_Standard_Deviation_on_Concentration_Data:  dcAvgStd  = hmrE_RunAvgStd2_oldversion_nirs(dcAvgRuns, dcSum2Runs, mlActRuns, nTrialsRuns)
% Run_Average_Standard_Deviation_on_Delta_OD_Data:       dodAvgStd = hmrE_RunAvgStd2_oldversion_nirs(dodAvgRuns, dodSum2Runs, mlActRuns, nTrialsRuns)
%

function yAvgStdOut = hmrE_RunAvgStd2_oldversion_nirs(yAvgRuns, ySum2Runs, mlActRuns, nTrialsRuns)

yAvgStdOut = DataClass().empty();

nDataBlks = length(yAvgRuns{1});
nTrials_tot = cell(nDataBlks,1);

for iBlk = 1:nDataBlks

    grp1 = [];

    for iRun = 1:length(yAvgRuns)

        yAvgStdOut(iBlk) = DataClass();

        yAvg    = yAvgRuns{iRun}(iBlk).GetDataTimeSeries('reshape');
        if isempty(yAvg)
            continue;
        end

        yAvgStd = zeros(size(yAvg,1), size(yAvg,2), size(yAvg,3), size(yAvg,4));
        ySum2   = ySum2Runs{iRun}(iBlk).GetDataTimeSeries('reshape');
        tHRF    = yAvgRuns{iRun}(iBlk).GetTime();
        nTrials = nTrialsRuns{iRun}{iBlk};
        if isempty(mlActRuns{iRun})
            mlActRuns{iRun} = cell(length(nDataBlks),1);
        end

        %
        datatype  = yAvgRuns{iRun}(iBlk).GetDataTypeLabel();
        if strncmp(datatype{1}, 'HRF Hb', length('HRF Hb'))
            ml    = yAvgRuns{iRun}(iBlk).GetMeasListSrcDetPairs('reshape');
        elseif strcmp(datatype{1}, 'HRF dOD')
            ml    = yAvgRuns{iRun}(iBlk).GetMeasList('reshape');
        end
        if isempty(mlActRuns{iRun}{iBlk})
            mlActRuns{iRun}{iBlk} = ones(size(ml,1),1);
        end
        mlAct = mlActRuns{iRun}{iBlk}(1:size(ml,1));

        nCond = size(nTrials,2);
        yAvgStdOut(iBlk).SetTime(tHRF);

        if strcmp(datatype{1}, 'HRF dOD')

            if isempty(grp1)
                grp1 = zeros(size(yAvg,1), size(yAvg,2), nCond);
                grp1Sum2 = zeros(size(yAvg,1), size(yAvg,2), nCond);
                nTrials_tot{iBlk} = zeros(size(yAvg,2), nCond);
            end

            lstChInc = find(mlAct==1);
            for iC = 1:nCond
                nT = nTrials(iC);
                if nT>0
                    if iRun==1
                        grp1(:,lstChInc,iC) = yAvg(:,lstChInc,iC) * nT;
                        grp1Sum2(:,lstChInc,iC) = ySum2(:,lstChInc,iC);
                        nTrials_tot{iBlk}(lstChInc,iC) = nT;
                    else
                        for iCh=1:length(lstChInc) %size(yAvg,2)
                            % Make sure 3rd arg to interp1 is column vector to guarauntee interp1 output is column vector
                            % which matches grp1 dimensions when adding the two.
                            grp1(:,lstChInc(iCh),iC) = grp1(:,lstChInc(iCh),iC) + interp1(tHRF,yAvg(:,lstChInc(iCh),iC),tHRF(:)) * nT;
                            grp1Sum2(:,lstChInc(iCh),iC) = grp1Sum2(:,lstChInc(iCh),iC) + interp1(tHRF,ySum2(:,lstChInc(iCh),iC),tHRF(:));
                            nTrials_tot{iBlk}(lstChInc(iCh),iC) = nTrials_tot{iBlk}(lstChInc(iCh),iC) + nT;
                        end
                    end
                end
            end

            if ~isempty(grp1)
                for iC = 1:size(grp1,3)
                    for iCh = 1:size(grp1,2)
                        yAvg(:,iCh,iC) = grp1(:,iCh,iC) / nTrials_tot{iBlk}(iCh,iC);

                        % We want to distinguish between no trials and 1 trial:
                        % If there are no trials, we have no HRF data and no std which
                        % the first case will calculate as opposed to one trial (2nd case)
                        % where we have all zeros.
                        if(nTrials_tot{iBlk}(iCh,iC)~=1)
                            yAvgStd(:,iCh,iC) = ( (1/(nTrials_tot{iBlk}(iCh,iC)-1))*grp1Sum2(:,iCh,iC) - (nTrials_tot{iBlk}(iCh,iC)/(nTrials_tot{iBlk}(iCh,iC)-1))*(grp1(:,iCh,iC) / nTrials_tot{iBlk}(iCh,iC)).^2).^0.5 ;
                        else
                            yAvgStd(:,iCh,iC) = zeros(size(grp1Sum2(:,iCh,iC)));
                        end

                        %%%% Snirf stuff: Once we get to the last run, we've accumulated our averages.
                        %%%% Now we can set channel descriptors for avg and standard deviation
                        if iRun == length(yAvgRuns)
                            yAvgStdOut(iBlk).AddChannelDod(ml(iCh,1), ml(iCh,2), ml(iCh,4), iC);
                        end
                    end

                    %%%% Snirf stuff: Once we get to the last run, we've accumulated our averages.
                    %%%% Now we can set channel descriptors for avg and standard deviation
                    if iRun == length(yAvgRuns)
                        yAvgStdOut(iBlk).AppendDataTimeSeries(yAvgStd(:,:,iC));
                    end
                end
            end

        elseif strncmp(datatype{1}, 'HRF Hb', length('HRF Hb'))

            if isempty(grp1)
                grp1 = zeros(size(yAvg,1), size(yAvg,2), size(yAvg,3), nCond);
                grp1Sum2 = zeros(size(yAvg,1), size(yAvg,2), size(yAvg,3), nCond);
                nTrials_tot{iBlk} = zeros(size(yAvg,3), nCond);
            end

            lstChInc = find(mlAct==1);
            for iC = 1:1:nCond
                nT = nTrials(iC);
                if nT>0
                    if iRun==1
                        grp1(:,:,lstChInc,iC) = yAvg(:,:,lstChInc,iC) * nT;
                        grp1Sum2(:,:,lstChInc,iC) = ySum2(:,:,lstChInc,iC);
                        nTrials_tot{iBlk}(lstChInc,iC) = nT;
                    else
                        for iCh=1:length(lstChInc) %size(yAvg,3)
                            for iHb=1:size(yAvg,2)
                                % Make sure 3rd arg to interp1 is column vector to guarauntee interp1 output is column vector
                                % which matches grp1 dimensions when adding the two.
                                grp1(:,iHb,lstChInc(iCh),iC) = grp1(:,iHb,lstChInc(iCh),iC) + interp1(tHRF,yAvg(:,iHb,lstChInc(iCh),iC),tHRF(:)) * nT;
                                grp1Sum2(:,iHb,lstChInc(iCh),iC) = grp1Sum2(:,iHb,lstChInc(iCh),iC) + interp1(tHRF,ySum2(:,iHb,lstChInc(iCh),iC),tHRF(:));
                            end
                            nTrials_tot{iBlk}(lstChInc(iCh),iC) = nTrials_tot{iBlk}(lstChInc(iCh),iC) + nT;
                        end
                    end
                end
            end

            if ~isempty(grp1)
                for iC = 1:size(grp1,4)
                    for iCh = 1:size(grp1,3)
                        yAvg(:,:,iCh,iC) = grp1(:,:,iCh,iC) / nTrials_tot{iBlk}(iCh,iC);

                        % We want to distinguish between no trials and 1 trial:
                        % If there are no trials, we have no HRF data and no std which
                        % the first case will calculate as opposed to one trial (2nd case)
                        % where we have all zeros.
                        if(nTrials_tot{iBlk}(iCh,iC)~=1)
                            yAvgStd(:,:,iCh,iC) = ( (1/(nTrials_tot{iBlk}(iCh,iC)-1))*grp1Sum2(:,:,iCh,iC) - (nTrials_tot{iBlk}(iCh,iC)/(nTrials_tot{iBlk}(iCh,iC)-1))*(grp1(:,:,iCh,iC) / nTrials_tot{iBlk}(iCh,iC)).^2).^0.5 ;
                        else
                            yAvgStd(:,:,iCh,iC) = zeros(size(grp1Sum2(:,:,iCh,iC)));
                        end

                        %%%% Snirf stuff: Once we get to the last run, we've accumulated our averages.
                        %%%% Now we can set channel descriptors for avg and standard deviation
                        if iRun == length(yAvgRuns)
                            yAvgStdOut(iBlk).AddChannelHbO(ml(iCh,1), ml(iCh,2), iC);
                            yAvgStdOut(iBlk).AddChannelHbR(ml(iCh,1), ml(iCh,2), iC);
                            yAvgStdOut(iBlk).AddChannelHbT(ml(iCh,1), ml(iCh,2), iC);
                        end
                    end

                    %%%% Snirf stuff: Once we get to the last run, we've accumulated our averages.
                    %%%% Now we can set channel descriptors for avg and standard deviation
                    if iRun == length(yAvgRuns)
                        yAvgStdOut(iBlk).AppendDataTimeSeries(yAvgStd(:,:,:,iC));
                    end
                end
            end
        end
    end
end


function K = sdlfmvXsdrbfKernCompute(sdlfmvKern, sdrbfKern, t1, t2)

% SDLFMVXSDRBFKERNCOMPUTE Cross kernel between a SDLFMV and a SDRBF kernels.
% FORMAT
% DESC computes cross kernel terms between the velocity switching dynamical
% LFM and the switching dynamicl RBF.
% ARG sdlfmvKern : the kernel structure associated with the velocity of the
% first SDLFM kernel.
% ARG sdrbfKern : the kernel structure associated with the SDRBF kernel.
% ARG t : inputs for which kernel is to be computed.
% RETURN K : block of values from kernel matrix.
%
% FORMAT
% DESC computes cross kernel terms between the velocity switching dynamical
% LFM and the switching dynamicl RBF.
% ARG sdlfmvKern : the kernel structure associated with the velocity of the
% first SDLFM kernel.
% ARG sdrbfKern : the kernel structure associated with the SDRBF kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% RETURN K : block of values from kernel matrix.
%
% SEEALSO : sdlfmvKernParamInit, sdlfmvKernCompute, sdlfmKernParamInit
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if nargin < 4
    t2 = t1;
end

K = sdlfmXsdrbfKernCompute(sdlfmvKern, sdrbfKern, t1, t2, 'Vel');


%-------------------------------------------------------------------------------------------------------------------%
%
% 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:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	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 (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the CHANNEL_CHANNEL-EXAMPLE geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Channel_Channel()

%
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%
Nx =  512;       % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
Ny =  512;       % # of Eulerian Grid Pts. in y-Direction (MUST BE EVEN!!!)
Lx = 1.0;        % Length of Eulerian Grid in x-Direction
Ly = 1.0;        % Length of Eulerian Grid in y-Direction

% Immersed Structure Geometric / Dynamic Parameters %
ds= min(Lx/(2*Nx),Ly/(2*Ny));  % Lagrangian spacing
L = 0.9*Lx;                    % Length of Channel
w = 0.2*Ly;                    % Width of Channel
x0 = 0.3;                      % x-Center for Cylinder
y0 = 0.5;                      % y-Center for Cylinder
r = w/10;                      % Radii of Cylinder
struct_name = 'channel'; % Name for .vertex, .spring, etc files.

% Call function to construct geometry
[xLag,yLag] = give_Me_Channel_Immsersed_Boundary_Geometry(ds,L,w,Lx,Ly);
[xLag_C,yLag_C] = give_Me_Cylinder_Immsersed_Boundary_Geometry(ds,r,x0,y0);
[xLag_V,yLag_V] = give_Me_Piston_IB_Geometry(ds,w,Lx,Ly);

Npts = length(xLag_C);
NPistonStart = length(xLag);
fprintf('\n\n          POSSIBLE WARNING!!!      \n\n');
fprintf('# of pts in a cell: %d\n',Npts);
fprintf('If it is not EVEN...there will be a problem :)\n');
fprintf('Change radius, r, or ds slightly to try to make it even\n\n\n');

% Compute Curvatures
C =  compute_Curvatures(xLag_C,yLag_C);

% Plot Geometry to test
plot(xLag(1:end/2),yLag(1:end/2),'r-'); hold on;
plot(xLag(end/2+1:end),yLag(end/2+1:end),'r-'); hold on;
plot(xLag_C,yLag_C,'r-'); hold on;
plot(xLag_V,yLag_V,'k.-'); hold on;

plot(xLag,yLag,'*'); hold on;
plot(xLag_C,yLag_C,'g*'); hold on;
xlabel('x'); ylabel('y');
axis([0 Lx 0 Ly]);

% Create different Bubbles
xLag_C1 = xLag_C; yLag_C1 = yLag_C;              % Bubble 1
xLag_C2 = xLag_C+33.5*r; yLag_C2 = yLag_C+2*r;      % Bubble 2
xLag_C3 = xLag_C+1.5*r; yLag_C3 = yLag_C-2.5*r;  % Bubble 3

% Combine Geometry Files into ONE Vector
%xLag = [xLag_C1 xLag_C2 xLag_C3 xLag xLag_V];
%yLag = [yLag_C1 yLag_C2 yLag_C3 yLag yLag_V];
xLag = [xLag_C1 xLag_C2 xLag_C3 xLag_V];
yLag = [yLag_C1 yLag_C2 yLag_C3 yLag_V];
yLag = yLag - 0.375;

first_Index = 3*length(xLag_C1)+1;
Ncell_Lag_Pts = [length(xLag_C1) length(xLag_C2) length(xLag_C3)];

% Combine CELL Geometry files into ONE vector for springs/beams (not necessary...just easier to count)
xLag_Cells = [xLag_C1 xLag_C2 xLag_C3];
yLag_Cells = [yLag_C1 yLag_C2 yLag_C3];

plot(xLag_Cells,yLag_Cells,'k*'); hold on;

%
% Prints .vertex file!
%
print_Lagrangian_Vertices(xLag,yLag,struct_name);

%
% Prints .spring file!
%
k_Spring = 2.5e7;
NPistonStart = first_Index; % Gives starting index of piston instead of num of channel pts
NPiston = length(xLag_V);
print_Lagrangian_Springs(xLag_C,yLag_C,xLag,yLag,k_Spring,ds,r,struct_name,NPistonStart,NPiston);
plot(xLag(NPistonStart),yLag(NPistonStart),'r*'); hold on;
plot(xLag(NPistonStart+NPiston-1),yLag(NPistonStart+NPiston-1),'g*'); hold on

%
% Prints .beam file!
%
k_Beam = 1e12;
print_Lagrangian_Beams(xLag_C,yLag_C,k_Beam,C,struct_name);

%
% Prints .coagulation file!
%
starting_index = 1;         % index of first cellular Lagrangian Pt.
threshold_radius = 0.075;   % how close do cells need to be to form a bond
bond_strength = 1e3;        % bond strength
fracture_force = 1e2;       % how strong of force to break bond
print_Lagrangian_Coagulation_Inputs(struct_name,Ncell_Lag_Pts,starting_index,threshold_radius,fracture_force,bond_strength);

%
% Prints .target file!
%
k_Target = 1e7;
fprintf('For update_target_pts: last target pt index before piston: %d\n\n\n',0);
print_Lagrangian_Target_Pts(xLag,k_Target,struct_name,first_Index);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints VERTEX points to a file called struct.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag);

    vertex_fid = fopen([struct_name '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        X_v = xLag(s);
        Y_v = yLag(s);
        fprintf(vertex_fid, '%1.16e %1.16e\n', X_v, Y_v);
    end

    fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints COAGULATION Parameters to the file struct.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Coagulation_Inputs(struct_name,Ncell_Lag_Pts,starting_index,threshold_radius,fracture_force,bond_strength)

    % Ncell_Lag_Pts:    -gives # of Lag. Pts in each cell (# of Lag. Pts that compose each cell)
    % starting_index:   -gives first index of first cell Lag. Pt. in vertex file
    %                   -NOTE: IB2d Assumes all cell Lag. Pts are next to each other in .vertex file
    % threshold_radius: -how close do cells need to be to form a bond
    % fracture_force:   -how strong of force to break bond between cells

    Ncells = length(Ncell_Lag_Pts);   % gives total number of cells

    coag_fid = fopen([struct_name '.coagulation'], 'w');

    fprintf(coag_fid, '%d\n', starting_index );   % Prints first index of a Lag. Pt. associated to a Cell
    fprintf(coag_fid, '%d\n', threshold_radius ); % Prints threshold radii
    fprintf(coag_fid, '%d\n', bond_strength );    % Prints bond strength
    fprintf(coag_fid, '%d\n', fracture_force );   % Prints fracture_force

    %Loops over all CELLs.
    for s = 1:Ncells
        fprintf(coag_fid, '%1.16e\n', Ncell_Lag_Pts(s) );
    end

    fclose(coag_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints Target points to a file called struct.target
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name,first_index)

    N = length(xLag) - first_index + 1;  % # of target points (e.g., the walls of channel)

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 0:N-1                   % Loops over N target pts...just starts at zero bc of how first_index is defined
        fprintf(target_fid, '%d %1.16e\n', s+first_index, k_Target);
    end

    fclose(target_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints SPRING points to a file called struct.spring
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Springs(xLag,yLag,xLagAll,yLagAll,k_Spring,ds,r,struct_name,NPistonStart,NPiston)

    N = length(xLag);

    spring_fid = fopen([struct_name '.spring'], 'w');

    fprintf(spring_fid, '%d\n', 3*N + 3/2*N + (NPiston-1) ); %N MUST BE EVEN!

    %spring_force = kappa_spring*ds/(ds^2);

    %SPRINGS BETWEEN VERTICES
    for s = 1:3*N
            if s < N
                x1 = xLag(s);    y1 = yLag(s);
                x2 = xLag(s+1);  y2 = yLag(s+1);
                ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            elseif s==N
                %Case s=N
                x1 = xLag(s);    y1 = yLag(s);
                x2 = xLag(1);    y2 = yLag(1);
                ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, 1,   k_Spring, ds_Rest);
            elseif ( ( s>N ) && ( s<2*N ) )
                x1 = xLagAll(s);    y1 = yLagAll(s);
                x2 = xLagAll(s+1);  y2 = yLagAll(s+1);
                ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            elseif s==2*N
                x1 = xLagAll(s);    y1 = yLagAll(s);
                x2 = xLagAll(N+1);  y2 = yLagAll(N+1);
                ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, N+1,   k_Spring, ds_Rest);
            elseif ( ( s>2*N ) && ( s<3*N ) )
                x1 = xLagAll(s);    y1 = yLagAll(s);
                x2 = xLagAll(s+1);  y2 = yLagAll(s+1);
                ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            elseif s==3*N
                x1 = xLagAll(s);      y1 = yLagAll(s);
                x2 = xLagAll(2*N+1);  y2 = yLagAll(2*N+1);
                ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, 2*N+1,   k_Spring, ds_Rest);
            end
    end

    for s=1:N/2
        x1 = xLagAll(s);      y1 = yLagAll(s);
        x2 = xLagAll(s+N/2);  y2 = yLagAll(s+N/2);
        ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+N/2, k_Spring/5000, ds_Rest);
    end

    for s=1:N/2
        x1 = xLagAll(N+s);      y1 = yLagAll(N+s);
        x2 = xLagAll(N+s+N/2);  y2 = yLagAll(N+s+N/2);
        ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', N+s, N + s+N/2, k_Spring/5000, 2*r);
    end

    for s=1:N/2
        x1 = xLagAll(2*N+s);      y1 = yLagAll(2*N+s);
        x2 = xLagAll(2*N+s+N/2);  y2 = yLagAll(2*N+s+N/2);
        ds_Rest = sqrt( (x1-x2)^2 + (y1-y2)^2 );
        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', 2*N+s, 2*N + s+N/2, k_Spring/5000, 2*r);
    end

    for s=NPistonStart:NPistonStart+NPiston-2
        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds);
    end
    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAM (Torsional Spring) points to a file called struct.beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name)

    % k_Beam: beam stiffness
    % C: beam curvature

    N = length(xLag); % NOTE: Total number of beams = Number of Total Lag Pts. - 2

    beam_fid = fopen([struct_name '.beam'], 'w');

    fprintf(beam_fid, '%d\n', 3*N );

    %spring_force = kappa_spring*ds/(ds^2);

    %BEAMS BETWEEN VERTICES
    for s = 1:N
            if s==1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',N, s, s+1,   k_Beam, C(s) );
            elseif  s <= N-1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C(s) );
            elseif s==N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, 1,   k_Beam, C(s) );
            end
    end

    for s = N+1:2*N
            if s==N+1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',2*N, s, s+1,   k_Beam, C(s-N) );
            elseif  s <= 2*N-1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C(s-N) );
            elseif s==2*N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, N+1,   k_Beam, C(s-N) );
            end
    end

    for s = 2*N+1:3*N
            if s==2*N+1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',3*N, s, s+1,   k_Beam, C(s-2*N) );
            elseif  s <= 3*N-1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C(s-2*N) );
            elseif s==3*N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, 2*N+1,   k_Beam, C(s-2*N) );
            end
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes "curvature" of ellipse
%
% NOTE: not curvature in the traditional geometric sense, in the 'discrete'
% sense through cross product.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function C = compute_Curvatures(xLag,yLag)

%a-x component (rmin)
%b-y component (rmax)
%C = ab / ( sqrt( a^2*sin(t)^2 + b^2*cos(t)^2  )  )^3

N = length(xLag);
C = zeros( N );

%Note: needs to be done same order as you print .beam file!
for i=1:N

   % Pts Xp -> Xq -> Xr (same as beam force calc.)

   if ( (i > 1) && (i < N) )

        Xp = xLag(i-1); Xq = xLag(i); Xr = xLag(i+1);
        Yp = yLag(i-1); Yq = yLag(i); Yr = yLag(i+1);

   elseif (i==1)

        Xp = xLag(N); Xq = xLag(i); Xr = xLag(i+1);
        Yp = yLag(N); Yq = yLag(i); Yr = yLag(i+1);

   elseif (i==N)

        Xp = xLag(N-1); Xq = xLag(N); Xr = xLag(1);
        Yp = yLag(N-1); Yq = yLag(N); Yr = yLag(1);

   end

   C(i) = (Xr-Xq)*(Yq-Yp) - (Yr-Yq)*(Xq-Xp); %Cross product btwn vectors

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for channel
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Channel_Immsersed_Boundary_Geometry(ds,L,w,Lx,Ly)

% The immsersed structure is a channel %
x = (Lx-L)/2:ds:(L+(Lx-L)/2);  %xPts
yBot = (Ly-w)/2;               %yVal for bottom of Channel
yTop = Ly - (Ly-w)/2;          %yVal for top of Channel

xLag = [x x];
yLag = [yBot*ones(1,length(x)) yTop*ones(1,length(x))];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: gives Lag. Pts for Piston
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Piston_IB_Geometry(ds,w,Lx,Ly)

yBot = (Ly-w)/2;               %yVal for bottom of Channel
yTop = Ly - (Ly-w)/2;          %yVal for top of Channel

yLag = Ly/2-0.05:ds:Ly/2+0.05;
xLag = 0.25*ones(1,length(yLag));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for cylinder
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Cylinder_Immsersed_Boundary_Geometry(ds,r,x0,y0)

% The immsersed structure is a cylinder %

dtheta = ds/ (2.08*r);
theta = 0; i=1;
while theta < 2*pi
   xLag(i) = x0 - r*cos(theta);
   yLag(i) = y0 - r*sin(theta);
   theta = theta + dtheta;
   i=i+1;
end

%ds_Rest = sqrt( ( xLag(2) - xLag(3) )^2 + ( xLag(2) - xLag(3) )^2 );


function [FO,ntrials] = getFractionalOccupancy (Gamma,T,options,dim,alignment)
%
% Computes the fractional occupancy
% - across trials if dim==1 (i.e. FO is time by no.states)
%   This is indicating how the fractional occupancy evolves as a function
%   of time across trials.
% - across time if dim==2 (i.e. FO is no.trials/subjects by no.states)
%   This is indicating how much of each state each subject has.
% (default, dim=2)
%
% The parameter 'options' must be the same than the one supplied to the
% hmmmar function for training. The structure hmm.train can also be
% supplied
%
% If dim==1, how is the across-time average defined when trials have not
% the same length? The trials can be aligned in two different ways: such
% that they all start at the same time point, or such that they all finish
% at the same time point. The parameter 'alignment' defines this, and can
% take values 'start' or 'finish'.
%
% If dim==1, also, ntrials will return a (time by 1) vector telling how many
% trials were used to compute the average at that time point. This is
% useful when trials have different lengths.
%
% Note: this can be applied to the data as well, if you want to look at the
% evoked response.
%
% Author: Diego Vidaurre, OHBA, University of Oxford (2016)

if nargin<3, options = struct(); options.Fs = 1; options.downsample = 1; end
if ~isfield(options,'Fs'), options.Fs = 1; end
if ~isfield(options,'downsample'), options.downsample = options.Fs; end
if nargin<4, dim = 2; end
if nargin<5 && dim == 1, alignment = 'start'; end

is_vpath = (size(Gamma,2)==1 && all(rem(Gamma,1)==0));
if iscell(T)
    if size(T,1) == 1, T = T'; end
    for i = 1:length(T)
        if size(T{i},1)==1, T{i} = T{i}'; end
    end
    Nsubj = length(T);
    trials2subjects = zeros(length(cell2mat(T)),1); ii = 1;
    for i = 1:length(T)
        Ntrials = length(T{i});
        trials2subjects(ii:ii+Ntrials-1) = i;
        ii = ii + Ntrials;
    end
    T = cell2mat(T);
elseif dim == 2
    Nsubj = length(T);
    trials2subjects = 1:Nsubj;
end
N = length(T);

r = 1;
if isfield(options,'downsample') && options.downsample>0
    r = (options.downsample/options.Fs);
end

if ~isfield(options,'order') && ~isfield(options,'embeddedlags')
   options.order = (sum(T) - size(Gamma,1)) / length(T);
end

if isfield(options,'tuda') && options.tuda | ...
        isfield(options, 'order') && options.order == 0
    T = ceil(r * T);
elseif isfield(options,'order') && options.order > 0
    T = ceil(r * T);
    T = T - options.order;
elseif isfield(options,'embeddedlags') && length(options.embeddedlags) > 1
    d1 = -min(0,options.embeddedlags(1));
    d2 = max(0,options.embeddedlags(end));
    T = T - (d1+d2);
    T = ceil(r * T);
end

if is_vpath % viterbi path
    vpath = Gamma;
    if options.dropstates==1
        K = length(unique(vpath));
    else
        K = options.K;
    end
    Gamma = zeros(length(vpath),K);
    for k = 1:K
       Gamma(vpath==k,k) = 1;
    end
else
    K = size(Gamma,2);
    %Gamma = Gamma > (2/3);
end

if dim == 2
    FO = zeros(Nsubj,K);
    for j = 1:N
        t0 = sum(T(1:j-1));
        ind = (1:T(j)) + t0;
        jj = trials2subjects(j);
        if length(ind) > 1
            FO(jj,:) = FO(jj,:) + sum(Gamma(ind,:));
        else
            FO(jj,:) = FO(jj,:) + Gamma(ind,:);
        end
    end
    FO = FO ./ repmat(sum(FO,2),1,K);
    ntrials = [];
else
    if all(T==T(1))
        Gamma = reshape(Gamma,[T(1),N,K]);
        FO = squeeze(mean(Gamma,2));
        ntrials = N * ones(T(1),1);
    else
        maxT = max(T);
        FO = zeros(maxT,K);
        ntrials = zeros(maxT,1);
        for j = 1:N
            t0 = sum(T(1:j-1));
            ind_gamma = (1:T(j)) + t0;
            if strcmpi(alignment,'start')
                ind_FO = 1:length(ind_gamma);
            else
                ind_FO = (1:length(ind_gamma)) + (maxT-length(ind_gamma));
            end
            FO(ind_FO,:) = FO(ind_FO,:) + Gamma(ind_gamma,:);
            ntrials(ind_FO) = ntrials(ind_FO) + 1;
        end
        FO = FO ./ repmat(ntrials,1,K);
    end
end

end


 function ob = Gtomo2_wtmex(arg1, varargin)
%function ob = Gtomo2_wtmex(sg, ig, options)
%function ob = Gtomo2_wtmex(file, options)
%function ob = Gtomo2_wtmex(arg_pairs, options)
%
% Generate a tomographic system model based on aspire's wtfmex() routine.
%
% in
%	sg	strum		sino_geom()
%	ig	strum		image_geom()
%
% options
%	'grouped'		row or col (default: row)
%	'nthread'		pthreads for multiple processors (default: 1)
%	'mask'	[nx,ny]		logical support mask, has precedence
%	'pairs' {}		cell array of name/value pairs for aspire_pair()
%
% out
%	ob	[nb*na,np]	Fatrix system "matrix", np = sum(ig.mask(:))
%
% Copyright 2006-3-3, Jeff Fessler, The University of Michigan

if nargin == 1 && streq(arg1, 'test'), Gtomo2_wtmex_test, return, end
if nargin < 1, ir_usage, end

% options
arg.grouped = 'row';
arg.nthread = 1;
arg.mask = [];
arg.chat = 0;
arg.pairs = {};

% given wtf or argument pairs
if ischar(arg1)

	arg = vararg_pair(arg, varargin);

	if exist(arg1, 'file') % file.wtf
		if ~isempty(arg.mask), error 'no mask option given .wtf', end
		arg.file = arg1;
		[nx ny nb na] = wtfmex('read', arg.file);

	else % arg_pair
		arg.arg = arg1;
		if isempty(arg.mask)
			mask_arg = {};
		else
			if ~islogical(arg.mask), error 'need logical mask', end
			mask_arg = uint8(arg.mask);
		end

		wtfmex('gensys', arg.arg', arg.grouped, mask_arg{:});

		nx = str2num(arg_get(arg.arg, 'nx'));
		ny = str2num(arg_get(arg.arg, 'ny'));
		nb = str2num(arg_get(arg.arg, 'nb'));
		na = str2num(arg_get(arg.arg, 'na'));
	end

	arg.ig = image_geom('nx', nx, 'ny', ny, 'dx', -1, ...
		'mask', wtfmex('mask') > 0);
	arg.sg = sino_geom('par', 'nb', nb, 'na', na);

% given sino_geom() and image_geom()
else

	arg.sg = arg1;
	arg.ig = varargin{1};
	varargin = {varargin{2:end}};
	arg = vararg_pair(arg, varargin);
	if ~isempty(arg.mask)
		arg.ig.mask = arg.mask;
	end

	arg.aspire_arg = aspire_pair(arg.sg, arg.ig, 'support', 'array', ...
		arg.pairs{:}); % trick: pairs can override default support

	wtfmex('gensys', arg.aspire_arg', arg.grouped, uint8(arg.ig.mask));

end

arg.power = 1;
arg.nd = arg.sg.nb * arg.sg.na;
arg.np = sum(arg.ig.mask(:));
dim = [arg.nd arg.np]; % trick: make it masked by default!

%
% build Fatrix object
%
ob = Fatrix(dim, arg, 'caller', 'Gtomo2_wtmex', ...
	'forw', @Gtomo2_wtmex_forw, 'back', @Gtomo2_wtmex_back, ...
	'free', @Gtomo2_wtmex_free, ...
	'mtimes_block', @Gtomo2_wtmex_mtimes_block);

if arg.chat
	wtfmex('print');
end

%
% Gtomo2_wtmex_forw(): y = G * x
%
function y = Gtomo2_wtmex_forw(arg, x)
y = Gtomo2_wtmex_mtimes_block(arg, 0, x, 1, 1);

%
% Gtomo2_wtmex_back(): x = G' * y
% full backprojection
%
function x = Gtomo2_wtmex_back(arg, y)
if arg.power == 2
	[flag_column y] = Gtomo2_wtmex_y_shape(arg, y, istart, nblock);
	x = wtfmex('chat', arg.chat, 'back2', single(y));
	if flag_column, x = arg.ig.maskit(x); end
else
	x = Gtomo2_wtmex_mtimes_block(arg, 1, y, 1, 1);
end

%
% Gtomo2_wtmex_mtimes_block()
%
function y = Gtomo2_wtmex_mtimes_block(arg, is_transpose, x, istart, nblock)

if is_transpose
	y = Gtomo2_wtmex_block_back(arg, x, istart, nblock);
else
	y = Gtomo2_wtmex_block_forw(arg, x, istart, nblock);
end

%
% Gtomo2_wtmex_block_forw()
%
function y = Gtomo2_wtmex_block_forw(arg, x, istart, nblock)

if arg.power ~= 1, error('power=%d not done', arg.power), end

% if needed, convert concise column to 3d array
flag_column = 0;
if size(x,1) == arg.np
	flag_column = 1;
	x = arg.ig.embed(x);
end

if nblock == 1
	y = wtfmex('chat', arg.chat, 'mult', single(x));
else
	y = wtfmex('chat', arg.chat, 'proj,block', single(x), ...
			int32(istart-1), int32(nblock));

	% fix: extract the relevant columns - should do in wtfmex?
	ia = istart:nblock:arg.sg.na;
	y = y(:,ia,:);
end

y = double6(y);

if flag_column % column in yields column out.
	y = reshape(y, arg.sg.nb*size(y,2), []);
end

%
% Gtomo2_wtmex_block_back()
%
function x = Gtomo2_wtmex_block_back(arg, y, istart, nblock)

if arg.power ~= 1, error('power=%d not done', arg.power), end

[flag_column y] = Gtomo2_wtmex_y_shape(arg, y, istart, nblock);
if nblock == 1
	x = wtfmex('chat', arg.chat, 'back', single(y));
else
	x = wtfmex('chat', arg.chat, 'back,block', single(y), ...
			int32(istart-1), int32(nblock));
end
x = double6(x);

if flag_column, x = arg.ig.maskit(x); end

%
% Gtomo2_wtmex_y_shape(arg, y, istart, nblock)
%
function [flag_column, y] = Gtomo2_wtmex_y_shape(arg, y, istart, nblock)

flag_column = 0;
nb = arg.sg.nb;
na = arg.sg.na;

if nblock == 1
	if size(y,1) == nb * na % [nb*na,?]
		flag_column = 1;
		y = arg.sg.shape(y); % [nb,na,?]
	end
else
	ia = istart:nblock:na;
	nv = length(ia);
	if size(y,1) == nb * nv % [nb*nv,?]
		flag_column = 1;
		tmp = zeros(nb, na, size(y,2));
		tmp(:,ia,:) = reshape(y, nb, nv, []);
		y = reshape(tmp, nb*na, []); % [nb*na,?]
	elseif size(y,1) == nb
		error todo
	else
		error bug
	end
end

%
% Gtomo2_wtmex_free()
%
function Gtomo2_wtmex_free(arg)
printm('freeing wtfmex static memory')
wtfmex('free');

%
% Gtomo2_wtmex_power()
%
function ob = Gtomo2_wtmex_power(ob, sup)
ob.arg.power = ob.arg.power * sup;


function x = p34_start ( n )

%*****************************************************************************80
%
%% P34_START returns a starting point for optimization for problem 34.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables X.
%
%    Output, real X(N), a starting point for the optimization.
%
  x = [ -1.5, 0.5 ]';

  return
end


%DEFINE_CONSTANTS  Defines useful constants for indexing data, etc.
%
%   This is simply a convenience script that defines the constants
%   listed below, consisting primarily of named indices for the
%   columns of the data matrices: bus, branch, gen and gencost.
%   This includes input columns defined in caseformat as well as
%   columns that are added in the power flow and OPF output. It also
%   defines constants for the change tables used by apply_changes().
%
%   bus:
%      PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM,
%      VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN
%
%   branch:
%      F_BUS, T_BUS, BR_R, BR_X, BR_B, RATE_A, RATE_B, RATE_C,
%      TAP, SHIFT, BR_STATUS, PF, QF, PT, QT, MU_SF, MU_ST,
%      ANGMIN, ANGMAX, MU_ANGMIN, MU_ANGMAX
%
%   gen:
%      GEN_BUS, PG, QG, QMAX, QMIN, VG, MBASE, GEN_STATUS, PMAX, PMIN,
%      MU_PMAX, MU_PMIN, MU_QMAX, MU_QMIN, PC1, PC2, QC1MIN, QC1MAX,
%      QC2MIN, QC2MAX, RAMP_AGC, RAMP_10, RAMP_30, RAMP_Q, APF
%
%   gencost:
%      PW_LINEAR, POLYNOMIAL, MODEL, STARTUP, SHUTDOWN, NCOST, COST
%
%   change tables:
%       CT_LABEL, CT_PROB, CT_TABLE, CT_TBUS, CT_TGEN, CT_TBRCH, CT_TAREABUS,
%       CT_TAREAGEN, CT_TAREABRCH, CT_ROW, CT_COL, CT_CHGTYPE, CT_REP,
%       CT_REL, CT_ADD, CT_NEWVAL, CT_TLOAD, CT_TAREALOAD, CT_LOAD_ALL_PQ,
%       CT_LOAD_FIX_PQ, CT_LOAD_DIS_PQ, CT_LOAD_ALL_P, CT_LOAD_FIX_P,
%       CT_LOAD_DIS_P, CT_TGENCOST, CT_TAREAGENCOST, CT_MODCOST_F,
%       CT_MODCOST_X
%
%   See CASEFORMAT, IDX_BUS, IDX_BRCH, IDX_GEN, IDX_COST and IDX_CT for
%   details on the meaning of these constants. Internally
%   DEFINE_CONSTANTS calls IDX_BUS, IDX_BRCH, IDX_GEN, IDX_COST and IDX_CT.
%   In performance sensitive code, such as internal MATPOWER functions
%   that are called frequently, it is preferred to call these
%   functions directly rather than using the DEFINE_CONSTANTS script,
%   which is less efficient.
%
%   This script is included for convenience for interactive use or
%   for high-level code where maximum performance is not a concern.

%   MATPOWER
%   Copyright (c) 2009-2016, Power Systems Engineering Research Center (PSERC)
%   by Doug Mitarotonda & Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% define named indices into data matrices
[PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
    VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;
[F_BUS, T_BUS, BR_R, BR_X, BR_B, RATE_A, RATE_B, RATE_C, ...
    TAP, SHIFT, BR_STATUS, PF, QF, PT, QT, MU_SF, MU_ST, ...
    ANGMIN, ANGMAX, MU_ANGMIN, MU_ANGMAX] = idx_brch;
[GEN_BUS, PG, QG, QMAX, QMIN, VG, MBASE, GEN_STATUS, PMAX, PMIN, ...
    MU_PMAX, MU_PMIN, MU_QMAX, MU_QMIN, PC1, PC2, QC1MIN, QC1MAX, ...
    QC2MIN, QC2MAX, RAMP_AGC, RAMP_10, RAMP_30, RAMP_Q, APF] = idx_gen;
[PW_LINEAR, POLYNOMIAL, MODEL, STARTUP, SHUTDOWN, NCOST, COST] = idx_cost;
[CT_LABEL, CT_PROB, CT_TABLE, CT_TBUS, CT_TGEN, CT_TBRCH, CT_TAREABUS, ...
    CT_TAREAGEN, CT_TAREABRCH, CT_ROW, CT_COL, CT_CHGTYPE, CT_REP, ...
    CT_REL, CT_ADD, CT_NEWVAL, CT_TLOAD, CT_TAREALOAD, CT_LOAD_ALL_PQ, ...
    CT_LOAD_FIX_PQ, CT_LOAD_DIS_PQ, CT_LOAD_ALL_P, CT_LOAD_FIX_P, ...
    CT_LOAD_DIS_P, CT_TGENCOST, CT_TAREAGENCOST, CT_MODCOST_F, ...
    CT_MODCOST_X] = idx_ct;


% JM: 2006/07/19
% viewer for data Ic on domain Omega

function varargout = viewImage(Ic,Omega,m,varargin);
% this is just output and hence not commented; fell free to improve (-;

global VIEWER VIEWPARA

% default parameter:
fig       = 1;
name      = 'image';
plots     = 1;
sub       = [1,1,1];
figname   = '';

%% overwrite default parameter
for k=1:1:length(varargin)/2,
  %disp([varargin{2*k-1},'=varargin{',int2str(2*k),'};']);
  eval([varargin{2*k-1},'=varargin{',int2str(2*k),'};']);
end;

if ~plots, return;  end;          % do nothing, just return;

if nargin == 0,
  fprintf('VIEWER = %s\n',VIEWER);
  VIEWPARA
  return;
end;

if isstr(Ic) & strcmp(Ic,'set'),
%   fprintf('set VIEWPARA\n');
  VIEWPARA = {Omega,m,varargin{:}};
  for j = 1:length(VIEWPARA),
    if strcmp(VIEWPARA{j},'viewer'),
      VIEWER = VIEWPARA{j+1};
      break;
    end;
  end;
%   viewImage;
  return;
end;

% setup figure
fig = figure(fig);
subplot(sub(1),sub(2),sub(3)); cla;

if ~isempty(figname),
  set(fig,'numbertitle','off','name',sprintf('[JM-%d]: %s',fig,figname));
end;

% call the viewer
fh = feval(VIEWER,Ic,Omega,m,VIEWPARA{:});
str = sprintf('%s',name);
title(str);

if nargout>0,  varargout{1} = fh;  end;
return;
%==============================================================================

%==============================================================================
function ih = image2D(B,Omega,m,varargin)

colmap = [];

%% overwrite default parameter
for k=1:1:length(varargin)/2,
  %disp([varargin{2*k-1},'=varargin{',int2str(2*k),'};']);
  eval([varargin{2*k-1},'=varargin{',int2str(2*k),'};']);
end;

% compute pixel size and generate a regular grid
h  = Omega./m;
z1 = h(1)/2:h(1):Omega(1);
z2 = h(2)/2:h(2):Omega(2);

% we are now ready to use MATLAB's image
ih = image(z1,z2,reshape(B,m)');
axis image; axis xy                     % apply some nice axis
if ~isempty(colmap), colormap(colmap);  end;
return;

%==============================================================================
function ih = imagesc2D(B,Omega,m,varargin)

colmap = [];

%% overwrite default parameter
for k=1:1:length(varargin)/2,
  %disp([varargin{2*k-1},'=varargin{',int2str(2*k),'};']);
  eval([varargin{2*k-1},'=varargin{',int2str(2*k),'};']);
end;

% compute pixel size and generate a regular grid
h  = Omega./m;
z1 = h(1)/2:h(1):Omega(1);
z2 = h(2)/2:h(2):Omega(2);

% we are now ready to use MATLAB's image
ih = imagesc(z1,z2,reshape(B,m)');
axis image; axis xy                     % apply some nice axis
if ~isempty(colmap), colormap(colmap);  end;
return;

%==============================================================================
function p = volview(I,Omega,m,varargin)

I = reshape(I,m);
% I = flipdim(I,3);
isovalue    = 0;
viewpoint   = [-37.5,30];
facecolor   = .75*[1,1,1];
facealpha   = .8;

%% overwrite default parameter
for k=1:1:length(varargin)/2,
  %disp([varargin{2*k-1},'=varargin{',int2str(2*k),'};']);
  eval([varargin{2*k-1},'=varargin{',int2str(2*k),'};']);
end;

I(I<isovalue) = 0;

p(1) = patch(isosurface(I,isovalue));

switch computer,
case 'PCWIN',
  set(p(1),...
    'FaceColor',facecolor,...
    'EdgeColor','none');
otherwise,
  set(p(1),...
    'FaceColor',facecolor,...
    'EdgeColor','none',...
    'FaceAlpha',facealpha);
end;

isonormals(I,p(1))

p(2) = patch(isocaps(I,isovalue));

set(p(2),...
  'FaceColor','interp',...
  'EdgeColor','none');

view(viewpoint);
axis equal
axis([1,size(I,2),1,size(I,1),1,size(I,3)]);
camlight; lighting phong
material dull
drawnow
%==============================================================================

%==============================================================================
function ih = montage(I,Omega,m,varargin)

I = reshape(I,m);

threshold = 0;
framesx   = [];
framesy   = [];
direction = 'x';
number    = 'off';
colmap    = [];
%% overwrite default parameter
for k=1:1:length(varargin)/2,
  %disp([varargin{2*k-1},'=varargin{',int2str(2*k),'};']);
  eval([varargin{2*k-1},'=varargin{',int2str(2*k),'};']);
end;

if threshold ~= 0,
  I(I<threshold) = 0;
end;

I = squeeze(I);
l = length(size(I));

fig = gcf;
switch l,
  case 1,
    hh = plot(1:size(I,1),I);
  case 2,
    hh = subimage(I,colmap);
    if ~isempty(colmap), colormap(colmap); end;
  case 3,
    [m1,m2,m3] = size(I);

    switch direction,
      case 'x',
        aa = m1; b1 = m2; b2 = m3; I1 = 1:m2; I2 = 1:m3;
      case 'y',
        aa = m2; b1 = m1; b2 = m3; I1 = 1:m1; I2 = 1:m3;
      case 'z',
        aa = m3; b1 = m1; b2 = m2; I1 = 1:m1; I2 = 1:m2;

      otherwise, jmerror(direction);
    end;

    if isempty(framesx) & isempty(framesy),
      framesx = ceil(sqrt(aa));
      framesy = ceil(aa/framesx);
    elseif isempty(framesx),
      framesx = ceil(aa/framesy);
    else
      framesy = ceil(aa/framesx);
    end;

    %   fprintf('image is %dx%dx%d, frames [%d,%d]\n',...
    %     m1,m2,m3,framesx,framesy);

    B = zeros(b1*framesx,b2*framesy);

    a1 = 0; a2 = 0;
    for j=1:aa,
      switch direction,
        case 'x',  P = squeeze(I(j,:,:));
        case 'y',  P = squeeze(I(:,j,:));
        case 'z',  P = squeeze(I(:,:,j));
      end;
      B(a1+I1,a2+I2) = P;
      a2 = a2 + b2;
      if ~rem(j,framesy), a1 = a1 + b1; a2 = 0; end;
    end;

    ih = image(B);
    if ~isempty(colmap), colormap(colmap); end;
    hold on

    if strcmp(number,'on'),
      a1 = 0; a2 = 0;
      for j=1:aa,
        text(a2+10,a1+20,['#',int2str(j)],'fontsize',5,'color','r');
        a2 = a2 + b2;
        if ~rem(j,framesy), a1 = a1 + b1; a2 = 0; end;
      end;
    end;

    for j=0:framesx,
      plot(0.5+[0,framesy*b2],0.5+b1*j*[1,1],'b-');
    end;
    for j=0:framesy,
      plot(0.5+b2*j*[1,1],0.5+[0,framesx*b1],'b-');
    end;

    axis equal
    axis([0 framesy*b2+1, 0, framesx*b1+1])
    axis off
    hold off;
  otherwise,
    jmerror('dimension=?');
end;
% ==============================================================================

%==============================================================================


classdef SideSlipAngleTermCondition < AbstractEventTerminationCondition
    %SideSlipAngleTermCondition Summary of this class goes here
    %   Detailed explanation goes here

    properties
        steeringModel(1,1) AbstractSteeringModel = RollPitchYawPolySteeringModel.getDefaultSteeringModel();
        targetSlipAngle(1,1) double = 0;
        bodyInfo KSPTOT_BodyInfo
    end

    methods
        function obj = SideSlipAngleTermCondition(targetSlipAngle)
            obj.targetSlipAngle = targetSlipAngle;
        end

        function evtTermCondFcnHndl = getEventTermCondFuncHandle(obj)
            evtTermCondFcnHndl = @(t,y) obj.eventTermCond(t,y, obj.targetSlipAngle, obj.steeringModel, obj.bodyInfo);
        end

        function initTermCondition(obj, initialStateLogEntry)
            obj.steeringModel = initialStateLogEntry.steeringModel;
            obj.bodyInfo = initialStateLogEntry.centralBody;
        end

        function name = getName(obj)
            name = sprintf('Side Slip Angle (%.3f deg)', rad2deg(obj.targetSlipAngle));
        end

        function tf = shouldBeReinitOnRestart(obj)
            tf = false;
        end

        function params = getTermCondUiStruct(obj)
            params = struct();

            params.paramName = 'Target Side Slip Angle';
            params.paramUnit = 'deg';
            params.useParam = 'on';
            params.useStages = 'off';
            params.useTanks = 'off';
            params.useEngines = 'off';
            params.useStopwatches = 'off';

            params.value = rad2deg(obj.targetSlipAngle);
            params.refStage = LaunchVehicleStage.empty(1,0);
            params.refTank = LaunchVehicleEngine.empty(1,0);
            params.refEngine = LaunchVehicleEngine.empty(1,0);
            params.refStopwatch = LaunchVehicleStopwatch.empty(1,0);
        end

        function optVar = getNewOptVar(obj)
            optVar = SideSlipAngleTermCondOptimVar(obj);
        end

        function optVar = getExistingOptVar(obj)
            optVar = obj.optVar;
        end

        function tf = usesStage(obj, stage)
            tf = false;
        end

        function tf = usesEngine(obj, engine)
            tf = false;
        end

        function tf = usesTank(obj, tank)
            tf = false;
        end

        function tf = usesEngineToTankConn(obj, engineToTank)
            tf = false;
        end

        function tf = usesStopwatch(obj, stopwatch)
            tf = false;
        end
    end

    methods(Static)
        function termCond = getTermCondForParams(paramValue, stage, tank, engine, stopwatch)
            termCond = SideSlipAngleTermCondition((paramValue));
        end
    end

    methods(Static, Access=private)
        function [value,isterminal,direction] = eventTermCond(t,y, targetSlipAngle, steeringModel, bodyInfo)
            ut = t;
            rVect = y(1:3);
            vVect = y(4:6);

            dcm = steeringModel.getBody2InertialDcmAtTime(ut, rVect, vVect, bodyInfo);
            [~,~,angOfSideslip] = computeAeroAnglesFromBodyAxes(ut, rVect, vVect, bodyInfo, dcm(:,1), dcm(:,2), dcm(:,3));

            angOfSideslip = AngleZero2Pi(angOfSideslip);

            value = angOfSideslip - targetSlipAngle;
            isterminal = 1;
            direction = 0;
        end
    end
end


function CaptureFigVid(ViewZ, FileName,OptionZ)
% CaptureFigVid(ViewZ, FileName,OptionZ)
% Captures a video of the 3D plot in the current axis as it rotates based
% on ViewZ and saves it as 'FileName.mpg'. Option can be specified.
%
% ViewZ:     N-rows with 2 columns, each row are the view angles in
%            degrees, First column is azimuth (pan), Second is elevation
%            (tilt) values outside of 0-360 wrap without error,
%            *If a duration is specified, angles are used as nodes and
%            views are equally spaced between them (other interpolation
%            could be implemented, if someone feels so ambitious).
%            *If only an initial and final view is given, and no duration,
%            then the default is 100 frames.
% FileName:  Name of the file of the produced animation. Because I wrote
%            the program, I get to pick my default of mpg-4, and the file
%            extension .mpg will be appended, even if the filename includes
%            another file extension. File is saved in the working
%            directory.
% (OptionZ): Optional input to specify parameters. The ones I use are given
%            below. Feel free to add your own. Any or all fields can be
%            used
% OptionZ.FrameRate: Specify the frame rate of the final video (e.g. 30;)
% OptionZ.Duration: Specify the length of video in seconds (overrides
%    spacing of view angles) (e.g. 3.5;)
% OptionZ.Periodic: Logical to indicate if the video should be periodic.
%    Using this removed the final view so that when the video repeats the
%    initial and final view are not the same. Avoids having to find the
%    interval between view angles. (e.g. true;)
%
% % % % Example (shown in published results, video attached) % % % %
% figure(171);clf;
% surf(peaks,'EdgeColor','none','FaceColor','interp','FaceLighting','phong')
% daspect([1,1,.3]);axis tight;
% OptionZ.FrameRate=15;OptionZ.Duration=5.5;OptionZ.Periodic=true;
% CaptureFigVid([-20,10;-110,10;-190,80;-290,10;-380,10],'WellMadeVid',OptionZ)
%
% Known issues: MPEG-4 video option only available on Windows machines. See
% fix where the VideoWriter is called.
%
% Getframe is used to capture image and current figure must be on monitor 1
% if multiple displays are used. Does not work if no display is used.
%
% Active windows that overlay the figure are captured in the movie.  Set up
% the current figure prior to calling the function. If you don't specify
% properties, such as tick marks and aspect ratios, they will likely change
% with the rotation for an undesirable effect.

% Cheers, Dr. Alan Jennings, Research assistant professor,
% Department of Aeronautics and Astronautics, Air Force Institute of Technology

%% preliminaries

% initialize optional argument
if nargin<3;     OptionZ=struct([]); end

% check orientation of ViewZ, should be two columns and >=2 rows
if size(ViewZ,2)>size(ViewZ,1); ViewZ=ViewZ.'; end
if size(ViewZ,2)>2
warning('AJennings:VidWrite',...
    'Views should have n rows and only 2 columns. Deleting extraneous input.');
ViewZ=ViewZ(:,1:2); %remove any extra columns
end

% Create video object
daObj=VideoWriter(FileName,'MPEG-4'); %my preferred format
% daObj=VideoWriter(FileName); %for default video format.
% MPEG-4 CANNOT BE USED ON UNIX MACHINES
% set values:
% Frame rate
if isfield(OptionZ,'FrameRate')
    daObj.FrameRate=OptionZ.FrameRate;
end
% Durration (if frame rate not set, based on default)
if isfield(OptionZ,'Duration') %space out view angles
    temp_n=round(OptionZ.Duration*daObj.FrameRate); % number frames
    temp_p=(temp_n-1)/(size(ViewZ,1)-1); % length of each interval
    ViewZ_new=zeros(temp_n,2);
    % space view angles, if needed
    for inis=1:(size(ViewZ,1)-1)
        ViewZ_new(round(temp_p*(inis-1)+1):round(temp_p*inis+1),:)=...
            [linspace(ViewZ(inis,1),ViewZ(inis+1,1),...
             round(temp_p*inis)-round(temp_p*(inis-1))+1).',...
             linspace(ViewZ(inis,2),ViewZ(inis+1,2),...
             round(temp_p*inis)-round(temp_p*(inis-1))+1).'];
    end
    ViewZ=ViewZ_new;
end
% space view angles, if needed
if length(ViewZ)==2 % only initial and final given
    ViewZ=[linspace(ViewZ(1,1),ViewZ(end,1)).',...
           linspace(ViewZ(1,2),ViewZ(end,2)).'];
end
% Periodicity
if isfield(OptionZ,'Periodic')&&OptionZ.Periodic==true
ViewZ=ViewZ(1:(end-1),:); %remove last sample
end
% open object, preparatory to making the video
open(daObj);

%% rotate the axis and capture the video
for kathy=1:size(ViewZ,1)
    view(ViewZ(kathy,:)); drawnow;
    writeVideo(daObj,getframe(gcf)); %use figure, since axis changes size based on view
end

%% clean up
close(daObj);


% Copyright 2017 Lime Microsystems Ltd.
%
% Licensed under the Apache License, Version 2.0 (the "License");
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
%
%    http://www.apache.org/licenses/LICENSE-2.0
%
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an "AS IS" BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and
% limitations under the License.
%
% provide EVM for random sequences
% W-CDMA does not enforce equal amplitude signals, so we need AGC for each signal
% we assume we are dealing with BPSK or QAM, so rms amplitude should be unity
function evm=WCDMAULevm(x,qam)
  agc=1/sqrt(sum(x.*conj(x))/length(x));
  y=x*agc;
	evmref2=sum(abs(qam))/length(qam);
	[evm,loc]=min(abs(repmat(conj(qam'),1,length(y))-repmat(y,length(qam),1))); % decode QAM symbols
%	evm=evm./abs(qam(loc)); % some EVMs define relative to desired symbol
	evm/=evmref2; % some EVMs define relative to rms of QAM symbol table
end


function model = vargplvmUpdateStats(model, X_u)
% VARGPLVMUPDATESTATS Update stats for VARGPLVM model.
%
% COPYRIGHT : Michalis K. Titsias, 2009-2011
% COPYRIGHT : Neil D. Lawrence, 2009-2011
% COPYRIGHT : Andreas C. Damianou, 2010-2011
%
% SEEALSO : vargplvmOptimise, vargplvmExpandParam

% VARGPLVM

jitter = 1e-6;
%model.jitter = 1e-6;

% %%% Precomputations for the KL term %%%
%%% Maybe we should add something like model.dynamics.X =
%%% model.dynamics.vardist.means (if visualisation requires that).
 if isfield(model, 'dynamics') && ~isempty(model.dynamics)
     % This actually only operates on model.dynamics. The only reason we
     % send the whole model is that we need to get model.vardist.means.
     model = vargplvmDynamicsUpdateStats(model);
 end

%%% Precomputations for (the likelihood term of) the bound %%%

model.K_uu = kernCompute(model.kern, X_u);

% Always add jitter (so that the inducing variables are "jitter" function variables)
% and the above value represents the minimum jitter value
% Putting jitter always ("if" in comments) is like having a second
% whiteVariance in the kernel which is constant.
%if (~isfield(model.kern, 'whiteVariance')) | model.kern.whiteVariance < jitter
   % There is no white noise term so add some jitter.
if ~strcmp(model.kern.type, 'rbfardjit')
       model.K_uu = model.K_uu ...
                         + sparseDiag(repmat(jitter, size(model.K_uu, 1), 1));
end
%end

model.Psi0 = kernVardistPsi0Compute(model.kern, model.vardist);
model.Psi1 = kernVardistPsi1Compute(model.kern, model.vardist, X_u);
[model.Psi2, AS] = kernVardistPsi2Compute(model.kern, model.vardist, X_u);

%K_uu_jit = model.K_uu + model.jitter*eye(model.k);
%model.Lm = chol(K_uu_jit, 'lower');

                                      % M is model.k
%model.Lm = chol(model.K_uu, 'lower');
model.Lm = jitChol(model.K_uu)';      % M x M: L_m (lower triangular)   ---- O(m^3)
model.invLm = model.Lm\eye(model.k);  % M x M: L_m^{-1}                 ---- O(m^3)
model.invLmT = model.invLm'; % L_m^{-T}
model.C = model.invLm * model.Psi2 * model.invLmT;
model.TrC = sum(diag(model.C)); % Tr(C)
% Matrix At replaces the matrix A of the old implementation; At is more stable
% since it has a much smaller condition number than A=sigma^2 K_uu + Psi2
model.At = (1/model.beta) * eye(size(model.C,1)) + model.C; % At = beta^{-1} I + C

model.Lat = jitChol(model.At)'; % UNCOMMENT THIS IF YOU ARE NOT DOING THE DEBUG BELOW

%try %%%% DEBUG1
% model.Lat = jitChol(model.At)';
%catch e %%% DEBUG
%    model_At = model.At;
%    save('modellAt', 'model_At');
%    e.throw %%% DEBUG
%end %%%% DEBUG

% DEBUG2
% warning('') %%% DEBUG
% model.Lat = jitChol(model.At)';
% tmpWarn = lastwarn; %%% DEBUG
% if length(tmpWarn) > 41 && strcmp(tmpWarn(1:42), 'Matrix is not positive definite in jitChol') %%% DEBUG
%     % do stuff...
%     %%%model.id
%     warning('') % This is a good place to put a stop for the debugger...
% end %%% DEBUG

model.invLat = model.Lat\eye(size(model.Lat,1));
model.invLatT = model.invLat';
model.logDetAt = 2*(sum(log(diag(model.Lat)))); % log |At|

model.P1 = model.invLat * model.invLm; % M x M

% First multiply the two last factors; so, the large N is only involved
% once in the calculations (P1: MxM, Psi1':MxN, Y: NxD)
model.P = model.P1 * (model.Psi1' * model.m);

% Needed for both, the bound's and the derivs. calculations.
model.TrPP = sum(sum(model.P .* model.P));

%%% Precomputations for the derivatives (of the likelihood term) of the bound %%%

%model.B = model.invLmT * model.invLatT * model.P; %next line is better
model.B = model.P1' * model.P;
model.invK_uu = model.invLmT * model.invLm;
Tb = (1/model.beta) * model.d * (model.P1' * model.P1);
	Tb = Tb + (model.B * model.B');
model.T1 = model.d * model.invK_uu - Tb;

model.X = model.vardist.means;

%{
%%%%-------------------------- TEMP (for analysing how ind. pts are optimised) !!!!!!!!!
d=dist2(model.X_u, model.vardist.means);
[C,I]=min(d,[],2);
dNorm=dist2(model.X_u, model.vardist.means)/norm(model.X_u)^2;
[Cnorm,Inorm]=min(dNorm,[],2);

if Inorm-I ~= 0
    fprintf(1, 'Something is wrong with the distances! (Update Stats)');
end

try
    load TEMPInducingMeansDist
catch exception
    TEMPInducingMeansDist=[];
end
try
    load TEMPInducingIndices
catch exception
    TEMPInducingIndices=[];
end
try
    load TEMPInducingMeansDistNorm
catch exception
    TEMPInducingMeansDistNorm=[];
end
TEMPInducingMeansDist = [TEMPInducingMeansDist sum(C)];
save 'TEMPInducingMeansDist.mat' 'TEMPInducingMeansDist';
%fprintf(1,'# Total X_u - vardistMeans = %d\n',sum(C));
TEMPInducingIndices = [TEMPInducingIndices I];
save 'TEMPInducingIndices.mat' 'TEMPInducingIndices';

TEMPInducingMeansDistNorm = [TEMPInducingMeansDistNorm sum(Cnorm)];
save 'TEMPInducingMeansDistNorm.mat' 'TEMPInducingMeansDistNorm';
%}


function [labels] = predictor_linear_classifier(X,B_hat,mapping)
%PREDICTOR_LINEAR_CLASSIFIER
%
%
%

[~,~,labels]  = linear_classifier('predict',X,[],[],'classification',B_hat,mapping);

end


function test_bug3195

% MEM 8gb
% WALLTIME 00:20:00
% DEPENDENCY ft_prepare_sourcemodel ft_inside_headmodel

%%
% load the data

cd(dccnpath('/home/common/matlab/fieldtrip/data/test/bug3195'));

load('template_FEM.mat')
load('sourcemodel.mat')

% the system matrix has not been computed yet, hence ft_headmodeltype fails
template_FEM.type = 'simbio';

%%
% take a subset of the cortical sheet vertices

sel = randperm(8004);
sel = sel(1:1000);

cfg = [];
cfg.headmodel = template_FEM;
cfg.sourcemodel.pos = sourcemodel.pos(sel,:);
sourcemodel1 = ft_prepare_sourcemodel(cfg);

figure
ft_plot_mesh(sourcemodel1.pos(sourcemodel1.inside,:))

%%
% construct a coarse 3D grid

cfg = [];
cfg.headmodel = template_FEM;
cfg.resolution = 20;
sourcemodel2 = ft_prepare_sourcemodel(cfg);

figure
ft_plot_mesh(sourcemodel2.pos(sourcemodel2.inside,:))


function f = compose(f, op)
%COMPOSE   Compose command for SPHEREFUNV objects.
%   F = COMPOSE(F, G) returns the composition G(F) of the SPHEREFUNV object
%   F and a CHEBFUN3 or CHEBFUN3V with three components G.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Deal with empty CHEBFUN objects:
if ( isempty(f) || isempty(op) )
    if ( isa(op, 'chebfun3v') )
        f = spherefunv();
    else
        f = spherefun();
    end
end

% F is real and has three components, so we can compose with a CHEBFUN3 or
% CHEBFUN3V.
% TODO? compose when op is a spherefun object?

if ( isa(op, 'chebfun3') )
    % Get components of F:
    f1 = f.components{1};
    f2 = f.components{2};
    f3 = f.components{3};

    % Check that image(f) is in domain(op):
    rangef = minandmax2est(f);              % Estimate of image(f).
    tol = 100 * chebfun2eps * max(vscale(f), vscale(op)) * ...
            norm(f1.domain, inf);           % Tolerance.
    if ( ~isSubset(rangef, op.domain, tol) )
        error('CHEBFUN:SPHEREFUNV:COMPOSE:DomainMismatch3', ...
            'OP(F) is not defined, since image(F) is not contained in domain(OP).')
    end

    % Call constructor:
    f = spherefun(@(x,y,z) op(feval(f1, x, y, z), feval(f2, x, y, z), ...
        feval(f3, x, y, z)));

elseif ( isa(op, 'chebfun3v') )
    % Check that OP has three components:
    if ( op.nComponents ~= 3 )
        error('CHEBFUN:SPHEREFUNV:compose:nComponents', ...
            'CHEBFUN3V(SPHEREFUNV) is defined for CHEBFUN3V objects with 3 components.')
    end

    % Get components of op:
    op1 = op(1);
    op2 = op(2);
    op3 = op(3);

    % Get components of f:
    f1 = f.components{1};
    f2 = f.components{2};
    f3 = f.components{3};

    % Check that image(f) is in domain(op):
    rangef = minandmax2est(f);              % Estimate of image(f).
    tol = 100 * chebfun2eps * max(vscale(f), vscale(op)) * ...
            norm(f1.domain, inf);           % Tolerance.
    if ( ~isSubset(rangef, op1.domain, tol) )
        error('CHEBFUN:SPHEREFUNV:COMPOSE:DomainMismatch3v', ...
            'OP(F) is not defined, since image(F) is not contained in domain(OP).')
    end

    % Calling directly the constructor for [ op1(f), op2(f), op3(f) ] as below
    % seems to be slightly faster than building a spherefun for each component
    % and then calling the spherefunv constructor on the three spherefuns.

    % Call constructor:
    f = spherefunv(@(x,y,z) op1(feval(f1, x, y, z), feval(f2, x, y, z), ...
        feval(f3, x, y, z)), ...
        @(x,y,z) op2(feval(f1, x, y, z), feval(f2, x, y, z), ...
        feval(f3, x, y, z)), ...
        @(x,y,z) op3(feval(f1, x, y, z), feval(f2, x, y, z), ...
        feval(f3, x, y, z)));

else
    error('CHEBFUN:SPHEREFUNV:COMPOSE:OP', ...
        'Can compose a SPHEREFUNV with a CHEBFUN3 or CHEBFUN3V.')
end

end


function tmp = rmSliceGet(model,slice,id)
% rmSliceGet - extract slice from model struct and place it
% into temporary struct, also convert to single here.
%
% tmp = rmSliceGet(model,slice,id);
%
% 2008/01 SOD: extracted from rmGridFit.

if ~exist('model','var') || isempty(model), error('Need model'); end
if ~exist('slice','var') || isempty(slice), slice = 1;           end
if ~exist('id','var') || isempty(id),       id = 1:numel(model); end

% loop over models
tmp = cell(numel(id),1);
for n=id,
    f = {'x0','y0','s','x02','y02','s2','s_major','s_minor','s_theta','rss','rss2','rawrss','rawrss2', 'exponent'};

    % for all models
    tmp{n}.desc = rmGet(model{n},'desc');
    tmp{n}.df   = single(rmGet(model{n},'dfglm'));
    for fn = 1:numel(f),
        val    = rmGet(model{n},f{fn});
        if ~isempty(val),
            % switch on the number of dimensions, since inplane data has
            % diff dimensionality than other views
            switch length(size(val))
                case 3 % presumably means INPLANE view, where slice is the third dimension
                    temp = single(val(:,:,slice));
                    tmp{n}.(f{fn}) = temp(:)';
                otherwise % otherwise slice is the first dimensions
                    tmp{n}.(f{fn}) = single(val(slice,:));
            end
        end;
    end;

    % put all beta values in one matrix
    val      = rmGet(model{n},'b');
    switch length(size(val)) % switch on the number of dimensions, since inplane data has diff dimesnsionality than other views
        case 4 % 4 dimensions means Inplane model:
               %  3 dimensions of coords, and one dimension of beta values.
               %  Hence the dims are x, y, slice, betas. We want to make a
               %  matrix of betas for one slice. This matrix be 2-D, with
               %  one dimension indexing the x and y values (hence
               %  length x * y), and the other dimension representing the
               %  beta values (hence length size(val, 4))
            nbetas = size(val,4);
            nvoxelsPerSlice = size(val,2)*size(val,1);
            tmp{n}.b = zeros(nbetas,nvoxelsPerSlice,'single');
            for fn = 1:nbetas,
                temp  = single(val(:,:,slice,fn));
                tmp{n}.b(fn,:) = temp(:);
            end;
        otherwise
            tmp{n}.b = zeros(size(val,3),size(val,2),'single');
            for fn = 1:size(val,3),
                tmp{n}.b(fn,:) = single(val(slice,:,fn));
            end;
    end
end;

return;


function cS = times(alpha,cS)

if ~isa(cS,'crystalShape')
  [cS,alpha] = deal(alpha,cS);
end

alpha = repmat(alpha(:).',size(cS.V,1),size(cS.V,2)/numel(alpha));
cS.V = alpha .* cS.V;


function vol = read_asa_vol(fn)

% READ_ASA_VOL reads an ASA volume conductor file
%
% all data is converted to the following units
%   vertices        mm
%   conductivities  S/m

% Copyright (C) 2002, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

Nbnd  = read_asa(fn, 'NumberBoundaries=', '%d');
UnitC = read_asa(fn, 'UnitConduct', '%s');
UnitP = read_asa(fn, 'UnitPosition', '%s');
cond  = read_asa(fn, 'Conductivities', '%f');
radii = read_asa(fn, 'Radii', '%f');
pos   = read_asa(fn, 'Positions', '%f');
bnd1  = read_asa(fn, 'Boundary1', '%s');
bnd2  = read_asa(fn, 'Boundary2', '%s');
bnd3  = read_asa(fn, 'Boundary3', '%s');
bnd4  = read_asa(fn, 'Boundary4', '%s');

if ~isempty(radii) || ~isempty(pos)
  % this appears to be a spherical volume conductor
  if strcmpi(UnitP,'mm')
    radii = 1*radii;
    pos   = 1*pos;
  elseif strcmpi(UnitP,'cm')
    radii = 100*radii;
    pos   = 100*pos;
  elseif strcmpi(UnitP,'m')
    radii = 1000*radii;
    pos   = 1000*pos;
  else
    ft_error(sprintf('Unknown unit of distance for volume (%s)', UnitP));
  end
end

if strcmpi(UnitC,'s/m')
  cond = cond/1;
elseif strcmpi(UnitC,'s/cm')
  cond = cond/100;
elseif strcmpi(UnitC,'s/mm')
  cond = cond/1000;
else
  ft_error(sprintf('Unknown unit of conductivity for volume (%s)', UnitC));
end

if ~isempty(radii)
  % this appears to be a spherical volume conductor
  vol.radius = radii;
  vol.cond   = cond;
  vol.center = pos;
else
  % this appears to be a realistical volume conductor
  [path, name, ext] = fileparts(fn);
  if Nbnd>=1
    vol.bnd(1) = read_asa_bnd(fullfile(path, bnd1));
  end
  if Nbnd>=2
    vol.bnd(2) = read_asa_bnd(fullfile(path, bnd2));
  end
  if Nbnd>=3
    vol.bnd(3) = read_asa_bnd(fullfile(path, bnd3));
  end
  if Nbnd>=4
    vol.bnd(4) = read_asa_bnd(fullfile(path, bnd4));
  end
  if Nbnd>=5
    ft_error('cannot read more than 4 boundaries');
  end

  % if there is a precomputed matrix, read it from an external file
  mat_file = read_asa(fn, 'Matrix', '%s');
  if ~isempty(mat_file)
    nr = read_asa(fullfile(path, mat_file), 'NumberRows=', '%d');
    nc = read_asa(fullfile(path, mat_file), 'NumberColumns=', '%d');
    mab_file = read_asa(fullfile(path, mat_file), 'Matrix', '%s');
    fid = fopen_or_error(fullfile(path, mab_file), 'rb', 'ieee-le');
    vol.mat = fread(fid, [nr nc], 'float32');
    fclose(fid);
    % remove the factor 2 that ASA assumes in the system matrix
    vol.mat = vol.mat/2;
    % scale the system matrix corresponding to vertex coordinates in mm
    vol.mat = vol.mat*100;
  end

  vol.cond   = cond;
end

% remove all empty fields
field=fieldnames(vol);
for i=1:length(field)
  if isempty(getfield(vol, field{i}))
    vol = rmfield(vol, field{i});
  end
end