function [h, t] = vl_plotbox(boxes, varargin)
% PLOTBOX  Plot boxes
%   PLOTBOX(BOX) plots a box specified by the 4-dimensional vector
%   BOXES = [XMIN YMIN XMAX YMAX]'. If BOXES is a 4 x N matrix, a box
%   for each of the N columns is plotted.
%
%   H = PLOTBOX(BOXES) returns an handle to the line drawings
%   representing the boxes. For multiple boxes, H is a row vector with
%   one handle per box.
%
%   PLOTBOX(BOXES, 'LABEL', LABEL) annotates the box with the string
%   LABEL. If BOXES contains multiple boxes, then LABEL can be a cell
%   array with one entry for each box. H is then a 2 x N array with
%   handles to boxes and labels.
%
%   PLOTBOX(BOXES, ...) passes any extra argument to the underlying
%   plotting function. The first optional argument can be a line
%   specification string such as the one used by PLOT().
%
%   See also:: VL_PLOTFRAME().

% Author:: Andrea Vedaldi

% Copyright (C) 2008-13 Andrea Vedaldi
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

opts.label = {} ;

% if the first optional argument is a linespec expand it
if length(varargin) > 0
  lineprop = vl_linespec2prop(varargin{1}) ;
  varargin = {lineprop{:}, varargin{2:end}} ;
end

% parse optional arguments
[opts, varargin] = vl_argparse(opts, varargin) ;
if ischar(opts.label)
  opts.label = {opts.label} ;
end

if size(boxes,2) == 0
  h = [] ;
  return ;
end

if size(boxes,1) ~= 4
  error('BOXES must be a 4 x N matrix') ;
end

Lx = [1 0 0 0 ;
      0 0 1 0 ;
      0 0 1 0 ;
      1 0 0 0 ;
      1 0 0 0 ] ;

Ly = [0 1 0 0 ;
      0 1 0 0 ;
      0 0 0 1 ;
      0 0 0 1 ;
      0 1 0 0 ] ;

fig = newplot ;
h = line(Lx * boxes, Ly * boxes, varargin{:}) ;
h = h' ;

if ~isempty(opts.label)
  ish = ishold ;
  hold on ;
  t = zeros(1,length(opts.label)) ;
  for r = 1:size(boxes,2)
    cl = get(h(1,r), 'Color') ;
    q = mod(r - 1, length(opts.label)) + 1 ;
    h(2,r) = text(mean(boxes([1 3],r)), boxes(2,r), opts.label{q}, ...
                  'Background', cl, ...
                  'VerticalAlignment', 'bottom', ...
                  'HorizontalAlignment', 'center') ;
  end
  if ~ish, hold off ; end
end

if nargout == 0
  clear h ;
end


dir = 'C:\Users\xiuye\Dropbox\ZBrainOutput\light-sheet data (11 fish)\cell locations';

Collection = cell(1,11);
for i_fish = 1:11,
    i_fish
    fishdir = fullfile(dir,['F',num2str(i_fish),'_cell_info.mat']);
    load(fishdir);
    XY = reshape([cell_info.center],2,[]);
    Z = [cell_info.slice];
    m = vertcat(XY,Z)';
    text = cell(length(m),1);
    for i = 1:length(m),
        text{i} = [num2str(m(i,1)),' ',num2str(m(i,2)),' ',num2str(m(i,3))];
    end
    Collection{i_fish} = text;
end

save(fullfile(dir,'Allfish.mat'),'Collection');

%%

for i_fish = 1:11,
    i_fish
    %%
    fileID = fopen(['F',num2str(i_fish),'_XYZ.txt'],'w');

    fishdir = fullfile(dir,['F',num2str(i_fish),'_cell_info.mat']);
    load(fishdir);
    XY = reshape([cell_info.center],2,[]);
    Z = [cell_info.slice];
    m = vertcat(XY,Z)';
    for i = 1:length(m),
       s = [num2str(m(i,2)),' ',num2str(m(i,1)),' ',num2str(m(i,3))];
       fprintf(fileID,[s,'\r\n']);
    end

    fclose(fileID);
    %%
end

%%

for i_fish = 1:11,
    fileID = fopen(['F',num2str(i_fish),'_XYZ.txt'],'w');
    text =Collection{i_fish};
    for i = 1:length(text);
        fprintf(fileID,[text{i},'\r\n']);
    end
    fclose(fileID);
    %%
end


classdef GeometricPointSet < matlab.mixin.SetGet
    %GeometricPointSet Summary of this class goes here
    %   Detailed explanation goes here

    properties
        points(1,:) AbstractGeometricPoint

        lvdData LvdData
    end

    methods
        function obj = GeometricPointSet(lvdData)
            obj.lvdData = lvdData;
        end

        function addPoint(obj, point)
            obj.points(end+1) = point;
        end

        function removePoint(obj, point)
            obj.points([obj.points] == point) = [];
        end

        function [listBoxStr, points] = getListboxStr(obj)
            listBoxStr = {};

            for(i=1:length(obj.points)) %#ok<*NO4LP>
                listBoxStr{end+1} = obj.points(i).getListboxStr(); %#ok<AGROW>
            end

            points = obj.points;
        end

        function indPoints = getPointsForInds(obj, inds)
            indPoints = obj.points(inds);
        end

        function inds = getIndsForPoints(obj, indPoints)
            inds = [];
            for(i=1:length(indPoints))
                for(j=1:length(obj.points))
                    if(indPoints(i) == obj.points(j))
                        inds(end+1) = j; %#ok<AGROW>
                        break;
                    end
                end
            end
        end

        function indPoint = getPointAtInd(obj, ind)
            if(ind > 0 && ind <= length(obj.points))
                indPoint = obj.points(ind);
            else
                indPoint = AbstractGeometricPoint.empty(1,0);
            end
        end

        function [plotPoints, listBoxStr] = getPlottablePoints(obj)
            plotPoints = AbstractGeometricPoint.empty(1,0);
            listBoxStr = {};
            for(i=1:length(obj.points))
                if(obj.points(i).canBePlotted())
                    plotPoints(end+1) = obj.points(i); %#ok<AGROW>
                    listBoxStr{end+1} = obj.points(i).getListboxStr(); %#ok<AGROW>
                end
            end
        end

        function numPoints = getNumPoints(obj)
            numPoints = length(obj.points);
        end

        function tf = usesGroundObj(obj, groundObj)
            tf = false;
            for(i=1:length(obj.points))
                tf = tf || obj.points(i).usesGroundObj(groundObj);
            end
        end

        function tf = usesGeometricPoint(obj, point)
            tf = false;
            for(i=1:length(obj.points))
                tf = tf || obj.points(i).usesGeometricPoint(point);
            end
        end

        function tf = usesGeometricVector(obj, vector)
            tf = false;
            for(i=1:length(obj.points))
                tf = tf || obj.points(i).usesGeometricVector(vector);
            end
        end

        function tf = usesGeometricCoordSys(obj, coordSys)
            tf = false;
            for(i=1:length(obj.points))
                tf = tf || obj.points(i).usesGeometricCoordSys(coordSys);
            end
        end

        function tf = usesGeometricRefFrame(obj, refFrame)
            tf = false;
            for(i=1:length(obj.points))
                tf = tf || obj.points(i).usesGeometricRefFrame(refFrame);
            end
        end

        function tf = usesGeometricAngle(obj, angle)
            tf = false;
            for(i=1:length(obj.points))
                tf = tf || obj.points(i).usesGeometricAngle(angle);
            end
        end

        function tf = usesGeometricPlane(obj, plane)
            tf = false;
            for(i=1:length(obj.points))
                tf = tf || obj.points(i).usesGeometricPlane(plane);
            end
        end

        %graphical analysis tasks
        function ptGAStr = getAllPointGraphAnalysisTaskStrs(obj)
            ptGAStr = horzcat(obj.getPointPositionXGraphAnalysisTaskStrs(), ...
                              obj.getPointPositionYGraphAnalysisTaskStrs(), ...
                              obj.getPointPositionZGraphAnalysisTaskStrs(), ...
                              obj.getPointVelocityXGraphAnalysisTaskStrs(), ...
                              obj.getPointVelocityYGraphAnalysisTaskStrs(), ...
                              obj.getPointVelocityZGraphAnalysisTaskStrs());
        end

        function [ptPosXGAStr, points] = getPointPositionXGraphAnalysisTaskStrs(obj)
            points = obj.points;

            ptPosXGAStr = cell(1,length(points));
            A = length(points);
            formSpec = sprintf('%%0%uu',floor(log10(abs(A)+1)) + 1);
            for(i=1:length(points))
                ptPosXGAStr{i} = sprintf(sprintf('Point %s Position (X) - "%s"',formSpec, points(i).getName()), i);
            end
        end

        function [ptPosXGAStr, points] = getPointPositionYGraphAnalysisTaskStrs(obj)
            points = obj.points;

            ptPosXGAStr = cell(1,length(points));
            A = length(points);
            formSpec = sprintf('%%0%uu',floor(log10(abs(A)+1)) + 1);
            for(i=1:length(points))
                ptPosXGAStr{i} = sprintf(sprintf('Point %s Position (Y) - "%s"',formSpec, points(i).getName()), i);
            end
        end

        function [ptPosXGAStr, points] = getPointPositionZGraphAnalysisTaskStrs(obj)
            points = obj.points;

            ptPosXGAStr = cell(1,length(points));
            A = length(points);
            formSpec = sprintf('%%0%uu',floor(log10(abs(A)+1)) + 1);
            for(i=1:length(points))
                ptPosXGAStr{i} = sprintf(sprintf('Point %s Position (Z) - "%s"',formSpec, points(i).getName()), i);
            end
        end

        function [ptVelXGAStr, points] = getPointVelocityXGraphAnalysisTaskStrs(obj)
            points = obj.points;

            ptVelXGAStr = cell(1,length(points));
            A = length(points);
            formSpec = sprintf('%%0%uu',floor(log10(abs(A)+1)) + 1);
            for(i=1:length(points))
                ptVelXGAStr{i} = sprintf(sprintf('Point %s Velocity (X) - "%s"',formSpec, points(i).getName()), i);
            end
        end

        function [ptVelYGAStr, points] = getPointVelocityYGraphAnalysisTaskStrs(obj)
            points = obj.points;

            ptVelYGAStr = cell(1,length(points));
            A = length(points);
            formSpec = sprintf('%%0%uu',floor(log10(abs(A)+1)) + 1);
            for(i=1:length(points))
                ptVelYGAStr{i} = sprintf(sprintf('Point %s Velocity (Y) - "%s"',formSpec, points(i).getName()), i);
            end
        end

        function [ptVelZGAStr, points] = getPointVelocityZGraphAnalysisTaskStrs(obj)
            points = obj.points;

            ptVelZGAStr = cell(1,length(points));
            A = length(points);
            formSpec = sprintf('%%0%uu',floor(log10(abs(A)+1)) + 1);
            for(i=1:length(points))
                ptVelZGAStr{i} = sprintf(sprintf('Point %s Velocity (Z) - "%s"',formSpec, points(i).getName()), i);
            end
        end
    end
end


function tol = cputime_tol
    % cputime_res applies to either t1 or t2 individually;
    % tol applies to t2-t1;
    tol = 2*cputime_res + sqrt(eps);
end

%!test
%! tol = cputime_tol;


clearvars -except yolonet yoloml yolojb
close all

% Download yolo network if it does not exist in the current folder (which it won't the
% first time you run the code). This takes a while. Code snippet courtesy of mathworks.

if exist('yolonet.mat','file') == 0
    url = 'https://www.mathworks.com/supportfiles/gpucoder/cnn_models/Yolo/yolonet.mat';
    websave('yolonet.mat',url);
end

%load yolo network from current folder (this can take a while) if it does not exist in
%workspace (which it won't the first time you run code)
if exist('yolonet') ~= 1
    load yolonet.mat
end

% the first time we run the script, we need to modify yolonet and save it
% with a new name (yoloml)
if exist('yoloml.mat','file') == 0
    display('modifying network')

    % extract a layer graph from the network. We need to modify this graph.
    lgraph = layerGraph(yolonet.Layers);

    % the yolo network from MATLAB is built like a classifier.
    % We need to convert it to a regression network. This means modifying the
    % last two layers
    lgraph = removeLayers(lgraph,'ClassificationLayer');
    lgraph = removeLayers(lgraph,'softmax');

    % According to the original YOLO paper, the last transfer function
    % is not a leaky, but a normal ReLu (I think).
    % In MATLAB, this is equivalent to a leaky ReLu with Scale = 0.
    alayer = leakyReluLayer('Name','linear_25','Scale',0);
    rlayer = regressionLayer('Name','routput');
    lgraph = addLayers(lgraph,rlayer);
    lgraph = replaceLayer(lgraph,'leakyrelu_25',alayer);
    lgraph = connectLayers(lgraph,'FullyConnectedLayer1','routput');
    yoloml = assembleNetwork(lgraph);

    %save the network with a new name
    display('saving modified network')
    save yoloml yoloml

% if we have created and saved yoloml but not loaded it to workspace, load
% it now.
elseif exist('yoloml') ~= 1
    display('loading modified network')
    load('yoloml.mat')
end


% set the dimensions of the two leg links

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

% create the leg links based on DH parameters
%                    theta   d     a  alpha
links(1) = Link([    0       0    0   pi/2 ], 'standard');
links(2) = Link([    0       0    L1   0   ], 'standard');
links(3) = Link([    0       0   -L2   0   ], 'standard');

% now create a robot to represent a single leg
leg = SerialLink(links, 'name', 'leg', 'offset', [pi/2   0  -pi/2]);

% define the key parameters of the gait trajectory, walking in the
% x-direction
xf = 5; xb = -xf;   % forward and backward limits for foot on ground
y = 5;              % distance of foot from body along y-axis
zu = 2; zd = 5;     % height of foot when up and down
% define the rectangular path taken by the foot
segments = [xf y zd; xb y zd; xb y zu; xf y zu] * 0.01;

% build the gait. the points are:
%   1 start of walking stroke
%   2 end of walking stroke
%   3 end of foot raise
%   4 foot raised and forward
%
% The segments times are :
%   1->2  3s
%   2->3  0.5s
%   3->4  1s
%   4->1  0.5ss
%
% A total of 4s, of which 3s is walking and 1s is reset.  At 0.01s sample
% time this is exactly 400 steps long.
%
% We use a finite acceleration time to get a nice smooth path, which means
% that the foot never actually goes through any of these points.  This
% makes setting the initial robot pose and velocity difficult.
%
% Intead we create a longer cyclic path: 1, 2, 3, 4, 1, 2, 3, 4. The
% first 1->2 segment includes the initial ramp up, and the final 3->4
% has the slow down.  However the middle 2->3->4->1 is smooth cyclic
% motion so we "cut it out" and use it.
segments = [segments; segments];
tseg = [3 0.25 0.5 0.25]';
tseg = [1; tseg; tseg];
x = mstraj(segments, [], tseg, segments(1,:), 0.01, 0.1);

% pull out the cycle
xcycle = x(100:500,:);
qcycle = leg.ikine( transl(xcycle), [], [1 1 1 0 0 0], 'pinv' );

% dimensions of the robot's rectangular body, width and height, the legs
% are at each corner.
W = 0.1; L = 0.2;

% a bit of optimization.  We use a lot of plotting options to
% make the animation fast: turn off annotations like wrist axes, ground
% shadow, joint axes, no smooth shading.  Rather than parse the switches
% each cycle we pre-digest them here into a plotopt struct.
% plotopt = leg.plot({'noraise', 'nobase', 'noshadow', ...
%     'nowrist', 'nojaxes'});
% plotopt = leg.plot({'noraise', 'norender', 'nobase', 'noshadow', ...
%     'nowrist', 'nojaxes', 'ortho'});

plotopt = {'noraise', 'nobase', 'noshadow', 'nowrist', 'nojaxes', 'delay', 0};

% create 4 leg robots.  Each is a clone of the leg robot we built above,
% has a unique name, and a base transform to represent it's position
% on the body of the walking robot.
legs(1) = SerialLink(leg, 'name', 'leg1');
legs(2) = SerialLink(leg, 'name', 'leg2', 'base', transl(-L, 0, 0));
legs(3) = SerialLink(leg, 'name', 'leg3', 'base', transl(-L, -W, 0)*trotz(pi));
legs(4) = SerialLink(leg, 'name', 'leg4', 'base', transl(0, -W, 0)*trotz(pi));

% create a fixed size axis for the robot, and set z positive downward
clf; axis([-0.3 0.1 -0.2 0.2 -0.15 0.05]); set(gca,'Zdir', 'reverse')
hold on
% draw the robot's body
patch([0 -L -L 0], [0 0 -W -W], [0 0 0 0], ...
    'FaceColor', 'r', 'FaceAlpha', 0.5)
% instantiate each robot in the axes
for i=1:4
    legs(i).plot(qcycle(1,:), plotopt{:});
end
hold off

% walk!
k = 1;
%A = Animate('walking');
%while 1
for i=1:500
    legs(1).animate( gait(qcycle, k, 0,   0));
    legs(2).animate( gait(qcycle, k, 100, 0));
    legs(3).animate( gait(qcycle, k, 200, 1));
    legs(4).animate( gait(qcycle, k, 300, 1));
    drawnow
    k = k+1;
    %A.add();
end


clear ra
for i = 1 : length(list)
    s = size(imread(list{i}));
    s = max(s);
    t=gt{i};
    t = t(:,3)-t(:,1);
    t=t/s*2048;
    ra{i} = t;
end
t = cellfun(@(x) unique(11-floor(log2(x))), ra, 'uni', 0);
 class = cellfun(@(x) max(1, min(ceil(log2(x   )*10)-50, 60)), ra, 'uni', 0);


function [gx] = g_dummy(x,P,u,in)
gx = 1./(1+exp(-x./P));


classdef TestConvexHull
    %TestConvexHull

    methods (Static)
        function test_input_1
            % cell array
            points = {[0 0], [1 1], [2 2], [3 3], [4 4]};
            hull = cv.convexHull(points);
        end

        function test_input_2
            % Nx2 matrix
            points = [0 0; 1 0; 2 2; 3 3; 3 4];
            hull = cv.convexHull(points);
        end

        function test_input_3
            % 1xNx2 matrix
            points = shiftdim([0 0; 1 0; 2 2; 3 3; 3 4], -1);
            hull = cv.convexHull(points);
        end

        function test_input_4
            % Nx1x2 matrix
            points = permute([0 0; 1 0; 2 2; 3 3; 3 4], [1 3 2]);
            hull = cv.convexHull(points);
        end

        function test_type_1
            points = single([0 0; 1 0; 2 2; 3 3; 3 4]);
            hull = cv.convexHull(points);
        end

        function test_type_2
            points = int32([0 0; 1 0; 2 2; 3 3; 3 4]);
            hull = cv.convexHull(points);
        end

        function test_option_1
            points = [0 0; 1 0; 2 2; 3 3; 3 4];
            hull = cv.convexHull(points, 'Clockwise',false);
            hull = cv.convexHull(points, 'Clockwise',true);
        end

        function test_output_1
            points = int32([0 0; 1 0; 2 2; 3 3; 3 4]);

            hull = cv.convexHull(points, 'ReturnPoints',true);
            validateattributes(hull, {'numeric'}, {'2d', 'ncols',2});
            assert(isa(hull, class(points)));

            hull = cv.convexHull(points, 'ReturnPoints',false);
            validateattributes(hull, {'int32'}, {'vector', 'nonnegative'});
        end

        function test_output_2
            points = num2cell(int32([0 0; 1 0; 2 2; 3 3; 3 4]),2);

            hull = cv.convexHull(points, 'ReturnPoints',true);
            validateattributes(hull, {'cell'}, {'vector'});
            assert(all(cellfun(@numel, hull) == 2));

            hull = cv.convexHull(points, 'ReturnPoints',false);
            validateattributes(hull, {'int32'}, {'vector', 'nonnegative'});
        end

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

end


function [camCorners, usedImIdx] = getCameraCorners(calibInfo)
%% Extract checkerboard corners given calibInfo.

%% License
% ACADEMIC OR NON-PROFIT ORGANIZATION NONCOMMERCIAL RESEARCH USE ONLY
% Copyright (c) 2018 Bingyao Huang
% All rights reserved.

% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:

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

% If you publish results obtained using this software, please cite our paper.

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

%%
% Get a list of images that has 'light' in file name.
imLightNames = ImgProc.getImageNames(calibInfo.path, 'lightGrid', calibInfo.sets);

% resize image to width = 640, since MATLAB detectCheckerboardPoints may fail on large images.
w = 640;
imCheckerboard = readAllImgs(imLightNames);

if(size(imCheckerboard, 2) > w)
    imCheckerboardSmall = imresize(imCheckerboard, [nan, w]);
else
    imCheckerboardSmall = imCheckerboard;
end

% extract checkerboard corners
[camCornersTemp, boardSize, usedImIdx] = detectCheckerboardPoints(imCheckerboardSmall);

if(size(imCheckerboard, 2) > w)
    % scale checkerboard coordinates to the orginal size
    camCornersTemp = camCornersTemp * size(imCheckerboard, 2) / w;
end

if(nnz(boardSize == calibInfo.boardSize) < 2)
    error('Extracted checkerboard size does not match calib-info.yml, please check calib-info.yml and checkerboard image');
end

% initialize corner coordiantes with [-1, -1] if fail to extract corners
% from some images, this is to avoid MT GUI file not found error
numImages = numel(usedImIdx);
numCorners = (boardSize(1)-1)*(boardSize(2)-1);

camCorners = -ones(numCorners,2, numImages);
camCorners(:,:,usedImIdx) = camCornersTemp;

if(nnz(usedImIdx) < length(usedImIdx))
    warning(['Unable to extract checkerboard corners in set(s): ','[', num2str(find(usedImIdx == 0))',']'])
end
end

function imD = readAllImgs(imgPath, ext, idx)

if(nargin < 2)
    imgDs = imageDatastore(imgPath);
else
    imgDs = imageDatastore(imgPath, 'FileExtensions', ext);
end

numImgs = length(imgDs.Files);

if(nargin < 3)
    idx = 1:numImgs;
end

% read all images as cell (very fast)
imD = imgDs.readall;

% keep only the idx
imD = imD(idx);

% convert to 4D mat
imD = cat(4, imD{:});
end


function SRhdplot(HD,VARopt)
% =======================================================================
% Plot the HD shocks computed with SR (sign restriction procedure)
% =======================================================================
% SRhdplot(HD,VARopt)
% -----------------------------------------------------------------------
% INPUT
%   - HD: structure from SR
%   - VARopt: options of the VAR (from VARmodel and SR)
% =======================================================================
% Ambrogio Cesa Bianchi, March 2015
% ambrogio.cesabianchi@gmail.com

%% Check inputs
%================================================
if ~exist('VARopt','var')
    error('You need to provide VAR options (VARopt from VARmodel)');
end
% If there is VARopt check that vnames and snames are not empty
vnames = VARopt.vnames;
snames = VARopt.snames;
if isempty(vnames)
    error('You need to add label for endogenous variables in VARopt');
end
if isempty(snames)
    error('You need to add label for shocks in VARopt');
end

%% Check inputs Define some parameters
%===============================================
filename = [VARopt.figname 'HD_SR_'];
quality = VARopt.quality;
suptitle = VARopt.suptitle;
pick = VARopt.pick;

% Initialize HD matrix
nshocks = length(snames); [nsteps, nvars, ~] = size(HD);

% If one shock is chosen, set the right value for nshocks
if pick<0 || pick>nvars
    error('The selected shock is non valid')
else
    if pick==0
        pick=1;
    else
        nshocks = pick;
    end
end

%% Plot
%================================================
FigSize
for ii=pick:nvars
    colormap(parula)
    BarPlot(HD(:,1:nshocks,ii));
    xlim([1 nsteps]);
    title([vnames{ii}], 'FontWeight','bold','FontSize',10);
    % Save
    FigName = [filename num2str(ii)];
    if quality
        if suptitle==1
            Alphabet = char('a'+(1:nvars)-1);
            SupTitle([Alphabet(jj) ') HD of '  vnames{ii}])
        end
        opt = LegOption; LegSubplot(vnames,opt);
        set(gcf, 'Color', 'w');
        export_fig(FigName,'-pdf','-png','-painters')
    else
        legend(snames)
        print('-dpng','-r100',FigName);
        print('-dpdf','-r100',FigName);
    end
    clf('reset');
end

close all


function meshCanopy(I,Mdata,Mmap,height)
%function meshCanopy: display a mesh above a grayscale image
%SCd 11/18/2010
%
%Updates:
%   -11/22/2010: Added example (requires IPT)
%                Added height input argument
%
%Input Arguments:
%   -I: 2-dimensional grayscale image slice.  The values are expected to
%       range from 0 to 255.  If the maximum value is greater than 255 or
%       the minimum value is less than 0, it will be scaled to 0:255.  Else
%       it will remain unchanged.
%   -Mdata: 2-dimensional mesh data (Z for a meshplot).
%       NOTE: the mesh command will be called with:
%       >>[ii jj] = meshgrid(ceil(cumsum(diff([0 linspace(1,size(I,2),size(Mdata,2))]))),ceil(cumsum(diff([0 linspace(1,size(I,1),size(Mdata,1))]))));
%       >>mesh(ii,jj,Mdata);
%       and thus does not need to be the same size as the image!
%   -Mmap: string, function_handle or nx3, mesh color map.  See:
%       >>doc colormap
%       for valid options.  The argument is optional and defaults to 'jet'
%       Examples: 'jet', @jet, [0 0 1; 0.5 0;.1 .1 .1]
%   -height: scalar height of the mesh above the image so you can see both.
%       Optional defaults to 80.
%
%Output Arguments:
%   -None!
%
%Example: (Requires the Image Processing Toolbox)
%   %Display a Mesh Canopy of a standard deviation image, above the original image
%   I = imread('cameraman.tif');
%   M = stdfilt(I);
%   meshCanopy(I,M,@spring)
%
%See also: mesh colormap
%

%Error Checking:
assert(nargin==2||nargin==3||nargin==4,'The number of input arguments is expected to be 2, 3 or 4.\n  It was %i',nargin);
assert(ndims(I)==2,'The first input argument, I, is required to be 2-dimensional');
assert(ndims(Mdata)==2,'The second input argument, Mdata, is required to be 2-dimensional');

%Assigning and checking the mesh colormap/height
if nargin == 2
    Cmap = [gray(256); jet(256)]; %Default
elseif ischar(Mmap)
    %String is used, assert it's right.
    valid_maps = {'jet','hsv','hot','cool','spring','summer','autumn','winter','gray','bone','copper','pink','lines'};
    assert(any(ismember(valid_maps,Mmap)),'If a string is used as a colormap, it is expected to match one of these:\n%s',sprintf('%s\n',valid_maps{:}));
    Mmap = str2func(Mmap);
    Cmap = [gray(256); Mmap(256)];
elseif isa(Mmap,'function_handle')
    %Function handle is user, assert it's right
    valid_maps = {'jet','hsv','hot','cool','spring','summer','autumn','winter','gray','bone','copper','pink','lines'};
    assert(any(ismember(valid_maps,func2str(Mmap))),'If a function_handle is used as a colormap, it is expected to match one of these:\n%s',sprintf('%s\n',valid_maps{:}));
    Cmap = [gray(256); Mmap(256)];
else
    %Explicit color map is used, make sure it's ok
    assert(size(Mmap,2)==3,'If a matrix colormap is used the second dimension must be 3');
    assert(all(Mmap(:)<=1&Mmap(:)>=0),'If a matrix colormap is used the values must all: 0 <= Mmap <= 1');
    Cmap = [gray(256); Mmap];
end
if ~exist('height','var');
    height = 80;
else
    assert(isscalar(height),'The fourth argument, height, is expected to be a scalar');
end

%Making required pieces
I = double(I); %Needs to be double for slice() and all other calculations
Mdata = double(Mdata);
[ii jj] = meshgrid(ceil(cumsum(diff([0 linspace(1,size(I,2),size(Mdata,2))]))),ceil(cumsum(diff([0 linspace(1,size(I,1),size(Mdata,1))]))));

Mdata = (Mdata - min(Mdata(:)))+height; %Scale so minimum is height (so it doesn't conflict with image)
Midx = ceil(min((length(Cmap)-256),round((length(Cmap)-255)*(Mdata-min(Mdata(:)))/(max(Mdata(:))-min(Mdata(:))))+1))+256;

if any(I(:)<0)||any(I(:)>255)
    %Scale whole image to 1:256 for the map (only if it was out of bounds before!
    I = ceil(min(256,round((255)*(I-min(I(:)))/(max(I(:))-min(I(:))))+1));
else
    %Else adjust to 1:256 integer increment
    I = ceil(I+1);
end

%Plotting7
figure;
H(1) = slice(repmat(I,[1 1 2]),[],[],1); %slice() requires at least 2x2x2
set(H(1),'EdgeColor','none') %required so image isn't just an edge
hold on
H(2) = mesh(ii,jj,double(Mdata));
hold off

%Plot Properties
axis vis3d
axis ij
axis tight
colormap(Cmap)
set(H(1),'CData',I);
set(H(2),'CData',Midx);
caxis([1 length(Cmap)])


% IMU information (todo: move this to the yaml file)
update_rate = 400;

% xsens mti-G-700 (page 32 and 33)
% https://www.xsens.com/download/usermanual/MTi_usermanual.pdf
gyroscope_noise_density = 0.01; %datasheet => deg/s/sqrt(Hz)
accelerometer_noise_density = 60; %datasheet => µg/sqrt(Hz)

% Convert to kalibr format
gyroscope_noise_density = pi/180*gyroscope_noise_density; %convert to => rad/s/sqrt(Hz)
accelerometer_noise_density = 9.81*accelerometer_noise_density/1000000; %convert to => m/s^2/sqrt(Hz)

%% Output results for yaml file
fprintf('\n\n=================================\n');
fprintf('rostopic: /imu0\n');
fprintf('update_rate: %.2f\n',update_rate);
fprintf('#Accelerometers\n');
fprintf('accelerometer_noise_density: %.5f.\n',accelerometer_noise_density);
fprintf('accelerometer_random_walk: %.5f.\n',0.0);
fprintf('#Gyroscopes\n');
fprintf('gyroscope_noise_density: %.6f.\n',gyroscope_noise_density);
fprintf('gyroscope_random_walk: %.6f.\n',0.0);
fprintf('=================================\n');


function [prediction rfParams] = rmPlotGUI_scaleFactor(M, pred, rfParams, recompFit);
%
%
%
%
% ras, 11/2008.
model     = M.model{M.modelNum};
modelName = rmGet(model, 'desc');
voxel     = get(M.ui.voxel.sliderHandle, 'Value');
coords	  = M.coords(:,voxel);
if isequal(M.viewType, 'Gray')  % convert coords into an index
    coords = M.coords(voxel);
end

%% get/make trends
[trends, ntrends, dcid] = rmMakeTrends(M.params, 0);
if isfield(M.params.analysis,'allnuisance')
    trends = [trends M.params.analysis.allnuisance];
end

%% Compute prediction; compute beta scale factors according to the model
% we also add this to the rfParams, to report later
switch modelName,
    case {'2D pRF fit (x,y,sigma, positive only)',...
            '2D RF (x,y,sigma) fit (positive only)'};
        if recompFit==0,
            beta = rmCoordsGet(M.viewType, model, 'b', coords);
            beta = beta([1 dcid+1]);
        else
            beta = pinv([pred trends(:,dcid)])*M.tSeries(:,voxel);
            beta(1) = max(beta(1),0);

            % also recompute variance explained
            prediction = [pred trends(:,dcid)] * beta;
            prediction = prediction(:,1);
        end

        prediction = [pred trends(:,dcid)] * beta;
        RFs        = RFs .* (beta(1) .* M.params.analysis.HrfMaxResponse);

        rfParams(4) = beta(1);

    case {'2D pRF fit (x,y,sigma_major,sigma_minor)' ...
			'oval 2D pRF fit (x,y,sigma_major,sigma_minor,theta)'};
        if recompFit==0,
            beta = rmCoordsGet(M.viewType, model, 'b', coords);
            beta = beta([1 dcid+1]);
        else
            beta = pinv([pred trends(:,dcid)])*M.tSeries(:,voxel);
            beta(1) = max(beta(1),0);

            % also recomput variance explained
            prediction = [pred trends(:,dcid)] * beta;
            prediction = prediction(:,1);
            rss = sum((M.tSeries(:,voxel)-prediction).^2);
            rawrss = sum(M.tSeries(:,voxel).^2);
            varexp = 1 - rss./rawrss;
        end

        prediction = [pred trends(:,dcid)] * beta;
        RFs        = RFs .* (beta(1) .* M.params.analysis.HrfMaxResponse);

        rfParams(4) = beta(1);

    case 'unsigned 2D pRF fit (x,y,sigma)';
        if recompFit==0,
            beta = rmCoordsGet(M.viewType, model, 'b', coords);
            beta = beta([1 dcid+1]);
        else
            beta = pinv([pred trends(:,dcid)])*M.tSeries(:,voxel);

            % also recomput variance explained
            prediction = [pred trends(:,dcid)] * beta;
            prediction = prediction(:,1);
        end

        prediction = [pred trends(:,dcid)] * beta;
        RFs        = RFs .* (beta(1) .* M.params.analysis.HrfMaxResponse);

        rfParams(4) = beta(1);

   case {'Double 2D pRF fit (x,y,sigma,sigma2, center=positive)'},
        if recompFit==0,
            beta = rmCoordsGet(M.viewType, model, 'b', coords);
            beta = beta([1 2 dcid+2]);
        else
            beta = pinv([pred trends(:,dcid)])*M.tSeries(:,voxel);
            beta(1) = max(beta(1),0);
            beta(2) = max(beta(2),-abs(beta(1)));

            % also recomput variance explained
            prediction = [pred trends(:,dcid)] * beta;
            prediction = prediction(:,1);
        end

        prediction = [pred trends(:,dcid)] * beta;
        RFs        = RFs * (beta(1:2).*M.params.analysis.HrfMaxResponse);

        rfParams(:,4) = beta(1);

    case {'Two independent 2D pRF fit (2*(x,y,sigma, positive only))'},
        if recompFit==0,
            beta = rmCoordsGet(M.viewType, model, 'b', coords);
            beta = beta([1 2 dcid+2]);
        else
            beta = pinv([pred trends(:,dcid)])*M.tSeries(:,voxel);
            beta(1:2) = max(beta(1:2),0);

            prediction = [pred trends(:,dcid)] * beta;
            prediction = prediction(:,1);
        end

        prediction = [pred trends(:,dcid)] * beta;
        RFs        = RFs * (beta(1:2) .* M.params.analysis.HrfMaxResponse);

        rfParams(:,4) = beta(1:2);
        rfParams = rfParams(1,:);

  case {'Sequential 2D pRF fit (2*(x,y,sigma, positive only))'},
        if recompFit==0,
            beta = rmCoordsGet(M.viewType, model, 'b', coords);
            beta = beta([1 2 dcid+2]);
        else
            beta = pinv([pred trends(:,dcid)])*M.tSeries(:,voxel);
            beta(1:2) = max(beta(1:2),0);
            % also recomput variance explained
            prediction = [pred trends(:,dcid)] * beta;
            prediction = prediction(:,1);
        end

        prediction = [pred trends(:,dcid)] * beta;
        RFs        = RFs * (beta(1:2) .* M.params.analysis.HrfMaxResponse);

        rfParams(:,4) = beta(1:2);
        rfParams = rfParams(1,:);

   otherwise,
        error('Unknown modelName: %s', modelName);
end;

return


% Extract the nearest exemplar from 3D pose library
% pool_2d: the 2D pose library for matching
% pool_3d: the 3D pose library
% pred: the prediction 2D pose
function [j_p] = NN_pose(pool_2d,pool_3d,pred)
    temp = zeros(1,28);
    temp_root = 0.5 * (pred(9,:) + pred(12,:));
    for a = 1:14
        temp(1,a*2-1) = pred(a,1) - temp_root(1);
        temp(1,a*2) = pred(a,2) - temp_root(2);
        y_c(a) = temp(1,2*a);
    end
    scale = max(y_c)-min(y_c);
    temp = temp / scale;

    [~,m_idx] = min(pdist2(temp, pool_2d));
    j_p = pool_3d(m_idx,:);
    j_p = reshape(j_p,3,14);
    j_p = double(j_p');
end


function [out_vec] = tgn_model(in_vec)
% tgn_model.m - TGn channel model simulation (load external coeff's)
global numPkts PER_snr snr_idx num_taps H H_idx_inc H_idx_ini H_idx H_cnt H_dline;

% PER graph settings (actual values are user-selected (GUI))
%%%%% numPkts = 200;
%%%%% PER_snr = 29:2:37;

% To select channel matrices
numCoherenceTimes = 10;                        % num of coherence times to test
numSampPerCohere = numPkts/numCoherenceTimes;  % max value = 442 (num of ch. samples in one coherence time)

% Input parameters
Clk = in_vec(1);
Ntx = in_vec(2);
Nrx = in_vec(3);

% Channel inputs
in_len = length(in_vec(4:end))/4;
ch_in  = zeros(4, in_len);
ch_in(1,:) = in_vec(0*in_len+3+(1:in_len)).';
ch_in(2,:) = in_vec(1*in_len+3+(1:in_len)).';
ch_in(3,:) = in_vec(2*in_len+3+(1:in_len)).';
ch_in(4,:) = in_vec(3*in_len+3+(1:in_len)).';
ch_in = ch_in(1:Ntx,:);  % keep only used Tx ant. inputs

% Channel outputs
ch_out = zeros(4, in_len);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Channel Model (external coeff's)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% for Channel D
chD_wdth   = [   10     10     10     10     10     10     10     10     10     20     30     30     30     40     50     50     50     50];

% define imag. component
j = sqrt(-1);

% Initialize channel model
if (Clk==0)
    % Initialize PER test
    snr_idx = 1;
    % Load Channel D coeff's
    load tgn_d_nlos;
    % Setup channel coeff's for PER test
    H_cohere_prd = ceil(size(H, 4)/100);  % ch. mat samples in one coherence time (100 simulated)
    H_idx_inc = round(H_cohere_prd / numSampPerCohere);
    % Setup indices for channel coeff's
    num_taps = size(H, 3);
    H_idx_ini = 100; %100; %2000;  % initial index for channel matrices
    H_idx = H_idx_ini;
    H_cnt = 1;

    % Initialize channel delay line
    tot_dly = sum(chD_wdth)/10;
    H_dline = zeros(Ntx, tot_dly);
end

% Use fixed channel for each packet
%     - assume 38400 samples for each packet (with silence added)
if 1  %(FIXED_CHANNEL)
 % Get channel coeff's
 H_tap(:,:,:) = H(:,:,:,H_idx);
 % Increment index after every packet...
 H_idx = H_idx + H_idx_inc;

 % Reuse channel coeff's for each SNR tested...
 if (mod(Clk,numPkts)==(numPkts-1))  H_idx = H_idx_ini;  end
end

% Process all input samples...
for i=1:in_len

    % Add new sample to delay line
    H_dline = [ch_in(:,i) H_dline(:,1:end-1)];

    % Apply channel filter
    H_dline_idx = 1;
    H_out = zeros(Nrx,1);
    for m=1:num_taps
if 1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Apply tap to inputs (considering tap width)
        H_samp = zeros(Ntx, 1);
        for k = 1:(chD_wdth(m)/10)
            % 10 ns (100 MHz) sampling
            H_samp = H_samp + H_dline(:,H_dline_idx);
            H_dline_idx = H_dline_idx + 1;
        end
        H_out = H_out + H_tap(:,:,m)*H_samp;
else %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Apply tap to inputs (considering tap width)
        for k = 1:(chD_wdth(m)/10)
  if 1 % 10 ns (100 MHz) sampling
            H_out = H_out + H_tap(:,:,m)*H_dline(:,H_dline_idx);
  else % 50 ns (20 MHz) sampling
            if mod(H_dline_idx,5)==1
                H_out = H_out + H_tap(:,:,m)*H_dline(:,1+floor(H_dline_idx/5));
            end
  end
            H_dline_idx = H_dline_idx + 1;
        end
end  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    end

    % Store channel output
    ch_out(1:Nrx,i) = H_out;
end

% Add AWGN to Rx streams (reduce SNR by 5 dB, due to upsampling)
snr_val = PER_snr(snr_idx);
for i=1:Nrx
    ch_out(i,:) = awgn(ch_out(i,:), snr_val-5, 'measured');
end

% Next SNR Value for PER Curve...
if (mod(Clk,numPkts)==(numPkts-1) && snr_idx<length(PER_snr))
    snr_idx = snr_idx+1;
end

% Return output
out_vec = reshape(ch_out.', 4*in_len, 1);


%% Analyzing Neural Time Series Data
% Matlab code for Chapter 4 script C
% Mike X Cohen
%
% This code accompanies the book, titled "Analyzing Neural Time Series Data"
% (MIT Press). Using the code without following the book may lead to confusion,
% incorrect data analyses, and misinterpretations of results.
% Mike X Cohen assumes no responsibility for inappropriate or incorrect use of this code.

%%

% The function 'clear' removes all data from the matlab buffer
clear

% you can also clear specific variables.
leave_me_alone   = 10;
remove_me_please = 20;
clear remove_me_please
whos

remove_me_please % gives an error because the variable doesn't exist anymore!

%% basic importing text data

% The most basic way to import data is to copy and paste. This is the best
% option for small amounts of numeric data that you'll need to put into matlab
% only once. (Hint: use square brackets.)

% The simplest way to read in text data is if all data in the next are
% numbers (no text). Open a text editor and make a small matrix (say, 3x4).
% Next, type:
data = load('chapter04_datafile.txt');

% slightly more advanced:
[file_name,file_path]=uigetfile('*.txt'); % ui = user-interface
data = load([ file_path file_name ]);

% you can also read in data from excel files, but BE CAREFUL because this
% function can act in unexpected ways, e.g., by removing empty columns and
% rows without warning (this can be seen in comparing "numberdata" to "raw_data").
% Therefore, it might be best to use the "raw" data output.
[numberdata,textdata,raw_data] = xlsread('chapter04_excel_data.xls');

%% advanced importing text data

% Here we borrow from C language to flexibly read in mixed data. Let's say
% you have some poorly organized behavioral data files to read in, but at
% least you know what text strings to look for:

fid = fopen('chapter04_headache_data.txt','r');
% fid is a pointer to a location on the physical hard disk (similar to how
% we used variables as handles to axes when plotting). The 'r' means read
% (later we'll use 'w' for write).

% In this particular example, we will extract the trial number, subject
% choice, reaction time (RT), and accuracy for each trial. Fields are separated by tabs.

behavioral_data=[]; % initialize... we can't initialize the full matrix, because we don't know how big this will be.

% The following code will remain inside a loop, reading in and processing new
% lines of data, until we reach the end of the file.
datarow=1;

while ~feof(fid) % feof tests whether we're at the end of the file.

    dataline = fgetl(fid); % read a line ("file get line")

    dataline = regexp(dataline,'\t','split');
    % regexp can be used to cut data according to delimiters. Here we will
    % cut this string of characters into a cell array in which elements of
    % the array are separated by tabs.

    % here we use strcmpi to compare strings. The "i" means to ignore case.
    if ~any(strcmpi('trial',dataline))
        continue % continue means to skip to the next iteration of the loop.
    end

    trial_column    = find(strcmpi('trial',   dataline));
    choice_column   = find(strcmpi('choice',  dataline));
    rt_column       = find(strcmpi('rt',      dataline));
    accuracy_column = find(strcmpi('accuracy',dataline));

    behavioral_data(datarow,1) = str2double(dataline{trial_column+1});      % Note that we didn't initialize the size of the variable "behavioral_data" so matlab gives a warning.
    behavioral_data(datarow,2) = str2double(dataline{choice_column+1});     % If the variable is relatively small, it doesn't matter.
    behavioral_data(datarow,3) = str2double(dataline{rt_column+1});         % If the variable is large, however, it's best to initialize it to something really big, and then cut it down to size afterwards.
    behavioral_data(datarow,4) = str2double(dataline{accuracy_column+1});   % See chapter 4 in the book for further discussion of matrix initializations.

    datarow=datarow+1; % increment row
end

fclose(fid); % don't forget to close the file after you finish it!

%% initializing variables

num_rows = 10;
num_cols = 35;

% initialize with zeros (typical approach)
largematrix = zeros(num_rows,num_cols);

for rowi=1:num_rows
    for coli=1:num_cols
        % processing here...
    end % end row-loop
end % end column-loop

% note that you can increase the size of a matrix without initializing it
largematrix(num_rows+1,1) = 10;

% similarly, you can increase the dimensionality of a matrix
largematrix(1,round(num_cols/2),3) = 100;

% these last two options (adding elements and dimensions to an existing
% matrix) should be avoided whenever possible. They can create confusion and errors.

% you can also decrease matrix sizes/dimensions, ether by re-assignment:
largematrix = largematrix(:,:,1);

% or by setting parts of the matrix to be empty:
size(largematrix)
largematrix(:,end-4:end) = []; % this removes the last 5 columns
size(largematrix)

% Again, changing matrix sizes and dimensions should be avoided when
% possible, and done carefully when necessary.

%% basic saving data

% save as a .mat file (only matlab can read these files):
save('my_matlab_variables.mat','data','amsterdam','x'); % Question: Why does matlab crash on this line?

% The function 'dlmwrite' is useful if you have a matrix of numbers
% and want to write a text file of only numbers:
dlmwrite('data_written_from_matlab.txt',data,'\t');
% the final argument is the delimieter. This can be tab (\t), space ( ), comma (,), the letter X (X), etc.

%% advanced saving data

fid = fopen('data_output_SPSS_format.txt','w');

% we want the first row to be variable labels, then rows of mixed string-number data

% variable labels
variable_labels = {'Name';'trial';'choice';'rt';'accuracy'};

% let's add subject names
subject_names={'billy';'bob'};

for vari=1:length(variable_labels)
    fprintf(fid,'%s\t',variable_labels{vari});
    % the %s is for string; %g is for number.
end

% insert a new-line character
fprintf(fid,'\n');

for datarowi=1:size(behavioral_data,1)

    % print subject name
    fprintf(fid,'%s\t',subject_names{datarowi});

    % now loop through columns (variables)
    for columni=1:size(behavioral_data,2)
        fprintf(fid,'%g\t',behavioral_data(datarowi,columni));
    end
    fprintf(fid,'\n'); % end-of-line

    % You could also do this in one line:
    % fprintf(fid,'%s\t%g\t%g\t%g\t%g\n',subject_names{datarowi},behavioral_data(datarowi,1),behavioral_data(datarowi,2),behavioral_data(datarowi,3),behavioral_data(datarowi,4));

    fprintf('Finished writing line %g of %g\n',datarowi,size(behavioral_data,1));
end

fclose(fid);

% Now you can easily import these data into SPSS or Excel.

% Note that if you omit the first argument to fprintf, it puts the output
% in the command instead of the text file, as in the final line of this for-loop.

%% end.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Max manipulability index ALONG A LINE.
% Use stomp like to optimize along a surface/line
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function experiment2B_K5_N30
close all;
global parameters

%STOMP PARAMETERS
%number of particles
K = 5;

%repeat experiment number of times
parameters.n_repeat = 3;
parameters.experiment_name = 'experiment2B_K5_N30_time.mat';
parameters.experiment_number = 2;
parameters.animate = 0;

%repeat the experiment E times
random_manips=[];
Gout = [];
ela = [];
for i=1:parameters.n_repeat
    close all
    tic;
    [pk, final_manip] = path_planning_SCO_4DOF(K);
    Gout{i}=pk;
    random_manips = [random_manips; final_manip];
    save(parameters.experiment_name)
    elapsed = toc;
    ela = [ela elapsed];
end
fprintf('TIC TOC mean: %g\n', mean(ela));
fprintf('TIC TOC cov: %g\n', cov(ela));
fprintf('TIC TOC 2sigma: %g\n', 2*sqrt(cov(ela)));

'ended'
parameters.experiment_name


function DEM_demo_fMRI_PEB
% Demonstration of PEB for multisession spectral DCM studies
%__________________________________________________________________________
% This demonstration routine illustrates the analysis of a multisession
% fMRI study using spectral DCM. Crucially, the between session effects are
% characterised using empirical Bayes and Bayesian model reduction. This
% means that the original session data are only inverted once (at the
% within session level). The resulting posterior estimates and then used to
% make inferences about between session effects (e.g., time or drug
% effects). The basic question addressed in this sort of analysis is where
% between session effects are expressed in terms of connectivity or
% parameters of neuronal fluctuations. These sorts of effects are specified
% in a second level design matrix in the usual way and can be identified
% using Bayesian model reduction.
%
% in this example, we analyse three sessions with a monotonic change in the
% intrinsic (self) connectivity over three sessions. This involves
% decreases in diagonal A parameters at the first two levels of a simple
% three node hierarchy - and an increase at the highest (third) level.
% Physiologically, this corresponds to a decrease in self-inhibition (or
% increase in excitability) in the lower notes for regions, as time goes
% on.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: DEM_demo_fMRI_PEB.m 7679 2019-10-24 15:54:07Z spm $

% Simulate timeseries
%==========================================================================
rng('default')

% assume we have data from eight subjects with a TR of two seconds
% -------------------------------------------------------------------------
T  = 256*8;                           % number of observations (scans)
TR = 2;                               % repetition time or timing
t  = (1:T)*TR;                        % observation times
n  = 3;                               % number of regions or nodes

% setup model
% -------------------------------------------------------------------------
options.nonlinear  = 0;
options.two_state  = 0;
options.stochastic = 0;
options.centre     = 1;
options.induced    = 1;

A   = ones(n,n);
B   = zeros(n,n,0);
C   = zeros(n,n);
D   = zeros(n,n,0);
pP  = spm_dcm_fmri_priors(A,B,C,D,options);

% true parameters - a simple hierarchy of three nodes
% -------------------------------------------------------------------------
pP.A = [  0  -.2    0;
         .2    0  -.1
          0   .3    0];
pP.C = eye(n,n);
pP.transit = randn(n,1)/16;

% spectral density of neuronal fluctuations and observation noise (eg)
% -------------------------------------------------------------------------
[Gu,Gn,Hz,dt] = spm_csd_fmri_gu(pP,TR);
Gu            = Gu(:,1,1)*ones(1,n);
Gn            = Gn(:,1,1)*ones(1,n);

% session specific parameters: here, intrinsic connectivity
% -------------------------------------------------------------------------
X    = exp(-(0:2))';                    % between session decay
B    = [  1    0    0;                  % of intrinsic connections
          0    1    0
          0    0   -1];
X    = X - mean(X);                     % detrended session-effects

% simulate response to endogenous fluctuations
%==========================================================================
for s = 1:numel(X)

    % parameters in this session
    % ---------------------------------------------------------------------
    P    = pP;
    P.A  = P.A + B*X(s);

    % integrate states with endogenous fluctuations (u)
    % ---------------------------------------------------------------------
    M.f  = 'spm_fx_fmri';
    M.x  = sparse(n,5);
    U.u  = spm_rand_power_law(Gu,Hz,dt,T);
    U.dt = TR;
    x    = spm_int_J(P,M,U);

    % haemodynamic observer
    % ---------------------------------------------------------------------
    for i = 1:T
        y(i,:) = spm_gx_fmri(spm_unvec(x(i,:),M.x),[],P)';
    end

    % response with observation noise (e)
    % ---------------------------------------------------------------------
    e       = spm_rand_power_law(Gn,Hz,dt,T);
    Y(s).y  = y + e;
    Y(s).dt = TR;
    PP(s)   = P;

end

% show simulated response (last session)
%--------------------------------------------------------------------------
i = 1:128;
spm_figure('Getwin','Figure 1'); clf
subplot(2,2,1)
plot(t(i),U.u(i,:))
title('Endogenous fluctuations','FontSize',16)
xlabel('Time (seconds)')
ylabel('Amplitude')
axis square

subplot(2,2,2), hold off
plot(t(i),x(i,n + 1:end),'c'), hold on
plot(t(i),x(i,1:n)), hold off
title('Hidden states','FontSize',16)
xlabel('Time (seconds)')
ylabel('Amplitude')
axis square

subplot(2,2,3)
plot(t(i),y(i,:),t(i),e(i,:),':')
title('Hemodynamic response and noise','FontSize',16)
xlabel('Time (seconds)')
ylabel('Amplitude')
axis square

% nonlinear system identification (DCM for CSD)
%==========================================================================
dcm.options = options;

dcm.a    = logical(pP.A);
dcm.b    = zeros(n,n,1);
dcm.c    = zeros(n,1);
dcm.d    = zeros(n,n,0);

% add responsse to each session
% -------------------------------------------------------------------------
for s = 1:numel(X)
    DCM{s,1}   = dcm;
    DCM{s,1}.Y = Y(s);
end

% first level inversion - spectral DCM
% =========================================================================
CSD = spm_dcm_peb_fit(DCM);

% show estimates for a single session
% -------------------------------------------------------------------------
spm_figure('Getwin','Figure 1');

j  = 1:n^2;
d  = 3;
subplot(2,2,4); hold off
spm_plot_ci(CSD{d}.Ep.A(:),CSD{d}.Cp(j,j)), hold on
bar(PP(d).A(:),1/4), hold off
title('True and MAP connections (Deterministic)','FontSize',16)
axis square

% Parametric empirical Bayes
%==========================================================================
% having inverted every session, we can construct a between session model
% at the second level. We are interested in identifying which parameters
% change according to the session specific effects encoded in the design
% matrix. This design matrix is supplemented with a constant term, creating
% two times the number of parameters at the second level. Using Bayesian
% model reduction, we can then examine models with and without each
% parameter for the  constant (first) and hypothesised (second) explanatory
% variables in the design matrix. Crucially, we want to explain as much  as
% possible using just between session differences. This can be implemented
% by setting the between session covariance of random effects to a
% relatively small value; here the original prior variance divided by 32.
%--------------------------------------------------------------------------
clear M;
beta  = 32;

M.X   = [X.^0 X];                  % between session explanatory variables
M.hE  = 0;                         % prior expectation of log precision
M.hC  = 1/16;                      % prior covariance of precision
M.bE  = CSD{1}.M.pE;               % prior expectations over sessions
M.bC  = CSD{1}.M.pC;               % prior covariance over sessions
M.pC  = CSD{1}.M.pC/beta;          % prior covariance between sessions

field = {'A','a'};                 % parameters of interest
PEB   = spm_dcm_peb(CSD,M,field);  % empirical Bayesian inversion
BMA   = spm_dcm_peb_bmc(PEB);      % Bayesian model reduction and averaging

% overlay true between session effects over BMA density over parameters
%--------------------------------------------------------------------------
j     = spm_find_pC(CSD{1},'A');
p     = spm_vec(B); p = p(j);
subplot(3,2,4), hold on, bar(p,1/4), hold off

return


function Flicker_Caclulation_function (endval,endval1,Sn,Sk,n,Un,Fn,i,Kp,Ki,F_Sample,Simulation_dynamic_cut_time,N10,N120,U1N_NS,U2N_NS,U3N_NS,I1_NS,I2_NS,I3_NS,time)
% Initial Input data for calculating a complete flicker valuses of a
% meassured data.
% endval1 is the number of calculation to be done (for different Grid phase
% angle
% Sn is Appparent power in kW
% Sk is Short circuit apparent power in MW
% n is % the rate between Sfic/Sn as defined by IEC norm
% Un is Nominal Voltage where the measurment were done in V.
% Fn is Nominal Frequency of the grid were the measurment were done in Hz.
% i is a selector between i = 1 Continuous operation and i = 2 and switching operation.
% Kp Proportional Gain Factor of the PLL Block.
% Ki Integral Gain Factor of the PLL Block.
% F_Sample is Sampling Frequencz used in recording the measurments in S.
% Simulation_dynamic_cut_time is the estimated time needed by the PI
% controller when calculating the Frequency. (this simulation dynamic time
% need to be removed)
% N10 is Number of switching operation with in 10 min (As given in IEC norm)
% N120 is Number of switching operation with in 2 h (As given in IEC norm)
% dataname is the measured data name that should be analyized and it should
% have the format .mat and need to contain the measured three phase
% voltages and current signals with there time axie.
% this file assume that the measured three pahse voltage and current are identical and have the same:
% 1- Sampling rate (time intervals)
% 2- data length (Which is according to the IEC 10 min of measured data)
% This function will also call the Flickermeter simulater (File ID: #24423
% by Patrik Jourdan 12 Jun 2009 (Updated 11 Jan 2010)) in order to
% calculate the flicker values calculated by the Fictitious grid module (so
% called Flickermeter6140021_new_GL_Final_3ph.mdl).
% Please refer to the PDF documentation file in order to get more
% information on how this function works
Sfic = (n*Sn)/1000;% Fictitious Short cicuit apparent power grid in MW
P_st_fic_complete_ave = 0;
P_st_complete_ave = 0;
P_lt_complete_ave = 0;
if i == 2
    P_st_fic_complete = zeros(1,endval);
    P_st_complete = zeros(1,endval);
    P_lt_complete = zeros(1,endval);
    kf_psyk_complete = zeros(1,endval);
    ku_psyk_complete = zeros(1,endval);
    kf_psyk_complete_ave = 0;
    ku_psyk_complete_ave = 0;
    save ('Initial values','time','U1N_NS','U2N_NS','U3N_NS','I1_NS','I2_NS','I3_NS','endval','endval1','Sn','Sk','n','Sfic','Un','Fn','i','Kp','Ki','F_Sample','Simulation_dynamic_cut_time','N10','N120','P_st_fic_complete','P_st_complete','P_lt_complete','kf_psyk_complete','ku_psyk_complete','P_st_fic_complete_ave','P_st_complete_ave','P_lt_complete_ave','kf_psyk_complete_ave','ku_psyk_complete_ave')
elseif i == 1
    P_st_fic_complete = zeros(1,endval);
    P_st_complete = zeros(1,endval);
    P_lt_complete = zeros(1,endval);
    c_psyk_complete = zeros(1,endval);
    c_psyk_complete_ave = 0;
    save ('Initial values','time','U1N_NS','U2N_NS','U3N_NS','I1_NS','I2_NS','I3_NS','endval','endval1','Sn','Sk','n','Sfic','Un','Fn','i','Kp','Ki','F_Sample','Simulation_dynamic_cut_time','N10','N120','P_st_fic_complete','P_st_complete','P_lt_complete','c_psyk_complete','P_st_fic_complete_ave','P_st_complete_ave','P_lt_complete_ave','c_psyk_complete_ave')
else
end
clear
load 'Initial values'
%-------------------------------------------------------------------------------------End--------------------------------------------------------------------------------------------
for jj=1:endval1
    prompt4 = {'Please Enter The new phase angle Alpha in degree'};
    name = 'Initial Input data';
    numlines = 1;
    defaultanswer4 = {'30'};
    answer4 = inputdlg(prompt4,name,numlines,defaultanswer4);
    Alpha = str2double(answer4{1});
    if jj>1 && i == 2
        save ('Initial values','time','U1N_NS','U2N_NS','U3N_NS','I1_NS','I2_NS','I3_NS','endval','endval1','Sn','Sk','n','Sfic','Un','Fn','i','Kp','Ki','F_Sample','Simulation_dynamic_cut_time','N10','N120','j','jj','Sequence_Path','Searchdir','P_st_fic_complete','P_st_complete','P_lt_complete','kf_psyk_complete','ku_psyk_complete','P_st_fic_complete_ave','P_st_complete_ave','P_lt_complete_ave','kf_psyk_complete_ave','ku_psyk_complete_ave')
        clear
        load 'Initial values'
    elseif jj>1 && i == 1
        save ('Initial values','time','U1N_NS','U2N_NS','U3N_NS','I1_NS','I2_NS','I3_NS','endval','endval1','Sn','Sk','n','Sfic','Un','Fn','i','Kp','Ki','F_Sample','Simulation_dynamic_cut_time','N10','N120','j','jj','Alpha','P_st_fic_complete','P_st_complete','P_lt_complete','c_psyk_complete','P_st_fic_complete_ave','P_st_complete_ave','P_lt_complete_ave','c_psyk_complete_ave')
        clear
        load 'Initial values'
    end
    for j=1:endval
        if i == 2
            save ('Initial values','time','U1N_NS','U2N_NS','U3N_NS','I1_NS','I2_NS','I3_NS','endval','endval1','Sn','Sk','n','Sfic','Un','Fn','i','Kp','Ki','F_Sample','Simulation_dynamic_cut_time','N10','N120','j','jj','Alpha','P_st_fic_complete','P_st_complete','P_lt_complete','kf_psyk_complete','ku_psyk_complete','P_st_fic_complete_ave','P_st_complete_ave','P_lt_complete_ave','kf_psyk_complete_ave','ku_psyk_complete_ave')
            I1_NS = [time' I1_NS'];
            I2_NS = [time' I2_NS'];
            I3_NS = [time' I3_NS'];
            U1_NS = [time' U1N_NS'];
            U2_NS = [time' U2N_NS'];
            U3_NS = [time' U3N_NS'];
            savenamerowdata = sprintf('Input_Data_Matlab_%d',j);
            save (savenamerowdata,'I1_NS','I2_NS','I3_NS','U1_NS','U2_NS','U3_NS','j','i');
            loadfile = [savenamerowdata];
            load ([loadfile '.mat'])
            % Input info for the Fictitious grid Module & Flickermeter
            % afterword and the name of the saved result file
            Tp = eval(sprintf('Simulation_Time_%d',j));
            Fs = 1/F_Sample; %in Hz & must be greater than 2000 Hz
            File_save_name = sprintf('Final_results_%d_%d',j,Alpha);
            filesavename = File_save_name;
            flicker_Calculation_complete (j,i,n,Sk,Sn,Alpha,Un,Kp,Ki,Fn,F_Sample,Tp,Simulation_dynamic_cut_time,N10,N120,File_save_name);
            loadfile_final = [File_save_name];
            load ([loadfile_final '.mat'])
            P_st_fic_complete(j) = P_st_fic_ave;
            P_st_complete(j) = Pst_ave;
            P_lt_complete(j) = Plt_ave;
            kf_psyk_complete(j) = kf_psyk_ave;
            ku_psyk_complete(j) = ku_psyk_max_ave;
            P_st_fic_complete_ave = mean(P_st_fic_complete);
            P_st_complete_ave = mean(P_st_complete);
            P_lt_complete_ave = mean(P_lt_complete);
            kf_psyk_complete_ave = mean(kf_psyk_complete);
            ku_psyk_complete_ave = mean(ku_psyk_complete);
            complet_results = sprintf('Complete_Results_%d',Alpha);
            save (complet_results,'P_st_fic_complete','P_st_complete','P_lt_complete','kf_psyk_complete','ku_psyk_complete','P_st_fic_complete_ave','P_st_complete_ave','P_lt_complete_ave','kf_psyk_complete_ave','ku_psyk_complete_ave');
            save ('Initial values','time','U1N_NS','U2N_NS','U3N_NS','I1_NS','I2_NS','I3_NS','endval','endval1','Sn','Sk','n','Sfic','Un','Fn','i','Kp','Ki','F_Sample','Simulation_dynamic_cut_time','N10','N120','j','jj','Alpha','P_st_fic_complete','P_st_complete','P_lt_complete','kf_psyk_complete','ku_psyk_complete','P_st_fic_complete_ave','P_st_complete_ave','P_lt_complete_ave','kf_psyk_complete_ave','ku_psyk_complete_ave')
            clear
            load 'Initial values'
        elseif i == 1
            save ('Initial values','time','U1N_NS','U2N_NS','U3N_NS','I1_NS','I2_NS','I3_NS','endval','endval1','Sn','Sk','n','Sfic','Un','Fn','i','Kp','Ki','F_Sample','Simulation_dynamic_cut_time','N10','N120','j','jj','Alpha','P_st_fic_complete','P_st_complete','P_lt_complete','c_psyk_complete','P_st_fic_complete_ave','P_st_complete_ave','P_lt_complete_ave','c_psyk_complete_ave')
            load('Initial values.mat')
            I1_Ns = [time' I1_NS'];
            I2_Ns = [time' I2_NS'];
            I3_Ns = [time' I3_NS'];
            U1_NS = [time' U1N_NS'];
            U2_NS = [time' U2N_NS'];
            U3_NS = [time' U3N_NS'];
            Tp = time(end);
            %Fs = 1/F_Sample;
            %Simulation_Time = Cut_Time;
            savenamerowdata = sprintf('Input_Data_Matlab_%d',j);
            save (savenamerowdata,'I1_Ns','I2_Ns','I3_Ns','U1_NS','U2_NS','U3_NS','j','i','Tp');
            loadfile = [savenamerowdata];
            load ([loadfile '.mat'])
            % Input info for the Fictitious grid Module & Flickermeter
            % afterword and the name of the saved result file
            File_save_name = sprintf('Final_results_%d_%d',j,Alpha);
            flicker_Calculation_complete (j,i,n,Sk,Sn,Alpha,Un,Kp,Ki,Fn,F_Sample,Tp,Simulation_dynamic_cut_time,N10,N120,File_save_name,I1_Ns,I2_Ns,I3_Ns,U1_NS,U2_NS,U3_NS);
            loadfile_final = [File_save_name];
            load ([loadfile_final '.mat'])
            P_st_fic_complete(j) = P_st_fic_ave;
            P_st_complete(j) = Pst_ave;
            P_lt_complete(j) = Plt_ave;
            c_psyk_complete(j) =  cpsyk_ave;
            P_st_fic_complete_ave = mean(P_st_fic_complete);
            P_st_complete_ave = mean(P_st_complete);
            P_lt_complete_ave = mean(P_lt_complete);
            c_psyk_complete_ave = mean(c_psyk_complete);
            complet_results = sprintf('Complete_Results_%d',Alpha);
            save (complet_results,'P_st_fic_complete','P_st_complete','P_lt_complete','c_psyk_complete','P_st_fic_complete_ave','P_st_complete_ave','P_lt_complete_ave','c_psyk_complete_ave');
            save ('Initial values','time','endval','U1N_NS','U2N_NS','U3N_NS','I1_NS','I2_NS','I3_NS','endval1','Sn','Sk','n','Sfic','Un','Fn','i','Kp','Ki','F_Sample','Simulation_dynamic_cut_time','N10','N120','j','jj','Alpha','P_st_fic_complete','P_st_complete','P_lt_complete','c_psyk_complete','P_st_fic_complete_ave','P_st_complete_ave','P_lt_complete_ave','c_psyk_complete_ave')
            clear
            load 'Initial values'
        else
        end
        button = questdlg('The next Row Data calculation is about to start do you want to finish running this M-file?', ...
            'Exit Dialog','Yes','No','No');
        switch button
            case 'Yes',
                disp('Exiting MATLAB');
                %Save variables to matlab.mat
                save
            case 'No',
                quit cancel;
        end
    end
    button = questdlg('The next calculation for a new phase angle of the fictious grid Alpha is about to start do you want to finish running this M-file?', ...
        'Exit Dialog','Yes','No','No');
    switch button
        case 'Yes',
            disp('Exiting MATLAB');
            %Save variables to matlab.mat
            save
        case 'No',
            quit cancel;
    end
end


function [l, L_rf, L_sf, L_obs, L_n, N] = retroProjLmk(Rob,Sen,Obs,Opt)

% RETROPROJLMK  Retro project landmark.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

switch Sen.type

    % camera pinHole
    case {'pinHole'}
        % type of lmk to init
        switch Opt.init.initType
            case {'idpPnt'}
                % INIT LMK OF TYPE: Inverse depth point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjIdpPntFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;

            case {'hmgPnt'}
                % INIT LMK OF TYPE: Homogeneous point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjHmgPntFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;

            case {'ahmPnt'}
                % INIT LMK OF TYPE: Anchored Homogeneous point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjAhmPntFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;

            case {'fhmPnt'}
                % INIT LMK OF TYPE: Framed Homogeneous point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjFhmPntFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;

            case {'plkLin'}
                % INIT LMK OF TYPE: Plucker line
                [hm, HM_obs] = seg2hmgLin(Obs.meas.y);
                [l, L_rf, L_sf, L_k, L_hm, L_n] = ...
                    retroProjPlkLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    hm, ...
                    Opt.init.plkLin.nonObsMean) ;
                L_obs = L_hm*HM_obs;

                N = diag(Opt.init.plkLin.nonObsStd.^2) ;

            case 'aplLin'
                % INIT LMK OF TYPE: Anchored Plucker line
                [hm, HM_obs] = seg2hmgLin(Obs.meas.y);
                [l, L_rf, L_sf, L_k, L_hm, L_n] = ...
                    retroProjAplLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    hm, ...
                    Opt.init.plkLin.nonObsMean) ;
                L_obs = L_hm*HM_obs;

                N = diag(Opt.init.plkLin.nonObsStd.^2) ;

            case 'idpLin'
                % INIT LMK TYPE: Inverse-depth line.
                nMean = Opt.init.idpPnt.nonObsMean*[1;1]; % non-measured prior
                nStd  = Opt.init.idpPnt.nonObsStd*[1;1];

                [l, L_rf, L_sf, L_k, L_obs, L_n] = ...
                    retroProjIdpLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Obs.meas.y, ...
                    nMean) ;

                N = diag(nStd.^2);

            case 'hmgLin'
                % INIT LMK TYPE: Homogenous points line.
                nMean = Opt.init.idpPnt.nonObsMean*[1;1]; % non-measured prior
                nStd  = Opt.init.idpPnt.nonObsStd*[1;1];

                [l, L_rf, L_sf, L_k, L_obs, L_n] = ...
                    retroProjHmgLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Obs.meas.y, ...
                    nMean) ;

                N = diag(nStd.^2);

            case 'ahmLin'
                % INIT LMK TYPE: Anchored Homogenous points line.
                nMean = Opt.init.idpPnt.nonObsMean*[1;1]; % non-measured prior
                nStd  = Opt.init.idpPnt.nonObsStd*[1;1];

                [l, L_rf, L_sf, L_k, L_obs, L_n] = ...
                    retroProjAhmLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Obs.meas.y, ...
                    nMean) ;

                N = diag(nStd.^2);

            otherwise
                error('??? Unknown landmark type ''%s'' for initialization.',Opt.init.initType)
        end

        % Pin hole camera with depth information
    case 'pinHoleDepth'

        switch Opt.init.initType
            case 'eucPnt'
                % INIT LMK OF TYPE: Euclidean point
                [l, L_rf, L_sf, L_k, L_c, L_obs] = ...
                    retroProjEucPntFromPhdOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y) ;

                L_n = zeros(3,0);
                N   = [] ;

            otherwise
                error('??? Unknown landmark type ''%s'' for initialization.',Opt.init.initType)
        end

    % Omnidirectional camera
    case {'omniCam'}
        % type of lmk to init
        switch Opt.init.initType
            case {'ahmPnt'}
                % INIT LMK OF TYPE: Anchored Homogeneous point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjAhmPntFromOmniCamOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;
            otherwise
                error('??? Unknown landmark type ''%s'' for initialization.',Opt.init.initType)
        end

    otherwise % -- Sen.type
        % Print an error and exit
        error('??? Unknown sensor type ''%s''.',Sen.type);
end % -- Sen.type

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function value = year_length_months_julian ( y )

%*****************************************************************************80
%
%% YEAR_LENGTH_MONTHS_JULIAN returns the number of months in a Julian year.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year to be checked.
%
%    Output, integer VALUE, the number of
%    months in the year.
%
  value = 12;

  return
end


function Y = acsch(X)
    % Symbolic inverse hyperbolic cosecant.

    % Convert inputs to SymExpression
    % X = SymExpression(X);

    % construct the operation string
    sstr = ['ArcCsch[' X.s ']'];

    % create a new object with the evaluated string
    Y = SymExpression(sstr);
end


function [trl] = read_fcdc_trl(fn);

% READ_FCDC_TRL reads trial definitions from a file
%
% Given a file which defines N trials, this function returns a Nx3
% matrix with the begin latency, end latency, and the latency offset
% of the first sample of each trial. The latencies are in seconds.
%
% [trl] = read_fcdc_trl(filename)
%
% An FCD trial definition file is formatted like
%   begin   end     offset
%   0.0000  1.0000  0.0000
%   3.0000  4.0000  0.0000
%   5.0000  5.5000  0.0000
%   ...
%
% The trial begin and end are given in seconds relative to the start
% of the recorded datafile. The offset is given in seconds and indicates
% the latency of the first sample, relative to the trial marker or
% trigger. E.g., given a trigger at 7000ms (relative to the recording
% begin), a trial of 1000ms with a pretrigger interval of 300ms would
% correspond to "6.700 7.700 -0.300".

% Copyright (C) 2003, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.ru.nl/neuroimaging/fieldtrip
% 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: read_fcdc_trl.m 945 2010-04-21 17:41:20Z roboos $

fid = fopen(fn, 'rt');
if fid<0
   error('could not open file');
end

trl = [];
while ~feof(fid)
  tmp = fscanf(fid, '%f %f %f', 3);
  if ~isempty(tmp)
    trl = [trl; tmp'];
  end
end

fclose(fid);


function rr = PZX(rel_data, wins, up)
%PZX estimates the RR using a combination of zero-crossing detection, and
% peak detection.
%	            PZX(option, up)
%
%	Inputs:
%       rel_data    .t  vector of times
%                   .v  vector of resp Sig values
%                   .fs sampling freq
%       up              universal parameters structure
%       wins        .t  vector of start times
%
%	Outputs:
%       rr          .t  vector of times of estimated RRs
%                   .v  vector of estimated RRs
%

%% Cycle through windows
rr.t = mean([wins.t_start(:)' ; wins.t_end(:)']); rr.t = rr.t(:);
rr.v = nan(length(rr.t),1);

for win_no = 1 : length(wins.t_start)

    % extract relevant data
    rel_els = find(rel_data.t >= wins.t_start(win_no) & rel_data.t < wins.t_end(win_no));
    data.v = rel_data.v(rel_els);
    data.t = rel_data.t(rel_els);

    good_els = ~isnan(data.v);
    data.v = data.v(good_els); data.v = data.v(:);
    data.t = data.t(good_els); data.t = data.t(:);

    %% identify individual breaths from the raw signal using 3-pt peak detection:
    diffs_on_left_of_pt = diff(data.v); diffs_on_left_of_pt = diffs_on_left_of_pt(1:(end-1)); diffs_on_left_of_pt = logical(diffs_on_left_of_pt>0);
    diffs_on_right_of_pt = diff(data.v); diffs_on_right_of_pt = diffs_on_right_of_pt(2:end); diffs_on_right_of_pt = logical(diffs_on_right_of_pt<0);
    peaks = find(diffs_on_left_of_pt & diffs_on_right_of_pt)+1;

    %% identify individual breaths from the raw signal using 3-pt trough detection:
    diffs_on_left_of_pt = diff(-1*data.v); diffs_on_left_of_pt = diffs_on_left_of_pt(1:(end-1)); diffs_on_left_of_pt = logical(diffs_on_left_of_pt>0);
    diffs_on_right_of_pt = diff(-1*data.v); diffs_on_right_of_pt = diffs_on_right_of_pt(2:end); diffs_on_right_of_pt = logical(diffs_on_right_of_pt<0);
    troughs = find(diffs_on_left_of_pt & diffs_on_right_of_pt)+1;

    %% eliminate peaks or troughs which aren't in their half (above or below mean)
    mean_val = mean(data.v);
    temp_log = data.v(peaks) > mean_val;
    peaks = peaks(temp_log);
    temp_log = data.v(troughs) < mean_val;
    troughs = troughs(temp_log);

    %% Eliminate peaks or troughs which occur within 0.5s of the previous peak or trough
    thresh = 0.5;
    % peaks
    diffs = diff(data.t(peaks));
    rel_diffs = find(diffs > thresh); rel_diffs = rel_diffs(:);
    if isempty(rel_diffs)
        rr.v(win_no) = nan;
        continue
    end
    rel_diffs = [1; rel_diffs+1];
    peaks = peaks(rel_diffs);
    % troughs
    diffs = diff(data.t(troughs));
    rel_diffs = find(diffs > thresh); rel_diffs = rel_diffs(:);
    if isempty(rel_diffs)
        rr.v(win_no) = nan;
        continue
    end
    rel_diffs = [1; rel_diffs+1];
    troughs = troughs(rel_diffs);

    %% Eliminate peaks or troughs which aren't followed by their opposite
    comp.t = [peaks; troughs];
    comp.log = [ones(length(peaks),1); zeros(length(troughs),1)];
    [comp.t, orders] = sort(comp.t);
    comp.log = comp.log(orders);
    diffs = diff(comp.log); diffs = [1; diffs];
    comp.t = comp.t(diffs ~= 0);
    comp.log = comp.log(diffs ~= 0);
    peaks = comp.t(comp.log==1);
    troughs = comp.t(comp.log==0);

    %% Calc RR
    breaths = peaks;
    % Only using time period spanning a whole number of breaths
    win_length = data.t(breaths(end)) - data.t(breaths(1));
    rr.v(win_no) = 60*(length(breaths)-1)/win_length;

end

end


classdef CutMeshProvisionalQuadrilater < CutMesh

    properties (Access = private)
       connec
       coord

       cutMesh
       subMesh
       subCutSubMesh

       subMesher

       fullSubCells
       cutSubCells
       levelSetSubMesh
    end

    properties (Access = private)
        lastNode
    end

    methods (Access = public)

        function obj = CutMeshProvisionalQuadrilater(cParams)
            obj.init(cParams);
            obj.lastNode = cParams.lastNode;
        end

        function compute(obj)
            obj.createSubMesher();
            obj.createSubMesh();
            obj.computeLevelSetInSubMesh();
            obj.classifyCells();
            obj.computeSubCutSubMesh();
            obj.computeXcoord();
            obj.computeCoord();
            obj.computeConnec();
            obj.computeCellContainingSubCell();
            obj.computeMesh();
            obj.computeBoundaryMesh();
            obj.computeBoundaryXCoordsIso();
            obj.computeBoundaryCellContainingSubCell();
            obj.computeInnerCutMesh();
            obj.computeBoundaryCutMesh();
        end

    end

    methods (Access = private)

        function createSubMesher(obj)
            s.mesh        = obj.backgroundMesh;
            s.lastNode    = obj.lastNode;
            obj.subMesher = SubMesher(s);
        end

        function createSubMesh(obj)
            obj.subMesh = obj.subMesher.subMesh;
        end

        function computeLevelSetInSubMesh(obj)
            ls = obj.levelSet;
            s.mesh   = obj.backgroundMesh;
            s.fValues = ls;
            f = P1Function(s);
            q = Quadrature.set(obj.backgroundMesh.type);
            q.computeQuadrature('CONSTANT');
            xV = q.posgp;
            lsSubMesh = squeeze(f.evaluate(xV));
            obj.levelSetSubMesh = [ls;lsSubMesh];
        end

       function classifyCells(obj)
            lsInElem = obj.computeLevelSetInElem();
            isFull  = all(lsInElem<0,2);
            isEmpty = all(lsInElem>0,2);
            isCut = ~isFull & ~isEmpty;
            obj.fullSubCells  = find(isFull);
            obj.cutSubCells   = find(isCut);
       end

        function lsElem = computeLevelSetInElem(obj)
            ls = obj.levelSetSubMesh;
            nodes = obj.subMesh.connec;
            nnode = size(nodes,2);
            nElem = size(nodes,1);
            lsElem = zeros(nElem,nnode);
            for inode = 1:nnode
                node = nodes(:,inode);
                lsElem(:,inode) = ls(node);
            end
        end

        function computeSubCutSubMesh(obj)
            s.backgroundMesh = obj.subMesh;
            s.cutCells       = obj.cutSubCells;
            s.levelSet       = obj.levelSetSubMesh;
            cMesh = CutMesh.create(s);
            cMesh.compute();
            obj.subCutSubMesh = cMesh;
        end

        function  computeCellContainingSubCell(obj)
            cellSubMesh = obj.subCutSubMesh.innerCutMesh.cellContainingSubcell;
            fCells      = obj.fullSubCells;
            cellSubMesh = [fCells;cellSubMesh];

            cell = obj.computeSubTriangleOfSubCell();
            obj.cellContainingSubcell = cell(cellSubMesh);
        end

        function cell = computeSubTriangleOfSubCell(obj)
            nnode  = size(obj.backgroundMesh.connec,2);
            cElems = transpose(obj.cutCells);
            cell = repmat(cElems,nnode,1);
            cell = cell(:);
        end

        function globalToLocal = computeGlobalToLocal(obj)
            bConnec = obj.backgroundMesh.connec;
            nnode   = size(bConnec,2);
            nElem   = size(bConnec,1);
            cell = repmat((1:nnode)',1,nElem);
            globalToLocal = cell(:);
        end

        function computeXcoord(obj)
            s.fullCells     = obj.fullSubCells;
            s.cutCells      = obj.subCutSubMesh.innerCutMesh.cellContainingSubcell;
            s.globalToLocal = obj.computeGlobalToLocal();
            s.localMesh     = obj.subMesher.localMesh;
            s.xIsoCutCoord  = obj.subCutSubMesh.innerCutMesh.xCoordsIso;
            xC = XcoordIsoComputer(s);
            obj.xCoordsIso = xC.compute();
        end

        function computeConnec(obj)
            connecCutInterior = obj.subCutSubMesh.innerCutMesh.mesh.connec;
            connecFull        = obj.subMesh.connec(obj.fullSubCells,:);
            obj.connec = [connecFull;connecCutInterior];
        end

        function computeCoord(obj)
            obj.coord  = obj.subCutSubMesh.innerCutMesh.mesh.coord;
        end

        function computeMesh(obj)
            sM.connec = obj.connec;
            sM.coord  = obj.coord;
            sM.kFace  = obj.backgroundMesh.kFace;
            obj.mesh = Mesh(sM);
        end

       function computeBoundaryMesh(obj)
            m = obj.subCutSubMesh.boundaryCutMesh.mesh;
            obj.boundaryMesh = m;
        end

        function computeBoundaryXCoordsIso(obj)
            xCutIso = obj.subCutSubMesh.xCoordsIsoBoundary;
            s.fullCells     = obj.fullSubCells;
            s.cutCells      = obj.cutSubCells;
            s.globalToLocal = obj.computeGlobalToLocal();
            s.localMesh     = obj.subMesher.localMesh;
            s.xIsoCutCoord  = xCutIso;
            xC = XcoordIsoComputer(s);
            xCutG = xC.computeXSubCut();
            obj.xCoordsIsoBoundary = xCutG;
        end

        function cellCont = computeBoundaryCellContainingSubCell(obj)
            cutC = obj.cutSubCells;
            cell = obj.computeSubTriangleOfSubCell();
            cellCont = cell(cutC);
            obj.cellContainingSubCellBoundary = cellCont;
        end

    end

end


function tests = test_ft_specest_hilbert

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_specest_hilbert

if nargout
  % assume that this is called by RUNTESTS
  tests = functiontests(localfunctions);
else
  % assume that this is called from the command line
  fn = localfunctions;
  for i=1:numel(fn)
    feval(fn{i});
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function testOptions(testCase)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fsample = 1000;
nchan   = 8;
nsample = 1000;

dat     = randn(nchan, nsample) + 1 + linspace(0,1,nsample);
time    = (0:(nsample-1))/fsample;
verbose = false;
freqoi  = (10:10:100)';
width   = 5;

result = {};

% first explore the different filter types and orders
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but',   'bpfiltord', 2,   'bpfiltdir', 'twopass', 'timeoi', time);
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but',   'bpfiltord', 2,   'bpfiltdir', 'twopass', 'timeoi', time(1:2:end));
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but',   'bpfiltord', 3,   'bpfiltdir', 'twopass', 'timeoi', time(1:2:end));

% bpfiltdir=twopass is the default, use a different filter order for the following ones
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but', 'bpfiltord', 1, 'bpfiltdir', 'onepass');
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but', 'bpfiltord', 1, 'bpfiltdir', 'onepass-reverse');
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but', 'bpfiltord', 1, 'bpfiltdir', 'twopass');
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but', 'bpfiltord', 1, 'bpfiltdir', 'twopass-reverse');
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but', 'bpfiltord', 1, 'bpfiltdir', 'twopass-average');

% padtype=zero is the default, use a different filter order for the following ones
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but', 'bpfiltord', 2, 'bpfiltdir', 'twopass', 'pad', 2, 'padtype', 'zero'      );
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but', 'bpfiltord', 2, 'bpfiltdir', 'twopass', 'pad', 2, 'padtype', 'mean'      );
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but', 'bpfiltord', 2, 'bpfiltdir', 'twopass', 'pad', 2, 'padtype', 'localmean' );
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but', 'bpfiltord', 2, 'bpfiltdir', 'twopass', 'pad', 2, 'padtype', 'edge'      );
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but', 'bpfiltord', 2, 'bpfiltdir', 'twopass', 'pad', 2, 'padtype', 'mirror'    );

% polyorder=0 is the default, use a different filter order for the following ones
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but', 'bpfiltord', 3, 'bpfiltdir', 'twopass', 'polyorder', 0);
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but', 'bpfiltord', 3, 'bpfiltdir', 'twopass', 'polyorder', 1);
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but', 'bpfiltord', 3, 'bpfiltdir', 'twopass', 'polyorder', 2);
result{end+1} = ft_specest_hilbert(dat, time, 'verbose', verbose, 'freqoi', freqoi, 'width', width, 'bpfilttype', 'but', 'bpfiltord', 3, 'bpfiltdir', 'twopass', 'polyorder', 3);

% all iterations were done with (slightly) different options, hence the results should not be equal
for i=1:numel(result)
  for j=(i+1):numel(result)
    assert(~isequal(result{i}, result{j}), 'the results %d and %d should not be equal', i, j);
  end
end


function[varargout]=orbitbreaks(varargin)
%ORBITBREAKS  Separate orbit into passes based on turning points.
%
%   ORBITBREAKS is a low-level function called by ABOUT_TPJAOS.
%
%   [LAT,LON]=ORBITBREAKS(LAT,LON), where LAT and LON are column vectors
%   representing a satellite orbital groundtrack, is used to separate
%   different passes based on the turning points in latitude.
%
%   NANs are inserted into both LAT and LON wherever LAT has a turning
%   point.  Note that no data is removed.
%
%   All latitudes and lontidues are in degrees.  The output longitude has
%   range [-180, 180].
%
%   Calling COL2MAT(LAT,LON) will then reshape LAT and LON into matrices,
%   with each orbital pass in its own column.
%
%   [LAT,LON,X1,X2,... XN]=ORBITBREAKS(LAT,LON,X1,X2,... XN) will insert
%   NANs into the other arguments in the same locations as for LAT and LON.
%
%   The input arguments should contain no NANs.  For missing data, use INFs
%   instead.
%
%   ORBITBREAKS(LAT,LON, ...) with no output arguments overwrites the
%   original variables.
%
%   See also COLBREAKS, COL2MAT.
%
%   'orbitbreak --t' runs a test.
%
%   Usage: [lat,lon]=orbitbreaks(lat,lon);
%          [lat,lon,x1,x2,x3]=orbitbreaks(lat,lon,x1,x2,x3);
%          orbitbreaks(lat,lon);
%          orbitbreaks(lat,lon,x1,x2,x3);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2006--2015 J.M. Lilly --- type 'help jlab_license' for details

%This functionality is removed:
%Also, INFs are inserted into LON whenever LON jumps by more
%than 180 degrees, to be able to PLOT(LON,LAT) without spurious lines.

if strcmpi(varargin{1}, '--t')
    orbitbreaks_test,turningpoint_test,return
end

if nargin<2
    error('At least LAT and LON must be given.')
end

for i=1:nargin
    if ~iscolumn(varargin{i})
        error('All input arguments must be column vectors.')
    end
end

x=varargin{1};
index=find(isfinite(x));
if ~isempty(index)
    if maxmax(abs(x(index)))>90
         error('Looks like you have LAT and LON arguments swapped.')
    end
end
mat=zeros(size(varargin{1},1),nargin);

for i=1:nargin
   mat(:,i+1)=varargin{i};
end
if anyany(isnan(mat))
    error('Input arguments should contain no NaNs --- use INFs for missing data.')
end

mat=orbitbreaks_turningpoints(mat);
%mat=orbitbreaks_lonjumps(mat);

for i=1:nargin
   varargout{i}=mat(:,i+1);
end

if nargout==0
   eval(to_overwrite(nargin));
end

function[mat]=orbitbreaks_turningpoints(mat)
lat=vswap(mat(:,2),inf,nan);
bool=turningpoint(lat);
bool([1 end],1)=0;
bool=vshift(bool,-1,1);

mat(:,1)=cumsum(bool);
mat=colbreaks(mat);

function[mat]=orbitbreaks_lonjumps(mat)

mat=vswap(mat,nan,-inf);
mat(:,3)=deg180(mat(:,3));

lon=vswap(mat(:,3),-inf,nan);
bool=abs(lon-vshift(lon,-1,1))>90;
mat(:,1)=cumsum(bool);
mat=colbreaks(mat);
vswap(mat,nan,inf);
vswap(mat,-inf,nan);

%Remove last row
mat=mat(1:end-1,:);
function[]=orbitbreaks_test

tol=1e-6;
x1=[ 4 5 6 7     6 5     6    ]';
y1=[ 4 5 6 7 nan 6 5 nan 6 nan]';

[y1a,y1b]=orbitbreaks(x1,x1);

reporttest('ORBITBREAKS simple',aresame([y1 y1],[y1a y1b],tol))

x2=[ 1 1 6 -10     2 2     1    ]';
y2=[ 1 1 6 -10 nan 2 2 nan 1 nan]';
x3=x1;
y3=y1;
[y1o,y2o,y3o]=orbitbreaks(x1,x2,x3);

reporttest('ORBITBREAKS multiple',aresame([y1o y2o y3o],[y1 y2 y3],tol))

orbitbreaks(x1,x2,x3);

reporttest('ORBITBREAKS multiple overwriting',aresame([x1 x2 x3],[y1 y2 y3],tol))

% x1=[ 4     5    6          7      8    9       10   11  12]';
% y1=[ 4     5    6    inf   7      8    9  inf  10   11  12 nan]';
% x2=[ 100  110  120        -170  -160 -150     100  110 120  ]';
% y2=[ 100  110  120   inf  -170  -160 -150 inf 100  110 120 nan]';
% x3=x1;
% y3=y1;
% [y1o,y2o,y3o]=orbitbreaks(x1,x2,x3);
%
% reporttest('ORBITBREAKS multiple with lon breaks',aresame([y1o y2o y3o],[y1 y2 y3],tol))

function[bool]=turningpoint(x)
%TURNINGPOINT  True for turning points, i.e. local extrema, along rows.
%
%   BOOL=TURNINGPOINT(X) where X is a column vector returns BOOL, a
%   vector of the same size as X containing zeros and ones.  BOOL
%   is equal to one if the corresponding element of X is a local
%   maximum or minimum, and zero otherwise.
%
%   X may also be an array of any dimensionality, in which case
%   local maxima and minima are found with respect to differentiation
%   along the first dimension, i.e. along rows.
%
%   Note that if any two adjacent points are identical, TURNINGPOINT
%   adds a very small amount of numerical noise to one of the points
%   in order to make a choise about which is larger.
%
%   See also CROSSINGS.
%
%   'turningpoint --t' runs a test.
%
%   Usage: bool=turningpoint(x);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2006--2015 J.M. Lilly --- type 'help jlab_license' for details

%if strcmpi(x, '--t')
%    turningpoint_test,return
%end

%Add very small amout of noise to keep identical points from occurring
index=find(x==vshift(x,-1,1));
if ~isempty(index)
    x(index)=x(index)+randn(size(x(index)))*1e-10;
end

boolmin=~(x>vshift(x,1,1)|x>vshift(x,-1,1));
boolmax=x>vshift(x,1,1)&x>vshift(x,-1,1);
bool=boolmin|boolmax;
bool(1,:)=0;
bool(end,:)=0;

function[]=turningpoint_test

x=[ 4 5 6 7 6 5 6]';
b=[ 0 0 0 1 0 1 0]';

reporttest('TURNINGPOINT',aresame(b,turningpoint(x)))

x= [ 4 5 6 7 7 6 5 6]';
b1=[ 0 0 0 1 0 0 1 0]';
b2=[ 0 0 0 0 1 0 1 0]';

y1=turningpoint(x);
reporttest('TURNINGPOINT with repeated entry',aresame(y1,b1) || aresame(y1,b2))


function [ x, y ] = p09_dat ( data_num )

%*****************************************************************************80
%
%% P09_DAT returns the data vector for problem 9.
%
%  Discussion:
%
%    This example is due to Max Waldmeier.
%
%    This data represents a measure of sunspot activity over the
%    years 1700 to 1960.  The X value is the year, and the Y value
%    is a measure of the sunspot activity, which is usually, but
%    not always, an integer.
%
%    The X data is equally spaced.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Max Waldmeier,
%    The Sunspot-Activity in the Years 1610-1960,
%    Shulthess, Zurich, 1961.
%
%  Parameters:
%
%    Input, integer DATA_NUM, the number of data points.
%
%    Output, real X(DATA_NUM,1), the abscissa data.
%
%    Output, real Y(DATA_NUM,1), the ordinate data.
%
  x(1:261,1) = ( 1700 : 1960 )';

  y(1:261,1) = [ ...
    5.0,  11.0,  16.0,  23.0,  36.0, ...
   58.0,  29.0,  20.0,  10.0,   8.0, ...
    3.0,   0.0,   0.0,   2.0,  11.0, ...
   27.0,  47.0,  63.0,  60.0,  39.0, ...
   28.0,  26.0,  22.0,  11.0,  21.0, ...
   40.0,  78.0, 122.0, 103.0,  73.0, ...
   47.0,  35.0,  11.0,   5.0,  16.0, ...
   34.0,  70.0,  81.0, 111.0, 101.0, ...
   73.0,  40.0,  20.0,  16.0,   5.0, ...
   11.0,  22.0,  40.0,  60.0,  80.9, ...
   83.4,  47.7,  47.8,  30.7,  12.2, ...
    9.6,  10.2,  32.4,  47.6,  54.0, ...
   62.9,  85.9,  61.2,  45.1,  36.4, ...
   20.9,  11.4,  37.8,  69.8, 106.1, ...
  100.8,  81.6,  66.5,  34.8,  30.6, ...
    7.0,  19.8,  92.5, 154.4, 125.9, ...
   84.8,  68.1,  38.5,  22.8,  10.2, ...
   24.1,  82.9, 132.0, 130.9, 118.1, ...
   89.9,  66.6,  60.0,  46.9,  41.0, ...
   21.3,  16.0,   6.4,   4.1,   6.8, ...
   14.5,  34.0,  45.0,  43.1,  47.5, ...
   42.2,  28.1,  10.1,   8.1,   2.5, ...
    0.0,   1.4,   5.0,  12.2,  13.9, ...
   35.4,  45.8,  41.1,  30.1,  23.9, ...
   15.6,   6.6,   4.0,   1.8,   8.5, ...
   16.6,  36.3,  49.6,  64.2,  67.0, ...
   70.9,  47.8,  27.5,   8.5,  13.2, ...
   56.9, 121.5, 138.3, 103.2,  85.7, ...
   64.6,  36.7,  24.2,  10.7,  15.0, ...
   40.1,  61.5,  98.5, 124.7,  96.3, ...
   66.6,  64.5,  54.1,  39.0,  20.6, ...
    6.7,   4.3,  22.7,  54.8,  93.8, ...
   95.8,  77.2,  59.1,  44.0,  47.0, ...
   30.5,  16.3,   7.3,  37.6,  74.0, ...
  139.0, 111.2, 101.6,  66.2,  44.7, ...
   17.0,  11.3,  12.4,   3.4,   6.0, ...
   32.3,  54.3,  59.7,  63.7,  63.5, ...
   52.2,  25.4,  13.1,   6.8,   6.3, ...
    7.1,  35.6,  73.0,  85.1,  78.0, ...
   64.0,  41.8,  26.2,  26.7,  12.1, ...
    9.5,   2.7,   5.0,  24.4,  42.0, ...
   63.5,  53.8,  62.0,  48.5,  43.9, ...
   18.6,   5.7,   3.6,   1.4,   9.6, ...
   47.4,  57.1, 103.9,  80.6,  63.6, ...
   37.6,  26.1,  14.2,   5.8,  16.7, ...
   44.3,  63.9,  69.0,  77.8,  64.9, ...
   35.7,  21.2,  11.1,   5.7,   8.7, ...
   36.1,  79.7, 114.4, 109.6,  88.8, ...
   67.8,  47.5,  30.6,  16.3,   9.6, ...
   33.2,  92.6, 151.6, 136.3, 134.7, ...
   83.9,  69.4,  31.5,  13.9,   4.4, ...
   38.0, 141.7, 190.2, 184.8, 159.0, ...
  112.3 ]';

  return
end