% POP_CHANCENTER - recenter cartesian X,Y,Z channel coordinates
%
% Usage:
%    >> chanlocs = pop_chancenter(chanlocs); % pop up interactive window
%    >> [chanlocs centerloc] = pop_chancenter(chanlocs, center, omitchan);
%
% Inputs:
%    chanlocs  = eeglab channel location structure (see READLOCS)
%    center    = [X Y Z] known center different from [0 0 0]
%                [] will optimize the center location according
%                to the best sphere. Default is [0 0 0].
%    omitchan  = indices of channel to omit when computing center
%
% Outputs:
%    chanlocs  = updated channel location structure
%    centerloc = 3-D location of the new center (in the old coordinate
%                frame).
%
% Author: Arnaud Delorme, SCCN/INC/UCSD, La Jolla, Feb 2004
%
% See also: CHANCENTER, SPHERROR, CART2TOPO

% Copyright (C) 2004, Arnaud Delorme, SCCN/INC/UCSD, arno@salk.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function [ chanlocs, newcenter, com] = pop_chancenter( chanlocs, center, omitchans)

optim = 0;

if nargin<1
    help pop_chancenter
    return;
end

com = '';
newcenter = [];
if nargin < 3
    omitchans = [];
end
if nargin < 2
    cb_browse = [ 'tmpchans = get(gcbf, ''userdata'');' ...
                  'set(findobj(gcbf, ''tag'', ''chans''), ''string'', ' ...
                  'int2str(pop_chansel( { tmpchans.labels } )));' ];
    cb_opt    = [ 'if get(gcbo, ''value''), ' ...
                  '    set(findobj(gcbf, ''tag'', ''center''), ''enable'', ''off'');' ...
                  'else,' ...
                  '    set(findobj(gcbf, ''tag'', ''center''), ''enable'', ''on'');' ...
                  'end;' ];
    geometry = { [1.3 0.28 1 1] [1] [1] [2 1] };
    uilist = { { 'Style', 'text',       'string', 'Optimize center location', 'fontweight', 'bold'   } ...
			   { 'Style', 'checkbox',   'value',  1  'callback' cb_opt } ...
               { 'Style', 'text',       'string', 'or specify center', 'fontweight', 'bold'  } ...
               { 'Style', 'edit',       'string', '0 0 0', 'tag' 'center' 'enable' 'off' } ...
               { } ...
			   { 'Style', 'text',       'string', 'Channel indices to ignore for best-sphere matching'  } ...
			   { 'Style', 'edit',       'string', '', 'tag', 'chans' } ...
			   { 'Style', 'pushbutton', 'string', 'Browse', 'callback', cb_browse } };

    results = inputgui( geometry, uilist, 'pophelp(''pop_chancenter'');', ...
                        'Convert channel locations -- pop_chancenter()', chanlocs );
	if isempty(results), return; end
	if results{1}
        center = [];
    else
        center  = eval( [ '[' results{2} ']' ] );
    end
    if ~isempty(results{3})
        omitchans =  eval( [ '[' results{3} ']' ] );
    end
end

% remove channels
% ---------------
c = setdiff_bc([1:length(chanlocs)], union(omitchans, find(cellfun('isempty', { chanlocs.theta }))));

% optimize center
% ---------------
[X, Y, Z, newcenter]= chancenter( [ chanlocs(c).X ]', [ chanlocs(c).Y ]', [ chanlocs(c).Z ]', center);
for index = 1:length(c)
    chanlocs(c(index)).X  = X(index);
    chanlocs(c(index)).Y  = Y(index);
    chanlocs(c(index)).Z  = Z(index);
end
disp('Note: automatically convert XYZ coordinates to spherical and polar');
chanlocs = convertlocs(chanlocs, 'cart2all');
if ~isempty(omitchans)
    disp('Important warning: the location of omitted channels has not been modified');
end
if nargout > 2
    com = sprintf('%s = pop_chancenter( %s, %s);', inputname(1), inputname(1), vararg2str({ center omitchans }));
end


clear all;
close all;
clc;

image = 'cow';
% image = 'bungee';
% image = 'man';

Aorg = imread([image '.png']);
Morg = imread([image '-mask.png']);

tic
A=patch_inpaint(Aorg,Morg);
toc

imshow(A)


classdef gammaSections < ODFSections

  properties
    gamma
    sR
    maxGamma
  end

  methods

    function oS = gammaSections(varargin)
      % gamma sections for ODF and orientation plotting
      %
      % Syntax
      %
      %   oS = gammaSections(cs1,cs2,'sections',5)
      %   oS = gammaSections(cs1,cs2,'gamma',(0:15:90)*degree)
      %
      % Input
      %  cs1, cs2 - @crystalSymmetry, @specimenSymmetry
      %
      % Options
      %  sections - number of sections
      %  gamma - explicite section values
      %

      oS = oS@ODFSections(varargin{:});

      % get fundamental plotting region
      [alpha,beta,oS.maxGamma] = fundamentalRegionEuler(oS.CS1,oS.CS2,varargin{:}); %#ok<*PROP>
      oS.sR = sphericalRegion('maxTheta',beta,'maxRho',alpha);

      % get sections
      nsec = get_option(varargin,'sections',6);
      oS.gamma = linspace(0,oS.maxGamma,nsec+1);
      oS.gamma(end) = [];
      oS.gamma = get_option(varargin,'gamma',oS.gamma,'double');

      oS.updateTol(oS.gamma);
    end

    function ori = makeGrid(oS,varargin)

      oS.plotGrid = plotS2Grid(oS.sR,varargin{:});
      oS.gridSize = (0:numel(oS.gamma)) * length(oS.plotGrid);
      alpha = repmat(oS.plotGrid.rho,[1,1,numel(oS.gamma)]);
      beta = repmat(oS.plotGrid.theta,[1,1,numel(oS.gamma)]);
      gamma = repmat(reshape(oS.gamma,1,1,[]),[size(oS.plotGrid) 1]);

      ori = orientation.byEuler(alpha,beta,gamma,'ZYZ',oS.CS,oS.SS);

    end

    function ori = quiverGrid(oS,varargin)

      maxbeta = oS.sR.thetaMax;
      maxalpha = oS.sR.rhoMax;
      res = get_option(varargin,'resolution',15*degree);

      [alpha,beta,gamma] = meshgrid(res/2:res:maxalpha,res/2:res:maxbeta,oS.gamma);

      ori = orientation.byEuler(alpha,beta,gamma,'ZYZ',oS.CS,oS.SS);

    end

    function n = numSections(oS)
      n = numel(oS.gamma);
    end

    function [S2Pos,secPos] = project(oS,ori,varargin)

      % maybe this can be done more efficiently
      ori = ori.symmetrise('proper').';
      [alpha,beta,gamma] = Euler(ori,'ZYZ'); %#ok<*PROPLC>

      secPos = oS.secList(gamma,oS.gamma);
      S2Pos = vector3d.byPolar(beta,alpha);

    end

    function ori = iproject(oS,alpha,beta,igamma)
      ori = orientation.byEuler(alpha,beta,oS.gamma(igamma),'ZYZ',oS.CS,oS.SS);
    end

    function h = plotSection(oS,ax,sec,v,data,varargin)

      % plot data
      h = plot(v,data{:},oS.sR,'TR',[int2str(oS.gamma(sec)./degree),'^\circ'],...
        'parent',ax,'projection','plain','xAxisDirection','east',...
        'xlabel','$\alpha$','ylabel','$\beta$','dynamicMarkerSize',...
        'zAxisDirection','intoPlane',varargin{:},'doNotDraw');

    end

    function h = quiverSection(oS,ax,sec,v,data,varargin)

      % translate rotational data into tangential data
      if iscell(data) && isa(data{1},'quaternion')
        [v2,sec2] = project(oS,data{1},'noSymmetry','preserveOrder');
        data{1} = v2 - v;
        data{1}(sec2 ~= sec)=NaN;
      end
      if check_option(varargin,'normalize'), data{1} = normalize(data{1}); end

      % plot data
      h = quiver(v,data{:},oS.sR,'TR',[int2str(oS.gamma(sec)./degree),'^\circ'],...
      'parent',ax,'projection','plain','xAxisDirection','east',...
        'xlabel','$\alpha$','ylabel','$\beta$',...
        'zAxisDirection','intoPlane',varargin{:},'doNotDraw');

    end

  end
end


function rhs=ch_rom_deim_rhs(tspan, a,dummy,P_NL,P_Psi,L)
N=P_Psi*a;
rhs=L*a + i*P_NL*((abs(N).^2).*N);


function Z = variable_replace(X,Y,W)

% Check so that Y is a simple unit variable
Ybase = getbase(Y);
Yvariables = getvariablesSORTED(Y);
Xbase = getbase(X);
Xvariables = getvariables(X);
[i,j,k] = find(Ybase);
if ~isequal(sort(i),1:length(i))
end
if ~isequal(sort(j),2:(length(i)+1))
end
if ~all(k == 1)
end

[mt,variabletype] = yalmip('monomtable');
% Linear, or at least linear in Y
if all(variabletype(Xvariables) == 0) %| all(sum(mt(any(mt(getvariables(X),getvariables(Y)),2),:),2)==1)
    % Simple linear replacement
    v = 1;
    v1 = [];
    v2 = [];
    i1 = [];
    i2 = [];
    for i = 1:length(Xvariables)
        XisinY = find(Xvariables(i) == Yvariables);
        if ~isempty(XisinY)
         %   v = [v;W(XisinY)];
            v1 = [v1 XisinY];
            i1 = [i1 i];
        else
         %   v = [v;recover(Xvariables(i))];
            v2 = [v2 Xvariables(i)];
            i2 = [i2 i];
        end
    end
    v = sparse(i1,ones(length(i1),1),W(v1),length(Xvariables),1);
    v = v + sparse(i2,ones(length(i2),1),recover(v2),length(Xvariables),1);

    Z = Xbase*[1;v];
    %Z = Xbase*[v];
    Z = reshape(Z,size(X,1),size(X,2));
else
    if nnz(mt(getvariables(X),getvariables(Y)))==0
        Z = X;
    else
        Z = nonlinearreplace(X,Y,W);
    end
    return
%    error('Nonlinear replacement not supported yet')
end

% This has not been tested (copied from variable_replace) so it is placed
% in a catch to be safe.
try
    Xvariables = getvariables(Z);
    extvar = yalmip('extvariables');
    Xext = find(ismember(Xvariables,extvar));
    if ~isempty(Xext)
        %We must dig down in extended operators to see if they use the replaced
        %set of variables
        for i = 1:length(Xext)
            extstruct = yalmip('extstruct',Xvariables(Xext(i)));
            anychanged = 0;
            for j = 1:length(extstruct.arg)
                if isa(extstruct.arg{j},'sdpvar')
                    XinY = find(ismembc(getvariables(extstruct.arg{j}),Yvariables));
                    if ~isempty(XinY)
                        anychanged = 1;
                        extstruct.arg{j} = replace(extstruct.arg{j},Y,W);
                    else
                    end
                end
            end
            if anychanged
                Zi = yalmip('define',extstruct.fcn,extstruct.arg{:});
                Xvariables(Xext(i)) = getvariables(Zi);
            end
        end
        % And now recover this sucker
        Z = struct(Z);
        Z.lmi_variables = Xvariables;
        % Messed up order (lmi_variables should be sorted)
        if any(diff(Z.lmi_variables)<0)
            [i,j]=sort(Z.lmi_variables);
            Z.basis = [Z.basis(:,1) Z.basis(:,j+1)];
            Z.lmi_variables = Z.lmi_variables(j);
        end
        Z = sdpvar(Z.dim(1),Z.dim(2),[],Z.lmi_variables,Z.basis);
    end
catch
end

function Yvariables = getvariablesSORTED(Y);
Y = Y(:);
for i = 1:length(Y)
    Yvariables(i) = getvariables(Y(i));
end


function [cmc,ind]=get_cmc_multi_cam(Y_gallery,Y_cam_gallery,Y_probe,Y_cam_probe,dist)
[~, ind]=sort(dist,2);
Y_result=Y_gallery(ind);
Y_cam_result=Y_cam_gallery(ind);
valid_probe_sample_count=0;
gallery_unique_count=length(unique(Y_gallery));
match_counter=zeros(1,gallery_unique_count);
probe_sample_count=length(Y_probe);
for i=1:probe_sample_count
    % remove gallery samples from the same camera of the probe
    Y_result_i=Y_result(i,:);
    Y_result_i(Y_cam_result(i,:)==Y_cam_probe(i))=[];
    % remove duplicated id
    Y_result_unique_i=unique(Y_result_i,'stable');
    % match for probe i
    match_i=(Y_result_unique_i==Y_probe(i));
    if sum(match_i)~=0 % if there is true matching in gallery
        valid_probe_sample_count=valid_probe_sample_count+1;
        for r=1:length(match_i)
            match_counter(r)=match_counter(r)+match_i(r);
        end
    end
end
rankk=match_counter/valid_probe_sample_count;
cmc=cumsum(rankk);
end


function irepistruct = irepifit(irepistruct)
% irepistruct = irepifit(irepistruct)
% Performs LMS fit.
%

s = irepistruct; % copy into s for easy handling

% s = irepitiming(s); Need to do this if fitting seq params

% Need to be able to cache so not recomputing every slice
% Need this way too for incremental
s = irepisynth(s);

s.X0 = s.yRead;

s.iexclude1 = [];
if(s.nexclude > 0)
  % Just skip the first n
  % s.iexclude1 = [1:s.nexclude]';

  % This skips time points according to a rule. If a given time
  % point was the last acquired slice, then the next time point
  % for that slice is skipped.
  %indSlice = find((s.EventSliceNo == s.sliceno | s.EventSliceNo < 0) & s.IsReadOut);
  %nthSliceAcq = s.AcqSliceNo(indSlice);
  %indLast = find(nthSliceAcq == s.nslices);
  %if(~isempty(indLast))
  %  indEx = indLast + 1;
  %  ok = find(indEx <= s.ntp);
  %  indEx = indEx(ok);
  %else
  %  indEx = [];
  %end
  %s.iexclude1 = indEx;

  % This skips time points of the first acquired slice after inversion
  indSlice = find(s.EventSliceNo == s.sliceno | s.EventSliceNo < 0);
  indROS = find(s.IsReadOut(indSlice));
  nthSliceAcq = s.AcqSliceNo(indSlice(indROS));
  s.iexclude1 = find(nthSliceAcq <= s.nexclude);
  %s.iexclude1 = find(nthSliceAcq > s.nexclude);
end
s.iexclude2 = [];
if(s.nminexclude > 0)
  [ysort isort] = sort(s.y);
  s.iexclude2 = sort(isort(1:s.nminexclude));
end

if(~isempty(s.iexclude1) & isempty(s.iexclude2))
  s.iexclude = s.iexclude1;
elseif(isempty(s.iexclude1) & ~isempty(s.iexclude2))
  s.iexclude = s.iexclude2;
else
  s.iexclude = unique([s.iexclude1; s.iexclude2]);
end
s.indkeep = setdiff([1:s.ntp],s.iexclude);

% remove the mean
%s.X0 = s.X0-repmat(mean(s.X0),[size(s.X0,1) 1]);
%s.y = s.y-mean(s.y);

s.yFit = s.y(s.indkeep,:);
s.X = s.X0(s.indkeep,:);

s.tFit = s.tRead(s.indkeep);

nX = size(s.X,2);
Xvar = sum(s.X.*s.X); % need to cache, unless exclude
s.M0 = sum(s.X.*repmat(s.yFit,[1 nX]))./Xvar;
s.yhat = s.X.*repmat(s.M0,[length(s.yFit) 1]);
s.res = repmat(s.yFit,[1 nX])-s.yhat;
s.yhat0 = s.X0.*repmat(s.M0,[length(s.y) 1]);
if(0)
  s.M0 = inv(s.X'*s.X)*(s.X'*s.y);
  s.yhat = s.M0*s.X;
  s.res = s.y-s.yhat;
  s.yhat0 = s.M0*s.X0;
end
s.rstd = std(s.res);
err = s.rstd;
s.err = err;

irepistruct = s;

return;


%{
 * Copyright (C) 2020-2030, The Regents of The University of Michigan.
 * All rights reserved.
 * This software was developed in the Biped Lab (https://www.biped.solutions/)
 * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
 * be available under alternative licensing terms; contact the address above.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * The views and conclusions contained in the software and documentation are those
 * of the authors and should not be interpreted as representing official policies,
 * either expressed or implied, of the Regents of The University of Michigan.
 *
 * AUTHOR: Bruce JK Huang (bjhuang[at]umich.edu)
 * WEBSITE: https://www.brucerobot.com/
%}

function difference = H_diff(H1, H2)
    difference = H1 \ H2;
    logR = logm(difference(1:3,1:3));
    v = [-logR(1,2) logR(1,3) -logR(2,3) difference(1:3,4)'];
    difference = norm(v);
end


function datapt = lvd_ThrottleTask(stateLogEntry, subTask, inFrame)
%lvd_ThrottleTask Summary of this function goes here
%   Detailed explanation goes here

    switch subTask
        case 'throttle'
            datapt = 100*stateLogEntry.throttle;

        case 't2w'
            throttle = stateLogEntry.throttle;
            tankStates = stateLogEntry.getAllActiveTankStates();

            tankMasses = zeros(size(tankStates));
            for(i=1:length(tankStates))
                tankMasses(i) = tankStates(i).tankMass;
            end

            powerStorageStates = stateLogEntry.getAllActivePwrStorageStates();
            storageSoCs = NaN(size(powerStorageStates));
            for(i=1:length(powerStorageStates))
                storageSoCs(i) = powerStorageStates(i).getStateOfCharge();
            end

            datapt = computeTWRatio(throttle, stateLogEntry.time, stateLogEntry.position, stateLogEntry.velocity, tankMasses, stateLogEntry.getTotalVehicleDryMass(), ...
                                    stateLogEntry.stageStates, stateLogEntry.lvState, tankStates, stateLogEntry.centralBody, storageSoCs, powerStorageStates);
        case 'totalthrust'
            [totalThrust] = getThrustParameters(stateLogEntry);
            datapt = totalThrust;

        case 'thrust_x'
            vehElemSet = stateLogEntry.getCartesianElementSetRepresentation();
            [~, ~, ~, rotMat] = inFrame.getOffsetsWrtInertialOrigin(stateLogEntry.time, vehElemSet);

            [~, thrustForceVector] = getThrustParameters(stateLogEntry);
            thrustForceVector = thrustForceVector(:);
            thrustForceVector = rotMat * thrustForceVector;

            datapt = thrustForceVector(1);
            if(isnan(datapt))
                datapt = 0;
            end

        case 'thrust_y'
            vehElemSet = stateLogEntry.getCartesianElementSetRepresentation();
            [~, ~, ~, rotMat] = inFrame.getOffsetsWrtInertialOrigin(stateLogEntry.time, vehElemSet);

            [~, thrustForceVector] = getThrustParameters(stateLogEntry);
            thrustForceVector = thrustForceVector(:);
            thrustForceVector = rotMat * thrustForceVector;

            datapt = thrustForceVector(2);
            if(isnan(datapt))
                datapt = 0;
            end

        case 'thrust_z'
            vehElemSet = stateLogEntry.getCartesianElementSetRepresentation();
            [~, ~, ~, rotMat] = inFrame.getOffsetsWrtInertialOrigin(stateLogEntry.time, vehElemSet);

            [~, thrustForceVector] = getThrustParameters(stateLogEntry);
            thrustForceVector = thrustForceVector(:);
            thrustForceVector = rotMat * thrustForceVector;

            datapt = thrustForceVector(3);
            if(isnan(datapt))
                datapt = 0;
            end

        case 'thrust_vector'
            [~, thrustForceVector] = getThrustParameters(stateLogEntry);

            datapt = thrustForceVector(:);
            if(any(isnan(datapt)))
                datapt = [0;0;0];
            end

        otherwise
            error('Unrecongized task: %s', subTask);

    end
end

function [totalThrust, thrustForceVector] = getThrustParameters(stateLogEntry)
    if(stateLogEntry.event.propagatorObj.canProduceThrust())
        ut = stateLogEntry.time;
        rVect = stateLogEntry.position;
        vVect = stateLogEntry.velocity;

        bodyInfo = stateLogEntry.centralBody;
        tankStates = stateLogEntry.getAllActiveTankStates();
        stageStates = stateLogEntry.stageStates;
        lvState = stateLogEntry.lvState;

        dryMass = stateLogEntry.getTotalVehicleDryMass();
        tankStatesMasses = [tankStates.tankMass]';

        throttleModel = stateLogEntry.throttleModel;
        steeringModel = stateLogEntry.steeringModel;

        altitude = norm(rVect) - bodyInfo.radius;
        pressure = getPressureAtAltitude(bodyInfo, altitude);

        powerStorageStates = stateLogEntry.getAllActivePwrStorageStates();
        storageSoCs = NaN(size(powerStorageStates));
        for(i=1:length(powerStorageStates)) %#ok<*NO4LP>
            storageSoCs(i) = powerStorageStates(i).getStateOfCharge();
        end

        attState = LaunchVehicleAttitudeState();
        attState.dcm = steeringModel.getBody2InertialDcmAtTime(ut, rVect, vVect, bodyInfo);

        throttle = throttleModel.getThrottleAtTime(ut, rVect, vVect, tankStatesMasses, dryMass, stageStates, lvState, tankStates, bodyInfo, storageSoCs, powerStorageStates);

        [~, totalThrust, thrustForceVector] = LaunchVehicleStateLogEntry.getTankMassFlowRatesDueToEngines(tankStates, tankStatesMasses, stageStates, throttle, lvState, pressure, ut, rVect, vVect, bodyInfo, steeringModel, storageSoCs, powerStorageStates, attState);
        thrustForceVector = 1000*thrustForceVector; %in order to recover kN
    else
        totalThrust = 0;
        thrustForceVector = [0;0;0];
    end
end


function a = r8vec_permute ( n, a, p )

%*****************************************************************************80
%
%% R8VEC_PERMUTE permutes an R8VEC in place.
%
%  Discussion:
%
%    This routine permutes an array of real "objects", but the same
%    logic can be used to permute an array of objects of any arithmetic
%    type, or an array of objects of any complexity.  The only temporary
%    storage required is enough to store a single object.  The number
%    of data movements made is N + the number of cycles of order 2 or more,
%    which is never more than N + N/2.
%
%    P(I) = J means that the I-th element of the output array should be
%    the J-th element of the input array.  P must be a legal permutation
%    of the integers from 1 to N, otherwise the algorithm will
%    fail catastrophically.
%
%  Example:
%
%    Input:
%
%      N = 5
%      P = (   2,   4,   5,   1,   3 )
%      A = ( 1.0, 2.0, 3.0, 4.0, 5.0 )
%
%    Output:
%
%      A    = ( 2.0, 4.0, 5.0, 1.0, 3.0 ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 May 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of objects.
%
%    Input, real A(N), the array to be permuted.
%
%    Input, integer P(N), the permutation.
%
%    Output, real A(N), the permuted array.
%
  ierror = perm1_check ( n, p );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_PERMUTE - Fatal error!\n' );
    fprintf ( 1, '  PERM1_CHECK finds the permuation illegal.\n' );
    error ( 'R8VEC_PERMUTE - Fatal error!' );
  end
%
%  Search for the next element of the permutation that has not been used.
%
  for istart = 1 : n

    if ( p(istart) < 0 )

      continue

    elseif ( p(istart) == istart )

      p(istart) = -p(istart);
      continue

    else

      a_temp = a(istart);
      iget = istart;
%
%  Copy the new value into the vacated entry.
%
      while ( 1 )

        iput = iget;
        iget = p(iget);

        p(iput) = -p(iput);

        if ( iget < 1 || n < iget )
          fprintf ( 1, '\n' );
          fprintf ( 1, 'R8VEC_PERMUTE - Fatal error!\n' );
          fprintf ( 1, '  A permutation index is out of range.\n' );
          fprintf ( 1, '  P(%d) = %d\n', iput, iget );
          error ( 'R8VEC_PERMUTE - Fatal error!' );
        end

        if ( iget == istart )
          a(iput) = a_temp;
          break
        end

        a(iput) = a(iget);

      end

    end

  end
%
%  Restore the signs of the entries.
%
% p(1:n) = -p(1:n);

  return
end


function maximise( varargin )

%MAXIMISE Specifiies a concave (or affine) objective to be maximized.

global cvx___
prob = evalin( 'caller', 'cvx_problem', '[]' );
if ~isa( prob, 'cvxprob' ),
    error( 'No CVX model exists in this scope.' );
elseif isempty( cvx___.problems ) || cvx___.problems( end ).self ~= prob,
    error( 'Internal CVX data corruption. Please CLEAR ALL and rebuild your model.' );
elseif nargin < 1,
    error( 'Objective expression missing.' );
elseif iscellstr( varargin ),
    x = evalin( 'caller', sprintf( '%s ', varargin{:} ) );
elseif nargin > 1,
    error( 'Too many input arguments.' );
else
    x = varargin{1};
end
try
    newobj( prob, 'maximize', x );
catch exc
    rethrow( exc )
end

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [ y, next ] = p37_equil ( neqn, next )

%*****************************************************************************80
%
%% P37_EQUIL returns equilibrium solutions of problem p37.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, integer NEXT, the index of the previous
%    equilibrium, which should be 0 on first call.
%
%    Output, real Y(NEQN), the "next" equilibrium solution, if any.
%
%    Output, integer NEXT, the index of the current equilibrium,
%    or 0 if there are no more.
%
  if ( next == 0 )
    next = 1;
    y(1:neqn,1) = [ 0.0; 0.0 ];
  else
    next = 0;
    y = [];
  end

  return
end


% CONFIGSTATEMACHINE configures the state machine (type of demo, velocity
%                    of the demo, repeat movements, and so on).

%% --- Initialization ---

% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY)
Config.LEFT_RIGHT_MOVEMENTS = false;

% If equal to one, the desired values of the center of mass are smoothed internally
Config.SMOOTH_COM_DES       = true;

% If equal to one, the desired values of the postural tasks are smoothed internally
Config.SMOOTH_JOINT_DES     = true;

% Joint torques saturation [Nm]
Sat.torque                  = 60;

% Joint torques rate of change saturation
Sat.uDotMax                 = 300;

% max unsigned difference between two consecutive (measured) joint positions,
% i.e. delta_qj = abs(qj(k) - qj(k-1))
Sat.maxJointsPositionDelta  = 15*pi/180; % [rad]

%% Regularization parameters
Reg.pinvDamp_baseVel        = 1e-7;
Reg.pinvDamp                = 1;
Reg.pinvTol                 = 1e-5;
Reg.KP_postural             = 0.1;
Reg.KD_postural             = 0;
Reg.HessianQP               = 1e-7;

%% State Machine configuration

% time between two yoga positions
StateMachine.joints_pauseBetweenYogaMoves = 5;

% contact forces threshold
StateMachine.wrench_thresholdContactOn    = 50;
StateMachine.wrench_thresholdContactOff   = 100;

% threshold on CoM and joints error
StateMachine.CoM_threshold                = 0.01;
StateMachine.joints_thresholdNotInContact = 5;
StateMachine.joints_thresholdInContact    = 50;

% initial state for state machine
StateMachine.initialState                 = 1;

% other configuration parameters for state machine
StateMachine.tBalancing                   = 1;
StateMachine.tBalancingBeforeYoga         = 1;
StateMachine.yogaExtended                 = true;
StateMachine.skipYoga                     = false;
StateMachine.demoOnlyBalancing            = false;
StateMachine.demoStartsOnRightSupport     = false; % If false, the Yoga demo is performed on the left foot first
StateMachine.yogaAlsoOnRightFoot          = false; % TO DO: yoga on both feet starting from right foot (not available for now)

%%%% List of possible "Yoga in loop" %%%%

% the robot will repeat the FULL DEMO (two feet balancing, yoga on left
% foot, back on two feet, yoga right foot, back on two feet). The demo is
% repeated until the user stops the Simulink model. This option is ignored
% if Sm.demoStartsOnRightSupport = true.
StateMachine.twoFeetYogaInLoop            = false;

% the robot will repeat the ONE FOOT yoga for the number of times the user
% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two
% feet balancing in between to consecutive yoga. WARNING: if the option
% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga
% on left foot for the number of times the user specifies in the Sm.yogaCounter,
% and then it will repeat the yoga on the right foot for the same number of times.
StateMachine.oneFootYogaInLoop            = false;
StateMachine.yogaCounter                  = 5;

%% Parameters for motors reflected inertia

% transmission ratio (1/N)
Config.Gamma                 = 0.01*eye(ROBOT_DOF);

% modify the value of the transmission ratio for the hip pitch.
% TODO: avoid to hard-code the joint numbering
Config.Gamma(end-5, end-5)   = 0.0067;
Config.Gamma(end-11,end-11)  = 0.0067;

% motors inertia (Kg*m^2)
legsMotors_I_m               = 0.0827*1e-4;
torsoPitchRollMotors_I_m     = 0.0827*1e-4;
torsoYawMotors_I_m           = 0.0585*1e-4;
armsMotors_I_m               = 0.0585*1e-4;

% add harmonic drives reflected inertia
if Config.INCLUDE_HARMONIC_DRIVE_INERTIA

    legsMotors_I_m           = legsMotors_I_m + 0.054*1e-4;
    torsoPitchRollMotors_I_m = torsoPitchRollMotors_I_m + 0.054*1e-4;
    torsoYawMotors_I_m       = torsoYawMotors_I_m + 0.021*1e-4;
    armsMotors_I_m           = armsMotors_I_m + 0.021*1e-4;
end

Config.I_m                   = diag([torsoPitchRollMotors_I_m*ones(2,1);
                                     torsoYawMotors_I_m;
                                     armsMotors_I_m*ones(8,1);
                                     legsMotors_I_m*ones(12,1)]);

% parameters for coupling matrices. Updated according to the wiki:
%
% http://wiki.icub.org/wiki/ICub_coupled_joints
%
% and corrected according to https://github.com/robotology/robots-configuration/issues/39
t            = 0.615;
r            = 0.022;
R            = 0.04;

% coupling matrices
T_LShoulder  = [-1  0  0;
                -1 -t  0;
                 0  t -t];

T_RShoulder  = [ 1  0  0;
                 1  t  0;
                 0 -t  t];

T_torso      = [ 0.5    -0.5     0;
                 0.5     0.5     0;
                 r/(2*R) r/(2*R) r/R];

if Config.INCLUDE_COUPLING

    Config.T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12));
else
    Config.T = eye(ROBOT_DOF);
end

% gain for feedforward term in joint torques calculation. Valid range: a
% value between 0 and 1
Config.K_ff  = 0;

% Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA if true, the desired joints
% accelerations are used for computing the feedforward term in joint
% torques calculations. Not effective if Config.K_ff = 0.
Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA = false;

%% Constraints for QP for balancing

% The friction cone is approximated by using linear interpolation of the circle.
% So, numberOfPoints defines the number of points used to interpolate the circle
% in each cicle's quadrant
numberOfPoints               = 4;
forceFrictionCoefficient     = 1/3;
torsionalFrictionCoefficient = 1/75;
fZmin                        = 10;

% physical size of the foot
feet_size                    = [-0.07  0.12  ;    % xMin, xMax
                                -0.045 0.045 ];   % yMin, yMax

% Compute contact constraints (friction cone, unilateral constraints)
[ConstraintsMatrix, bVectorConstraints] = wbc.computeRigidContactConstraints ...
    (forceFrictionCoefficient, numberOfPoints, torsionalFrictionCoefficient, feet_size, fZmin);


function varargout = spm_bsplins(varargin)
% Sample a volume using B-spline interpolation - a compiled routine
% FORMAT [f,dfx,dfy,dfz] = spm_bsplins(c,x,y,z,d)
%   c - volume of B-spline coefficients (from spm_bsplinc)
%   x,y,z - co-ordinates of sampled points
%       d(1:3) - degree of B-spline (from 0 to 7) along different dimensions
%                - these must be same as used by spm_bsplinc
%       d(4:6) - 1/0 to indicate wrapping along the dimensions
%   f - sampled data
%   dfx,dfy,dfz - sampled first derivatives
%
% This function takes B-spline basis coefficients from spm_bsplinc,
% and re-convolves them with B-splines centred at the new sample points.
%
% Note that nearest neighbour interpolation is used instead of 0th
% degree B-splines, and the derivatives of trilinear interpolation are
% returned insted of those of 1st degree B-splines.  The difference is
% extremely subtle.
%
%_______________________________________________________________________
%
% References:
%   M. Unser, A. Aldroubi and M. Eden.
%   "B-Spline Signal Processing: Part I-Theory,"
%   IEEE Transactions on Signal Processing 41(2):821-832 (1993).
%
%   M. Unser, A. Aldroubi and M. Eden.
%   "B-Spline Signal Processing: Part II-Efficient Design and
%   Applications,"
%   IEEE Transactions on Signal Processing 41(2):834-848 (1993).
%
%   M. Unser.
%   "Splines: A Perfect Fit for Signal and Image Processing,"
%   IEEE Signal Processing Magazine, 16(6):22-38 (1999)
%
%   P. Thevenaz and T. Blu and M. Unser.
%   "Interpolation Revisited"
%   IEEE Transactions on Medical Imaging 19(7):739-758 (2000).
%_______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_bsplins.m 2696 2009-02-05 20:29:48Z guillaume $

%-This is merely the help file for the compiled routine
error('spm_bsplins.c not compiled - see Makefile');


%% Example of using KST class for interfacing with KUKA iiwa robots

% Used to test switching between various operation modes.

% First start the server on the KUKA iiwa controller
% Then run this script using Matlab

% Tested under Matlab2013b, for higher versions, the realtime plot might
% not work.

% This example works with Sunrise application version KST_1.7  and higher.
% Copyright Mohammad SAFEEA, 1st-March-2019

% Operation modes:
% 1st impedance motion
% 2nd (PTP) move on lines
% 3rd (PT) move on circles
% 4th direct servo
% 5th impedance again
% 6th (PTP) move lines again
close all;clear;clc;
warning('off');
%% Create the robot object
ip='172.31.1.147'; % The IP of the controller
arg1=KST.LBR7R800; % choose the robot iiwa7R800 or iiwa14R820
arg2=KST.Medien_Flansch_elektrisch; % choose the type of flange
Tef_flange=eye(4); % transofrm matrix of EEF with respect to flange
iiwa=KST(ip,arg1,arg2,Tef_flange); % create the object

%% Start a connection with the server
flag=iiwa.net_establishConnection();
if flag==0
return;
end
pause(1);
disp('Testing KST under various operation modes');

%% Move point to point to an initial position
jPos={0,0,0,-pi/2,0,pi/2,0};
relVel=0.15;
iiwa.movePTPJointSpace(jPos, relVel); % move to initial configuration

%% Pause for 3 seocnds
pause(3);

%% 1st
disp('First test, real time impedance mode');
% Tool/Impedance parameters
massOfTool=0.5; % the mass of the tool attached to flange in Kg
cOMx=0; % X coordinate of the center of mass of the tool in (mm)
cOMy=0; % Y coordinate of the center of mass of the tool in (mm)
cOMz=40; % Z coordinate of the center of mass of the tool in (mm)
cStiness=900; % cartizian stifness
rStifness=80; % rotational stifness
nStifness=50; % null space stifness

% Start the soft realtime control with impedance
iiwa.realTime_startImpedanceJoints(massOfTool,cOMx,cOMy,cOMz,...
cStiness,rStifness,nStifness);

w=0.6; % motion constants, frequency rad/sec
A=0.2; % motion constants, amplitude of motion

a=datevec(now);
t0=a(6)+a(5)*60+a(4)*60*60; % calculate initial time

dt=0;
tstart=t0;
counter=0;
duration=0.2*60; %0.2 minutes

% Control loop
% real time plot handles
 colors={'k','b','r','g','c','m','y'};
 figureHandle=figure('Units','inches','Position',[0 0 5 3.75]);
 numOfSamples=120; % number of samples to show in the plot
 timeVec=1:numOfSamples;
 tawVec=zeros(7,numOfSamples);
 plotHandle=[];
 for i=1:7
    plotHandle=[plotHandle,plot(timeVec,tawVec(i,:),colors{i},'LineWidth',2)];
    hold on;
 end
% format the plot
ylim([0,8]);
temp=title('External torques');
set(temp,'FontSize',16);
temp=xlabel('Time (seconds)');
set(temp,'FontSize',14);
temp=ylabel('Distance (m)');
set(temp,'FontSize',14);

while(dt<duration)
% perform trajectory calculation here
  a=datevec(now);
  time=a(6)+a(5)*60+a(4)*60*60;
  dt=time-t0;

  temp=A*(1-cos(w*dt));
    for dacount=1:7
        jPosCommand{dacount}=jPos{dacount}+temp;
    end
  counter=counter+1;
  % Send joint positions to robot
  taw=iiwa.sendJointsPositionsExTorque(jPosCommand);

  tawTemp=zeros(7,1);
  for i=1:7
      tawTemp(i)=taw{i};
      torque_array(i,counter)=taw{i};
  end
  % delete first measurment
  tawVec(:,1)=[];
  % add new measurment to end of arrays
  tawVec=[tawVec,tawTemp];
  % update plots data
  for i=1:6
    set(plotHandle(i),'YData',tawVec(i,:));
  end
  set(plotHandle(7),'YData',tawVec(i,:));
  % show plot data
  set(figureHandle,'Visible','on');

end

% Stop the realtime control with impedence motion
iiwa.realTime_stopImpedanceJoints();
fprintf('\n impedance motion stopped \n');
pause(2);

%% 2nd
disp('Second test, point to point motion mode, move on lines');
% move to initial position
pinit={0,pi*20/180,0,-pi*70/180,0,pi*90/180,0}; % initial confuguration
relVel=0.15; % relative velocity
iiwa.movePTPJointSpace(pinit, relVel); % point to point motion in joint space

% Get position orientation of end-effector
disp('Cartesian position')
Pos=iiwa.getEEFPos();
disp(Pos)

% Move the end-effector in the X direction
Vel=50; % velocity of the end effector, mm/sec
displacment=50;
index=1;

Pos{index}=Pos{index}+displacment;
iiwa.movePTPLineEEF(Pos, Vel)

pause(0.1)

Pos{index}=Pos{index}-displacment;
iiwa.movePTPLineEEF(Pos, Vel)

% Move the end-effector in the z direction
index=3;

Pos{index}=Pos{index}+displacment;
iiwa.movePTPLineEEF(Pos, Vel)

pause(0.1)

Pos{index}=Pos{index}-displacment;
iiwa.movePTPLineEEF(Pos, Vel);

% Move the end-effector to a distination frame,

Pos={400,0,580,-pi,0,-pi}; % first configuration
% the coordinates are:
% x=400mm, y=0mm, z=580 mm.
% the rotation angles are:
% alpha=-180 degrees, beta=0 degrees, gama=-180 degrees
iiwa.movePTPLineEEF(Pos, Vel);

Pos={500,0,580,-pi,0,-pi+pi/4}; %% second configuration
% the coordinates are:
% x=400mm, y=0mm, z=580 mm.
% the rotation angles are:
% alpha=-180 degrees, beta=0 degrees, gama=-165 degrees,
iiwa.movePTPLineEEF(Pos, Vel);

%% 3rd
disp('Third test, point to point motion mode, move on circles');
% move on cirlces
relVel=0.25; % override relative joint velocities

pos={0, -pi / 180 * 10, 0, -pi / 180 * 100, pi / 180 * 90,pi / 180 * 90, 0};   % initial cofiguration

iiwa.movePTPJointSpace( pos, relVel); % go to initial position
% Move in an arc, the orientation of EEF changes while performing the motion,
% The function utilized (movePTPCirc1OrintationInter)
% f2 is the final frame, at which the arc motion ends
% f1 is an intermidiate frame, through wich the robot passes while
% performing the motion.

f1=iiwa.getEEFPos();
f2=f1;
r=75; % radius of arc
f1{2}=f1{2}+r;
f1{3}=f1{3}-r;
f1{6}=f1{6}+pi/8;

f2{3}=f2{3}-2*r;
f2{6}=f2{6}+pi/2;

vel=150; % linear velocity of end-effector mm/sec
iiwa.movePTPCirc1OrintationInter( f1,f2, vel)

% Move robot in joint space to some initial configuration
pinit={0,pi*20/180,0,-pi*70/180,0,pi*90/180,0}; % joint angles of initial confuguration
relVel=0.15; % the relative velocity
iiwa.movePTPJointSpace(pinit, relVel); % point to point motion in joint space

% Move EEF -100 mm in Z direction
deltaX=0.0;deltaY=0;deltaZ=-100.; % relative displacemnets of end-effector
Pos{1}=deltaX;
Pos{2}=deltaY;
Pos{3}=deltaZ;
iiwa.movePTPLineEefRelBase(Pos, vel);

% Store the current position in the memory
Cen=iiwa.getEEFPos(); % Concider the current position as the center of the arcs

% Move EEF 50mm in X direction
deltaX=100;deltaY=0;deltaZ=0.;
Pos{1}=deltaX;
Pos{2}=deltaY;
Pos{3}=deltaZ;
iiwa.movePTPLineEefRelBase(Pos, vel);
% Store the current position in the memory
circle_Starting_Point=iiwa.getEEFPos(); % Consider the current point as circle starting point

% Move in an arc, the arc is drawn on an incliend plane
% using the function ((movePTPArc_AC))
theta=pi/2; % the angle subtended by the arc at the center ((c))
k=[1;1;1]; % normal vector of the plane, on which the circle is drawn
c=[Cen{1};Cen{2};Cen{3}]; % the center of the arc
vel=100; % the motion velocity mm/sec
iiwa.movePTPArc_AC(theta,c,k,vel)

% Go back to ((circle_Starting_Point)) coordinates
vel=150;
iiwa.movePTPLineEEF(circle_Starting_Point, vel);
% Move in an arc, the arc is drawn in XY plane
% using the function ((movePTPArcXY_AC))
theta=1.98*pi; % the angle subtended by the arc at the center ((c))
c=[Cen{1};Cen{2}]; % the XY coordinate of the center of the arc
vel=150; % the motion velocity mm/sec
iiwa.movePTPArcXY_AC(theta,c,vel)

%% 4th
disp('Fourth test, realtime motion using direct servo mode');
% Move to an intial position
jPos={0,0,0,-pi/2,0,pi/2,0};
relVel=0.15;
iiwa.movePTPJointSpace(jPos, relVel); % move to initial configuration

% Get Cartesian position of EEF
fprintf('Cartesian position')
eefpos=iiwa.getEEFPos();
eefposDist=eefpos;
disp(eefpos)

% Start direct servo in Cartesian space
iiwa.realTime_startDirectServoCartesian();
disp('Starting direct servo in Cartesian space');
w=2; % motion constants, frequency rad/sec
A=75; % motion constants, amplitude of motion (mm)
tic;
deltaT=0;
counter=0;
initiationFlag=0;
disp('Entering control loop, streaming EEF positions')

% Control loop
while(deltaT<(6*pi/w))
if(initiationFlag==0)
    initiationFlag=1;
    t_0=toc;
    t0=t_0;
else
    time=toc;
    deltaT=time-t0;
    %........................................
    % Perform trajectory calculation here
    eefposDist{3}=eefpos{3}-A*(1-cos(w*deltaT));
    %........................................
    % Send EEF position to robot
    if(toc-t_0>0.003)
        counter=counter+1;
        iiwa.sendEEfPositionf(eefposDist);
        t_0=toc;
    end
end
end
tstart=t0;
tend=time;
rate=counter/(tend-tstart);
% Stop the direct servo motion
iiwa.realTime_stopDirectServoCartesian( );
fprintf('\nThe rate of update per second is: \n');
disp(rate);
fprintf('\n')
pause(2);

%% 5th
disp('Fifth test, realtime impedance motion mode, repeat procedure for 3 times');
for i=1:3
    massOfTool=0.5; % the mass of the tool attached to flange in Kg
    cOMx=0; % X coordinate of the center of mass of the tool in (mm)
    cOMy=0; % Y coordinate of the center of mass of the tool in (mm)
    cOMz=40; % Z coordinate of the center of mass of the tool in (mm)
    cStiness=900; % cartizian stifness
    rStifness=80; % rotational stifness
    nStifness=50; % null space stifness

    % Start the soft realtime control with impedance
    iiwa.realTime_startImpedanceJoints(massOfTool,cOMx,cOMy,cOMz,...
    cStiness,rStifness,nStifness);

    w=0.6; % motion constants, frequency rad/sec
    A=0.2; % motion constants, amplitude of motion

    a=datevec(now);
    t0=a(6)+a(5)*60+a(4)*60*60; % calculate initial time

    dt=0;
    tstart=t0;
    counter=0;
    duration=10; %10 seconds

    % Control loop
    while(dt<duration)
    % perform trajectory calculation here
      a=datevec(now);
      time=a(6)+a(5)*60+a(4)*60*60;
      dt=time-t0;

      temp=A*(1-cos(w*dt));
        for dacount=1:7
            jPosCommand{dacount}=jPos{dacount}+temp;
        end
      counter=counter+1;
      % Send joint positions to robot
      iiwa.sendJointsPositions(jPosCommand);
    end
    % Stop the realtime control with impedence motion
    iiwa.realTime_stopImpedanceJoints();
    fprintf('\n Impedance motion stopped \n');
    pause(2);

    daPos_temp=iiwa.getEEFPos();
end

%% 6th
disp('Sexth test, repeated from point to point motion mode, move on lines');
% move to initial position
pinit={0,pi*20/180,0,-pi*70/180,0,pi*90/180,0}; % initial confuguration
relVel=0.15; % relative velocity
iiwa.movePTPJointSpace(pinit, relVel); % point to point motion in joint space

% Get position orientation of end effector
disp('Cartesian position')
Pos=iiwa.getEEFPos();
disp(Pos)

% Move the end-effector in the X direction
Vel=50; % velocity of the end effector, mm/sec
displacment=50;
index=1;

Pos{index}=Pos{index}+displacment;
iiwa.movePTPLineEEF(Pos, Vel)

pause(0.1)

Pos{index}=Pos{index}-displacment;
iiwa.movePTPLineEEF(Pos, Vel)

% Move the end-effector in the z direction
index=3;

Pos{index}=Pos{index}+displacment;
iiwa.movePTPLineEEF(Pos, Vel)

pause(0.1)

Pos{index}=Pos{index}-displacment;
iiwa.movePTPLineEEF(Pos, Vel);

% Move the end-effector to a distination frame,

Pos={400,0,580,-pi,0,-pi}; % first configuration
% the coordinates are:
% x=400mm, y=0mm, z=580 mm.
% the rotation angles are:
% alpha=-180 degrees, beta=0 degrees, gama=-180 degrees
iiwa.movePTPLineEEF(Pos, Vel);

Pos={500,0,580,-pi,0,-pi+pi/4}; %% second configuration
% the coordinates are:
% x=400mm, y=0mm, z=580 mm.
% the rotation angles are:
% alpha=-180 degrees, beta=0 degrees, gama=-165 degrees,
iiwa.movePTPLineEEF(Pos, Vel);

%% Turn off the Sunrise.Workbench application
iiwa.net_turnOffServer


function varargout = interp1(varargin)
%INTERP1 (overloaded)

switch class(varargin{3})

    case 'sdpvar'

        if nargin < 4
            varargin{4} = 'linear';
        end

        if nargin < 5
            varargin{5} = [];
        end

        if isa(varargin{2},'function_handle')
            varargin{2} = varargin{2}(varargin{1});
        end

        if numel(varargin{3}) > 1
            d = size(varargin{3});

            if min(d) > 1
                % Matrix case, vectorize
                varargout{1} = interp1(varargin{1},varargin{2},reshape(varargin{3},[],1),varargin{4},varargin{5});
                varargout{1} = reshape(varargout{1},d);
                return
            end

            if any(any(isnan(varargin{1})))
                error('Interpolation grid contains NaNs');
            end
            if any(any(isnan(varargin{2})))
                error('Interpolation data contains NaNs');
            end

            % If only one data sequence, place in row vector
            if min(size(varargin{1})) == 1
                varargin{1} = repmat(varargin{1}(:)',max(d),1);
            end
            if min(size(varargin{2})) == 1
                varargin{2} = repmat(varargin{2}(:)',max(d),1);
            end

            out = [];
            varargin{3} = reshape(varargin{3},1,max(d));
            for i = 1:max(d)
                    Y.type = '()';
                    Y.subs{1} = 1;
                    Y.subs{2} = i;
                    xij = subsref(varargin{3},Y);
                    out = [out;interp1(varargin{1}(i,:),varargin{2}(i,:),xij,varargin{4},varargin{5})];
            end
            if d(1)==1
                out = out';
            end
            varargout{1} = out;
            return
        else
            if min(size(varargin{1})) > 1 || min(size(varargin{2})) > 1
                error('Data should be vectors for scalar interpolant');
            end
            % Column vector assumed format
            varargin{1} = reshape(varargin{1},[],1);
            varargin{2} = reshape(varargin{2},[],1);
        end

        if ~isa(varargin{1},'double') || ~isa(varargin{2},'double')
            error('First 2 arguments in interp1 should be approximation data');
        end

        %if any(diff(varargin{1})<0)
        %    error('First arguments in interp1 should be monotonically increasing');
        %end

        if isequal(varargin{4},'graph')
            if  ~isconvexdata(varargin{1},varargin{2}) && ~isconvexdata(varargin{1},-varargin{2})
                error('Data has to be convex or concave for graph approximant');
            end
        end

        % Reorder arguments to make sure sdpvar is first argument.
        % Use local version of interp to deal with this
        % Arguments xi yi x method -> x xi yi method
        varargout{1} = yalmip('define','interp1_internal',varargin{[3 1 2 4 5]});

    case 'double'
        if isa(varargin{1},'double') && isa(varargin{2},'sdpvar')
            % User is trying to do nonlinear programming where the knot
            % function values in an interpolant are free variables
            zi = varargin{3};
            [n,m] = size(zi);
            if n*m == 1
                varargout{1} = yalmip('define','interp1_nonlinear',varargin{1},varargin{2},varargin{3},varargin{4});
            else
                out = [];
                zi = reshape(zi,1,n*m);
                for i = 1:n*m
                    Y.type = '()';
                    Y.subs{1} = 1;
                    Y.subs{2} = i;
                    zij = subsref(zi,Y);
                    out = [out  yalmip('define','interp1_nonlinear',varargin{1},varargin{2},zij,varargin{4})];
                end
                varargout{1} = reshape(out,n,m);
            end

        elseif isa(varargin{1},'sdpvar') && isa(varargin{2},'sdpvar')
            % User is trying to do nonlinear programming where the knot
            % function values and locations in an interpolant are free variables

            zi = varargin{3};
            [n,m] = size(zi);
            if n*m == 1
                varargout{1} = yalmip('define','interp1_nonlinear',[varargin{1},varargin{2}],[],varargin{3},varargin{4});
            else
                out = [];
                zi = reshape(zi,1,n*m);
                for i = 1:n*m
                    Y.type = '()';
                    Y.subs{1} = 1;
                    Y.subs{2} = i;
                    zij = subsref(zi,Y);
                    out = [out   yalmip('define','interp1_nonlinear',[varargin{1},varargin{2}],[],zij,varargin{4})];
                end
                varargout{1} = reshape(out,n,m);
            end
        else
            error('SDPVAR/INTERP1 called with strange arguments!');
        end

    otherwise
        error('SDPVAR/INTERP1 called with strange argument!');
end

function isconvex = isconvexdata(xi,yi)
finterp = (yi(1:end-2) + yi(3:end))/2;
if all(finterp >= yi(2:end-1))
    isconvex = 1;
else
    isconvex = 0;
end


classdef QuadrilateralNodesCalculator < NodesCalculator

    properties (Access = public)
        nvert
        div
        boundNodes
        totalNodes
    end

    methods (Access = public)

        function obj = QuadrilateralNodesCalculator(cParams)
            obj.init(cParams);
            obj.computeBoundaryNodes();
            obj.computeTotalNodes();
        end

    end

    methods (Access = private)

        function computeTotalNodes(obj)
            divA = obj.div(1);
            divB = obj.div(2);
            obj.totalNodes = obj.boundNodes+(divA-1)*(divB-1);
        end

    end

end


function s=sss(n_id_1,n_id_2,slot_num);

% s=sss(n_id_1,n_id_2,slot_num);
%
% Return the sss for slot slot_num for the specified n_id_1 and n_id_2.
%
% s is of length 62 and only includes the non-zero subcarriers. The calling
% function must insert the DC subcarrier and the 5 zero subcarriers that
% surround the sss.

% Copyright 2012 Evrytania LLC (http://www.evrytania.com)
%
% Written by James Peroulas <james@evrytania.com>
%
% 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/>.

error(nargchk(3,3,nargin));
error(chk_param(n_id_1,'n_id_1','scalar','real','integer','>=',0,'<=',167));
error(chk_param(n_id_2,'n_id_2','scalar','real','integer','>=',0,'<=',2));
error(chk_param(slot_num,'slot_num','scalar','real','integer','>=',0,'<=',10));
if ((slot_num~=0)&&(slot_num~=10))
  error('slot_num must be either 0 or 10');
end

% Calculate m0 and m1 from n_id_1
qp=floor(n_id_1/30);
q=floor((n_id_1+qp*(qp+1)/2)/30);
mp=n_id_1+q*(q+1)/2;
m0=mod(mp,31);
m1=mod(m0+floor(mp/31)+1,31);

%s_td=[0 0 0 0 1];
%for t=1:26
%  s_td=[s_td mod(s_td(end-2)+s_td(end-4),2)];
%end
s_td=[0 0 0 0 1 0 0 1 0 1 1 0 0 1 1 1 1 1 0 0 0 1 1 0 1 1 1 0 1 0 1];
s_td=1-2*s_td;

%c_td=[0 0 0 0 1];
%for t=1:26
%  c_td=[c_td mod(c_td(end-1)+c_td(end-4),2)];
%end
c_td=[0 0 0 0 1 0 1 0 1 1 1 0 1 1 0 0 0 1 1 1 1 1 0 0 1 1 0 1 0 0 1];
c_td=1-2*c_td;

%z_td=[0 0 0 0 1];
%for t=1:26
%  z_td=[z_td mod(z_td(end)+z_td(end-2)+z_td(end-3)+z_td(end-4),2)];
%end
z_td=[0 0 0 0 1 1 1 0 0 1 1 0 1 1 1 1 1 0 1 0 0 0 1 0 0 1 0 1 0 1 1];
z_td=1-2*z_td;

s0_m0=s_td(mod(m0:30+m0,31)+1);
s1_m1=s_td(mod(m1:30+m1,31)+1);

c0=c_td(mod(n_id_2:30+n_id_2,31)+1);
c1=c_td(mod(n_id_2+3:30+n_id_2+3,31)+1);

z1_m0=z_td(mod((0:30)+mod(m0,8),31)+1);
z1_m1=z_td(mod((0:30)+mod(m1,8),31)+1);

if (slot_num==0)
  s(2:2:62)=s1_m1.*c1.*z1_m0;
  s(1:2:62)=s0_m0.*c0;
elseif (slot_num==10)
  s(2:2:62)=s0_m0.*c1.*z1_m1;
  s(1:2:62)=s1_m1.*c0;
else
  error('Check code...');
end


function d=drectangle(p,x1,x2,y1,y2)

%   Copyright (C) 2004-2006 Per-Olof Persson. See COPYRIGHT.TXT for details.

d=-min(min(min(-y1+p(:,2),y2-p(:,2)),-x1+p(:,1)),x2-p(:,1));


a = Twist(SE3([1 2 3]))
%a.SE
% a.Ad
x = inv(a.SE);
x.Ad

Xtrans([1 2 3])

a = Twist(SE3.Rx(pi/2))
%a.SE
x = inv(a.SE);
x.Ad

Xrotx(pi/2)


%% Affine invariant feature-based image matching
%
% This sample is similar to |feature_homography_demo.m|, but uses the affine
% transformation space sampling technique, called
% <http://www.ipol.im/pub/algo/my_affine_sift/ ASIFT>. While the original
% implementation is based on SIFT, you can try to use SURF or ORB detectors
% instead. Homography RANSAC is used to reject outliers.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/asift.py>
%

function asift_demo()
    % load input grayscale images
    files = {
        fullfile(mexopencv.root(),'test','aero1.jpg')
        fullfile(mexopencv.root(),'test','aero3.jpg')
    };
    for i=1:numel(files)
        if exist(files{i}, 'file') ~= 2
            disp('Downloading...')
            url = 'https://cdn.rawgit.com/opencv/opencv/3.2.0/samples/data/';
            [~,fname,ext] = fileparts(files{i});
            urlwrite([url fname ext], files{i});
        end
    end
    img1 = cv.imread(files{1}, 'Grayscale',true);
    img2 = cv.imread(files{2}, 'Grayscale',true);

    % ASIFT
    [vis, H] = run_asift(img1, img2, 'BRISK', true);
    imshow(vis), title('ASIFT')

    % interactive exploration
    if ~mexopencv.isOctave() && mexopencv.require('images')
        % interactively select region in img1
        pos1 = select_poly(gca) - 1;
        if isempty(pos1), return; end

        % apply homography
        pos2 = cv.perspectiveTransform(pos1, H);
        pos2(:,1) = pos2(:,1) + size(img1,2);

        % draw region in img1 and transformed region in img2
        vis = cv.polylines(vis, pos1, 'Closed',true, ...
            'Color',[0 255 0], 'Thickness',2, 'LineType','AA');
        vis = cv.polylines(vis, pos2, 'Closed',true, ...
            'Color',[0 255 0], 'Thickness',2, 'LineType','AA');
        imshow(vis), title('ASIFT')
    end
end

function [vis, H] = run_asift(img1, img2, feature_name, use_flann)
    %RUN_ASIFT  Run ASIFT algorithm, estimate homography and visualize matches

    % create detector and matcher objects
    [detector, matcher] = init_feature(feature_name, use_flann);

    % detect features using ASIFT method
    disp('Detecting...')
    tic, [kp1, desc1] = affine_detect(detector, img1); toc
    tic, [kp2, desc2] = affine_detect(detector, img2); toc
    fprintf('img1: %d features, img2: %d features\n', numel(kp1), numel(kp2));

    % match ASIFT features
    disp('Matching...')
    tic, matches = matcher.knnMatch(desc1, desc2, 2); toc
    fprintf('%d matches\n', numel(matches));

    % filter matches
    [matches, p1, p2] = filter_matches(kp1, kp2, matches);
    fprintf('%d good matches\n', numel(matches));
    assert(numel(matches) >= 4, 'not enough matches for homography estimation');

    % estimate homography with RANSAC method
    [H,inliers] = cv.findHomography(p1, p2, 'Method','Ransac');
    assert(~isempty(H), 'homography estimation failed');
    inliers = logical(inliers);
    fprintf('%d inliers\n', nnz(inliers));

    % visualize good and inlier matches
    vis = cv.drawMatches(img1, kp1, img2, kp2, matches, ...
        'NotDrawSinglePoints',true, 'MatchesMask',inliers);
end

function [detector, matcher] = init_feature(feature_name, use_flann)
    %INIT_FEATURE  Create detector and matcher objects

    % detector
    switch upper(feature_name)
        case 'SURF'
            detector = cv.SURF('HessianThreshold',400);
        case 'SIFT'
            detector = cv.SIFT();
        case 'ORB'
            detector = cv.ORB();
        case 'BRISK'
            detector = cv.BRISK();
        case 'AKAZE'
            detector = cv.AKAZE();
        case 'KAZE'
            detector = cv.KAZE();
        otherwise
            error('unrecognized feature: %s', feature_name)
    end

    % matcher
    if use_flann
        if ~isempty(strfind(detector.defaultNorm(), 'Hamming'))
            opts = {'LSH', 'TableNumber',6, 'KeySize',12, 'MultiProbeLevel',1};
        else
            opts = {'KDTree', 'Trees',5};
        end
        matcher = cv.DescriptorMatcher('FlannBasedMatcher', 'Index',opts);
    else
        matcher = cv.DescriptorMatcher('BFMatcher', ...
            'NormType',detector.defaultNorm());
    end
end

function [kpts, descs] = affine_detect(detector, img, mask)
    %AFFINE_DETECT  Affine-SIFT (ASIFT) algorithm
    %
    %     [kpts, descs] = affine_detect(detector, img)
    %     [kpts, descs] = affine_detect(detector, img, mask)
    %
    % ## Input
    % * __detector__ detector object
    % * __img__ input image
    % * __mask__ optional mask, default all image included
    %
    % ## Output
    % * __kpts__ concatenated keypoints
    % * __descs__ corresponding concatenated descriptors
    %
    % Applies a set of affine transormations to the image, detect keypoints,
    % and reproject them into initial image coordinates.
    % See: http://www.ipol.im/pub/algo/my_affine_sift/ for the details.
    %

    % default mask if not specified
    if nargin < 3
        mask = 255 * ones(size(img,1), size(img,2), 'uint8');
    end

    % parameter sampling: Tilt and Phi
    % (simulates all possible affine distortions caused by the change of
    % camera optical axis orientation from a frontal position)
    params = [1 0];  % no tilt and no rotation, thus original image
    for t = sqrt(2).^(1:5)       % geometric series 1, a, a^2, .., a^n; a=sqrt(2), n=5
        for p = 0:72/t:180-72/t  % arithmetic series 0, b/t, .., k*b/t < 180; b=72
            params(end+1,:) = [t, p];
        end
    end

    % for each pair of distortion parameters
    % (this loop is candidate for parallelization, i.e parfor)
    N = size(params,1);
    kpts = cell(N, 1);
    descs = cell(N, 1);
    for i=1:N
        % transform image
        [timg, tmask, Ai] = affine_skew(params(i,1), params(i,2), img, mask);

        % detect features using a similarity invariant matching method (SIFT)
        [kp, feat] = detector.detectAndCompute(timg, 'Mask',tmask);

        %TODO: Remove keypoints close to the boundary of the transformed image

        % project keypoints from the coordinates of the rotated and tilted
        % image back to the original image corrdinates
        for j=1:numel(kp)
            kp(j).pt = [kp(j).pt, 1] * Ai.';
        end
        kpts{i} = kp(:);
        descs{i} = feat;
    end

    % concatenate all features from all simulated images
    kpts = cat(1, kpts{:});
    descs = cat(1, descs{:});
end

function [img, mask, Ai] = affine_skew(tilt, phi, img, mask)
    %AFFINE_SKEW  Transform image/mask by an affine distortion
    %
    %     [img, mask, Ai] = affine_skew(tilt, phi, img, mask)
    %
    % ## Input
    % * __tilt__ tilt
    % * __phi__ rotation angle in degrees
    % * __img__ image
    % * __mask__ mask
    %
    % ## Output
    % * __img__ transformed image
    % * __mask__ transformed mask
    % * __Ai__ affine transform matrix from output/skewed `img` to input `img`
    %

    % initialize affine transformation matrix (identity)
    A = eye(2,3);
    [h,w,~] = size(img);

    % in the case of (tilt=1, phi=0), then no transformation (identity)

    if phi ~= 0
        % rotate image
        A = [cosd(phi) -sind(phi); sind(phi) cosd(phi)];
        corners = [0 0; w 0; w h; 0 h];
        corners = round(corners * A.');
        rect = cv.boundingRect(corners);
        A(:,3) = -rect(1:2);
        img = cv.warpAffine(img, A, 'DSize',rect(3:4), ...
            'Interpolation','Linear', 'BorderType','Replicate');
    end

    if tilt ~= 1
        % anti-aliasing filtering in the vertical direction, then tilt image
        % (subsample in vertical direction by a factor of tilt)
        s = 0.8 * sqrt(tilt^2 - 1);
        img = cv.GaussianBlur(img, 'KSize',[0 0], 'SigmaX',s, 'SigmaY',0.01);
        img = cv.resize(img, 1/tilt, 1, 'Interpolation','Nearest');
        A(1) = A(1) / tilt;
    end

    if phi ~= 0 || tilt ~= 1
        % apply same transformation on mask
        [h,w,~] = size(img);
        mask = cv.warpAffine(mask, A, 'DSize',[w h], 'Interpolation','Nearest');
    end

    % inverse affine transformation
    Ai = cv.invertAffineTransform(A);
end

function [matches, p1, p2] = filter_matches(kp1, kp2, matches, ratio)
    %FILTER_MATCHES  Filter out 2-NN matches using Lowe's ratio test

    if nargin < 4, ratio = 0.75; end

    % good matches
    idx = cellfun(@(m) (numel(m) == 2) && ...
        (m(1).distance < ratio * m(2).distance), matches);
    matches = cellfun(@(m) m(1), matches(idx));

    % corresponding points
    p1 = cat(1, kp1([matches.queryIdx]+1).pt);
    p2 = cat(1, kp2([matches.trainIdx]+1).pt);
end

function pos = select_poly(ax)
    %SELECT_POLY  Select polygon area using the mouse
    %
    %     pos = select_poly()
    %     pos = select_poly(ax)
    %
    % ## Input
    % * __ax__ axes handle, default gca
    %
    % ## Output
    % * __pos__ Nx2 matrix of points
    %

    if nargin < 1, ax = gca; end
    hRoi = impoly(ax);
    api = iptgetapi(hRoi);
    try
        api.setColor('green');
        api.setPositionConstraintFcn(makeConstrainToRectFcn('impoly', ...
            get(ax,'XLim').*[1 0.5], get(ax,'YLim')));
        pos = wait(hRoi);
    catch
        pos = zeros(0,2);
    end
    delete(hRoi);
end


 function [xs, info] = ir_pwls_os_rlalm(x, Ab, yi, R, varargin)
%function [xs, info] = ir_pwls_os_rlalm(x, Ab, yi, R, [options])
%|
%| penalized weighted least squares estimation / image reconstruction
%| using relaxed linearized augmented lagrangian method with
%| (optionally relaxed) ordered subsets. (relaxed OS-LALM)
%|
%| See ?. 201? IEEE T-MI by Hung Nien & J A Fessler
%| "Relaxed linearized algorithms for faster X-ray CT image reconstruction"
%|	http://dx.doi.org/10.1109/TMI.201?
%|
%|
%| cost(x) = (y-Ax)' W (y-Ax) / 2 + R(x)
%|
%| in
%|	x	[np 1]		initial estimate
%|	Ab	[nd np]		Gblock object (needs abs(Ab) method)
%|				or sparse matrix (implies nsubset=1)
%|	yi	[nb na]		measurements (noisy sinogram data)
%|	R	penalty		object (see Reg1.m), can be []
%|
%| option
%|	niter			# of iterations (default: 1)
%|	wi	[nb na]		weighting sinogram (default: [] for uniform)
%|	pixmax	[1] or [2]	max pixel value, or [min max] (default [0 inf])
%|	denom	[np 1]		precomputed denominator
%|	aai	[nb na]		precomputed row sums of |Ab|
%|	relax0	[1] or [2]	relax0 or (relax0, relax_rate) (default 1)
%|	rho			AL penalty parameter (default: [] for decreasing rho)
%|	alpha			over-relaxation parameter (default: 1.999)
%|	userfun	@		user-defined function handle (see default below)
%|					taking arguments (x, userarg{:})
%|	userarg	{}		user arguments to userfun (default {})
%|	chat
%|
%| out
%|	xs	[np niter]	iterates
%|	info	[niter 1]	time
%|
%| 2015-11-30, Hung Nien, based on ir_pwls_os_lalm
%| 2015-11-30, tweaks by Jeff Fessler

if nargin == 1 && streq(x, 'test'), ir_pwls_os_rlalm_test, return, end
if nargin < 4, ir_usage, end

% defaults
arg.niter = 1;
arg.isave = [];
arg.userfun = @userfun_default;
arg.userarg = {};
arg.pixmax = inf;
arg.chat = false;
arg.wi = [];
arg.aai = [];
arg.rho = []; % default: decreasing rho
arg.alpha = 1.999;
arg.relax0 = 1;
arg.denom = [];
arg.scale_nblock = true; % traditional scaling
arg.update_even_if_denom_0 = true;
arg = vararg_pair(arg, varargin);

arg.isave = iter_saver(arg.isave, arg.niter);

Ab = block_op(Ab, 'ensure'); % make it a block object (if not already)
nblock = block_op(Ab, 'n');
starts = subset_start(nblock);

cpu etic

wi = arg.wi;
if isempty(wi)
	wi = ones(size(yi), class(yi));
end
if isempty(arg.aai) && isempty(arg.denom)
	arg.aai = reshape(sum(abs(Ab)'), size(yi)); % a_i = sum_j |a_ij|
end

% check input sinogram sizes for OS
if (ndims(yi) ~= 2) || (size(yi,2) == 1 && nblock > 1)
	fail 'bad yi size'
end
if (ndims(wi) ~= 2) || (size(wi,2) == 1 && nblock > 1)
	fail 'bad wi size'
end

relax0 = arg.relax0(1);
if length(arg.relax0) == 1
	relax_rate = 0;
elseif length(arg.relax0) == 2
	relax_rate = arg.relax0(2);
else
	error relax
end

if length(arg.pixmax) == 2
	pixmin = arg.pixmax(1);
	pixmax = arg.pixmax(2);
elseif length(arg.pixmax) == 1
	pixmin = 0;
	pixmax = arg.pixmax;
else
	error pixmax
end

% likelihood denom, if not provided
denom = arg.denom;
if isempty(denom)
	denom = abs(Ab)' * col(arg.aai .* wi); % needs abs(Ab)
        arg = rmfield(arg, 'aai'); % clear to save memory - not needed below
end
if ~arg.update_even_if_denom_0
	% todo: this may not work for LALM because "denom" appears in numerator!
	denom(denom == 0) = inf; % trick: prevents pixels where denom=0 being updated
end

if isempty(R)
	pgrad = 0; % unregularized default
	Rdenom = 0;
end

alpha = arg.alpha;
if alpha<1 || alpha>2
	fail 'alpha should be between 1 and 2'
end

rho = arg.rho;
if isempty(rho)
	rho = @(k) pi/(alpha*k) * sqrt(1 - (pi/(2*(alpha*k)))^2) * (k>1) + (k==1);
else
	rho = @(k) rho; % constant user-specified value
end

[nb na] = size(yi);

x = x(:);
np = length(x);
xs = zeros(np, length(arg.isave), 'single');
if any(arg.isave == 0)
	xs(:, arg.isave == 0) = single(x);
end

%info = zeros(niter,?); % do not initialize since size may change

% initialization
iblock = nblock;
ia = iblock:nblock:na;
li = Ab{iblock} * x;
li = reshape(li, nb, length(ia));
resid = wi(:,ia) .* (li - yi(:,ia));
if arg.scale_nblock
	scale = nblock; % traditional way
else
	scale = na / numel(ia); % alternative - untested
end
zeta = scale * Ab{iblock}' * resid(:);

g = rho(1) * zeta;
h = denom .* x - zeta;

% iterate
for iter = 1:arg.niter
	ticker(mfilename, iter, arg.niter)

	relax = relax0 / (1 + relax_rate * (iter-1));

	% loop over subsets
	for iset = 1:nblock
		k = nblock*(iter-1)+iset;

		num = rho(k) * (denom .* x - h) + (1-rho(k)) * g;
		den = rho(k) * denom;
		if ~isempty(R)
			num = num + R.cgrad(R, x);
			den = den + R.denom(R, x);
		end
		x = x - relax * num ./ den;
		x = max(x, pixmin);
		x = min(x, pixmax);

		iblock = starts(iset);
		ia = iblock:nblock:na;

		li = Ab{iblock} * x;
		li = reshape(li, nb, length(ia));
		resid = wi(:,ia) .* (li - yi(:,ia));

		if arg.scale_nblock
			scale = nblock; % traditional way
		else
			scale = na / numel(ia); % alternative - untested
		end

		zeta = scale * Ab{iblock}' * resid(:); % A' * W * (y - A*x)
		g = (rho(k) * (alpha * zeta + (1-alpha) * g) + g) / (rho(k)+1);
		h = alpha * (denom .* x - zeta) + (1-alpha) * h;
	end

	if any(arg.isave == iter)
		xs(:, arg.isave == iter) = single(x);
	end
	info(iter,:) = arg.userfun(x, arg.userarg{:});
end

% default user function.
% using this evalin('caller', ...) trick, one can compute anything of interest
function out = userfun_default(x, varargin)
chat = evalin('caller', 'arg.chat');
if chat
%	x = evalin('caller', 'x');
	printm('minmax(x) = %g %g', min(x), max(x))
end
out = cpu('etoc');

% ir_pwls_os_rlalm_test()
% test with small quadratic case 1/2 |y - A x|_2^2 + 1/2 |C x|_2^2
% see also ir_ct_fan_beam_sqs_vs_lalm.m
function ir_pwls_os_rlalm_test
rng(0)
nd = 99; np = 38;
A = randn(nd,np);
A = abs(A);
R = Reg1(ones(np,1), 'type_penal', 'mat', 'type_diff', 'sparse', 'beta', 2^0);
C = R.C;
C = full(C);
if 0 % try to make it so unit step size is reasonable?
	D = abs(A)' * (abs(A) * ones(np,1)) + abs(C)' * (abs(C) * ones(np,1));
	A = A * diag(sqrt(1 ./ D));
	C = C * diag(sqrt(1 ./ D));
end
H = A' * A + C' * C;
pr cond(H)

xtrue = ones(np,1); xtrue(10:20) = 2;
yi = A * xtrue; yi = yi + 0.01 * max(yi(:)) * randn(nd,1);
xhat = H \ (A' * yi);
% nrms(xhat, xtrue)

x0 = zeros(np,1);
niter = 100;

printm 'Nesterov' % todo: replace with OGM
gradfun = @(x) A' * (A * x - yi) + C' * (C * x);
step = abs(A') * (abs(A) * ones(np,1)) + abs(C)' * (abs(C) * ones(np,1));
step = 1 / max(step); % todo: use D
xf = ir_fista(x0, 'gradfun', gradfun, 'step', step, 'niter', niter, ...
	'isave', 'all');

printm 'Relaxed LALM'
xl = ir_pwls_os_rlalm(x0, Gmatrix(A), yi, R, 'niter', niter, 'isave', 'all');

printm 'CG'
xc = qpwls_pcg1(x0, A, 1, yi, R.C, 'niter', niter, 'isave', 'all');

if im
	im plc 1 2
	im subplot 1
	plot([xtrue xhat xl(:,end) xc(:,end) xf(:,end)])
	im subplot 2
	xlabel 'iteration', ylabelf 'NRMSD $x^n$ vs $\hat{x}$'
	plot(	0:niter, nrms(xl, xhat), 'r-', ...
		0:niter, nrms(xf, xhat), 'g-', ...
		0:niter, nrms(xc, xhat), 'c-')
	legend('Relaxed LALM', 'FGM', 'CG')
end


% PREPARE_DATABASE Calculates the features from objects in a source
%
% Usage
%    database = PREPARE_DATABASE(src, feature_fun, options)
%
% Input
%    src (struct): The source specifying the objects.
%    feature_fun (cell): The feature functions applied to each object.
%    options (struct): Options for calculating the features:
%       options.feature_sampling (int): specifies how to sample the feature
%           vectors in time/space (default 1).
%       options.file_normalize (int): The normalization of each file before
%          being given to feature_fun. Can be empty, 1, 2, or Inf (default []).
%       options.parallel (boolean): If true, tries to use the Distributed
%          Computing Toolbox to speed up calculation (default true).
%       Other options are listed in the help for the FEATURE_WRAPPER function.
% Output
%    database (struct): The database of feature vectors.
%
% See also
%    CREATE_SRC, FEATURE_WRAPPER

function db = prepare_database(src,feature_fun,opt)
	if nargin < 3
		opt = struct();
	end

	opt = fill_struct(opt, 'feature_sampling', 1);
	opt = fill_struct(opt, 'file_normalize', []);
	opt = fill_struct(opt, 'parallel', 1);

	features = cell(1,length(src.files));
	obj_ind = cell(1,length(src.files));

	rows = 0;
	cols = 0;
	precision = 'double';

	if opt.parallel
		% parfor loop - note that the contents are the same as the serial loop, but
		% MATLAB doesn't seem to offer an easy way of deduplicating the code.

		% Loop through all the files in the source
		parfor k = 1:length(src.files)
			% Identify all the objects contained in the current file
			file_objects = find([src.objects.ind]==k);

			if length(file_objects) == 0
				% No objects, skip.
				continue;
			end

			tm0 = tic;

			% Load the complete file and normalize as needed.
			x = data_read(src.files{k});

			if ~isempty(opt.file_normalize)
				if opt.file_normalize == 1
					x = x/sum(abs(x(:)));
				elseif opt.file_normalize == 2
					x = x/sqrt(sum(abs(x(:)).^2));
				elseif opt.file_normalize == Inf
					x = x/max(abs(x(:)));
				end
			end

			% Due to parallelization constraints, each file stores its feature vectors
			% in one index of the features cell array. The corresponding object indices
			% are stored in the obj_ind cell array.
			features{k} = cell(1,length(file_objects));
			obj_ind{k} = file_objects;

			% Apply all the feature functions to the objects contained in x. The output
			% buf is a PxNxK vector, where P corresponds to number of feature coeffi-
			% cients, N corresponds to the number of features - the number of "windows"
			% - in each object, and K corresponds to the number of objects.
			buf = apply_features(x,src.objects(file_objects),feature_fun,opt);

			% Subsample among the features as needed.
			buf = buf(:,1:opt.feature_sampling:end,:);

			% Separate each of the objects into its own array element.
			for l = 1:length(file_objects)
				features{k}{l} = buf(:,:,l);
			end
			fprintf('.');
		end
	else
		time_start = clock;
		for k = 1:length(src.files)
			file_objects = find([src.objects.ind]==k);

			if length(file_objects) == 0
				continue;
			end

			tm0 = tic;
			x = data_read(src.files{k});

			if ~isempty(opt.file_normalize)
				if opt.file_normalize == 1
					x = x/sum(abs(x(:)));
				elseif opt.file_normalize == 2
					x = x/sqrt(sum(abs(x(:)).^2));
				elseif opt.file_normalize == Inf
					x = x/max(abs(x(:)));
				end
			end

			features{k} = cell(1,length(file_objects));
			obj_ind{k} = file_objects;

			buf = apply_features(x,src.objects(file_objects),feature_fun,opt);

			buf = buf(:,1:opt.feature_sampling:end,:);

			for l = 1:length(file_objects)
				features{k}{l} = buf(:,:,l);
			end
			time_elapsed = etime(clock, time_start);
			estimated_time_left = time_elapsed * (length(src.files)-k) / k;
			fprintf('.');
		end
	end
	fprintf('\n');

	% Identify the files for which objects have been computed.
	nonempty = find(~cellfun(@isempty,features));

	% Determine the size of the features array. Each row corresponds to a feature
	% coefficient, each column corresponds to a feature vector.
	rows = size(features{nonempty(1)}{1},1);
	cols = sum(cellfun(@(x)(sum(cellfun(@(x)(size(x,2)),x))),features(nonempty)));
	precision = class(features{nonempty(1)}{1});

	db.src = src;

	db.features = zeros(rows,cols,precision);

	db.indices = cell(1,length(src.objects));

	% Fill in the features array using the pre-calculated features.
	r = 1;
	for k = 1:length(features)
		for l = 1:length(features{k})
			ind = r:r+size(features{k}{l},2)-1;
			db.features(:,ind) = features{k}{l};
			db.indices{obj_ind{k}(l)} = ind;
			r = r+length(ind);
		end
	end
end

function out = apply_features(x,objects,feature_fun,opt)
	% For each feature function, apply it to the objects in x and concatenate
	% the results.

	if ~iscell(feature_fun)
		feature_fun = {feature_fun};
	end

	out = {};
	for k = 1:length(feature_fun)
		if nargin(feature_fun{k}) == 2
			% If only two inputs are given, the feature function will handle the cut
			% ting up of x into objects itself.
			out{k,1} = feature_fun{k}(x,objects);
		elseif nargin(feature_fun{k}) == 1
			% If only one input is given, the feature function relies on us to cut up
			% the input x into objects and feed them to it.
			out{k,1} = feature_wrapper(x,objects,feature_fun{k},opt);
		else
			error('Invalid number of inputs for feature function!')
		end
	end

	if length(feature_fun) == 1
		out = out{1};
	else
		% Concatenate along the first dimension - the feature dimension - of each
		% element.
		out = cellfun(@(x)(permute(x,[2 1 3])),out,'UniformOutput',false);
		out = permute([out{:}],[2 1 3]);
	end
end


function F = extractColumns(F, colIdx)
%EXTRACTCOLUMNS   Extract columns (or rows) of an array-valued CHEBFUN.
%   G = EXTRACTCOLUMNS(F, COLIDX) extracts the columns specified by the row
%   vector COLIDX from the CHEBFUN F so that G = F(:,COLIDX). COLIDX need not be
%   increasing in order or unique, but it must contain only integers in the
%   range [1, M] where F has M columns.
%
%   If F is a row CHEBFUN, then EXTRACTCOLUMNS(F, ROWIDX) behaves as described
%   above, except that extracts the rows of F so G = F(ROWIDX,:).
%
% See also MAT2CELL.

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

% Number of columns (or rows if f.isTransposed) of f:
numCols = numColumns(F);

% Trivial cases:
if ( isempty(F) )
    return

elseif ( ~isnumeric(colIdx) && strcmp(colIdx, ':') )
    return

elseif ( (numel(colIdx) == numel(numCols)) && all(colIdx == 1:numCols) )
    return

elseif ( max(colIdx) > numCols )
    error('CHEBFUN:CHEBFUN:extractColumns:dimensions', ...
        'Index exceeds CHEBFUN dimensions.')

end

if ( numel(F) > 1 )
    % Quasimatrix case:
    F = F(colIdx);

else
    % Array-valued CHEBFUN case:

    % Extract the columns of the FUNs:
    for k = 1:numel(F.funs)
        F.funs{k} = extractColumns(F.funs{k}, colIdx);
    end

    % Extract the columns from the impulses:
    F.pointValues = F.pointValues(:,colIdx);
end

end


function [a]=plus(b,c)
%A=B+C
%   [A]=PLUS(B,C) A is the sum of TT-matrices B and C
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------
if ( isempty(b) )
   a=c;
   return
elseif (isempty(c) )
   a=b;
   return
end
    a=tt_matrix;
a.n=b.n;
a.m=b.m;
a.tt=b.tt+c.tt;
return
end


function findedge3(node,edge,range,varargin)
%% FINDEDGE3 highlights edges in certain range in 3-D
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

hold on
% set up range
if (nargin<=2) || (isempty(range))
    range = (1:size(edge,1))';
end
if islogical(range)
    range = find(range);
end
if size(range,2)>size(range,1)
    range = range';
end
midEdge = (node(edge(range,1),:)+node(edge(range,2),:))/2;
if length(range)<size(edge,1)
    h = plot3([node(edge(range,1),1)'; node(edge(range,2),1)'],...
              [node(edge(range,1),2)'; node(edge(range,2),2)'],...
              [node(edge(range,1),3)'; node(edge(range,2),3)'],...
              'LineWidth',1.5,'Color','r');
else
    h = plot3(midEdge(:,1),midEdge(:,2),midEdge(:,3),'r.','MarkerSize', 18);
end
if nargin > 3
    if strcmp(varargin{1},'noindex') || strcmp(varargin{1},'index')
        startidx = 2;
    else
        startidx = 1;
    end
    set(h,varargin{startidx:end});
end
if (nargin <=3) || ~(strcmp(varargin{1},'noindex'))
    text(midEdge(:,1)+0.025,midEdge(:,2)-0.025,midEdge(:,3),int2str(range), ...
         'FontSize',12,'FontWeight','bold','Color','k');
end
%% TODO: help and example


function movePTPEllipse_1(iiwa,c,dir,ratio,theta,velocity,accel,TefTool)
%% This funciton is used to
% Move end-effector of the robot on an ellipse.
% this function is used from within the KST class

%% Syntax:
% movePTPEllipse_1(iiwa,c,dir,ratio,TefTool)

%% Arreguments:
% iiwa: KST obejct.
% c: 3x1 vector, is a vector defining the displacment of the the center
% point of the ellipse in relation to the start point of the ellipse,
% position units in (mm).
% dir: the direction vector of the (a) axis of the ellipse.
% ratio: the radious ratio (a/b) of the ellipse.
% theta: is the angle of the ellipe part , for drawing a complete
% ellipse this angle is equal to 2*pi.
% TefTool: 4X4 Transform matrix of the end-efector in the reference frame
% of the flange of the KUKA iiwa, position units in (mm).
% accel: is the acceleration (mm/sec2).
% velocity: is the motion velocity (mm/sec).

% Copy right: Mohammad SAFEEA
% 25th-June-2018

%% Convert inputs to a column vectors
c=colVec(c);
dir=colVec(dir);
ratio=colVec(ratio);
theta=colVec(theta);
velocity=colVec(velocity);
accel=colVec(accel);

%% Check input variables
if(size(c,1)~=3)
fprintf('Error: the vector (c) shall be a 3x1 vector \n');
return;
end

if(size(dir,1)~=3)
fprintf('Error: the vector (dir) shall be a 3x1 vector \n');
return;
end

if(size(ratio,1)~=1)
fprintf('Error: the ratio shall be a scalar  \n');
return;
end

if(size(theta,1)~=1)
fprintf('Error: the variable (theta) shall be a scalar  \n');
return;
end

if(size(TefTool,1)~=4)
fprintf('Error: the transform matrix (TefTool) shall be 4x4  \n');
return;
end

if(size(TefTool,2)~=4)
fprintf('Error: the transform matrix (TefTool) shall be 4x4  \n');
return;
end

if(size(velocity,1)~=1)
fprintf('Error: the velocity on the path shall be a scalar  \n');
return;
end

if(size(accel,1)~=1)
fprintf('Error: the acceleration on the path shall be a scalar  \n');
return;
end

%% Convert the units to (meter)
unitConverter=1000;
c=c/unitConverter;
velocity=velocity/unitConverter;
accel=accel/unitConverter;
TefTool(1:3,4)=TefTool(1:3,4)/unitConverter;

%% start the direct servo
     iiwa.realTime_startDirectServoJoints();

%% get current joints angles of robot
    jPos  = iiwa.getJointsPos();

%% Calculate the current position of TCP point of the robot (p0)
    qs=zeros(7,1);
for i=1:7
    qs(i)=jPos{i};
end
        T0=iiwa.directKinematics(qs); % TCP point transformation matrix
        p0=T0(1:3,4); % Current position of TCP point of the robot
        Tt=T0;

%% Calculate the parameters of the ellipse,
p=p0; % this is the starting point of the ellipse
c=c+p; % calculate the center coordinates from the center displacment vector
[R,theta0,a,b,c,errorFlag]=getEllipseParameters(p,c,ratio,dir);
if errorFlag==true % this happens when the start point is on the (dir) vector direction.
    % in such case the ellipse plane can not be specified in space.
    iiwa.realTime_stopDirectServoJoints();
    beep;
    fprintf('Error executing the ellipse function:\n');
    fprintf('Starting point of the ellipse shall not be on the line aligned with the (dir) vector and passing from the center (c)\n');
    return;
end

%% Get the length of the ellipse arc as a function of the parametric angle (theta)
theta1=theta0+theta;
[ thetaVec,sVec ] = getEllipseLengthVector( a,b, theta0,theta1 );
sizesVec=max(size(sVec));
L=sVec(end); % L is the length of the elliptic curve

%% Calculate the times:
[t0,t1,t2]=calculateInterpolationTimes(L,velocity,accel);

%% Joint space control

    [Ttemp,J]=iiwa.directKinematics(qs);
    vec=Ttemp(1:3,4);

%% dls solver parameters
 numberOfIterationForSolver=100;
 lambda=0.5;
 TefTool=eye(4);

    sCoordinate=0;
    interpolationCounter=1;
    thetaVar=theta0;

%% start of time
        dateVector0=datevec(now);
        time0=dateVector0(6)+dateVector0(5)*60+dateVector0(4)*60*60; % calculate time at this instant
while true

    % Calculate the elapsed time
        dateVector=datevec(now);
        timeNow=dateVector(6)+dateVector(5)*60+dateVector(4)*60*60; % calculate time at this instant
        deltaT=timeNow-time0; % elapsed is zero at first excution

    % calculate position of servo point
    if deltaT<t0
        sCoordinate=0.5*accel*deltaT*deltaT;
    elseif(deltaT<t1)
        s0=0.5*accel*t0*t0;
        sCoordinate=velocity*(deltaT-t0)+s0;
    elseif(deltaT<t2)
           s0=0.5*accel*t0*t0;
           s1=velocity*(t1-t0)+s0;
           sCoordinate=velocity*(deltaT-t1)-0.5*accel*(deltaT-t1)*(deltaT-t1)+s1;
    end
 %% when time ends break the loop
    if deltaT>t2
        break;
    end
%% Interpolate theta from the sVec,thetaVec
    for counter=interpolationCounter:(sizesVec-1)
        if(sCoordinate>sVec(counter))
            tetaRange=thetaVec(counter+1)-thetaVec(counter);
            sRange=sVec(counter+1)-sVec(counter);
            thetaVar=(sCoordinate-sVec(counter))...
                *tetaRange/sRange+thetaVec(counter);
            interpolationCounter=counter;
        end
    end

    pPrime=R*ellipseParametricFunction( a,b,thetaVar );
    p=c+pPrime;

    % calculate target transform
    Tt(1:3,4)=p;
    [ qs ] = iiwa.gen_InverseKinematics( qs, Tt,numberOfIterationForSolver,lambda );

    for i=1:7
        jPos{i}=qs(i);
    end

	%% Send joint positions to robot
	iiwa.sendJointsPositions( jPos);

end

    % Stop the reltime motion
    iiwa.realTime_stopDirectServoJoints();

end

function [R,theta0,a,b,c,errorFlag]=getEllipseParameters(p,c,ratio,dir)

%% About
% This function returns the parameters of the ellipse

%% Arreguments:
% p: the starting point of the ellipse
% c: the center of the ellipse
% ratio: the ratio (a/b) of the ellipse
% dir: direction of the

%% Return values:
% R, the rotation matrix from the ellipse frame to the base frame of the
% robot
% theta0: the starting angle of the ellipse (given by the parametric equation of the ellipse)
% a,b: the big and the small radious of the ellipse
% c: is the center of the ellipse
% error flag: returns true if an error happens, this error is when the
% vector (p-c) is coinsident with the vector (dir).

p=colVec(p);
c=colVec(c);
dir=colVec(dir);

% X dirction vector
i=dir/norm(dir);

u=p-c;
%
k=cross(u,i);
normk=norm(k);

if(normk==0)
    errorFlag=true;
else
    errorFlag=false;
end

k=k/norm(k);
j=cross(k,i);

% rotation matrix from ellipse frame to base frame
R=[i,j,k];
% calculate a,b minimum,maximum radious of the ellipse
x=u'*i; % x coordinate of the starting point of the ellipse in the ellipse frame
y=u'*j; % y coordinate of the starting point of the ellipse in the ellipse frame
a=(x*x+y*y/(ratio*ratio))^0.5;
b=ratio*a;
% calculate theta0, begining angle of the ellipse
theta0=atan2(y,x);

end

function y=colVec(x)
    if size(x,2)==1
        y=x;
    else
        y=x';
    end
end

function [ thetaVec,sVec ] = getEllipseLengthVector( a,b, theta0,theta1 )
%% About:
% This function is used to return the length of the ellipse as a function
% of the angle theta.

%% Inputs:
% theta0, theta1: start/end angle of the arc of the ellipse (from the parametric equation of the ellipse)
% a,b: the big and the small radious of the ellipse.

thetaSpan = [theta0 theta1];
s0 = 0;
opts = odeset('RelTol',1e-2,'AbsTol',1e-4);
[thetaVec,sVec] = ode45(@(theta,s) ((a*a*sin(theta)*sin(theta)+b*b*cos(theta)*cos(theta))^0.5), thetaSpan, s0,opts);

end

function [ vec ] = ellipseParametricFunction( a,b,theta )
%% About:
% Calculate the X and Y coordinates of the ellipse point corrsponding to
% angle (theta) from the parametric equation

%% inputs:
% theta: angle of point on the ellipse (from the parametric equation of the ellipse)
% a,b: the big and the small radious of the ellipse

%% Return value:
% The return value is a column vector, vec=[x;y;0] where:
% x: the x coordiante of the point on the ellipse correspodning to angle
% (theta) as described in the frame of the ellipse
% y: the y coordiante of the point on the ellipse  correspodning to angle
% (theta) as described in the frame of the ellipse

x=a*cos(theta);
y=b*sin(theta);
vec=[x;y;0];

end

function [t0,t1,t2]=calculateInterpolationTimes(L,v,a)
%% About:
% calculates the times of the acceleration, stable motion, deceleration

%% Arreguments:
% L: length of the curve (mm).
% v: linear velocity of eef on the the curve (mm/sec).
% a: linear acceleration of eef on the the curve (mm/sec2).

%% Return values:
% t0; time for acceleration.
% t1: time for acceleration, constant velocity.
% t2: time for acceleration, constant velocity and for deceleration

taw=v/a;

if(L>a*taw*taw)
    t0=taw;
    deltat=(L-a*taw*taw)/v;
    t1=t0+deltat;
    t2=t1+taw;
else
    taw1=(L/a)^0.5;
    t0=taw1;
    t1=taw1;
    t2=2*taw1;
end

end


function out=icassoGet(sR,field,index)
%function out=icassoGet(sR,field,[index])
%
%PURPOSE
%
%Auxiliary function for obtaining various information from the
%Icasso result data structure. Using icassoGet, you can return
%information related to original data, e.g.: data matrix,
%(de)whitening matrix, total number of estimates, number of
%estimates on each round. You can also return specified estimated
%independent components (sources), and rows of demixing matrices
%from. (However, it is easier to use function icassoResult to return
%the final estimation results from the complete Icasso procedure.)
%
%EXAMPLES OF BASIC USAGE
%
%   M=icassoGet(sR,'m');
%
%returns total number of estimates.
%
%   nic=icassoGet(sR,'numOfIC')
%
%returns number of estimated IC components on each round in an Nx1
%vector (The number may differ in deflatory estimation mode due to
%convergence problems.)
%
%   r=icassoGet(sR,'round',[25 17 126]);
%
%workspace variable r contains now a 3x2 matrix where the first
%column shows the number of estimation cycle. The second column
%is the order of appearance of the estimate within the cycle:
%e.g. 2  5 : estimate 25 was 5th estimate in cycle 2
%     1 17 :          17     1st                   1
%     7  6 :         126     6th                   7
%
%   W=icassoGet(sR,'demixingmatrix',[25 17 126]);
%
%returns the demixing matrix rows for estimates 25, 17, and 126.
%
%   s=icassoGet(sR,'source');
%
%returns _all_ M estimated independent components (sources), i.e.,
%estimates from all resampling cycles.
%
%INPUT
%
%[An argument in brackets is optional. If it isn't  given or it's
% an empty matrix/string, the function will use a default value.]
%
% sR    (struct) Icasso result data structure (from icassoEst).
% field (string) (case insensitive) determines what is returned:
%    'data'           the original data centered
%    'N'              the number of estimation cycles
%    'M'              the total number of estimates available;
%                     equal to sum(icassoGet(sR,'numofic'));
%    'numofic'        number of ICs extracted on each cycle (Nx1
%                     vector)
%    'odim'           original data dimension
%    'rdim'           reduced data dimension (after PCA)
%    'round'          output is an Mx2 matrix that identifies from
%                     which estimation  round each estimate
%                     originates:
%                     out(i,1) is the estimation round for
%                     estimate i,
%                     out(i,2) is the ordinal number of the
%                     estimate within the round.
%    'source'         estimated source signals
%    'dewhitemat'     dewhitening matrix for the original data
%    'whitemat'       dewhitening matrix for the original data
%    'demixingmatrix' demixing matrix rows ('W' works as well)
%
% [index] (vector of integers) specifies which estimates to return
%          Applies only for 'demixingmatrix', and 'source'
%          Default: leaving this argument out corresponds to
%          selecting all estimates, i.e., giving [1:M] as index
%          vector.
%
%OUTPUT
%
% out   (type and meaning vary according to input argument 'field')
%
%SEE ALSO
% icassoResult

%COPYRIGHT NOTICE
%This function is a part of Icasso software library
%Copyright (C) 2003-2005 Johan Himberg
%
%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 2
%of the License, or 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, write to the Free Software
%Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

% ver 1.2 100105

M=size(sR.index,1);

if nargin<3,
   index=1:M;
end

switch lower(field)
 case 'm'
   out=size(sR.index,1);
 case 'n'
  out=length(sR.A);
 case 'odim'
  out=size(sR.whiteningMatrix,2);
 case 'rdim'
  out=size(sR.whiteningMatrix,1);
 case 'numofic'
  for i=1:length(sR.A),
    out(i,1)=size(sR.A{i},2);
  end
 case {'w','demixingmatrix'}
  out=getDeMixing(sR,index);
 case 'data'
  out=sR.signal;
 case {'dewhitemat'}
  out=sR.dewhiteningMatrix;
 case {'whitemat'}
  out=sR.whiteningMatrix;
 case 'source'
  out=getSource(sR,index);
 case 'round'
  out=sR.index(index,:)
 otherwise
   error('Unknown operation.');
end

%function A=getMixing(sR,index);
%
%function A=getMixing(sR,index);
%

% reindex
%index2=sR.index(index,:);
%N=size(index2,1); A=[];
%for i=1:N,
%   A(:,i)=sR.A{index2(i,1)}(:,index2(i,2));
%end

function W=getDeMixing(sR,index);
%
%function W=getDeMixing(sR,index);
%

% reindex
index2=sR.index(index,:);
N=size(index2,1); W=[];
for i=1:N,
   W(i,:)=sR.W{index2(i,1)}(index2(i,2),:);
end

function S=getSource(sR,index)
%function S=getSource(sR,index)
%

X=sR.signal;
W=getDeMixing(sR,index);
S=W*X;

%% Old stuff
%function dWh=getDeWhitening(sR,index);
%
%function dWh=getDeWhitening(sR,index);
%
%
%index=sR.index(index,:);
%Nindex=size(index,1); W=[];
%for i=1:Nindex,
%   dWh(:,i)=sR.dewhiteningMatrix{1}(:,index(i,2));
%end

%function W=getWhitening(sR,index);
%
%function W=getWhitening(sR,index);
%
%
%index=sR.index(index,:);
%Nindex=size(index,1); W=[];
%for i=1:Nindex,
%   W(:,i)=sR.whiteningMatrix{index(i,1)}(:,index(i,2));
%end