function out = isdecay(f)
%ISDECAY   Test if a SINGFUN decays faster than a single root at endpoints.
%   ISDECAY(F) returns a 1x2 row vector each of which indicates whether F
%   vanishes at one of the endpoints faster than a single root. An entry TRUE is
%   returned if F has a boundary root with multiplicity larger than one, FALSE
%   otherwise.
%
%   Note that ISDECAY is designed for and expected to be called only by UNBNDFUN
%   class for handling functions defined on unbounded domains.

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

out = isdecay(f.smoothPart);

%% Left endpoint:

if ( ~out(1) && f.exponents(1) > 1 )
    out(1) = 1;
end

%% Right endpoint:

if ( ~out(2) && f.exponents(2) > 1 )
    out(2) = 1;
end

end


function lax_wendroff_2d_display ( )

%*****************************************************************************80
%
%% LAX_WENDROFF_2D_DISPLAY illustrates the Lax-Wendroff procedure in 2D.
%
%  Discussion:
%
%    A sequence of ball and stick drawings is displayed in steps 0 through 9.
%
%    Hitting return causes the next item to be drawn.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2003
%
%  Author:
%
%    John Burkardt
%
  for frame = 0 : 9
%
%  Clear the plotting frame.
%
    clf
%
%  Make all the subsequent plotting "cumulative".
%
    hold on

    if ( 0 == frame )
      title ( 'Lax-Wendroff 2D scheme' )
    end
%
%  Central nodes on the blue level.
%
    if ( 1 <= frame )
      plot3 (  0.0,  0.0, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
    end
    if ( frame == 1 )
      title ( 'A node where we have the solution.' )
    end
%
%  The region associated with the central node, blue level.
%
    if ( 2 <= frame )
      plot3 (  [ -0.5,  0.5 ],  [ -0.5, -0.5 ],  [ 0.0, 0.0 ], 'b', 'LineWidth', 3 )
      plot3 (  [  0.5,  0.5 ],  [ -0.5,  0.5 ],  [ 0.0, 0.0 ], 'b', 'LineWidth', 3 )
      plot3 (  [  0.5, -0.5 ],  [  0.5,  0.5 ],  [ 0.0, 0.0 ], 'b', 'LineWidth', 3 )
      plot3 (  [ -0.5, -0.5 ],  [  0.5, -0.5 ],  [ 0.0, 0.0 ], 'b', 'LineWidth', 3 )
    end
    if ( frame == 2 )
      title ( 'The region associated with the node.' )
    end
%
%  Neighbor nodes on the blue level.
%
    if ( 3 <= frame )
      plot3 ( -1.0,  0.0, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  1.0,  0.0, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0, -1.0, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0,  1.0, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
    end
    if ( frame == 3 )
      title ( 'The neighboring nodes.' )
    end
%
%  Midside nodes on the blue level.
%
    if ( 4 <= frame )
      plot3 ( -0.5,  0.0, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.5,  0.0, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0, -0.5, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0,  0.5, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
    end
    if ( frame == 4 )
      plot3 (  [ -1.0, +1.0 ],  [  0.0,  0.0 ],  [ 0.0, 0.0 ], 'b', 'LineWidth', 3 )
      plot3 (  [  0.0,  0.0 ],  [ -1.0,  1.0 ],  [ 0.0, 0.0 ], 'b', 'LineWidth', 3 )
    end
    if ( frame == 4 )
      title ( 'Values at midsides by averaging.' )
    end
%
%  Center node on the green level.
%  Line from center node on blue to center node on green.
%
    if ( 5 <= frame )
      plot3 ( -0.5,  0.0, 0.5, 'o', 'MarkerFaceColor', 'g', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.5,  0.0, 0.5, 'o', 'MarkerFaceColor', 'g', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0, -0.5, 0.5, 'o', 'MarkerFaceColor', 'g', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0,  0.5, 0.5, 'o', 'MarkerFaceColor', 'g', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  [ -0.5,  0.5 ],  [ -0.5, -0.5 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
      plot3 (  [  0.5,  0.5 ],  [ -0.5,  0.5 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
      plot3 (  [  0.5, -0.5 ],  [  0.5,  0.5 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
      plot3 (  [ -0.5, -0.5 ],  [  0.5, -0.5 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
    end
    if ( frame == 5 )
      plot3 (  [ -0.5, -0.5 ],  [  0.0,  0.0 ],  [ 0.0, 0.5 ], 'k', 'LineWidth', 3 )
      plot3 (  [  0.5,  0.5 ],  [  0.0,  0.0 ],  [ 0.0, 0.5 ], 'k', 'LineWidth', 3 )
      plot3 (  [  0.0,  0.0 ],  [ -0.5, -0.5 ],  [ 0.0, 0.5 ], 'k', 'LineWidth', 3 )
      plot3 (  [  0.0,  0.0 ],  [  0.5,  0.5 ],  [ 0.0, 0.5 ], 'k', 'LineWidth', 3 )
    end
    if ( frame == 5 )
      title ( 'Values at midside, half time step.' )
    end
%
%  Lines from midside nodes to center.
%
    if ( 6 == frame )
      plot3 (  [  0.5,  0.0 ],  [  0.0,  0.0 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
      plot3 (  [ -0.5,  0.0 ],  [  0.0,  0.0 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
      plot3 (  [  0.0,  0.0 ],  [  0.5,  0.0 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
      plot3 (  [  0.0,  0.0 ],  [ -0.5,  0.0 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
    end
    if ( frame == 6 )
      title ( 'Estimate fluxes' )
    end
%
%  Carry neighbor nodes to green level, and draw lines.
%
    if ( 7 <= frame )
      plot3 (  0.0,  0.0, 0.5, 'o', 'MarkerFaceColor', 'g', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
    end
    if ( frame == 7 )
      title ( 'Estimate derivative at node, half time step.' )
    end
%
%  Advance solution to full time.
%
    if ( 8 <= frame )
      plot3 (  [  0.0,  0.0 ],  [  0.0,  0.0 ],  [ 0.0, 1.0 ], 'k', 'LineWidth', 3 )
      plot3 (  0.0,  0.0, 1.0, 'o', 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  [ -0.5,  0.5 ],  [ -0.5, -0.5 ],  [ 1.0, 1.0 ], 'r', 'LineWidth', 3 )
      plot3 (  [  0.5,  0.5 ],  [ -0.5,  0.5 ],  [ 1.0, 1.0 ], 'r', 'LineWidth', 3 )
      plot3 (  [  0.5, -0.5 ],  [  0.5,  0.5 ],  [ 1.0, 1.0 ], 'r', 'LineWidth', 3 )
      plot3 (  [ -0.5, -0.5 ],  [  0.5, -0.5 ],  [ 1.0, 1.0 ], 'r', 'LineWidth', 3 )
    end
    if ( frame == 8 )
      title ( 'Take full step to new time.' )
    end
%
%  Advance all data.
%
    if ( 9 <= frame )
      plot3 (  [ -1.0, -1.0 ],  [  0.0,  0.0 ],  [ 0.0, 1.0 ], 'k', 'LineWidth', 3 )
      plot3 (  [  1.0,  1.0 ],  [  0.0,  0.0 ],  [ 0.0, 1.0 ], 'k', 'LineWidth', 3 )
      plot3 (  [  0.0,  0.0 ],  [ -1.0, -1.0 ],  [ 0.0, 1.0 ], 'k', 'LineWidth', 3 )
      plot3 (  [  0.0,  0.0 ],  [  1.0,  1.0 ],  [ 0.0, 1.0 ], 'k', 'LineWidth', 3 )
      plot3 ( -1.0,  0.0, 1.0, 'o', 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  1.0,  0.0, 1.0, 'o', 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0, -1.0, 1.0, 'o', 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0,  1.0, 1.0, 'o', 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
    end
    if ( frame == 9 )
      title ( 'Advance all data to new time.' )
    end

    xlabel ( '- X -' )
    ylabel ( '- Y -' )
    zlabel ( '- Time -' )

    axis ( [ -1, +1, -1, +1, 0, 1] )
    view ( 3 )
%
%  "Release" the plotting screen.
%
    hold off

    pause

  end

  return
end


function M = ply_read(filename)
% Read PLY-formatted data from disk
% FORMAT M = ply_read(filename)
%
% filename - PLY-formatted file name
% M        - data structure
%__________________________________________________________________________
%
% Stanford Triangle Format Specification:
% https://en.wikipedia.org/wiki/PLY_%28file_format%29
%__________________________________________________________________________
% Copyright (C) 2017-2019 Wellcome Trust Centre for Neuroimaging

% Guillaume Flandin
% $Id: ply_read.m 7676 2019-10-22 10:19:29Z guillaume $

fid = fopen(filename,'rt');
if fid == -1
    error('Cannot open %s.',filename);
end

M = struct('vertices',[],'faces',[]);

%-Read header
isvertex = false;
nval = 0;
while true
    l = fgetl(fid);
    if strcmp(l,'end_header'), break; end
    [l,r] = strtok(l);
    if strcmp(l,'element')
        [l,r] = strtok(r);
        switch l
            case 'vertex'
                nv = str2double(r);
                isvertex = true;
            case 'face'
                nf = str2double(r);
                isvertex = false;
        end
    elseif strcmp(l,'property')
        if isvertex
            nval = nval + 1;
        end
    end
end

%-Read data
M.vertices = fscanf(fid,'%f',[nval nv])';
M.vertices = M.vertices(:,1:3);
M.faces    = fscanf(fid,'%d',[4 nf])';
M.faces    = M.faces(:,2:4) + 1;

fclose(fid);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   example 4 DOF planar robot.
%
%   Author: Arturo Gil. Universidad Miguel Hernandez de Elche.
%   email: arturo.gil@umh.es date:   05/03/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.
function robot = parameters()

robot.name='Example 4DOF planar arm';

%kinematic data DH parameters
robot.DH.theta='[q(1) q(2) q(3) q(4)]';
robot.DH.d='[0  0  0  0]';
robot.DH.a='[1  1  1  1]';
robot.DH.alpha='[0  0  0  0]';

%number of degrees of freedom
robot.DOF = 4;

%Jacobian matrix
% if not defined, compute it with manipulator_jacobian
robot.J=[];
%the proper rows of J to compute manipulability
robot.selJ =[1,2,6];
robot.kind=['R' 'R' 'R' 'R'];

%Function name to compute inverse kinematic
robot.inversekinematic_fn = 'inverse_kinematics_4dofplanar(robot, T, q)';
robot.directkinematic_fn = 'directkinematic(robot, q)';

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-180) deg2rad(180); %Axis 1, minimum, maximum
                deg2rad(-180) deg2rad(180);
                deg2rad(-180) deg2rad(180)]; %Axis 2, minimum, maximum

            %minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-90) deg2rad(90); %Axis 1, minimum, maximum
                deg2rad(-90) deg2rad(90);
                deg2rad(-180) deg2rad(180)]; %Axis 2, minimum, maximum

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = []; %empty, not available

robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time
% end effectors maximum velocity
robot.linear_velmax = 0; %m/s, example, not available

%base reference system
robot.T0 = eye(4);

%INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
%position, velocity and acceleration
robot=init_sim_variables(robot);
robot.path = pwd;

%this is an ad hoc transformation between the piece and the robot's end
%effector
% robot.Tcoupling=[-1 0 0  0;
%      0 1 0  0;
%      0 0 -1 0;
%      0 0 0 1];
% Tc1 = [cos(-pi/2) 0 sin(-pi/2) 0;
%        0 1 0 0;
%        -sin(-pi/2) 0 cos(-pi/2) 0;
%        0  0  0  1       ];
% Tc2 = [1 0 0 0;
%        0 cos(pi) -sin(pi) 0;
%        0  sin(pi) cos(pi) 0;
%        0 0 0 1];
% robot.Tcoupling =Tc1*Tc2;
robot.Tcoupling = eye(4);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%GRAPHICS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%read graphics files
robot.graphical.has_graphics=1;
robot.graphical.color = [255 20 40]./255;
%for transparency
robot.graphical.draw_transparent=0;
%draw DH systems
robot.graphical.draw_axes=1;
%DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for
%bigger robots
robot.graphical.axes_scale=1;
%adjust for a default view of the robot
robot.axis=[-3.5 3.5 -3.5 3.5 0 1]
robot = read_graphics(robot);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%DYNAMIC PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
robot.has_dynamics=1;

%consider friction in the computations
robot.dynamics.friction=0;

%link masses (kg)
robot.dynamics.masses=[1 1 1] ;

%COM of each link with respect to own reference system
robot.dynamics.r_com=[-0.5      0         0; %(rx, ry, rz) link 1
                      -0.5      0         0;
                      -0.5      0         0];%(rx, ry, rz) link 2

%link masses
m1 = robot.dynamics.masses(1);
m2 = robot.dynamics.masses(2);
m3 = robot.dynamics.masses(3);

a=eval(robot.DH.a);
L1 = a(1);
L2 = a(2);
L3 = a(3);

%Momentos de inercia de cada eslabon.
% Ixx	Iyy	Izz	Ixy	Iyz	Ixz, por cada fila
robot.dynamics.Inertia=[0   m1*L1^2/3   m1*L1^2/3    0	0	0;
                        0   m2*L2^2/3   m2*L2^2/3    0	0	0;
                        0   m3*L3^2/3   m3*L3^2/3    0	0	0];

%Actuator rotor inertia
robot.motors.Inertia=[0 0 0];
%Reduction ratio motor/joint speed
robot.motors.G=[1 1 1];
%consider friction
robot.friction=0;
%Viscous friction factor, motor referred
%robot.B = [1e-3  1e-3 1e-3];
robot.motors.Viscous = [10  10 10];
%Coulomb friction, motor referred
robot.motors.Coulomb = [0 0;
            0 0;
            0 0];

%SPECIAL PARAMETERS TO SOLVE THE INVERSE KINEMATICS
robot.parameters.step_time=0.1;
%Error in XYZ to stop inverse kinematics
robot.parameters.epsilonXYZ=0.01;
%Error in Quaternion to stop inverse kinematics.
robot.parameters.epsilonQ=0.01;
robot.parameters.stop_iterations=500;


classdef LevelSetWithCircleInclusion < LevelSetSphereNdim

    methods (Access = public)

        function obj = LevelSetWithCircleInclusion(cParams)
            obj.fracRadius = cParams.fracRadius;
            obj.compute(cParams);
        end
    end

    methods (Access = protected)
        function computeLevelSetValue(obj)
            ls = 1 - obj.dist;
            obj.levelSet = ls;
        end
    end

end


function tests = test_ft_preproc_detrend

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

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)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nchan   = 8;
nsample = 1000;
dat     = randn(nchan, nsample) + 1;

result = [];
result{end+1} = ft_preproc_detrend(dat, 1, 1000);
result{end+1} = ft_preproc_detrend(dat, 1, 900);
result{end+1} = ft_preproc_detrend(dat, 101, 1000);
result{end+1} = ft_preproc_detrend(dat, 101, 900);

% 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


classdef GradientSurfPerimeterComputer < handle

    properties (Access = private)
        outPutFolder
        iMesh
        figureID
        gExperiment
    end

    properties (Access = private)
        inputFiles
        outputFolder
        levelSetParams
        circleCase
    end

    methods (Access = public)

        function obj = GradientSurfPerimeterComputer(cParams)
            obj.init(cParams)
        end

        function compute(obj)
            for im = 1:numel(obj.inputFiles)
                obj.iMesh = im;
                obj.createGradientVariationExperiment();
                nEpsilon = length(obj.gExperiment.regularizedPerimeter.epsilons);
                for iEpsilon = 1:nEpsilon
                    obj.plotSurf(iEpsilon);
                    obj.addXYlabel();
                    obj.addTitle(iEpsilon);
                    obj.printSurf(iEpsilon);
                end
            end
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.inputFiles   = cParams.inputFiles;
            obj.outputFolder = cParams.outputFolder;
            obj.levelSetParams = cParams.levelSetParams;
            obj.circleCase = cParams.circleCase;
        end

        function createGradientVariationExperiment(obj)
            s.levelSetParams = obj.levelSetParams;
            s.inputFile      = obj.inputFiles{obj.iMesh};
            s.iMesh          = obj.iMesh;
            g = GradientVariationExperiment(s);
            obj.gExperiment = g;
        end

        function addXYlabel(obj)
            xlabel('$x$','interpreter','latex')
            ylabel('$y$','interpreter','latex')
        end

        function plotSurf(obj,iEpsilon)
            coord = obj.gExperiment.backgroundMesh.coord;
            x = coord(:,1);
            y = coord(:,2);
            dPer = obj.gExperiment.regularizedPerimeter.perimetersGradient;
            z = dPer(:,iEpsilon);
            tri = delaunay(x,y);
            f = figure();
            h = trisurf(tri,x,y,z);
            shading interp
            obj.figureID = f;
            ar = get(gca,'DataAspectRatio');
            obj.plotBoundaryCutMesh(tri,x,y,z)
            set(gca,'DataAspectRatio',ar);
            view(-60,25);
        end

        function plotBoundaryCutMesh(obj,tri,x,y,z)
            m = obj.computeBoundaryCutMesh();
            %nodes = unique(m.connec(:));
            Xi = m.coord(:,1);
            Yi = m.coord(:,2);
            Zi = interptri(tri,x,y,z,Xi,Yi);
            s.coord = [Xi Yi Zi];
            %s.connec(:,1) = 1:length(Xi);
            %s.connec(:,2) = 2:length(Xi)+1;
            %s.connec(end,2) = 1;
            %s.coord = m.coord;
            s.connec = m.connec;
            s.kFace = -2;
            mB = Mesh(s);
            hold on
            mB.plot
            % axes(axes_h)
            % axis(axis_h)
            % view(a,b)
        end

        function mBCut = computeBoundaryCutMesh(obj)
            g = obj.gExperiment;
            s.backgroundMesh = g.backgroundMesh;
            s.boundaryMesh   = g.boundaryMesh;
            uMesh = UnfittedMesh(s);
            uMesh.compute(g.levelSet);
            bCut = uMesh.boundaryCutMesh;
            mBCut = bCut.mesh;
        end

        function printSurf(obj,iEpsilon)
            part1 = [obj.outputFolder,obj.circleCase,'GradientMesh',num2str(obj.iMesh)];
            part2 = ['Epsilon',num2str(iEpsilon)];
            outFile = [part1,part2];
            printer = surfPrinter(obj.figureID);
            printer.print(outFile)
        end

        function addTitle(obj,iEpsilon)
            epsilons = obj.gExperiment.regularizedPerimeter.epsilons;
            h = obj.gExperiment.backgroundMesh.computeMeanCellSize;
            epsStr = num2str(epsilons(iEpsilon)/h);
            firstStr = 'dPer^R_\varepsilon \ \textrm{with} \ \,';
            secondStr = '\varepsilon/h \, = \, ';
            thirdStr = '\ \textrm{and} \ h/L \, = \, ';
            L = obj.gExperiment.domainLength;
            [n,d] = rat(h/L);
            if n == 1
                hStr = [num2str(n),'/',num2str(d)];
            else
                hStr = num2str(round(h/L,3));
            end
            titleStr = ['$',firstStr,secondStr,epsStr,thirdStr,hStr,'$'];
            title(titleStr,'interpreter','latex');
        end

    end

end


classdef AllEdges2CutEdgesComputer < handle

    properties (GetAccess = public, SetAccess = private)
        nCutEdgeByElem
    end

   properties (Access = private)
        isEdgeCutInElem
   end

   methods (Access = public)

       function obj = AllEdges2CutEdgesComputer(cParams)
          obj.init(cParams)
       end

       function edge = compute(obj,allEdges)
            nElem = size(allEdges,1);
            allEdges = transpose(allEdges);
            cutEdges = allEdges(obj.isEdgeCutInElem);
            cutEdges = reshape(cutEdges,obj.nCutEdgeByElem,nElem);
            edge     = transpose(cutEdges);
       end

   end

   methods (Access = private)

       function init(obj,cParams)
           obj.isEdgeCutInElem = cParams.isEdgeCutInElem;
           obj.nCutEdgeByElem = unique(sum(obj.isEdgeCutInElem,1));
       end

   end

end


function z = ge(x,y)
%GE Greater than or equal for sptensors.
%
%   See also SPTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Observations for sparse matrix case.
% The result of a >= 5 is sparse.
% The result of a >= 0 is sparse.
% The result of a >= full(a) is sparse.

%% Case 1: One argument is a scalar
if isscalar(y)
    subs1 = x.subs((x.vals >= y),:);
    if y <= 0
        subs2 = setdiff(allsubs(x),x.subs,'rows');
    else
        subs2 = [];
    end
    z = sptensor([subs1;subs2],true,size(x));
    return;
end

% Call back with the arguments reversed.
if isscalar(x)
    z = le(y,x);
    return;
end

%% Case 2: Both x and y are tensors of some sort
% Check that the sizes match
if ~isequal(x.size,y.size)
    error('Size mismatch');
end

% Case 2a: Two sparse tensors
if isa(x,'sptensor') && isa(y,'sptensor')
    z = le(y,x);
    return;
end

% Case 2b: One dense tensor
if isa(y,'tensor')

    % x zero
    subs1 = find(y <= 0);
    subs1 = setdiff(subs1,x.subs,'rows');

    % x nonzero
    subs2 = x.subs(x.vals >= y(x.subs,'extract'),:);

    % assemble
    z = sptensor([subs1;subs2],true,size(x));

    return;

end

%% Otherwise
error('The arguments must be two sptensors or an sptensor and a scalar.');


function w = drfiLearnSaliencyFusionWeight( train_dir, gt_dir, num_segmentation, is_resize )
    % Assume that all training images are placed under train_dir.
    % The saliency maps of i-th segmentation are under the folder "i" (e.g.,
    % saliency maps of 3rd segmentation are under the folder "3").
    % Detailed introduction on learning the saliency fusion weight can be
    % found in our supplementary material.

    M = num_segmentation;

    % Resize all training images to a fixed size 200*200
    sub_dir_list = dir(fullfile(train_dir, '*'));

    ind = [];
    for m = 1 : length(sub_dir_list)
        if strcmp(sub_dir_list(m).name, '.') || strcmp(sub_dir_list(m).name, '..')
            ind = [ind, m];
            continue;
        end
    end

    % Remove '.' and '..'
    sub_dir_list(ind) = [];

    normh = 200;
    normw = 200;

    % Resize
    if is_resize
        for m = 1 : length(sub_dir_list)
            image_list = dir(fullfile(train_dir, sub_dir_list(m).name, '*.png'));
            sub_dir_name = sub_dir_list(m).name;

            parfor n = 1 : length(image_list)
                image = imread(fullfile(train_dir, sub_dir_name, image_list(n).name));

                image = imresize(image, [normh, normw]);

                imwrite(image, fullfile(train_dir, sub_dir_name, image_list(n).name));

%                 if mod(jx, 500) == 0
%                     fprintf( 'sub_dir: %s, jx: %d\n', sub_dir_name, jx );
%                 end
            end

            fprintf( '%d / %d\n', m, length(sub_dir_list) );
        end
    end

    image_list = dir(fullfile(train_dir, sub_dir_list(end).name, '*.png'));
    num_image = length(image_list);

    % prepare H and f
    H = zeros(M, M);
    f = zeros(M, 1);

    for ii = 1 : M * M
        [m, n] = ind2sub([M, M], ii);
        sub_dir_name_n = sub_dir_list(n).name;
        sub_dir_name_m = sub_dir_list(m).name;
        if m >= n
            temp = zeros(1, num_image);
            parfor k = 1 : num_image
                image_name = image_list(k).name;
                Akm = im2double(imread(fullfile(train_dir, sub_dir_name_m, image_name)));
                Akn = im2double(imread(fullfile(train_dir, sub_dir_name_n, image_name)));

                if size(Akm, 3) > 1
                    Akm = rgb2gray( Akm );
                end

                if size(Akn, 3) > 1
                    Akn = rgb2gray( Akn );
                end

                Nk = 1;%size(Akm, 1) * size(Akm, 2);
                temp(k) = sum(sum(Akm .* Akn)) / Nk;
                % fprintf( 'ix: %d, jx: %d, n: %d\n', ix, jx, n );
            end
            H(m, n) = 2 * sum( temp );
        else
            H(m, n) = H(n, m);
        end

        fprintf( 'Computing H, m: %d, n: %d\n', m, n );
    end
    H = H / num_image;
    save( 'H.mat', 'H' );
%     load( 'H.mat' );

    for m = 1 : M
        temp = zeros(1, num_image);
        sub_dir_name = sub_dir_list(m).name;
        parfor k = 1 : num_image
            image_name = image_list(k).name;
            Akm = im2double(imread(fullfile(train_dir, sub_dir_name, image_name)));
            if size(Akm, 3) > 1
                Akm = rgb2gray(Akm);
            end

            A = imread(fullfile(gt_dir, image_name));
            A = imresize(A, [normh, normw]);
            A = im2double( A );
            if size(A, 3) > 1
                A = rgb2gray(A);
            end

            A( A > 0.5 ) = 1.0;
            A( A < 0.5 ) = 0;

            % f(ix) = f(ix) - 2 * sum(sum(A .* Ani));
            % temp = temp - 2 * sum(sum(A .* Ani));
            Nk = 1;%size(A, 1) * size(A, 2);
            temp(k) = - 2 * sum(sum(A .* Akm)) / Nk;
        end
        f(m) = sum( temp );
        fprintf( 'comupting f, m: %d\n', m );
    end
    f = f / num_image;
    save( 'f.mat', 'f' );

    % Solve the quadratic programming problem
    w_init = ones(M, 1) / M;

    Aeq = ones(1, M);
    beq = 1;

    lb = zeros(M, 1);
    ub = ones(M, 1);

    opt = optimset( 'Algorithm', 'interior-point-convex' );
    w = quadprog(H, f, [], [], Aeq, beq, lb, ub, w_init, opt );

    w( w < 1e-6 ) = 0;
end


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

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym acos (@var{x})
%% Symbolic acos function.
%%
%% Example:
%% @example
%% @group
%% syms x
%% y = acos (x)
%%   @result{} y = (sym) acos(x)
%% @end group
%% @end example
%%
%% Note: this file is autogenerated: if you want to edit it, you might
%% want to make changes to 'generate_functions.py' instead.
%%
%% @end defmethod

function y = acos(x)
  if (nargin ~= 1)
    print_usage ();
  end
  y = elementwise_op ('acos', x);
end

%!error acos (sym(1), 2)
%!assert (isequaln (acos (sym(nan)), sym(nan)))

%!shared x, d
%! d = 1;
%! x = sym('1');

%!test
%! f1 = acos(x);
%! f2 = acos(d);
%! assert( abs(double(f1) - f2) < 1e-15 )

%!test
%! D = [d d; d d];
%! A = [x x; x x];
%! f1 = acos(A);
%! f2 = acos(D);
%! assert( all(all( abs(double(f1) - f2) < 1e-15 )))

%!test
%! % round trip
%! y = sym('y');
%! A = acos (d);
%! f = acos (y);
%! h = function_handle (f);
%! B = h (d);
%! assert (A, B, -eps)


function ret=writesdpa(fname,A,b,c,K,pars)
%WRITESDPA Take a problem in SeDuMi format and writes it in SDPA format
%
%  Usage:
%
%  ret=writesdpa(fname,A,b,c,K,pars)
%
%      fname           Name of SDPpack file, in quotes
%      A,b,c,K         Problem in SeDuMi form
%      pars            Optional parameters.
%                      pars.printlevel=0           No printed output
%                      pars.printlevel=1 (default) Some printed output.
%
%      ret             ret=0 on success, ret=1 on failure.
%
%  Notes:
%
%     Problems with complex data are not allowed.
%
%     Quadratic cone and rotated cone constraints are not supported.
%
%     Nonsymmetric A.s and C.s matrices are symmetrized with A=(A+A')/2
%     a warning is given when this happens.
%
%     Free variables are not supported.
%
%     Floating point numbers are written out with 18 decimal digits for
%     accuracy.
%
%  Please contact the author (Brian Borchers, borchers@nmt.edu) with any
%  questions or bug reports.
%
%  Third Version: 3/3/06.  Corrected a bug in the handling of
%                          nonsymmetric constraint matrices.
%
%  Second Version: 7/14/04.  Modified to vastly speed up operations on sparse
%                 matrices.  On some problems, this version is 100 times
%                 faster!
%
%  First Version: 7/14/03.  Modified from old writesdp.m which wrote problems
%                 in SDPPack format.
%
%
%

% This file is part of the Matlab Toolbox for Dimensionality Reduction.
% The toolbox can be obtained from http://homepage.tudelft.nl/19j49
% You are free to use, change, or redistribute this code in any way you
% want for non-commercial purposes. However, it is appreciated if you
% maintain the name of the original author.
%
% (C) Laurens van der Maaten, Delft University of Technology

%
% First, check to see whether or not we should be quiet.
%
if (nargin > 5)
  if (isfield(pars,'printlevel'))
    if (pars.printlevel == 0)
      quiet=1;
    else
      quiet=0;
    end
  else
    quiet=0;
  end
else
  pars.printlevel=1;
  quiet=0;
end
%
%  First, check for complex numbers in A, b, or c.
%
if (isreal(A) ~= 1)
  if (quiet == 0)
    fprintf('A is not real!\n');
  end
  ret=1;
  return
end
if (isreal(b) ~= 1)
  if (quiet == 0)
    fprintf('b is not real!\n');
  end
  ret=1;
  return
end
if (isreal(c) ~= 1)
  if (quiet == 0)
    fprintf('c is not real!\n');
  end
  ret=1;
  return
end
%
%  Check for any quadratic cone constraints.
%
if (isfield(K,'q'))
  if ((~isempty(K.q)) & (K.q ~= 0))
    if (quiet == 0)
      fprintf('quadratic cone constraints are not supported.\n');
    end
    ret=1;
    return
  end
end
%
%  Check for any rotated cone constraints.
%
if (isfield(K,'r'))
  if ((~isempty(K.r)) & (K.r ~= 0))
    if (quiet == 0)
      fprintf('rotated cone constraints are not supported.\n');
    end
    ret=1;
    return
  end
end
%
% Check for any free variables.
%
if (isfield(K,'f'))
  if ((~isempty(K.f)) & (K.f ~= 0))
    if (quiet == 0)
      fprintf('Free variables are not supported.\n');
    end
    ret=1;
    return
  end
end
%
%  Find the number of constraints.
%
m=length(b);
%
%  Deal with the following special case.  If A is transposed, transpose
%  it again so that it is of the right size.
%
[Am,An]=size(A);
if (Am ~= m)
  if (An == m)
    if (quiet==0)
      fprintf('Transposing A to match b \n');
    end
    AT=A;
    A=A';
%
% Also swap Am and An so that they're correct.
%
    temp=Am;
    Am=An;
    An=temp;
  else
%
% In this case, A is just plain the wrong size.
%
    if (quiet==0)
      fprintf('A is not of the correct size to match b \n');
    end
    ret=1;
    return
  end
else
%
% No need to transpose A, but we'll need AT.
%
  AT=A';
end
%
%  Deal with the following special case:  if c==0, then c should really
%  be a zero vector of the appropriate size.
%
if (c == 0)
  if (quiet==0)
    fprintf('Expanding c to the appropriate size\n');
  end
  c=sparse(An,1);
end
%
% If c is empty, then act as if it was zero.
%
if (isempty(c))
  if (quiet==0)
    fprintf('Expanding empty c to zeros of the appropriate size\n');
  end
  c=sparse(An,1);
end
%
% If c is a row vector, make it a column vector.
%
[cm,cn]=size(c);
if (cn > cm)
  c=c';
end
%
%  Get the size data.
%
%
% First, the size of the LP block.
%
if (isfield(K,'l'))
  nlin=K.l;
  sizelin=nlin;
  if (isempty(sizelin))
    sizelin=0;
    nlin=0;
  end
  if (K.l == 0)
    nlin=0;
    sizelin=0;
  end
else
  nlin=0;
  sizelin=0;
end
%
% Get the sizes of the SDP blocks.
%
if (isfield(K,'s'))
  nsdpblocks=length(K.s);
  sizesdp=sum((K.s).^2);
  if (isempty(sizesdp))
    sizesdp=0;
    nsdpblocks=0;
  end
  if (K.s == 0)
    nsdpblocks=0;
    sizesdp=0;
  end
else
  sizesdp=0;
  nsdpblocks=0;
end
%
% Figure out the number of blocks.
%
nblocks=nsdpblocks;
if (nlin>0)
  nblocks=nblocks+1;
end
%
%  print out some size information
%
if (quiet==0)
  fprintf('Number of constraints: %d \n',m);
  fprintf('Number of SDP blocks: %d \n',nsdpblocks);
  fprintf('Number of LP vars: %d \n',nlin);
end
%
%  Open up the file for writing.
%
fid=fopen(fname,'w');
if (fid==-1)
  if (quiet==0)
    fprintf('Could not open file for output!');
  end
  ret=1;
  return
end
%
%  Print out m, the number of constraints.
%
fprintf(fid,'%d \n',m);
%
% Next, the number of blocks.
%
fprintf(fid,'%d \n',nblocks);
%
% Print out the block structure.
%
if (K.s > 0)
  fprintf(fid,'%d ',K.s);
end
if (nlin > 0)
  fprintf(fid,'%d ',-nlin);
end
fprintf(fid,'\n');
%
%  Next, b, with all on one line.
%
fprintf(fid,'%.18e  ',full(b));
fprintf(fid,'\n');
%
% First, the C matrix.
%
%
%  First, calculate where in c things start.
%
base=sizelin+1;
%
%  Next, work through the SDP blocks.
%
for i=1:nsdpblocks
%
% Get the current block of C.
%
  I=find(c);
  II=find(I>=base);
  I=I(II);
  II=find(I<=base+K.s(i)^2-1);
  I=I(II);
  II=I-(base-1)*ones(size(I));
  work=sparse(II,ones(size(II)),c(I),K.s(i)^2,1);
  work=reshape(work,K.s(i),K.s(i));

%
% Check this block for symmetry.
%
if (norm(work-work','fro') ~= 0)
  if (quiet==0)
    fprintf('Non symmetric C.s matrix!\n');
  end
  work=(work+work')/2;
end
%
% Write out the C.s matrix.
%
  work=triu(work);
  [II,JJ,V]=find(work);
  cnt=length(II);
  if (cnt ~= 0)
    fprintf(fid,'%d %d %d %d %.18e\n',[zeros(size(II)) i*ones(size(II)) II JJ -V]');
  end

%
%  Next, update to the next base.
%
  base=base+K.s(i)^2;
end
%
% Print out the coefficients for the linear block of C.
%
for i=1:nlin
  if (c(i) ~= 0.0)
    fprintf(fid,'%d %d %d %d %.18e\n',[0 nsdpblocks+1 i i -full(c(i))]);
  end
end

%
%  Now, loop through the constraints, one at a time.
%
for cn=1:m
%
%  Print out the SDP part of constraint cn.
%
  base=sizelin+1;
  rowcn=AT(:,cn);
  for i=1:nsdpblocks
    I=find(rowcn);
    II=find(I>=base);
    I=I(II);
    II=find(I<=(base+K.s(i)^2-1));
    I=I(II);
    II=I-(base-1)*ones(size(I));
    work=sparse(II,ones(size(II)),rowcn(I),K.s(i)^2,1);

    work=reshape(work,K.s(i),K.s(i));

    if (norm(work-work','fro') ~= 0)
      if (quiet==0)
        fprintf('Non symmetric A.s matrix! \n');
      end
      work=(work+work')/2;
    end
%
% Ignore the lower left triangle.
%
    work=triu(work);
%
% Get the nonzero entries.
%
    [II,JJ,V]=find(work);
    cnt=length(II);
    if (cnt ~= 0)
      fprintf(fid,'%d %d %d %d %.18e\n',[cn*ones(size(II)) i*ones(size(II)) II JJ V]');
    end
%
%  Next, update to the next base.
%
    base=base+K.s(i)^2;
  end
%
% Finally, the linear part.
%
  I=find(rowcn);
  II=find(I<=nlin);
  I=I(II);
  workrow=sparse(I,ones(size(I)),rowcn(I),nlin,1);
  [II,JJ,V]=find(workrow);
  if (length(II) > 0)
    fprintf(fid,'%d %d %d %d %.18e\n',[cn*ones(length(II),1) (nsdpblocks+1)*ones(length(II),1) II II V]');
  end
end
%
% Close the file.
%
fclose(fid);
%
% Return success
%
ret=0;
return


% gammaScanScript
global planC
indexS = planC{end};

[newXgrid, newYgrid, newZgrid] = getScanXYZVals(planC{indexS.scan}(1));
deltaX = abs(newXgrid(2) - newXgrid(1));
deltaY = abs(newYgrid(2) - newYgrid(1));
deltaZ = abs(newZgrid(2) - newZgrid(1));
doseAgreement = 15;
distAgreement = 0.3;
thresholdAbsolute = 4000;
doseArray1 = single(planC{indexS.scan}(1).scanArray);
doseArray2 = single(planC{indexS.scan}(2).scanArray);
gammaM = gammaDose3d(doseArray1, doseArray2, [deltaX deltaY deltaZ], doseAgreement, distAgreement, [], thresholdAbsolute);

newDoseNum = length(planC{indexS.dose}) + 1;
%Remove old caching info.
planC{indexS.dose}(newDoseNum).cachedMask = [];
planC{indexS.dose}(newDoseNum).cachedColor = [];
planC{indexS.dose}(newDoseNum).cachedTime = [];
%Set coordinates.
planC{indexS.dose}(newDoseNum).sizeOfDimension1 = length(newXgrid);
planC{indexS.dose}(newDoseNum).sizeOfDimension2 = length(newYgrid);
planC{indexS.dose}(newDoseNum).sizeOfDimension3 = length(newZgrid);
planC{indexS.dose}(newDoseNum).horizontalGridInterval = newXgrid(2)-newXgrid(1);
planC{indexS.dose}(newDoseNum).verticalGridInterval = newYgrid(2)-newYgrid(1);
planC{indexS.dose}(newDoseNum).depthGridInterval = newZgrid(2)-newZgrid(1);
planC{indexS.dose}(newDoseNum).coord1OFFirstPoint = newXgrid(1);
planC{indexS.dose}(newDoseNum).coord2OFFirstPoint = newYgrid(1);
planC{indexS.dose}(newDoseNum).coord3OfFirstPoint = newZgrid(1);
planC{indexS.dose}(newDoseNum).zValues = newZgrid;
planC{indexS.dose}(newDoseNum).doseUnits = 'Gy';
planC{indexS.dose}(newDoseNum).doseArray = gammaM;
planC{indexS.dose}(newDoseNum).doseUID = createUID('dose');
planC{indexS.dose}(newDoseNum).fractionGroupID = 'Gamma Scan 4mm, 50HU';
%Switch to new dose
sliceCallBack('selectDose', num2str(newDoseNum));


function r = gen_rand_hash(k,n,m);
persistent buffer
persistent loc
try
    % Preallocate 100 samples everytime and empty that buffer first
    if isempty(buffer) || length(buffer) < loc + n*m
        % Previous approach does not work with parallell toolbox!
        s = rng;
        rng(k);
        buffer = rand(n*m+100,1);
        r = reshape(buffer(1:n*m),n,m);
        loc = 1;
        rng(s);
    end
     r = reshape(buffer(loc:n*m+loc-1),n,m);
     loc = loc + n*m;

catch
    % but rng is not available in all versions...
    s = rand('state');
    rand('state',k)
    r = rand(n,m);
    rand('state',s);
end


classdef HomogVoigtPlaneStressHomogenizer < SequentialLaminateHomogenizer

    properties (Access = private)
    end

    methods (Access = public)

        function obj = HomogVoigtPlaneStressHomogenizer(c0,c1,dir,mi,theta)
            obj.init(c0,c1,dir,mi,theta);
            obj.computeAnisotropicTensors();
            obj.homogenize()
            obj.transformToVoigt()
            obj.makeHomogenizedTensorPlaneStress();
        end
    end

    methods (Access = protected)

        function transformToVoigt(obj)
            t = obj.homogTensor;
            t = Tensor2VoigtConverter.convert(t);
            obj.homogTensor = t;
        end

    end

end


function varargout = pivotplot(varargin)
%PIVOTPLOT   Semilogy plot of pivot values.
%   PIVOTPLOT( F ) semilogy plot related to the pivots values taken during
%   the construction of the CHEBFUN2 F.
%
%   H = PIVOTPLOT( F ) returns a handle H to the figure.
%
%   PIVOTPLOT( F, S ) allows further plotting options, such as linestyle,
%   linecolor, etc. If S contains a string 'LOGLOG', the pseudo sig will be
%   displayed on a log-log scale.

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

[varargout{1:nargout}] = pivotplot@separableApprox(varargin{:});

end


function neg_base_exponent(base)
%
%
%
global C
C = 0;

figure;
set(gcf,'position',[350 400 850 350])

subplot(1,2,1);
set(gcf,'windowButtonMotionFcn',@fun_mouseMove)
line([-3 3],[0 0],'color','k','linewidth',3)

function fun_mouseMove (object, eventdata)

C = get(gca, 'CurrentPoint');
title(gca, ['(X,Y) = (', num2str(C(1,1)), ', ',num2str(C(1,2)), ')']);

subplot(1,2,2);
y = base.^C(1,1);
plot(real(y), imag(y),'ro');

xlim([-5 5])
ylim([-5 5])

end

end


function navego_nav2csv(nav)
% navego_nav2csv: exports navigation data to a .csv file.
%
% INPUT
%   nav, INS/GNSS estimations.
%
% OUTPUT
%	.csv file, with the following columns: time (seconds), roll (deg),
%       pitch (deg), yaw (deg), vel N (m/s), vel E (m/s), vel D (m/s),
%        latitude (deg), longitud (deg), altitude (m).
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%
% Version: 002
% Date:    2021/12/09
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

R2D = (180/pi);     % Radians to degrees

% File name
if(is_octave)
    dtime = datestr(date, 'yymmdd_HHMMSS' );
else
    dtime = datetime('now','TimeZone','local','Format','yyMMdd-HHmm');
end

file_name = sprintf('navego_%s.csv', dtime);

% Headline
ht = 'time (seconds), ';
ha = 'roll (deg), pitch (deg), yaw (deg), ';
hv = 'vel N (m/s), vel E (m/s), vel D (m/s), ';
hp = 'latitude (deg), longitud (deg), altitude (m),';

% Navigation data
nav_data = [nav.t nav.roll*R2D nav.pitch*R2D nav.yaw*R2D ...
              nav.vel(:,1) nav.vel(:,2) nav.vel(:,3) ...
              nav.lat*R2D nav.lon*R2D nav.h  ]' ;

% Navigation data precision to be saved
pt = '%.6f, ';
pa = repmat('%.3f, ', 1, 3);
pv = repmat('%.3f, ', 1, 3);
pp = repmat('%.7f, ', 1, 2);
ph = '%.3f, ';
format_pattern = [pt pa pv pp ph '\n'];

% Saving the .cvs file
fprintf('navego_nav2cvs: saving navigation data to file %s ... \n', file_name)

fd = fopen(file_name, 'w');

% Saving headline
fprintf(fd, '%s%s%s%s \n', ht, ha, hv, hp);

% Saving data
fprintf(fd, format_pattern , nav_data );

fclose(fd);

end


% navego_example_real_mpu6000: post-processing integration of MPU-6000
% IMU and Ekinox GNSS data.
%
% The main goal is to integrate MPU-6000 IMU and Ekinox-D GNSS measurements.
%
% Sensors dataset was generated driving a car through the streets of
% Turin city (Italy).
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%   SBG Systems. SBG Ekinox-D High Accuracy Inertial System Brochure,
% Tactical grade MEMS Inertial Systems, v1.0. February 2014.
%
%   InvenSense Inc. MPU-6000/MPU-6050 Product Specification. Document
% Number: PS-MPU-6000A-00. Revision: 3.4. Release Date: 08/19/2013.
%
%   R. Gonzalez and P. Dabove. Performance Assessment of an Ultra Low-Cost
% Inertial Measurement Unit for Ground Vehicle Navigation. Sensors 2019,
% 19(18). https://www.mdpi.com/530156.
%
% Version: 006
% Date:    2021/12/08
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

% NOTE: NaveGo assumes that IMU is aligned with respect to body-frame as
% X-forward, Y-right and Z-down.
%
% NOTE: NaveGo assumes that yaw angle (heading) is positive clockwise.

clc
close all
clear
matlabrc

addpath ../../ins/
addpath ../../ins-gnss/
addpath ../../conversions/
addpath ../../performance-analysis/
addpath ../../misc/
addpath ../../plot/

navego_print_version;

fprintf('\nNaveGo: starting real INS/GNSS integration... \n')

%% PARAMETERS

% Comment any of the following parameters in order to NOT execute a
% particular portion of code

INS_GNSS = 'ON';
PLOT     = 'ON';

if (~exist('INS_GNSS','var')), INS_GNSS = 'OFF'; end
if (~exist('PLOT','var')),     PLOT     = 'OFF'; end

%% CONVERSION CONSTANTS

G =  9.80665;       % Gravity constant, m/s^2
G2MSS = G;          % g to m/s^2
MSS2G = (1/G);      % m/s^2 to g

D2R = (pi/180);     % degrees to radians
R2D = (180/pi);     % radians to degrees

KT2MS = 0.514444;   % knot to m/s
MS2KMH = 3.6;       % m/s to km/h

%% REF DATA

% Reference dataset was obtained by processing Ekinox IMU and Ekinox GNSS
% with tighly-coupled integration by Inertial Explorer software package.

fprintf('NaveGo: loading reference data... \n')

load ref

%% MPU-6000 IMU

fprintf('NaveGo: loading MPU-6000 IMU data... \n')

load mpu6000_imu

%% EKINOX GNSS

fprintf('NaveGo: loading Ekinox GNSS data... \n')

load ekinox_gnss

% ekinox_gnss contains the lever arm with respect to Ekinox IMU.
% ekinox_gnss.larm has to be changed for MPU-6000 IMU.
ekinox_gnss.larm = [-0.369, 0.0, -0.219]';

ekinox_gnss.eps = mean(diff(mpu6000_imu.t)) / 2; %  A rule of thumb for choosing eps.

%% NAVIGATION TIME

to = (ref.t(end) - ref.t(1));

fprintf('NaveGo: navigation time is %.2f minutes or %.2f seconds. \n', (to/60), to)

%% INS/GNSS INTEGRATION

if strcmp(INS_GNSS, 'ON')

    fprintf('NaveGo: processing INS/GNSS integration... \n')

    % Execute INS/GNSS integration
    % ---------------------------------------------------------------------
    nav_mpu6000 = ins_gnss(mpu6000_imu, ekinox_gnss, 'dcm');
    % ---------------------------------------------------------------------

    save nav_mpu6000.mat nav_mpu6000
else

    load nav_mpu6000
end

%% TRAVELED DISTANCE

distance = gnss_distance (nav_mpu6000.lat, nav_mpu6000.lon);

fprintf('NaveGo: distance traveled by the vehicle is %.2f meters or %.2f km. \n', distance, distance/1000)

%% ANALYSIS OF PERFORMANCE FOR A CERTAIN PART OF THE INS/GNSS DATASET

tmin_rmse = ref.t(1);
tmax_rmse = ref.t(end);

% Sincronize REF data to tmin and tmax
idx  = find(ref.t > tmin_rmse, 1, 'first' );
fdx  = find(ref.t < tmax_rmse, 1, 'last' );
if(isempty(idx) || isempty(fdx))
    error('ref: empty index')
end

ref.t       = ref.t    (idx:fdx);
ref.roll    = ref.roll (idx:fdx);
ref.pitch   = ref.pitch(idx:fdx);
ref.yaw     = ref.yaw  (idx:fdx);
ref.lat     = ref.lat  (idx:fdx);
ref.lon     = ref.lon  (idx:fdx);
ref.h       = ref.h    (idx:fdx);
ref.vel     = ref.vel  (idx:fdx, :);

%% INTERPOLATION OF INS/GNSS DATASET

% INS/GNSS estimates and GNSS data are interpolated according to the
% reference dataset.

[nav_i,  ref_n] = navego_interpolation (nav_mpu6000, ref);
[gnss_i, ref_g] = navego_interpolation (ekinox_gnss, ref);

%% NAVIGATION RMSE

rmse_v = print_rmse (nav_i, gnss_i, ref_n, ref_g, 'MPU-6000 INS/GNSS');

%% RMSE TO CVS FILE

csvwrite('mpu6000.csv', rmse_v);

%% NAVIGATION DATA TO CSV FILE

fprintf('\n');
navego_nav2csv(nav_mpu6000);

%% PLOTS

if (strcmp(PLOT,'ON'))

   navego_plot_main (ref, ekinox_gnss, nav_mpu6000, gnss_i, nav_i, ref_g, ref_n)
end

%% PERFORMANCE ANAYSYS OF THE KALMAN FILTER

fprintf('\nNaveGo: Kalman filter performance analysis...\n')

kf_analysis (nav_mpu6000)


HEURISTICS = 0;

%%%%%%%%%%%%%%%%%%%%%%%%
%     Normalize        %
%%%%%%%%%%%%%%%%%%%%%%%%

Normalize = @(x) (x - min(x(:)))/(max(x(:)) - min(x(:)));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       Input images        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%  Img = 'lena';
Img = 'clena';

  switch lower(Img)

    case{'lena'}
      Ig = double( imread('gray_imgs/lena_gray_512.png') ) / 255;

    case{'peppers'}
      Ig = double( imread('gray_imgs/peppers_gray.png') ) / 255;

    case{'goldhl.'}
      Ig = double( imread('gray_imgs/goldhill_gray.png') ) / 255;

    case{'clena'}
      Ig = double( imread('color_imgs/lena_color_512.png') ) / 255;

    case{'none'}
      disp(' ');
      disp('Select a test image (see code for an example)...');
      disp(' ');
      disp('Exiting test_adaptL1 code...');
      return;

    otherwise
      error('Not a valid image\n');

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %       noisy images        %
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

      Ig_01L1 = imnoise(Ig, 'salt & pepper', 0.1);
      Ig_02L1 = imnoise(Ig, 'salt & pepper', 0.2);
      Ig_03L1 = imnoise(Ig, 'salt & pepper', 0.3);
      Ig_05L1 = imnoise(Ig, 'salt & pepper', 0.5);
      Ig_07L1 = imnoise(Ig, 'salt & pepper', 0.7);
      Ig_09L1 = imnoise(Ig, 'salt & pepper', 0.9);

      Ig_05L2 = imnoise(Ig,'gaussian', 0, ((5/255)^2) );
      Ig_10L2 = imnoise(Ig,'gaussian', 0, ((10/255)^2) );
      Ig_20L2 = imnoise(Ig,'gaussian', 0, ((20/255)^2) );
      Ig_30L2 = imnoise(Ig,'gaussian', 0, ((30/255)^2) );

      InCell{1} = imnoise(Ig_10L2, 'salt & pepper', 0.2);
      InCell{2} = imnoise(Ig_20L2, 'salt & pepper', 0.2);
      InCell{3} = imnoise(Ig_30L2, 'salt & pepper', 0.2);
      InCell{4} = imnoise(Ig_10L2, 'salt & pepper', 0.4);

      % -----------

      InCell{5} = imnoise(Ig_05L2, 'salt & pepper', 0.3);
      InCell{6} = imnoise(Ig_05L2, 'salt & pepper', 0.5);

      InCell{7} = imnoise(Ig_10L2, 'salt & pepper', 0.3);
      InCell{8} = imnoise(Ig_10L2, 'salt & pepper', 0.5);

%        snr_noisy12 = snr(Ig, In);

%      figure; imagesc( Normalize(In) );
%      axis image; axis off; colormap gray;
%      title(sprintf('Noisy - L1/L2. SNR: %4.1fdB.\n ', ...
%                    snr_noisy12));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% pre-processing

InputDims = size(Ig);

if(length(InputDims) == 2)
  lambdaS_l2 = [ 0.0185, 0.03, 0.045, 0.0185, 0.0075, 0.0075, 0.0185, 0.0185 ];
else
  lambdaS_l2 = 1.5*[ 0.0185, 0.03, 0.045, 0.0185, 0.0075, 0.0075, 0.0185, 0.0185 ];
end

for k=1:length(InCell)

In = InCell{k};

  tstart = tic;

S0 = adaptMedian(In);

if(length(InputDims) == 2)

  if(HEURISTICS)
    lambda_l1l2(:,:,1)  =  0.85*(S0 == 3) + ...
                    1.1*(S0 == 5) + 1.2*(S0 == 7) + 1.4*(S0 == 9);
  else
    lambda_l1l2(:,:,1)  = 1.5*(S0 ~= 0);
  end

  lambda_l1l2(:,:,2) = lambdaS_l2(k)*(S0==0);

  planes=1;
else

  if(HEURISTICS)

    for d=1:InputDims(3),
    lambda_l1l2(:,:,d)  =  1.05*(S0(:,:,d) == 3) + ...
                    1.3*(S0(:,:,d) == 5) + 1.4*(S0(:,:,d) == 7) + 1.6*(S0(:,:,d) == 9);
    end
  else

    for d=1:InputDims(3),
      lambda_l1l2(:,:,d)  =  1.75*(S0(:,:,d) ~= 0);
    end
  end

  for d=1:InputDims(3),
    lambda_l1l2(:,:,3+d) = lambdaS_l2(k)*(S0(:,:,d)==0);
  end

  planes=3;
end

% ---------- Setup for IRN_L1L2  ------------------

pars_irn_adapt = irntvInputPars('l1tv_l2tv');

pars_irn_adapt.adapt_epsR   = 1;
pars_irn_adapt.epsR_cutoff  = 0.01;
pars_irn_adapt.adapt_epsF   = 1;
pars_irn_adapt.epsF_cutoff  = 0.05;

pars_irn_adapt.pcgtol_ini = 1e-4;

pars_irn_adapt.loops      = 1;

pars_irn_adapt.U0      = In;   % necessary the for adapt case
%  pars_irn_adapt.U0      = lmu;   % necessary the for adapt case

  Ig_L1L2 = irntv(In, {}, lambda_l1l2, pars_irn_adapt);

loops = 6;
%
for m = 2:loops

   for d=1:InputDims(3)
      lambda_l1l2(:,:,3+d) = (0.975^(m-1))*lambda_l1l2(:,:,3+d);
   end
   Ipass = Ig_L1L2;
   dI = Ipass - In;
   lambda_l1l2(:,:,1:planes) = adaptLambda(dI,  lambda_l1l2(:,:,1:planes), 0.5, S0, 0.8);
   pars_irn_adapt.U0      = Ipass;
   Ig_L1L2 = irntv(In, {}, lambda_l1l2, pars_irn_adapt);

end
l1l2_time(k) = toc(tstart)
%
figure; imagesc( Normalize(Ig_L1L2) ); axis image; axis off; colormap gray;

psnr_l1l2(k) = psnr(Ig, Ig_L1L2)

end

psnr_bing = [32.84, 30.23, 27.33, 29.96];
psnr_xiao = [36.20, 33.93, 33.19, 31.51];

[ psnr_l1l2' [psnr_bing'; psnr_xiao']]


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

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

function HeartTube()

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

% Immersed Structure Geometric / Dynamic Parameters %
ds = Lx/(2*Nx);  % Lagrangian Spacing
d = 1.0;         % Diameter of the tube
L = 3.0;         % Length of the heart-tube
struct_name = 'HeartTube'; % Name for .vertex, .spring, etc files.

% Call function to construct geometry
[xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(ds,L,d,Lx,Ly);

% Plot Geometry to test
plot(xLag(1:end/2),yLag(1:end/2),'r-'); hold on;
plot(xLag(end/2+1:end),yLag(end/2+1:end),'r-'); hold on;
plot(xLag,yLag,'*'); hold on;
xlabel('x'); ylabel('y');
axis([0 Lx 0 Ly]);

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

% Prints .spring file!
k_Spring = 1e7;
print_Lagrangian_Springs(xLag,k_Spring,ds,struct_name);

% Prints .muscle file! [ a_f * Fmax *exp( -( (Q-1)/SK )^2 ) * (1/P0)*(b*P0-a*v)/(v+b); Q = LF/LFO ]
LFO = d; SK = 0.3; a = 0.25; b = 4.0; Fmax = 1e5;
print_Lagrangian_Muscles(xLag,LFO,SK,a,b,Fmax,struct_name)

% Prints .beam file!
k_Beam = 7.5e7; C = 0.0;
print_Lagrangian_Beams(xLag,k_Beam,C,struct_name);

% Prints .target file!
k_Target = 1e6;
print_Lagrangian_Target_Pts(xLag,k_Target,struct_name);

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

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag);

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

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

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

    fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints Vertex points to a file called rubberband.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name)

    N = length(xLag); % Total Number of Lagrangian Pts
    num = 1;          % Number of target points on each end

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

    fprintf(target_fid, '%d\n', 4*num );

    %Loops over all Lagrangian Pts.
    %for s = 1:N

    %Left Bottom Target Points
    for s=1:num
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    %Right Bottom Target Points
    for s=(N/2)-(num-1):N/2
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    %Left Top Target Points
    for s=(N/2+1):(N/2+1)+(num-1)
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    %Right Top Target Points
    for s=N-(num-1):N
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    %end

    fclose(target_fid);

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

function print_Lagrangian_Beams(xLag,k_Beam,C,struct_name)

    % k_Beam: beam stiffness
    % C: beam curvature

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

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

    fprintf(beam_fid, '%d\n', N - 4 );

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

    %BEAMS BETWEEN VERTICES
    for s = 2:N-1
            if  s <= N/2-1
                % Bottom of tube
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C);
            elseif ( ( s >= N/2+2 ) && ( s <= N-1 ) )
                % Top of Tube
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1,   k_Beam, C);
            end
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints MUSCLE points to a file called "struct_name".muscle
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Muscles(xLag,LFO,SK,a,b,Fmax,struct_name)

    N = length(xLag); %Number of Lagrangian Pts. Total

    muscle_fid = fopen([struct_name '.muscle'], 'w');

    fprintf(muscle_fid, '%d\n', N/2-2 );

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

    %MUSCLES BETWEEN VERTICES
    for s = 2:N/2-1
            fprintf(muscle_fid, '%d %d %1.16e %1.16e %1.16e %1.16e %1.16e\n', s, s+N/2, LFO, SK, a,b,Fmax);
    end
    fclose(muscle_fid);

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

function print_Lagrangian_Springs(xLag,k_Spring,ds_Rest,struct_name)

    N = length(xLag); %Number of Lagrangian Pts. Total

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

    fprintf(spring_fid, '%d\n', N-2 );

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

    %SPRINGS BETWEEN VERTICES
    for s = 1:N
            if s < N/2
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            elseif ( ( s >= N/2+1 ) && ( s < N ) )
                %Case s=N
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1,   k_Spring, ds_Rest);
            end
    end
    fclose(spring_fid);

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

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(ds,L,d,Lx,Ly)

% The immsersed structure is a straight heart-tube %
x1 = [-L/2:ds:L/2 L/2];             % Constructs x-Values for bottom of tube
y1 = -d/2*ones(1,length(x1)); % Constructs y-Values for bottom of tube

x2 = x1;                      % Constructs x-Values for top of tube
y2 = -y1;                     % Constructs y-Values for top of tube

x1 = x1 + Lx/2;               % Shift into correct box
x2 = x2 + Lx/2;                % Shift into correct box

y1 = y1 + Ly/2;               % Shift into correct box
y2 = y2 + Ly/2;               % Shift into correct box

xLag = [x1 x2];
yLag = [y1 y2];


function y = vl_nnreshape(x, shape, varargin)
% VL_NNRESHAPE Feature reshaping
%   Y = VL_NNRESHAPE(X, SHAPE) reshpaes the input data X to have
%   the dimensions specified by SHAPE. X is a SINGLE array of
%   dimension H x W x D x N where (H,W) are the height and width of
%   the map stack, D is the image depth (number of feature channels)
%   and N the number of of images in the stack. SHAPE is a 1 x 3 cell
%   array, the contents of which are passed in order to the MATLAB
%   reshape function. As a consequence, `[]` an be used to specify a
%   dimension which should be computed from the other two. The batch size
%   (the fourth dimension of the input) is left unchanged by this
%   reshaping operation.
%
%   Alternatively, SHAPE can be specified in the "caffe style", as an
%   array, using `0` to indicate that a dimension should be preserved and
%   `-1` to indicate a dimension that should be computed from the others
%   (this serves the same role as [] in the standard MATLAB convention).
%
%   Example:
%       Inputs: X with shape [100 100 3 5] and SHAPE = { 100 3 [] }
%       will produce an output Y with shape [100 3 100 5]
%
%   DZDX = VL_NNRESHAPE(X, SHAPE, DZDY) computes the derivatives of the
%   block projected onto DZDY. DZDX and DZDY have the same dimensions
%   as X and Y respectively.
%
% Copyright (C) 2017 Samuel Albanie and Andrea Vedaldi.
% Licensed under The MIT License [see LICENSE.md for details]

  [~, dzdy] = vl_argparsepos(struct(), varargin) ;

  if isnumeric(shape) % apply caffe style conventions if needed
    shape_ = num2cell(shape) ;
    if numel(shape_) == 2, shape_{3} = [] ; end
    k = find(shape == -1) ; if k, shape_{k} = [] ; end
    k = find(shape == 0) ;
    if k, rep = arrayfun(@(i) {size(x,i)}, k) ; shape_(k) = rep ; end
    shape = shape_ ;
  end

  batchSize = size(x, 4);

  if isempty(dzdy)
    y = reshape(x, shape{1}, shape{2}, shape{3}, batchSize) ;
  else
    y = reshape(dzdy{1}, size(x)) ;
  end


function varargout = WFTKernel(varargin)
% Function:             Windowed Fourier transofmr (Kernel)
% Initially Developed:  Dr Qian Kemao (16 May 2009)
% Last modified:        Dr Qian Kemao (17 May 2009)
% Version:              1.0
% Copyrights:           All rights reserved.
% Contact:              mkmqian@ntu.edu.sg (Dr Qian Kemao)

%WFTKERNEL M-file for WFTKernel.fig
%      WFTKERNEL, by itself, creates a new WFTKERNEL or raises the existing
%      singleton*.
%
%      H = WFTKERNEL returns the handle to a new WFTKERNEL or the handle to
%      the existing singleton*.
%
%      WFTKERNEL('Property','Value',...) creates a new WFTKERNEL using the
%      given property value pairs. Unrecognized properties are passed via
%      varargin to WFTKernel_OpeningFcn.  This calling syntax produces a
%      warning when there is an existing singleton*.
%
%      WFTKERNEL('CALLBACK') and WFTKERNEL('CALLBACK',hObject,...) call the
%      local function named CALLBACK in WFTKERNEL.M with the given input
%      arguments.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help WFTKernel

% Last Modified by GUIDE v2.5 16-May-2009 10:09:25

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @WFTKernel_OpeningFcn, ...
                   'gui_OutputFcn',  @WFTKernel_OutputFcn, ...
                   'gui_LayoutFcn',  [], ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
   gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before WFTKernel is made visible.
function WFTKernel_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   unrecognized PropertyName/PropertyValue pairs from the
%            command line (see VARARGIN)

% Choose default command line output for WFTKernel
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);

% UIWAIT makes WFTKernel wait for user response (see UIRESUME)
% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line.
function varargout = WFTKernel_OutputFcn(hObject, eventdata, handles)
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;

function edit_sigmax_Callback(hObject, eventdata, handles)
% hObject    handle to edit_sigmax (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_sigmax as text
%        str2double(get(hObject,'String')) returns contents of edit_sigmax as a double

% --- Executes during object creation, after setting all properties.
function edit_sigmax_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_sigmax (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit_wxl_Callback(hObject, eventdata, handles)
% hObject    handle to edit_wxl (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_wxl as text
%        str2double(get(hObject,'String')) returns contents of edit_wxl as a double

% --- Executes during object creation, after setting all properties.
function edit_wxl_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_wxl (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit_wxi_Callback(hObject, eventdata, handles)
% hObject    handle to edit_wxi (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_wxi as text
%        str2double(get(hObject,'String')) returns contents of edit_wxi as a double

% --- Executes during object creation, after setting all properties.
function edit_wxi_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_wxi (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit_wxh_Callback(hObject, eventdata, handles)
% hObject    handle to edit_wxh (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_wxh as text
%        str2double(get(hObject,'String')) returns contents of edit_wxh as a double

% --- Executes during object creation, after setting all properties.
function edit_wxh_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_wxh (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit_sigmay_Callback(hObject, eventdata, handles)
% hObject    handle to edit_sigmay (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_sigmay as text
%        str2double(get(hObject,'String')) returns contents of edit_sigmay as a double

% --- Executes during object creation, after setting all properties.
function edit_sigmay_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_sigmay (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit_wyl_Callback(hObject, eventdata, handles)
% hObject    handle to edit_wyl (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_wyl as text
%        str2double(get(hObject,'String')) returns contents of edit_wyl as a double

% --- Executes during object creation, after setting all properties.
function edit_wyl_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_wyl (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit_wyi_Callback(hObject, eventdata, handles)
% hObject    handle to edit_wyi (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_wyi as text
%        str2double(get(hObject,'String')) returns contents of edit_wyi as a double

% --- Executes during object creation, after setting all properties.
function edit_wyi_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_wyi (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit_wyh_Callback(hObject, eventdata, handles)
% hObject    handle to edit_wyh (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_wyh as text
%        str2double(get(hObject,'String')) returns contents of edit_wyh as a double

% --- Executes during object creation, after setting all properties.
function edit_wyh_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_wyh (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit_thr_Callback(hObject, eventdata, handles)
% hObject    handle to edit_thr (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_thr as text
%        str2double(get(hObject,'String')) returns contents of edit_thr as a double

% --- Executes during object creation, after setting all properties.
function edit_thr_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_thr (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

% --- Executes on button press in push_WFTRun.
function push_WFTRun_Callback(hObject, eventdata, handles)
% hObject    handle to push_WFTRun (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
load result
H=findobj('Tag','radiobutton_wff');   val=get(H,'Value');
if val==1
    g.AlgorithmType='wff';
else
    g.AlgorithmType='wfr';
end
H=findobj('Tag','edit_sigmax'); sigmax=str2num(get(H,'String'));
H=findobj('Tag','edit_sigmax'); sigmax=str2num(get(H,'String'));
H=findobj('Tag','edit_wxl');    wxl=str2num(get(H,'String'));
H=findobj('Tag','edit_wxi');    wxi=str2num(get(H,'String'));
H=findobj('Tag','edit_wxh');    wxh=str2num(get(H,'String'));
H=findobj('Tag','edit_sigmay'); sigmay=str2num(get(H,'String'));
H=findobj('Tag','edit_wyl');    wyl=str2num(get(H,'String'));
H=findobj('Tag','edit_wyi');    wyi=str2num(get(H,'String'));
H=findobj('Tag','edit_wyh');    wyh=str2num(get(H,'String'));
H=findobj('Tag','edit_thr');    thr=str2num(get(H,'String'));
g0=wft2fw(g.AlgorithmType,g.f,sigmax,wxl,wxi,wxh,sigmay,wyl,wyi,wyh,thr);
if strcmp(g.AlgorithmType,'wff')
    g.filtered=g0.filtered;
else
    g.wx=g0.wx;
    g.wy=g0.wy;
    g.r=g0.r;
    g.phase=g0.phase;
    g.filtered=g.r.*exp(sqrt(-1)*g.phase);
end
save result g;
H=findobj('Name','Windowed Fourier Transform (WFT)');figure(H);imagesc(angle(g.filtered));
H=findobj('Name','WFT Kernel');close(H);


% LFDecodeLensletImageSimple - decodes a 2D lenslet image into a 4D light field, called by LFUtilDecodeLytroFolder
%
% Usage:
%
%   [LF, LFWeight, DecodeOptions, DebayerLensletImage, CorrectedLensletImage] = ...
%      LFDecodeLensletImageSimple( LensletImage, WhiteImage, LensletGridModel, DecodeOptions )
%
% This function follows the simple decode process described in:
% D. G. Dansereau, O. Pizarro, and S. B. Williams, "Decoding, calibration and rectification for
% lenslet-based plenoptic cameras," in Computer Vision and Pattern Recognition (CVPR), IEEE
% Conference on. IEEE, Jun 2013.
%
% This involves demosaicing, devignetting, transforming and slicing the input lenslet image to
% yield a 4D structure. More sophisticated approaches exist which combine steps into joint
% solutions, and they generally yield superior results, particularly near the edges of lenslets.
% The approach taken here was chosen for its simplicity and flexibility.
%
% Input LensletImage is a raw lenslet iamge as found within Lytro's LFP picture files. Though
% this function is written with Lytro imagery in mind, it should be possible to adapt it for use
% with other lenslet-based cameras. LensletImage is ideally of type uint16.
%
% Input WhiteImage is a white image as found within Lytro's calibration data. A white image is an
% image taken through a diffuser, and is useful for removing vignetting (darkening near edges of
% images) and for locating lenslet centers. The white image should be taken under the same zoom and
% focus settings as the light field. In the case of Lytro imagery, the helper functions
% LFUtilProcessWhiteImages and LFSelectFromDatabase are provided to assist in forming a list of
% available white images, and selecting the appropriate image based on zoom and focus settings.
%
% Input LensletGridModel is a model of the lenslet grid, as estimated, for example, using
% LFBuildLensletGridModel -- see that function for more on the required structure.
%
% Optional Input DecodeOptions is a structure containing:
%          [Optional] ResampMethod : 'fast'(default) or 'triangulation', the latter is slower
%          [Optional]   Precision : 'single'(default) or 'double'
%          [Optional] LevelLimits : a two-element vector defining the black and white levels
%
% Output LF is a 5D array of size [Nj,Ni,Nl,Nk,3]. See [1] and the documentation accompanying this
% toolbox for a brief description of the light field structure.
%
% Optional output LFWeight is of size [Nj,Ni,Nl,Nk]. LFWeight contains a confidence measure
% suitable for use in filtering applications that accept a weight parameter. This parameter is kept
% separate from LF rather than building in as a fourth channel in order to allow an optimization:
% when the parameter is not requested, it is not computed, saving significant processing time and
% memory.
%
% Optional output DebayerLensletImage is useful for inspecting intermediary results. The debayered
% lenslet image is the result of devignetting and debayering, with no further processing. Omitting
% this output variable saves memory.
%
% Optional output CorrectedLensletImage is useful for inspecting intermediary results. The
% corrected lenslet image has been rotated and scaled such that lenslet centers lie on straight lines, and
% every lenslet center lies on an integer pixel spacing. See [1] for more detail. Omitting
% this output variable saves memory.
%
% A more direct version of this function LFDecodeLensletImageDirect does not generate the
% intermediate corrected lenslet image.
%
% User guide: LFToolbox.pdf
% See also:  LFLytroDecodeImage, LFUtilDecodeLytroFolder, LFDecodeLensletImageDirect

% Copyright (c) 2013-2020 Donald G. Dansereau

function [LF, LFWeight, DecodeOptions, DebayerLensletImage, CorrectedLensletImage] = ...
    LFDecodeLensletImageSimple( LensletImage, WhiteImage, LensletGridModel, DecodeOptions )

%---Defaults---
 % LevelLimits defaults are a failsafe, should be passed in / come from the white image metadata
DecodeOptions = LFDefaultField( 'DecodeOptions', 'LevelLimits', [min(WhiteImage(:)), max(WhiteImage(:))] );
DecodeOptions = LFDefaultField( 'DecodeOptions', 'ResampMethod', 'fast' ); %'fast', 'triangulation'
DecodeOptions = LFDefaultField( 'DecodeOptions', 'Precision', 'single' );
DecodeOptions = LFDefaultField( 'DecodeOptions', 'DoDehex', true );
DecodeOptions = LFDefaultField( 'DecodeOptions', 'DoSquareST', true );

%---Rescale image values, remove black level---
DecodeOptions.LevelLimits = cast(DecodeOptions.LevelLimits, DecodeOptions.Precision);
BlackLevel = DecodeOptions.LevelLimits(1);
WhiteLevel = DecodeOptions.LevelLimits(2);
WhiteImage = cast(WhiteImage, DecodeOptions.Precision);
WhiteImage = (WhiteImage - BlackLevel) ./ (WhiteLevel - BlackLevel);

LensletImage = cast(LensletImage, DecodeOptions.Precision);
LensletImage = (LensletImage - BlackLevel) ./ (WhiteLevel - BlackLevel);
LensletImage = LensletImage ./ WhiteImage; % Devignette
% Clip -- this is aggressive and throws away bright areas; there is a potential for an HDR approach here
LensletImage = min(1, max(0, LensletImage));

if( nargout < 2 )
    clear WhiteImage
end

%---Demosaic---
% This uses Matlab's demosaic, which is "gradient compensated". This likely has implications near
% the edges of lenslet images, where the contrast is due to vignetting / aperture shape, and is not
% a desired part of the image
LensletImage = cast(LensletImage.*double(intmax('uint16')), 'uint16');
LensletImage = demosaic(LensletImage, DecodeOptions.DemosaicOrder);
LensletImage = cast(LensletImage, DecodeOptions.Precision);
LensletImage = LensletImage ./  double(intmax('uint16'));
DecodeOptions.NColChans = 3;

if( nargout >= 2 )
    DecodeOptions.NWeightChans = 1;
else
    DecodeOptions.NWeightChans = 0;
end

if( nargout > 3 )
    DebayerLensletImage = LensletImage;
end

%---Tranform to an integer-spaced grid---
fprintf('\nAligning image to lenslet array...');
InputSpacing = [LensletGridModel.HSpacing, LensletGridModel.VSpacing];
NewLensletSpacing = ceil(InputSpacing);
% Force even so hex shift is a whole pixel multiple
NewLensletSpacing = ceil(NewLensletSpacing/2)*2;
XformScale = NewLensletSpacing ./ InputSpacing;  % Notice the resized image will not be square

NewOffset = [LensletGridModel.HOffset, LensletGridModel.VOffset] .* XformScale;
RoundedOffset = round(NewOffset);
XformTrans =  RoundedOffset-NewOffset;

NewLensletGridModel = struct('HSpacing',NewLensletSpacing(1), 'VSpacing',NewLensletSpacing(2), ...
    'HOffset',RoundedOffset(1), 'VOffset',RoundedOffset(2), 'Rot',0, ...
    'UMax', LensletGridModel.UMax, 'VMax', LensletGridModel.VMax, 'Orientation', LensletGridModel.Orientation, ...
    'FirstPosShiftRow', LensletGridModel.FirstPosShiftRow);

%---Fix image rotation and scale---
RRot = LFRotz( LensletGridModel.Rot );

RScale = eye(3);
RScale(1,1) = XformScale(1);
RScale(2,2) = XformScale(2);
DecodeOptions.OutputScale(1:2) = XformScale;
DecodeOptions.OutputScale(3:4) = [1,2/sqrt(3)];  % hex sampling

RTrans = eye(3);
RTrans(end,1:2) = XformTrans;

% The following rotation can rotate parts of the lenslet image out of frame.
% todo[optimization]: attempt to keep these regions, offer greater user-control of what's kept
FixAll = maketform('affine', RRot*RScale*RTrans);
NewSize = size(LensletImage(:,:,1)) .* XformScale(2:-1:1);
LensletImage = imtransform( LensletImage, FixAll, 'YData',[1 NewSize(1)], 'XData',[1 NewSize(2)]);
if( nargout >= 2 )
    WhiteImage = imtransform( WhiteImage, FixAll, 'YData',[1 NewSize(1)], 'XData',[1 NewSize(2)]);
end
if( nargout >= 4 )
    CorrectedLensletImage = LensletImage;
end

LF = SliceXYImage( NewLensletGridModel, LensletImage, WhiteImage, DecodeOptions );
clear WhiteImage LensletImage

%---Correct for hex grid and resize to square u,v pixels---
LFSize = size(LF);
HexAspect = 2/sqrt(3);
switch( DecodeOptions.ResampMethod )
    case 'fast'
        fprintf('\nResampling (1D approximation) to square u,v pixels');
        NewUVec = 0:1/HexAspect:(size(LF,4)+1);  % overshoot then trim
        NewUVec = NewUVec(1:ceil(LFSize(4)*HexAspect));
        OrigUSize = size(LF,4);
        LFSize(4) = length(NewUVec);
        %---Allocate dest and copy orig LF into it (memory saving vs. keeping both separately)---
        LF2 = zeros(LFSize, DecodeOptions.Precision);
        LF2(:,:,:,1:OrigUSize,:) = LF;
        LF = LF2;
        clear LF2

        if( DecodeOptions.DoDehex )
            ShiftUVec = -0.5+NewUVec;
            fprintf(' and removing hex sampling...');
        else
            ShiftUVec = NewUVec;
            fprintf('...');
        end
        for( ColChan = 1:size(LF,5) )
            CurUVec = ShiftUVec;
            for( RowIter = 1:2 )
                RowIdx = mod(NewLensletGridModel.FirstPosShiftRow + RowIter, 2) + 1;
                ShiftRows = squeeze(LF(:,:,RowIdx:2:end,1:OrigUSize, ColChan));
                SliceSize = size(ShiftRows);
                SliceSize(4) = length(NewUVec);
                ShiftRows = reshape(ShiftRows, [size(ShiftRows,1)*size(ShiftRows,2)*size(ShiftRows,3), size(ShiftRows,4)]);
                ShiftRows = interp1( (0:size(ShiftRows,2)-1)', ShiftRows', CurUVec' )';
                ShiftRows(isnan(ShiftRows)) = 0;
                LF(:,:,RowIdx:2:end,:,ColChan) = reshape(ShiftRows,SliceSize);
                CurUVec = NewUVec;
            end
        end
        clear ShiftRows
        DecodeOptions.OutputScale(3) = DecodeOptions.OutputScale(3) * HexAspect;

    case 'triangulation'
        fprintf('\nResampling (triangulation) to square u,v pixels');
        OldVVec = (0:size(LF,3)-1);
        OldUVec = (0:size(LF,4)-1) * HexAspect;

        NewUVec = (0:ceil(LFSize(4)*HexAspect)-1);
        NewVVec = (0:LFSize(3)-1);
        LFSize(4) = length(NewUVec);
        LF2 = zeros(LFSize, DecodeOptions.Precision);

        [Oldvv,Olduu] = ndgrid(OldVVec,OldUVec);
        [Newvv,Newuu] = ndgrid(NewVVec,NewUVec);
        if( DecodeOptions.DoDehex )
            fprintf(' and removing hex sampling...');
            FirstShiftRow = NewLensletGridModel.FirstPosShiftRow;
            Olduu(FirstShiftRow:2:end,:) = Olduu(FirstShiftRow:2:end,:) + HexAspect/2;
        else
            fprintf('...');
        end

        DT = delaunayTriangulation( Olduu(:), Oldvv(:) );  % use DelaunayTri in older Matlab versions
        [ti,bc] = pointLocation(DT, Newuu(:), Newvv(:));
        ti(isnan(ti)) = 1;

        for( ColChan = 1:size(LF,5) )
            fprintf('.');
            for( tidx= 1:LFSize(1) )
                for( sidx= 1:LFSize(2) )
                    CurUVSlice = squeeze(LF(tidx,sidx,:,:,ColChan));
                    triVals = CurUVSlice(DT(ti,:));
                    CurUVSlice = dot(bc',triVals')';
                    CurUVSlice = reshape(CurUVSlice, [length(NewVVec),length(NewUVec)]);

                    CurUVSlice(isnan(CurUVSlice)) = 0;
                    LF2(tidx,sidx, :,:, ColChan) = CurUVSlice;
                end
            end
        end
        LF = LF2;
        clear LF2
        DecodeOptions.OutputScale(3) = DecodeOptions.OutputScale(3) * HexAspect;

    otherwise
        fprintf('\nNo valid dehex / resampling selected\n');
end

%---Resize to square s,t pixels---
% Assumes only a very slight resampling is required, resulting in an identically-sized output light field
if( DecodeOptions.DoSquareST )
    fprintf('\nResizing to square s,t pixels using 1D linear interp...');

    ResizeScale = DecodeOptions.OutputScale(1)/DecodeOptions.OutputScale(2);
    ResizeDim1 = 1;
    ResizeDim2 = 2;
    if( ResizeScale < 1 )
        ResizeScale = 1/ResizeScale;
        ResizeDim1 = 2;
        ResizeDim2 = 1;
    end

    OrigSize = size(LF, ResizeDim1);
    OrigVec = floor((-(OrigSize-1)/2):((OrigSize-1)/2));
    NewVec = OrigVec ./ ResizeScale;

    OrigDims = [1:ResizeDim1-1, ResizeDim1+1:5];

    UBlkSize = 32;
    USize = size(LF,4);
    LF = permute(LF,[ResizeDim1, OrigDims]);
    for( UStart = 1:UBlkSize:USize )
        UStop = UStart + UBlkSize - 1;
        UStop = min(UStop, USize);
        LF(:,:,:,UStart:UStop,:) = interp1(OrigVec, LF(:,:,:,UStart:UStop,:), NewVec);
        fprintf('.');
    end
    LF = ipermute(LF,[ResizeDim1, OrigDims]);
    LF(isnan(LF)) = 0;

    DecodeOptions.OutputScale(ResizeDim2) = DecodeOptions.OutputScale(ResizeDim2) * ResizeScale;
end

%---Trim s,t---
LF = LF(2:end-1,2:end-1, :,:, :);

%---Slice out LFWeight if it was requested---
if( nargout >= 2 )
    LFWeight = LF(:,:,:,:,end);
    LFWeight = LFWeight./max(LFWeight(:));
    LF = LF(:,:,:,:,1:end-1);
end

end

%------------------------------------------------------------------------------------------------------
function LF = SliceXYImage( LensletGridModel, LensletImage, WhiteImage, DecodeOptions )
% todo[optimization]: The SliceIdx and ValidIdx variables could be precomputed

fprintf('\nSlicing lenslets into LF...');

USize = LensletGridModel.UMax;
VSize = LensletGridModel.VMax;
MaxSpacing = max(LensletGridModel.HSpacing, LensletGridModel.VSpacing);  % Enforce square output in s,t
SSize = MaxSpacing + 1; % force odd for centered middle pixel -- H,VSpacing are even, so +1 is odd
TSize = MaxSpacing + 1;

LF = zeros(TSize, SSize, VSize, USize, DecodeOptions.NColChans + DecodeOptions.NWeightChans, DecodeOptions.Precision);

TVec = cast(floor((-(TSize-1)/2):((TSize-1)/2)), 'int16');
SVec = cast(floor((-(SSize-1)/2):((SSize-1)/2)), 'int16');
VVec = cast(0:VSize-1, 'int16');
UBlkSize = 32;
for( UStart = 0:UBlkSize:USize-1 )  % note zero-based indexing
    UStop = UStart + UBlkSize - 1;
    UStop = min(UStop, USize-1);
    UVec = cast(UStart:UStop, 'int16');

    [tt,ss,vv,uu] = ndgrid( TVec, SVec, VVec, UVec );

    %---Build indices into 2D image---
    LFSliceIdxX = LensletGridModel.HOffset + uu.*LensletGridModel.HSpacing + ss;
    LFSliceIdxY = LensletGridModel.VOffset + vv.*LensletGridModel.VSpacing + tt;

    HexShiftStart = LensletGridModel.FirstPosShiftRow;
    LFSliceIdxX(:,:,HexShiftStart:2:end,:) = LFSliceIdxX(:,:,HexShiftStart:2:end,:) + LensletGridModel.HSpacing/2;

    %---Lenslet mask in s,t and clip at image edges---
    CurSTAspect = DecodeOptions.OutputScale(1)/DecodeOptions.OutputScale(2);
    R = sqrt((cast(tt,DecodeOptions.Precision)*CurSTAspect).^2 + cast(ss,DecodeOptions.Precision).^2);
    ValidIdx = find(R < LensletGridModel.HSpacing/2 & ...
        LFSliceIdxX >= 1 & LFSliceIdxY >= 1 & LFSliceIdxX <= size(LensletImage,2) & LFSliceIdxY <= size(LensletImage,1) );

    %--clip -- the interp'd values get ignored via ValidIdx--
    LFSliceIdxX = max(1, min(size(LensletImage,2), LFSliceIdxX ));
    LFSliceIdxY = max(1, min(size(LensletImage,1), LFSliceIdxY ));

    %---
    LFSliceIdx = sub2ind(size(LensletImage), cast(LFSliceIdxY,'int32'), ...
        cast(LFSliceIdxX,'int32'), ones(size(LFSliceIdxX),'int32'));

    tt = tt - min(tt(:)) + 1;
    ss = ss - min(ss(:)) + 1;
    vv = vv - min(vv(:)) + 1;
    uu = uu - min(uu(:)) + 1 + UStart;
    LFOutSliceIdx = sub2ind(size(LF), cast(tt,'int32'), cast(ss,'int32'), ...
        cast(vv,'int32'),cast(uu,'int32'), ones(size(ss),'int32'));

    %---
    for( ColChan = 1:DecodeOptions.NColChans )
        LF(LFOutSliceIdx(ValidIdx) + numel(LF(:,:,:,:,1)).*(ColChan-1)) = ...
            LensletImage( LFSliceIdx(ValidIdx) + numel(LensletImage(:,:,1)).*(ColChan-1) );
    end
    if( DecodeOptions.NWeightChans ~= 0 )
        LF(LFOutSliceIdx(ValidIdx) + numel(LF(:,:,:,:,1)).*(DecodeOptions.NColChans)) = ...
            WhiteImage( LFSliceIdx(ValidIdx) );
    end
    fprintf('.');
end
end


function cur_shift = WardGetExpShift(img1, img2, ward_percentile, shift_bits)
%
%
%       cur_shift = WardGetExpShift(img1, img2, ward_percentile, shift_bits)
%
%       This function computes the Ward's MTB.
%
%       Input:
%           -img1: the target image
%           -img2: the image that needs to be aligned to img1
%
%       Output:
%           -cur_shift: shifting vector for aligning img2 into img1.
%
%     Copyright (C) 2012-15  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

if(~exist('ward_percentile', 'var'))
    ward_percentile = 0.5;
end

if(~exist('shift_bits', 'var'))
    shift_bits = 6;
end

cur_shift = zeros(2, 1);
shift_ret = zeros(2, 1);

while(shift_bits > 0)
    %computing MTB
    sml_img1 = imresize(img1, 2^(-shift_bits), 'bilinear');
    sml_img2 = imresize(img2, 2^(-shift_bits), 'bilinear');

    [tb1, eb1] = WardComputeThreshold(sml_img1, ward_percentile);
    [tb2, eb2] = WardComputeThreshold(sml_img2, ward_percentile);

    min_err = size(sml_img1, 1) * size(sml_img1, 2);

    tb1 = logical(tb1);
    eb1 = logical(eb1);

    for i=-1:1
        for j=-1:1
            xs = cur_shift(1) + i;
            ys = cur_shift(2) + j;

            shifted_tb2 = logical(imShift(tb2, [xs, ys]));
            shifted_eb2 = logical(imShift(eb2, [xs, ys]));

            diff_b = bitxor(tb1, shifted_tb2);
            diff_b = diff_b & eb1;
            diff_b = diff_b & shifted_eb2;

            err = sum(diff_b(:));

            if (err < min_err)
                shift_ret = [xs; ys];
                min_err = err;
            end
        end
    end

    shift_bits = shift_bits - 1;
    cur_shift  = shift_ret * 2;
end

end


function test_suite = test_angle2Points
%TESTCLIPLINE  One-line description here, please.
%   output = testAngle2Points(input)
%
%   Example
%   testAngle2Points
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2009-04-22,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.

test_suite = functiontests(localfunctions);

function testHoriz(testCase) %#ok<*DEFNU>
% all points inside window, possibly touching edges

p1 = [0 0];
p2 = [10 0];
angle = angle2Points(p1, p2);
testCase.assertEqual(0, angle, 'AbsTol', .01);

angle = angle2Points(p2, p1);
testCase.assertEqual(pi, angle, 'AbsTol', .01);

function testVert(testCase)
% all points inside window, possibly touching edges

p1 = [0 0];
p2 = [0 10];
angle = angle2Points(p1, p2);
testCase.assertEqual(pi/2, angle, 'AbsTol', .01);

angle = angle2Points(p2, p1);
testCase.assertEqual(3*pi/2, angle, 'AbsTol', .01);

function testMultiMulti(testCase)
% all points inside window, possibly touching edges

p1 = [0 0;0 0;0 0;0 0];
p2 = [10 0;10 10;0 10;-10 10];
angle = angle2Points(p1, p2);
testCase.assertEqual(size(p1, 1), size(angle, 1), 'AbsTol', .01);

res = [0;pi/4;pi/2;3*pi/4];
testCase.assertEqual(res, angle, 'AbsTol', .01);

function testOneMulti(testCase)
% all points inside window, possibly touching edges

p1 = [0 0];
p2 = [10 0;10 10;0 10;-10 10];
angle = angle2Points(p1, p2);
testCase.assertEqual(size(p2, 1), size(angle, 1), 'AbsTol', .01);

res = [0;pi/4;pi/2;3*pi/4];
testCase.assertEqual(res, angle, 'AbsTol', .01);


function d =  generate(a)

if a.seed>0 rand('seed',a.seed);   end;
x=(rand(a.l,a.n)-0.5)*2;
w=ones(1,a.n);
prem=a.n-min(a.relevant_n,a.n);
w(1:prem)=0;
y=sign(x*w');

d=data([get_name(a) ' '] ,x,y);


function [coordinate, rgb] = getPointsFromRGBD(RGBDframe)

X = reshape(RGBDframe(:,:,[1 2 3 5 6 4]),[],6)';
valid = X(6,:) > 0;

coordinate = X(4:6,valid);
rgb = X(1:3,valid);


function [ Psi ] = buildPsi2( options, psi, configuration )
%BUILDPSI functions creates vector \Psi(I, S) for given configuration
%   Detailed explanation goes here
%
% INPUT
%   options         ...     structure holds info about components (size, count) and S (statistics of component position)
%   psi             ...     cell Mx1 contains matrices \Psi^q(I, S_i)
%   configuration   ...     matrix 2xM [x0, ..., x_{M-1}; y_0, ..., y_{M-1}] with positions of components
%
% OUTPUT
%   Psi     ...     vector [\Psi_0^q(I, S_0); ... ; \Psi_{M-1}^q(I, S_{M-1}); g(S_0xS_1); ... ; g(S_0xS_{M-1})];
%
% 10-08-10 Michal Uricar
% 11-07-11 Michal Uricar, corners dataset modificatoin

    indices = zeros(1, options.M);

    Psi = [];

    for i = 1 : options.M
        psi_si = psi{i};
        P = configuration(:, i) - [options.S(1, i) - 1; options.S(2, i) - 1];
        siz = [options.S(4, i) - options.S(2, i) + 1; options.S(3, i) - options.S(1, i) + 1];
        indices(i) = (P(1) - 1)*siz(1) + P(2);
        Psi = [Psi; psi_si(:, indices(i))];
    end;

    % g
    %for i = 2 : options.M - 2
    for i = 2 : options.M - 3
        dxdy = configuration(:, i) - configuration(:, 1);
        g = [dxdy; dxdy.^2];
        Psi = [Psi; g];
    end;
    % g5
    dxdy = configuration(:, 6) - configuration(:, 2);
    g = [dxdy; dxdy.^2];
    Psi = [Psi; g];
    % g6
    dxdy = configuration(:, 7) - configuration(:, 3);
    g = [dxdy; dxdy.^2];
    Psi = [Psi; g];
    % g7
    dxdy = configuration(:, 8) - configuration(:, 1);
    g = [dxdy; dxdy.^2];
    Psi = [Psi; g];

end