function [energy,constrast,Entropy,Homogeneity,standard_dev,Ph,Slope] = getHaralicParams(structNum,planC)
%function getHaralicParams(structNum)
%
%This function returns the haralic parameters for structure structNum.
%
%APA,12/26/2006

if ~exist('planC')
    global planC
end
indexS = planC{end};

scanNum                             = getStructureAssociatedScan(structNum,planC);
[rasterSegments, planC, isError]    = getRasterSegments(structNum,planC);
[mask3M, uniqueSlices]              = rasterToMask(rasterSegments, scanNum, planC);
scanArray3M                         = getScanArray(planC{indexS.scan}(scanNum));
SUVvals3M                           = mask3M.*double(scanArray3M(:,:,uniqueSlices));
[minr, maxr, minc, maxc, mins, maxs]= compute_boundingbox(mask3M);
volToEval                           = SUVvals3M(minr:maxr,minc:maxc,mins:maxs);
volToEval(volToEval==0)             = NaN;
%volToEval                           = volToEval - min(volToEval(:));
volToEval                           = volToEval / max(volToEval(:));
%volToEval                           = sqrt(volToEval);
[f,Ph]                              = haralick3D(volToEval,16);
suv3M                               = scanArray3M(:,:,uniqueSlices);
maskScaled3D = suv3M(find(mask3M));
% maskScaled3D = maskScaled3D/
standard_dev                        = std(single(maskScaled3D));
%standard_dev                        = std(suv3M(find(mask3M)));
energy                              = f(1);
constrast                           = f(2);
Entropy                             = f(4);
Homogeneity                         = f(8);
%Calculate slope
indexS = planC{end};
init_th  = 10;
final_th = 80;
n_th     = 10;
Thresholds = linspace (init_th, final_th, n_th);
% Slope = calc_slope_grigsby(structNum,Thresholds,planC);
Slope = NaN;
disp(['Energy     : ',num2str(energy)])
disp(['Contrast   : ',num2str(constrast)])
disp(['Entropy    : ',num2str(Entropy)])
disp(['Homogenity : ',num2str(Homogeneity)])
disp(['Std. Dev.  : ',num2str(standard_dev)])
disp(['Slope      : ',num2str(Slope)])


function month_name = month_to_month_name_hebrew ( y, m )

%*****************************************************************************80
%
%% MONTH_TO_MONTH_NAME_HEBREW returns the name of a Hebrew month.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, M, the year and month.
%
%    Output, string MONTH_NAME, the month name.
%
  name = [ ...
    'Tishri   '; 'Heshvan  '; 'Kislev   '; 'Tebet    '; 'Shebat   '; ...
    'Adar     '; 'Veadar   '; 'Nisan    '; 'Iyar     '; 'Sivan    '; ...
    'Tammuz   '; 'Ab       '; 'Elul     ' ];

  if ( year_is_embolismic_hebrew ( y ) )

    if ( m < 1 || 13 < m )
      month_name = '?????';
    else
      month_name = name(m,:);
    end

  else

    if ( m < 1 || 12 < m )
      month_name = '?????';
    elseif ( m <= 6 )
      month_name = name(m,:);
    else
      month_name = name(m+1,:);
    end

  end

  return
end


% KUKA sunrise toolbox example.
% moving end-effector of the robot on a circle.

% Copy right: Mohammad SAFEEA
% 16th-Oct-2017

clear all;
close all;
clc;

% Initial configuration
jPos={pi / 180 * 30, pi / 180 * 30, 0, -pi / 180 * 60, 0,...
                        pi / 180 * 90, 0};
% Start the KST, move robot to initial configuration, start directServo function
% [tKuka,flag]=startDaDirectServo(jPos );
%
% if flag==false
%     fprintf('Can not connect to KST \n');
%     fprintf('Program terminated \n');
%     return;
% end

% calculate current position of flange point of the robot
qs=zeros(7,1);
for i=1:7
    qs(i)=jPos{i};
end
        TefTool=eye(4);
        T0=directKinematics(qs,TefTool); % EEF frame transformation matrix
        p0=T0(1:3,4);
        Tt=T0;

% parameters of the circle:
r=0.15; % radius of the circle

% Joint space control

figure ();
    [Ttemp,J]=directKinematics(qs,TefTool);
    vec=Ttemp(1:3,4);
    plot3(vec(1,:),vec(2,:),vec(3,:));

pause(0.1);

        a=datevec(now);
        time0=a(6)+a(5)*60+a(4)*60*60; % calculate time at this instant

        deltaT0=2;

        theangleVec=[];
        timeVec=[];
% dls solver parameters
        n=10;
        lambda=0.1;
        TefTool=eye(4);

while true

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

    % calculate position of servo point
    if deltaT<deltaT0
        accel=0.4;
        w=accel*deltaT;
        theta=0.5*accel*deltaT*deltaT;
    else
        accel=0.4;
        theta0=0.5*accel*deltaT0*deltaT0;
        theta=w*(deltaT-deltaT0)+theta0;
    end

    if theta>2.5*pi
        break;
    end

    x=p0(1)+r*(cos(theta)-1);
    y=p0(2)+r*sin(theta);
    z=p0(3);
    p=[x;y;z];
    % calculate target transform
    Tt(1:3,4)=p;

    [ qs ] = kukaDLSSolver( qs, Tt, TefTool,n,lambda );

    [Ttemp,J]=directKinematics(qs,TefTool);
    vec=[vec,Ttemp(1:3,4)];
    plot3(vec(1,:),vec(2,:),vec(3,:))
    axis equal;
    pause(0.001);
    theangleVec=[theangleVec,theta];
    timeVec=[timeVec,deltaT];
end
figure;
plot(timeVec,theangleVec())
% close(figureHandle);
    % turn off the server
%     net_turnOffServer( tKuka );
%     fclose(tKuka);


%kitexture 'Texture Feature Extraction Using LAW Metrics'
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros itexture.pane file
%
% Parameters:
% InputFile: i 'Input object ', required: 'input object'
% OutputFile: o 'Output object', required: 'output object'
% Toggle: L5L5 'L5L5', default: 0: 'If set, apply L5L5 texture kernel'
% Toggle: E5L5 'E5L5', default: 0: 'If set, apply E5L5 texture kernel'
% Toggle: S5L5 'S5L5', default: 0: 'If set, apply S5L5 texture kernel'
% Toggle: W5L5 'W5L5', default: 0: 'If set, apply W5L5 texture kernel'
% Toggle: R5L5 'R5L5', default: 0: 'If set, apply R5L5 texture kernel'
% Toggle: L5E5 'L5E5', default: 0: 'If set, apply L5E5 texture kernel'
% Toggle: E5E5 'E5E5', default: 0: 'If set, apply E5E5 texture kernel'
% Toggle: S5E5 'S5E5', default: 0: 'If set, apply S5E5 texture kernel'
% Toggle: W5E5 'W5E5', default: 0: 'If set, apply W5E5 texture kernel'
% Toggle: R5E5 'R5E5', default: 0: 'If set, apply R5E5 texture kernel'
% Toggle: L5S5 'L5S5', default: 0: 'If set, apply L5S5 texture kernel'
% Toggle: E5S5 'E5S5', default: 0: 'If set, apply E5S5 texture kernel'
% Toggle: S5S5 'S5S5', default: 0: 'If set, apply S5S5 texture kernel'
% Toggle: W5S5 'W5S5', default: 0: 'If set, apply W5S5 texture kernel'
% Toggle: R5S5 'R5S5', default: 0: 'If set, apply R5S5 texture kernel'
% Toggle: L5W5 'L5W5', default: 0: 'If set, apply L5W5 texture kernel'
% Toggle: E5W5 'E5W5', default: 0: 'If set, apply E5W5 texture kernel'
% Toggle: S5W5 'S5W5', default: 0: 'If set, apply S5W5 texture kernel'
% Toggle: W5W5 'W5W5', default: 0: 'If set, apply W5W5 texture kernel'
% Toggle: R5W5 'R5W5', default: 0: 'If set, apply R5W5 texture kernel'
% Toggle: L5R5 'L5R5', default: 0: 'If set, apply L5R5 texture kernel'
% Toggle: E5R5 'E5R5', default: 0: 'If set, apply E5R5 texture kernel'
% Toggle: S5R5 'S5R5', default: 0: 'If set, apply S5R5 texture kernel'
% Toggle: W5R5 'W5R5', default: 0: 'If set, apply W5R5 texture kernel'
% Toggle: R5R5 'R5R5', default: 0: 'If set, apply R5R5 texture kernel'
%
% Example: o = kitexture(i, {'i','';'o','';'L5L5',0;'E5L5',0;'S5L5',0;'W5L5',0;'R5L5',0;'L5E5',0;'E5E5',0;'S5E5',0;'W5E5',0;'R5E5',0;'L5S5',0;'E5S5',0;'S5S5',0;'W5S5',0;'R5S5',0;'L5W5',0;'E5W5',0;'S5W5',0;'W5W5',0;'R5W5',0;'L5R5',0;'E5R5',0;'S5R5',0;'W5R5',0;'R5R5',0})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% itexture - Texture Feature Extraction Using LAW Metrics
%
%  DESCRIPTION
% .I itexture
% will extract texture features from an image by convolving the original image
% with a set of special spatial filter kernels.  The spatial filters are
% comprised of 5 x 5 kernels
% derived from center-weighted vectors defined by the LAWS texture measure.
%
% Each texture measure calculated will be stored as width-height planes along
% the elements direction in the Output Object (o).
% The order in which the (selected) texture measures will be stored in the
% output object are:
% .DS
%         L5L5
%         L5E5
%         L5S5
%         L5W5
%         L5R5
%         E5L5
%         E5E5
%         E5S5
%         E5W5
%         E5R5
%         S5L5
%         S5E5
%         S5S5
%         S5W5
%         S5R5
%         W5L5
%         W5E5
%         W5S5
%         W5W5
%         W5R5
%         R5L5
%         R5E5
%         R5S5
%         R5W5
%         R5R5
% .DE
%
% If the input object elements dimension is greater than 1 then the results
% obtained by applying the first selected kernel to each band are stored by planes
% in the output object, followed by the results obtained by applying the
% next selected texture kernel to all bands, and so on. Thus a 3-band input
% image, operated on by the L5L5 and R5R5 kernels, will produce a 6-band
% output image with the results as follows:
% .DS
%       Band 0 - L5L5 on Band 0 of input image
%       Band 1 - L5L5 on Band 1 of input image
%       Band 2 - L5L5 on Band 2 of input image
%       Band 3 - R5R5 on Band 0 of input image
%       Band 4 - R5R5 on Band 1 of input image
%       Band 5 - R5R5 on Band 2 of input image
% .DE
%
% The five center-weighted vectors are:
% .DS
%          L5 = [ 1  4  6  4  1]
%          E5 = [-1 -2  0  2  1]
%          S5 = [-1  0  2  0 -1]
%          W5 = [-1  2  0 -2  1]
%          R5 = [ 1 -4  6 -4  1]
% .DE
%
% Each 5 x 5 kernel is derived from multiplying a particular combination of
% two of the above vectors.  This results in 25 possible 5 x 5 kernels.
% Note that 10 of the kernels are formed by taking the transpose.  For example,
% L5E5 is a 5 x 5 kernel formed by multiplying the vectors, L5 with E5.  It's
% transpose is E5L5, and is similarly formed by multiplying the vectors,
% E5 with L5.  One of these kernels is more sensitive to horizontal
% changes in texture, while the other is sensitive to vertical changes in
% texture.
%
% The kernels are applied with a centered hotspot.
%
% The sum of the elements of each kernel is zero, which results in the output
% image having a mean of zero.  Therefore the relevant texture information is
% contained in the image variance of the microtexture features.  The LAW
% kernels were designed to be sensitive to structures such as edges, ripples,
% and spots.
%
% Choosing which kernel to use will depend on the nature of the texture of
% interest, and will require trial and error to decide which produces the
% feature with the most discriminating power.  In general, if a texture kernel
% of say, L5S5 is used, one may also want to use its transpose, S5L5 for
% certain types of quasiperiodic variations commonly found in textured images.
%
% The colorspace model for the output object is always set to KNONE.
%
%
%
%  EXAMPLES
%
%  "SEE ALSO"
%
%  RESTRICTIONS
% .I itexture
% will operate on input objects of any data storage type.
%
% The output object will be of type KLONG, KDOUBLE, or
% KDCOMPLEX, determined by the data type of the input object.
% If the input object is of data type KBYTE, KUBYTE, KSHORT, or KUSHORT,
% then it is converted up to an KLONG image. If the input
% is of type KFLOAT or KDOUBLE, then the result will be KDOUBLE.
% Any complex input type is converted to type KDCOMPLEX.
%
%  REFERENCES
% K. I. Laws, "Rapid texture identification" Proc. SPIE, vol. 238, pp.376-380,
% 1980.
%
% J. Y. Hsiao, and A. A. Sawchuk, "Supervised textured image segmentation
% using feature smoothing and probabilistic relaxation techniques" IEEE Trans.
% Pattern Anal. Machine Intell., vol. 11, No. 12, pp 1279-1292, 1989.
%
%  COPYRIGHT
% Copyright (C) 1993 - 1997, Khoral Research, Inc. ("KRI")  All rights reserved.
%

function varargout = kitexture(varargin)
if nargin ==0
  Inputs={};arglist={'',''};
elseif nargin ==1
  Inputs=varargin{1};arglist={'',''};
elseif nargin ==2
  Inputs=varargin{1}; arglist=varargin{2};
else error('Usage: [out1,..] = kitexture(Inputs,arglist).');
end
if size(arglist,2)~=2
  error('arglist must be of form {''ParameterTag1'',value1;''ParameterTag2'',value2}')
 end
narglist={'i', '__input';'o', '__output';'L5L5', 0;'E5L5', 0;'S5L5', 0;'W5L5', 0;'R5L5', 0;'L5E5', 0;'E5E5', 0;'S5E5', 0;'W5E5', 0;'R5E5', 0;'L5S5', 0;'E5S5', 0;'S5S5', 0;'W5S5', 0;'R5S5', 0;'L5W5', 0;'E5W5', 0;'S5W5', 0;'W5W5', 0;'R5W5', 0;'L5R5', 0;'E5R5', 0;'S5R5', 0;'W5R5', 0;'R5R5', 0};
maxval={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
minval={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
istoggle=[0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1];
was_set=istoggle * 0;
paramtype={'InputFile','OutputFile','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle'};
% identify the input arrays and assign them to the arguments as stated by the user
if ~iscell(Inputs)
Inputs = {Inputs};
end
NumReqOutputs=1; nextinput=1; nextoutput=1;
  for ii=1:size(arglist,1)
  wasmatched=0;
  for jj=1:size(narglist,1)
   if strcmp(arglist{ii,1},narglist{jj,1})  % a given argument was matched to the possible arguments
     wasmatched = 1;
     was_set(jj) = 1;
     if strcmp(narglist{jj,2}, '__input')
      if (nextinput > length(Inputs))
        error(['Input ' narglist{jj,1} ' has no corresponding input!']);
      end
      narglist{jj,2} = 'OK_in';
      nextinput = nextinput + 1;
     elseif strcmp(narglist{jj,2}, '__output')
      if (nextoutput > nargout)
        error(['Output nr. ' narglist{jj,1} ' is not present in the assignment list of outputs !']);
      end
      if (isempty(arglist{ii,2}))
        narglist{jj,2} = 'OK_out';
      else
        narglist{jj,2} = arglist{ii,2};
      end

      nextoutput = nextoutput + 1;
      if (minval{jj} == 0)
         NumReqOutputs = NumReqOutputs - 1;
      end
     elseif isstr(arglist{ii,2})
      narglist{jj,2} = arglist{ii,2};
     else
        if strcmp(paramtype{jj}, 'Integer') & (round(arglist{ii,2}) ~= arglist{ii,2})
            error(['Argument ' arglist{ii,1} ' is of integer type but non-integer number ' arglist{ii,2} ' was supplied']);
        end
        if (minval{jj} ~= 0 | maxval{jj} ~= 0)
          if (minval{jj} == 1 & maxval{jj} == 1 & arglist{ii,2} < 0)
            error(['Argument ' arglist{ii,1} ' must be bigger or equal to zero!']);
          elseif (minval{jj} == -1 & maxval{jj} == -1 & arglist{ii,2} > 0)
            error(['Argument ' arglist{ii,1} ' must be smaller or equal to zero!']);
          elseif (minval{jj} == 2 & maxval{jj} == 2 & arglist{ii,2} <= 0)
            error(['Argument ' arglist{ii,1} ' must be bigger than zero!']);
          elseif (minval{jj} == -2 & maxval{jj} == -2 & arglist{ii,2} >= 0)
            error(['Argument ' arglist{ii,1} ' must be smaller than zero!']);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} < minval{jj})
            error(['Argument ' arglist{ii,1} ' must be bigger than ' num2str(minval{jj})]);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} > maxval{jj})
            error(['Argument ' arglist{ii,1} ' must be smaller than ' num2str(maxval{jj})]);
          end
        end
     end
     if ~strcmp(narglist{jj,2},'OK_out') &  ~strcmp(narglist{jj,2},'OK_in')
       narglist{jj,2} = arglist{ii,2};
     end
   end
   end
   if (wasmatched == 0 & ~strcmp(arglist{ii,1},''))
        error(['Argument ' arglist{ii,1} ' is not a valid argument for this function']);
   end
end
% match the remaining inputs/outputs to the unused arguments and test for missing required inputs
 for jj=1:size(narglist,1)
     if  strcmp(paramtype{jj}, 'Toggle')
        if (narglist{jj,2} ==0)
          narglist{jj,1} = '';
        end;
        narglist{jj,2} = '';
     end;
     if  ~strcmp(narglist{jj,2},'__input') && ~strcmp(narglist{jj,2},'__output') && istoggle(jj) && ~ was_set(jj)
          narglist{jj,1} = '';
          narglist{jj,2} = '';
     end;
     if strcmp(narglist{jj,2}, '__input')
      if (minval{jj} == 0)  % meaning this input is required
        if (nextinput > size(Inputs))
           error(['Required input ' narglist{jj,1} ' has no corresponding input in the list!']);
        else
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        end
      else  % this is an optional input
        if (nextinput <= length(Inputs))
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end;
     else
     if strcmp(narglist{jj,2}, '__output')
      if (minval{jj} == 0) % this is a required output
        if (nextoutput > nargout & nargout > 1)
           error(['Required output ' narglist{jj,1} ' is not stated in the assignment list!']);
        else
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
          NumReqOutputs = NumReqOutputs-1;
        end
      else % this is an optional output
        if (nargout - nextoutput >= NumReqOutputs)
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end
     end
  end
end
if nargout
   varargout = cell(1,nargout);
else
  varargout = cell(1,1);
end
global KhorosRoot
if exist('KhorosRoot') && ~isempty(KhorosRoot)
w=['"' KhorosRoot];
else
if ispc
  w='"C:\Program Files\dip\khorosBin\';
else
[s,w] = system('which cantata');
w=['"' w(1:end-8)];
end
end
[varargout{:}]=callKhoros([w 'itexture"  '],Inputs,narglist);


classdef mme_bus_opf_acc < mp.mme_bus_opf_ac

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

%     properties
%         name = 'bus';
%     end

    methods
        function add_constraints(obj, mm, nm, dm, mpopt)
            dme = obj.data_model_element(dm);
            nme = obj.network_model_element(nm);

            %% voltage angle reference constraint
            ref = find(nme.node_types(nm, dm) == mp.NODE_TYPE.REF);
            varef = dme.va_start(ref);
            fcn_vref = @(xx)va_fcn(nme, xx, ref, varef);
            hess_vref = @(xx, lam)va_hess(nme, xx, lam, ref);
            mm.add_nln_constraint('Vref', length(ref), 1, fcn_vref, hess_vref, {'Vr', 'Vi'});

            %% fixed voltage magnitudes
            veq = find(dme.vm_lb == dme.vm_ub);
            nveq = length(veq);
            if nveq
                fcn_vm2eq = @(xx)vm2_fcn(nme, xx, veq, dme.vm_ub(veq).^2);
                hess_vm2eq = @(xx, lam)vm2_hess(nme, xx, lam, veq);
                mm.add_nln_constraint('Veq', nveq, 1, fcn_vm2eq, hess_vm2eq, {'Vr', 'Vi'});
            end
            mm.userdata.veq = veq;

            %% voltage magnitude limits
            viq = find(dme.vm_lb ~= dme.vm_ub);
            nviq = length(viq);
            if nviq
                fcn_vlim = @(xx)vm2_fcn(nme, xx, viq, ...
                        {dme.vm_lb(viq).^2, dme.vm_ub(viq).^2} );
                hess_vlim = @(xx, lam)vm2_hess(nme, xx, lam, viq);
                mm.add_nln_constraint({'Vmin', 'Vmax'}, [nviq;nviq], 0, fcn_vlim, hess_vlim, {'Vr', 'Vi'});
            end
            mm.userdata.viq = viq;
        end

        function x0 = interior_x0(obj, mm, nm, dm, x0)
            vv = mm.get_idx();
            varef1 = mm.interior_va(nm, dm);
            vm = obj.interior_vm(mm, nm, dm);
            v_ = vm * exp(1j*varef1);
            x0(vv.i1.Vr:vv.iN.Vr) = real(v_);
            x0(vv.i1.Vi:vv.iN.Vi) = imag(v_);
        end

        function obj = data_model_update(obj, mm, nm, dm, mpopt)
            %% complex bus voltages
            nn = nm.get_idx('node');
            V = nm.soln.v(nn.i1.bus:nn.iN.bus);

            %% shadow prices on voltage magnitudes
            [nne, nni] = mm.get_idx('nle', 'nli');
            lambda = mm.soln.lambda;
            mu_vm_lb = zeros(nn.N.bus, 1);  %% init to all 0
            mu_vm_ub = mu_vm_lb;            %% init to all 0
            if mm.userdata.veq
                lam = lambda.eqnonlin(nne.i1.Veq:nne.iN.Veq);
                lam_p = zeros(size(lam));
                lam_n = zeros(size(lam));
                lam_p(lam > 0) =  lam(lam > 0);
                lam_n(lam < 0) = -lam(lam < 0);
                mu_vm_lb(mm.userdata.veq) = lam_n;
                mu_vm_ub(mm.userdata.veq) = lam_p;
            end
            mu_vm_lb(mm.userdata.viq) = lambda.ineqnonlin(nni.i1.Vmin:nni.iN.Vmin);
            mu_vm_ub(mm.userdata.viq) = lambda.ineqnonlin(nni.i1.Vmax:nni.iN.Vmax);

            vm = abs(V);
            mu_vm_lb = mu_vm_lb .* vm * 2;
            mu_vm_ub = mu_vm_ub .* vm * 2;

            %% shadow prices on node power balance
            [lam_p, lam_q] = mm.node_power_balance_prices(nm);
            lam_p = lam_p(nn.i1.bus:nn.iN.bus);     %% for bus nodes only
            lam_q = lam_q(nn.i1.bus:nn.iN.bus);     %% for bus nodes only

            %% update in the data model
            dme = obj.data_model_element(dm);
            dme.tab.va(dme.on) = angle(V) * 180/pi;
            dme.tab.vm(dme.on) = abs(V);
            dme.tab.lam_p(dme.on) = lam_p / dm.base_mva;
            dme.tab.lam_q(dme.on) = lam_q / dm.base_mva;
            dme.tab.mu_vm_lb(dme.on) = mu_vm_lb;
            dme.tab.mu_vm_ub(dme.on) = mu_vm_ub;
        end
    end     %% methods
end         %% classdef


classdef ParadigmTFR < ParadigmDataflowSimplified
    % Time/Frequency Regression. This is a new approach that is not yet published.
    %
    % This method learns a linear classifier (or regressor) of second-order dynamics in the EEG over
    % a particular set of time and frequency bins. The key trick lies in a set of extra assumptions
    % that make the (very high-dimensional) solution statistically tractable. The algorithm amounts
    % to a single convex optimization problem dependent on a small number of hyper-parameters that can
    % be optimized using grid search. For a reasonably exhaustive grid the solution is the globally
    % optimal second-order dynamics solution.
    %
    % Notes:
    %  This implementation is not yet optimally tuned -- the spectral estimation should be replaced by a
    %  similar approach to multi-taper CSP.
    %
    % Examples:
    %   % learn a spectral classifier for features within -2 to 2 seconds relative to some marker in some standard EEG frequency bands
    %   % here using area under curve (AUC) to optimize the regularization parameter (assuming that the number of exemplars per class is imbalanced)
    %   % and setting the set of relative regularization term weights that should be searched over to {[1 1 1 1]}, i.e., not searching over those, for speed.
    %   myapproach = {'TFR' 'SignalProcessing',{'EpochExtraction',[-2 2]},'Prediction',{'FeatureExtraction',{'Times',[-1.5,-1,-0.5,0,0.5,1,1.5],'Frequencies',[4 7 11 14 20 30]}, ...
    %       'MachineLearning',{'Learner',{'proximal','LambdaSearch',{'ParameterMetric','auc'},'TermWeights',{[1 1 1 1]}}}}};
    %
    % Name:
    %   Time/Frequency Regression
    %
    %                            Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
    %                            2013-03-08

    methods

        function defaults = preprocessing_defaults(self)
            % define the default pre-processing parameters of this paradigm
            defaults = {'FIRFilter',{[0.5 1],'highpass'}, 'EpochExtraction',[-3 3], 'Resampling',100};
        end

        function defaults = machine_learning_defaults(self)
            % global sharing approach
            defaults = {'proximal', 'lambdaSearch',{'lambdas',2.^(5:-0.1:-4),'foldmargin',0},'regularizers',{ ...
                    'term1',{'trace'}, ...                                                                      % the weights are a small sum of second-order spatial filters per T/F resel
                    'term2',{'trace','LinearOperator','@(x)reshape(x,a*b,c*d)'}, ...                            % across all of time/frequency we learn combinations of few latent spatial filters
                    'term3',{'l2', 'LinearOperator','@(x)vec(diff(x,[],4))','NonorthogonalTransform',true}, ... % temporal smoothness
                    'term4',{'l2', 'LinearOperator','@(x)vec(diff(x,[],3))','NonorthogonalTransform',true}, ... % spectral smoothness
                }, 'regweights',{[1 1 1 1],[1 1 0.5 0.5],[1 1 2 2],[1 1 0.25 0.25],[1 1 4 4],[1 2 1 2],[2 1 1 2],[1 2 2 1],[2 1 2 1]}};
        end

        function model = feature_adapt(self,varargin)
            % adapt a feature representation using the CSP algorithm
            args = arg_define(varargin, ...
                arg_norep('signal'), ...
                arg({'freqpoints','Frequencies'},[4 7 11 14 20 30],[0 0.2 200 1000],'Frequency points to consider.'),...
                arg({'timepoints','Times'},-2.5:0.5:2.5,[],'Time points to consider.'), ...
                arg({'from_edges','FromEdges'},true,[],'Run up to epoch edges. This is both in time and frequency.'), ...
                arg({'vectorize_features','VectorizeFeatures'},false,[],'Vectorize the features. For compatibility with basic classifiers.'));
            model.args = rmfield(args,'signal');
            model.chanlocs = args.signal.chanlocs;
        end

        function [features,shape] = feature_extract(self,signal,featuremodel)
            % first perform the time/freq decomposition
            args = featuremodel.args;
            if args.from_edges
                args.timepoints = [signal.xmin args.timepoints signal.xmax];
                args.freqpoints = [0 args.freqpoints signal.srate/2];
            end
            time2idx = @(t) min(signal.pnts,max(1,1+round((t-signal.xmin)*signal.srate)));
            half_hann = @(a,b) 0.5*(1-cos(pi*((((a+1):b) - a) / (b - a))));
            shape = [size(signal.data,1),size(signal.data,1),length(args.freqpoints)-2,length(args.timepoints)-2];
            features = zeros([shape signal.trials]);
            for t=2:length(args.timepoints)-1
                % calculate time window function
                last_tp = time2idx(args.timepoints(t-1));
                cur_tp = time2idx(args.timepoints(t));
                next_tp = time2idx(args.timepoints(t+1));
                wndrange = last_tp+1:next_tp;
                wndfunc = [half_hann(last_tp,cur_tp) 1-half_hann(cur_tp,next_tp)];
                % window and fourier-transform the data
                X = fft(bsxfun(@times,wndfunc,signal.data(:,wndrange,:)),[],2);
                % multiply out the cross-spectral covariance
                tmp = zeros(size(X,1),size(X,1),ceil(size(X,2)/2),size(X,3));
                freq2idx = @(f) min(size(tmp,3),max(1,1+round(f*length(wndrange)/signal.srate)));
                for f=1:size(tmp,3)
                    for n=1:size(tmp,4)
                        tmp(:,:,f,n) = 2*real(X(:,f,n)*X(:,f,n)');
                    end
                end
                % average around frequency centers
                for f=2:length(args.freqpoints)-1
                    last_fp = freq2idx(args.freqpoints(f-1));
                    cur_fp = freq2idx(args.freqpoints(f));
                    next_fp = freq2idx(args.freqpoints(f+1));
                    wndrange = last_fp+1:next_fp;
                    wndfunc = [half_hann(last_fp,cur_fp) 1-half_hann(cur_fp,next_fp)];
                    features(:,:,f-1,t-1,:) = mean(bsxfun(@times,tmp(:,:,wndrange,:),reshape(wndfunc,1,1,[])),3);
                end
            end
            % do final vectorization if desired
            if featuremodel.args.vectorize_features
                features = reshape(features,[],signal.trials)'; end
        end

        function layout = dialog_layout_defaults(self)
            layout = {'SignalProcessing.Resampling.SamplingRate','SignalProcessing.FIRFilter.Frequencies', ...
                '', 'SignalProcessing.EpochExtraction','', ...
                'Prediction.FeatureExtraction.Frequencies', 'Prediction.FeatureExtraction.Times', ...
                'Prediction.FeatureExtraction.FromEdges','', ...
                'Prediction.MachineLearning.Learner.LossType', '', ...
                'Prediction.MachineLearning.Learner.LambdaSearch.Lambdas', ...
                'Prediction.MachineLearning.Learner.TermWeights', ...
                'Prediction.MachineLearning.Learner.LambdaSearch.ParameterMetric'};
        end

    end
end


%% Models
% Instantiate a Transitionamic model
transition_model = ConstantVelocityX('NumDims',2,'VelocityErrVariance',0.0001);

% Instantiate an Observation model
measurement_model = LinearGaussianX('NumMeasDims',2,'NumStateDims',4,'MeasurementErrVariance',0.02,'Mapping',[1 3]);
%measurement_model = RangeBearing2CartesianX('NumStateDims',4,'MeasurementErrVariance',[0.001,0.02],'Mapping',[1 3]);

% Compile the State-Space model
model = StateSpaceModelX(transition_model,measurement_model);

%% Groud-Truth Simulator
num_timesteps = 100;
timestep_duration = duration(0,0,5);
initial_state = GroundTruthStateX([0; 0; 0; 0], datetime());
gnd_sim = SingleTargetGroundTruthSimulatorX('Model',model, ...
                                            'InitialState', initial_state,...
                                            'NumTimesteps', num_timesteps,...
                                            'TimestepDuration', timestep_duration);

%% Simulate Ground-Truth
track = gnd_sim.simulate();

%% Measurement simulator
det_sim = SingleTargetMeasurementSimulatorX(model);

%% Simulate measurements
measurements = det_sim.simulate(track);

%% Plot the output
plotter = BasicPlotterX(model);

plotter.figure();
plotter.plotObject(track);
plotter.plotObject(measurements);


function bnet = learn_params_dbn(bnet, data)
% LEARN_PARAM_DBN Estimate params of a DBN for a fully observed model
% bnet = learn_params_dbn(bnet, data)
%
% data(i,t) is the value of node i in slice t (can be a cell array)
% We currently assume there is a single time series
%
% We set bnet.CPD{i} to its ML/MAP estimate.
%
% Currently we assume each node in the first 2 slices has its own CPD (no param tying);
% all nodes in slices >2 share their params with slice 2 as usual.

[ss T] = size(data);

% slice 1
for j=1:ss
  if adjustable_CPD(bnet.CPD{j})
    fam = family(bnet.dag,j);
    bnet.CPD{j} = learn_params(bnet.CPD{j}, data(fam,1));
  end
end

% slices 2:T
% data2(:,t) contains [data(:,t-1); data(:,t)].
% Then we extract out the rows corresponding to the parents in the current and previous slice.
data2 = [data(:,1:T-1);
	 data(:,2:T)];
for j=1:ss
  j2 = j+ss;
  if adjustable_CPD(bnet.CPD{j2})
    fam = family(bnet.dag,j2);
    bnet.CPD{j2} = learn_params(bnet.CPD{j2}, data2(fam,:));
  end
end


function [ Y ] = classify1( Model, X )

    % Extract Features from the test data
    data = getImages(X);

    weights = Model.weights;
    projection = Model.projection;

    % Project the test data on the new dimension
    xTest = getFeatures(data, projection);

    % Classify the images
    [Y, ~] = LRClassify( weights, xTest );
end

function [Features] =  getImages(data)
    Features = [];
    for i = 1:size(data,1)
        image = reshape(data(i,:),[32,32,3]);
        image = imresize(image,4);
        feat = extract_feature(image);
        Features = horzcat(Features,feat);
    end
    Features = Features';
end

function [Feats] = getFeatures(Features, projection)
    featureSize = size(Features,2);

    % Perform Mean Normalization on the Feature Matrix.
    % Each pixel can have intensity values from 0 - 255
    for i = 1 : featureSize
        Features(:,i) = Features(:,i) - mean(Features(:,i));
    end

    % Project the Features on the new dimension
    Feats = projection * Features';
    Feats = Feats';
end


function DEMO_GROUP_PEB
% Demonstration routine for empirical Bayes and Bayesian model reduction
%--------------------------------------------------------------------------
% This routine illustrates the use of Bayesian model reduction when
% inverting hierarchical (linear) models - it is essentially a software
% validation demo and proof of concept. It uses a parametric empirical
% Bayesian model (i.e., nested linear models) to eschew local minima issues
% and to assure the Laplace assumption is correct. In brief, the data are
% generated for multiple subjects, under a linear model with subject
% specific parameters at the first level and group specific parameters at
% the second. These model a group effect common to all subjects in a subset
% of parameters and differences in a further subset. In this demo, we
% consider the full hierarchical inversion of a multisubject  study by
% updating the priors at the first level, using the empirical priors from
% the second level. Crucially, this is done during the optimisation  at the
% first level (i.e., after  every iteration - or small number of iterations
% - at the first level.
%
% This provides a generic scheme for the hierarchical inversion of
% nonlinear and possibly dynamic models in which the first level
% optimisation is informed by the sufficient statistics of the second level
% (namely the empirical priors). This should be contrasted with the summary
% statistic approach, in which the second level optimisation, based upon
% the sufficient statistics of the first level (posteriors and priors) are
% computed after convergence of the first level. The results of inversion
% are compared in terms of the second level posteriors (and the second
% level free energy over iterations). Specifically, we compare a gold
% standard (PEB) inversion, with the summary statistic approach to
% empirical Bayes and the hierarchical inversion demonstrated in this
% routine.
%
% The parameterisation of the models uses the format of DCM. This means
% parameters are specified as a structure with key parameters being in the
% fields A, B and C.
%
% See also: spm_dcm_bmr, spm_dcm_peb and spm_dcm_peb_bma
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Karl Friston, Peter Zeidman
% $Id: DEMO_GROUP_PEB.m 6737 2016-03-03 12:05:51Z karl $

% set up
%==========================================================================
rng('default')

% model space - defined in terms of combinations of some parameters
%--------------------------------------------------------------------------
k     = spm_perm_mtx(3);
for i = 1:8;
    B{i} = k(i,:);
end

% model space
%--------------------------------------------------------------------------
mw  = 3;                              % true model (within)
mx  = 4;                              % true model (between)
Ns  = 16;                             % number of subjects
C   = 32;                             % within:between [co]variance ratio

% create subject-specifc GLM
%==========================================================================

% within subject effects:  condition specific effects 'B' (2 s.d.)
%--------------------------------------------------------------------------
pC          = 1/8;
sd          = sqrt(pC/C);
DCM.Ep.A    = randn(4,1)*sd;
DCM.Ep.B{1} = B{mw}*2*sd;
Np          = spm_length(DCM.Ep);
DCM.M.pE    = spm_zeros(DCM.Ep);
DCM.M.pC    = eye(Np,Np)*pC;

% between subject effects: constant and group difference
%--------------------------------------------------------------------------
X           = [ones(Ns,1) kron([-1;1],ones(Ns/2,1))];
DCM.Ex      = spm_zeros(DCM.Ep);
DCM.Ex.B{1} = B{mx}*2*sd;

% create subject-specifc DCM
%--------------------------------------------------------------------------
Ex    = spm_vec(DCM.Ex);
Ep    = spm_vec(DCM.Ep);
pC    = DCM.M.pC;
Cp    = sd*diag(~~spm_vec(Ep));
Ny    = 16;
for i = 1:Ns

    % generate data
    %----------------------------------------------------------------------
    Pp    = X(i,1)*Ep + X(i,2)*Ex + Cp*randn(Np,1);

    % generate data
    %----------------------------------------------------------------------
    Z{i,i} = randn(Ny,Np);
    y{i,1} = Z{i,i}*Pp + randn(Ny,1)/8;

    % design matrix and data
    %----------------------------------------------------------------------
    GCM{i,1}.xU    = Z{i,i};
    GCM{i,1}.xY.y  = y{i,1};
    GCM{i,1}.xY.X0 = [];

    % likelihood model and priors
    %----------------------------------------------------------------------
    GCM{i,1}.M.IS  = @(P,M,U) U*spm_vec(P);
    GCM{i,1}.M.pE  = DCM.M.pE;
    GCM{i,1}.M.pC  = pC;
    GCM{i,1}.Tp    = Pp;

end

% PEB (GLM) for inversion to provide a reference for BMR
%==========================================================================
Nx    = size(X,2);
Q     = spm_Ce(ones(1,Np));
for i = 1:Np
    Q{i} = kron(eye(Ns,Ns),Q{i})/128;
end
P{1}.X = spm_cat(Z);
P{1}.C = spm_Ce(ones(1,Ns)*Ny);
P{2}.X = kron(X,eye(Np,Np));
P{2}.C = Q;
P{3}.X = kron(zeros(Nx,1),zeros(Np,1));
P{3}.C = kron(eye(Nx,Nx),pC);

% Full hierarchical parametric empirical Bayes inversion
%--------------------------------------------------------------------------
[qP,~,F] = spm_PEB(spm_cat(y),P,1);

% record estimates as a reference
%--------------------------------------------------------------------------
PB.F  = F;
PB.Ep = qP{3}.E;
PB.Cp = qP{3}.C;

% repeat using non-linear empirical Bayes
%==========================================================================

% second level model
%--------------------------------------------------------------------------
M.X   = X;
M.pE  = DCM.M.pE;
M.pC  = DCM.M.pC;

% hierarchical inversion
%--------------------------------------------------------------------------
[gcm,peb,M] = spm_dcm_peb_fit(GCM,M);

% repeated using Bayesian model reduction summary statistic approach
%==========================================================================
GCM  = spm_dcm_fit(GCM);
PEB  = spm_dcm_peb(GCM,M);

% second level parameter estimates
%==========================================================================
spm_figure('GetWin','Figure 1'); clf

% estimated and true second level parameters
%--------------------------------------------------------------------------
subplot(2,2,1), spm_plot_ci(PB.Ep,PB.Cp), hold on, bar([Ep;Ex],1/2), hold off
xlabel('parameters'), ylabel('expectation'),
title('Parametric Bayes','FontSize',16), axis square

subplot(2,2,2), spm_plot_ci(PEB.Ep(:),PEB.Cp), hold on, bar([Ep;Ex],1/2), hold off
xlabel('parameters'), ylabel('expectation'),
title('Sufficient statistics','FontSize',16), axis square

subplot(2,2,3), spm_plot_ci(peb.Ep(:),peb.Cp), hold on, bar([Ep;Ex],1/2), hold off
xlabel('parameters'), ylabel('expectation'),
title('Hierarchical inversion','FontSize',16), axis square

subplot(2,2,4), bar(gcm{1}.FEB)
xlabel('iterations'), ylabel('free energy'),
title('second level free energy','FontSize',16), axis square

return

% Bayesian model reduction with and without hierarchical inversion
%==========================================================================

% define the model space in terms of a matrix
%--------------------------------------------------------------------------
K     = ones(length(B),spm_length(DCM.Ep));
k     = spm_fieldindices(DCM.M.pE,'B');
for i = 1:length(B)
    K(i,k) = spm_vec(B{i})';
end

% defined model in terms of prior covariance
%--------------------------------------------------------------------------
Nm    = size(K,2);
for i = 1:Ns
    for j = 1:Nm
        gcm{i,j}      = gcm{i,1};
        GCM{i,j}      = GCM{i,1};
        gcm{i,j}.M.pC = diag(K(j,:))*M.pC*diag(K(j,:));
        GCM{i,j}.M.pC = diag(K(j,:))*M.pC*diag(K(j,:));
    end
end

rcm   = spm_dcm_bmr(gcm);
RCM   = spm_dcm_bmr(GCM);

% Free energies
%--------------------------------------------------------------------------
for i = 1:Ns
    for j = 1:Nm
        G(i,j,1) = rcm{i,j}.F - rcm{i,1}.F;
        G(i,j,2) = RCM{i,j}.F - RCM{i,1}.F;
    end
end

%  free energy model comparison
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 2');clf

p  = spm_softmax(sum(G(:,:,1))'); [m i] = max(p);
subplot(2,2,1), bar(p)
text(i - 1/4,m/2,sprintf('%-2.0f%%',m*100),'Color','w','FontSize',8)
xlabel('model'), ylabel('probability'), title('Iterative PEB','FontSize',16)
axis([0 (length(p) + 1) 0 1]), axis square

p  = spm_softmax(sum(G(:,:,1))'); [m i] = max(p);
subplot(2,2,2), bar(p)
text(i - 1/4,m/2,sprintf('%-2.0f%%',m*100),'Color','w','FontSize',8)
xlabel('model'), ylabel('probability'), title('Bayesian model reduction','FontSize',16)
axis([0 (length(p) + 1) 0 1]), axis square


clear all;
randn('seed',0);
format compact;
param.num_threads=-1; % all cores (-1 by default)
param.verbose=false;   % verbosity, false by default
param.lambda=0.1; % regularization parameter
param.it0=1;      % frequency for duality gap computations
param.max_it=100; % maximum number of iterations
param.L0=0.1;
param.tol=1e-5;
param.intercept=false;
param.pos=false;

graph.eta_g=[1 1 1 1 1];
graph.groups=sparse([0 0 0 1 0;
                     0 0 0 0 0;
                     0 0 0 0 0;
                     0 0 0 0 0;
                     0 0 1 0 0]);   % g5 is included in g3, and g2 is included in g4
graph.groups_var=sparse([1 0 0 0 0;
                         1 0 0 0 0;
                         1 0 0 0 0 ;
                         1 1 0 0 0;
                         0 1 0 1 0;
                         0 1 0 1 0;
                         0 1 0 0 1;
                         0 0 0 0 1;
                         0 0 0 0 1;
                         0 0 1 0 0]); % represents direct inclusion relations

X=randn(100,10);
param.verbose=true;
%X=eye(10);
X=X-repmat(mean(X),[size(X,1) 1]);
X=mexNormalize(X);
Y=randn(100,1);
Y=Y-repmat(mean(Y),[size(Y,1) 1]);
Y=mexNormalize(Y);
W0=zeros(size(X,2),size(Y,2));
% Regression experiments
% 100 regression problems with the same design matrix X.
fprintf('\nVarious regression experiments\n');
param.compute_gram=true;
fprintf('\nFISTA + Regression graph\n');
param.loss='square';
param.regul='graph';
tic
[W optim_info]=mexFistaGraph(Y,X,W0,graph,param);
t=toc;
fprintf('mean loss: %f, mean relative duality_gap: %f, time: %f, number of iterations: %f\n',mean(optim_info(1,:)),mean(optim_info(3,:)),t,mean(optim_info(4,:)));

fprintf('\nADMM + Regression graph\n');
param.admm=true;
param.lin_admm=true;
param.c=1;
param.delta=1;
tic
[W optim_info]=mexFistaGraph(Y,X,W0,graph,param);
t=toc;
fprintf('mean loss: %f, stopping criterion: %f, time: %f, number of iterations: %f\n',mean(optim_info(1,:)),mean(optim_info(3,:)),t,mean(optim_info(4,:)));

param.admm=false;
param.max_it=5;
param.it0=1;
tic
[W optim_info]=mexFistaGraph(Y,X,W,graph,param);
t=toc;
fprintf('mean loss: %f, mean relative duality_gap: %f, time: %f, number of iterations: %f\n',mean(optim_info(1,:)),mean(optim_info(3,:)),t,mean(optim_info(4,:)));

% works also with non graph-structured regularization. graph is ignored
fprintf('\nFISTA + Regression Fused-Lasso\n');
param.regul='fused-lasso';
param.lambda2=0.01;
param.lambda3=0.01; %
tic
[W optim_info]=mexFistaGraph(Y,X,W0,graph,param);
t=toc;
fprintf('mean loss: %f, time: %f, number of iterations: %f\n',mean(optim_info(1,:)),t,mean(optim_info(4,:)));

fprintf('\nFISTA + Regression graph with intercept \n');
param.intercept=true;
param.regul='graph';
tic
[W optim_info]=mexFistaGraph(Y,[X ones(size(X,1),1)],[W0; zeros(1,size(W0,2))],graph,param);
t=toc;
fprintf('mean loss: %f, mean relative duality_gap: %f, time: %f, number of iterations: %f\n',mean(optim_info(1,:)),mean(optim_info(3,:)),t,mean(optim_info(4,:)));
param.intercept=false;

% Classification
fprintf('\nOne classification experiment\n');
Y=2*double(randn(100,size(Y,2)) > 0)-1;
fprintf('\nFISTA + Logistic + graph-linf\n');
param.regul='graph';
param.loss='logistic';
param.lambda=0.01;
tic
[W optim_info]=mexFistaGraph(Y,X,W0,graph,param);
t=toc;
fprintf('mean loss: %f, mean relative duality_gap: %f, time: %f, number of iterations: %f\n',mean(optim_info(1,:)),mean(optim_info(3,:)),t,mean(optim_info(4,:)));
% can be used of course with other regularization functions, intercept,...

% Multi-Class classification
Y=double(ceil(5*rand(100,size(Y,2)))-1);
param.loss='multi-logistic';
param.regul='graph';
fprintf('\nFISTA + Multi-Class Logistic + graph \n');
tic
nclasses=max(Y(:))+1;
W0=zeros(size(X,2),nclasses*size(Y,2));
[W optim_info]=mexFistaGraph(Y,X,W0,graph,param);
t=toc;
fprintf('mean loss: %f, mean relative duality_gap: %f, time: %f, number of iterations: %f\n',mean(optim_info(1,:)),mean(optim_info(3,:)),t,mean(optim_info(4,:)));
% can be used of course with other regularization functions, intercept,...

% Multi-Task regression
Y=randn(100,size(Y,2));
Y=Y-repmat(mean(Y),[size(Y,1) 1]);
Y=mexNormalize(Y);
param.compute_gram=false;
param.verbose=true;   % verbosity, false by default
W0=zeros(size(X,2),size(Y,2));
param.loss='square';
fprintf('\nFISTA + Regression multi-task-graph \n');
param.regul='multi-task-graph';
param.lambda2=0.01;
tic
[W optim_info]=mexFistaGraph(Y,X,W0,graph,param);
toc
fprintf('mean loss: %f, mean relative duality_gap: %f, time: %f, number of iterations: %f\n',mean(optim_info(1,:)),mean(optim_info(3,:)),t,mean(optim_info(4,:)));

% Multi-Task Classification
fprintf('\nFISTA + Logistic + multi-task-graph \n');
param.regul='multi-task-graph';
param.lambda2=0.01;
param.loss='logistic';
Y=2*double(randn(100,size(Y,2)) > 0)-1;
tic
[W optim_info]=mexFistaGraph(Y,X,W0,graph,param);
toc
fprintf('mean loss: %f, mean relative duality_gap: %f, time: %f, number of iterations: %f\n',mean(optim_info(1,:)),mean(optim_info(3,:)),t,mean(optim_info(4,:)));
% Multi-Class + Multi-Task Regularization

param.verbose=false;
fprintf('\nFISTA + Multi-Class Logistic +multi-task-graph \n');
Y=double(ceil(5*rand(100,size(Y,2)))-1);
param.loss='multi-logistic';
param.regul='multi-task-graph';
tic
nclasses=max(Y(:))+1;
W0=zeros(size(X,2),nclasses*size(Y,2));
[W optim_info]=mexFistaGraph(Y,X,W0,graph,param);
t=toc;
fprintf('mean loss: %f, mean relative duality_gap: %f, time: %f, number of iterations: %f\n',mean(optim_info(1,:)),mean(optim_info(3,:)),t,mean(optim_info(4,:)));
% can be used of course with other regularization functions, intercept,...


% Jiao Xianjun (putaoshu@msn.com; putaoshu@gmail.com)
% analyze PSS detection under big frequency offset
% A script of project: https://github.com/JiaoXianjun/rtl-sdr-LTE

% according to simulation result, gold configuration is that use whole PSS (length 137), and fo step 50kHz
% simulation result:
% len pss 137 fo step 50kHz SNR 0dB 10000 cost 29.9569s fail rate 0.003
% len pss 137 fo step 50kHz SNR 3dB 10000 cost 27.4461s fail rate 0
% len pss 137 fo step 25kHz SNR 0dB 10000 cost 40.6559s fail rate 0.0025
% len pss 137 fo step 25kHz SNR 3dB 10000 cost 41.6056s fail rate 0
% len pss 128 fo step 50kHz SNR 0dB 10000 cost 23.6994s fail rate 0.0091
% len pss 128 fo step 50kHz SNR 3dB 10000 cost 23.0074s fail rate 0.0002
% len pss 128 fo step 25kHz SNR 0dB 10000 cost 41.1991s fail rate 0.0078
% len pss 128 fo step 25kHz SNR 3dB 10000 cost 41.4224s fail rate 0

% function test_pss_fo_corr
clear all;
close all;

[fd_pss, td_pss] = pss_gen;

pss_idx = 1;
% pss = td_pss(end-127:end, pss_idx);
pss = td_pss(:, pss_idx);
len_pss = length(pss);

fo_search_set = [-200e3:50e3:200e3];
% fo_search_set = 0;
pss_set = kron(ones(1, length(fo_search_set)), pss).*exp(1i.*2.*pi.*(1./1.92e6).*(0:(length(pss)-1)).'*fo_search_set);
fo_step = (fo_search_set(2) - fo_search_set(1))/1e3;

var_pss = mean(abs(td_pss(:, pss_idx)).^2);

r = zeros(1, 2*137);
tmp_total = zeros(1, length(fo_search_set));
num_test = 10000;
num_fail = 0;
snr = 3;
sigma2 = var_pss/(10^(snr/10));
correct_pos = 147 - (length(pss)-128);
tic;
for idx = 1:num_test
    r_pss = [(randn(137,1)+1i.*randn(137,1)).*sqrt(var_pss./2); td_pss(:, pss_idx); (randn(137,1)+1i.*randn(137,1)).*sqrt(var_pss./2)];
    r_pss = r_pss + sqrt(sigma2/2).*(randn(length(r_pss),1)+1i.*randn(length(r_pss),1));

    fo = (2*rand-1)*200e3;
%     fo = (-7.5e3/1) + 0*15e3;
    r_pss_fo = r_pss.*exp(1i.*2.*pi.*fo.*(1./1.92e6).*(0:(length(r_pss)-1))');

    for i=1:(2*137)
        s = r_pss_fo(i:(i+length(pss)-1));

%         %  %---------method 1-----------------------
%         for j=1:length(fo_search_set)
%             tmp = conj(pss_set(:,j)).*s;
%             tmp = vec2mat(tmp, 8);
%             tmp = sum( abs(sum(tmp, 2)).^2 );
% %             r(i) = r(i) + tmp;
%             tmp_total(j) = tmp;
%         end
%         r(i) = max(tmp_total);
%         %  %---------end of method 1-----------------------

%         %  %---------method 2-----------------------
%         r(i) = abs(s(1:64)'*s(128:-1:65))^2;
%         %  %---------end of method 2-----------------------

        %  %---------method 3-----------------------
        tmp = conj(pss).*s;
        r(i) = max(abs(fft(tmp)).^2);
        %  %---------end of method 3-----------------------
    end
    plot(r); drawnow;
    % plot(138, r(138), 'rs');

    if mod(idx, 100) == 0
        disp(['len pss ' num2str(len_pss) ' fo step ' num2str(fo_step) 'kHz SNR ' num2str(snr) 'dB ' num2str(idx) ' cost ' num2str(toc) 's fail rate ' num2str(num_fail/idx) ]);
        tic;
    end

    [~, max_idx] = max(r);
    if max_idx ~= correct_pos
%         plot(r); hold on;
%         plot(correct_pos, r(correct_pos), 'rs');
        num_fail = num_fail + 1;
        disp(['idx ' num2str(idx) ' max_idx ' num2str(max_idx) ' fail rate ' num2str(num_fail/idx)]);
%         break;
    end
end

disp(['fail rate ' num2str(num_fail/idx)]);


function obj = getColorPrior(image,k)
    x = reshape(permute(image,[3 1 2]),[3 size(image,1) * size(image,2)]);
    vgg_mean =  [103.939, 116.779, 123.68];
    x = double(x([3 2 1],:));
    x = bsxfun(@minus,x,vgg_mean');
    options = statset('MaxIter',1000);
    while k>0
        try
            fprintf('color GMM, try clusters=%d...\n',k);
            obj = gmdistribution.fit(x',k,'Options',options);
            break;
        catch
            k = k-1;
        end;
    end;
end


function [DCM] = VBA_spm_sdcm_estimate2(P)
% Estimate parameters of a stochastic DCM for fMRI data (deconv HRF)
% FORMAT [DCM] = spm_sdcm_estimate2(DCM)
%
% DCM  - the DCM or its filename
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Jean Daunizeau

% load DCM structure
if ~nargin
    P = VBA_spm_select(1,'^DCM.*\.mat$','select DCM_???.mat');
    if isempty(P)
        return
    end
end
if isstruct(P)
    DCM = P;
    P   = ['DCM-' date];
else
    load(P)
end

disp(' ')
disp('-----------------------------')
disp('Inverting stochastic DCM...')

% Get data and remove confounds
y = DCM.Y.y';
X0 = DCM.Y.X0;
iX0 = pinv(X0'*X0)*X0';
nreg = size(y,1);
for i=1:nreg
    beta = iX0*y(i,:)';
    yc = X0*beta;
    y(i,:) = y(i,:) - yc';
end

% Unpack DCM specification
dt = DCM.U.dt;
uu = DCM.U.u';
[nu,nt] = size(uu);
[p,ny] = size(y);
A = DCM.a - eye(nreg);
for i=1:nu
    B{i} = DCM.b(:,:,i);
end
C = DCM.c;
if isfield(DCM,'d')
    for i=1:nreg
        try
            D{i} = DCM.d(:,:,i);
        catch
            D{i} = zeros(nreg,nreg);
        end
    end
else
    for i=1:nreg
        D{i} = zeros(nreg,nreg);
    end
end

% Prepare optional input for inversion routine
TR = DCM.Y.dt;
f_fname = @f_dcm4fmri;
g_fname = @g_Id;
microDT = 1e-1;             % micro-time resolution (in sec)
[inF] = prepare_dcm(A,B,C,D);
options.decim = max([1,floor(TR./microDT)]);
inF.deltat = TR./options.decim;
options.inF = inF;
options.microU = 1;
dim.n_theta = inF.indself;
dim.n_phi = 0;
dim.n = nreg;
dim.p = p;

% Build priors
alpha = 1e2/TR;
[priors] = getPriors(nreg,dim,alpha,ny);
options = getOptions(options,priors);

% Resample inputs on microtime integration grid
[u] = resampleU(uu,dt,nt,nu,ny,options);
u = [zeros(nu,1),u]; % add initial conditions

% Deconvolve fMRI data from HRF
[hrf] = VBA_spm_hrf(TR);
y0 = y;
y = [];
SNR = VBA_spm_getSNR(y0,uu,hrf);
for i=1:nreg
    try
        D = round(DCM.delays(i)./TR);
        y0i = [zeros(1,D),y0(i,1:end-D)];
    catch
        y0i = y0(i,:);
    end
    y(i,:) = VBA_spm_deconv(y0i,hrf,SNR);
end

% invert stochastic DCM using discrete-time VB routine:
[posterior,out] = VBA_NLStateSpaceModel(y,u,f_fname,g_fname,dim,options);

% extract relevant info and fills in DCM structure
[DCM] = exportDCMfromVBNLSS(posterior,out,DCM);

% change DCM output (account for HRF convolution)
for i=1:nreg
    y0i = conv(out.suffStat.gx(i,:),hrf);
    DCM.y(:,i) = y0i(1:end-length(hrf)+1)'; % predicted data
    DCM.R(:,i) = y0(i,:)' - DCM.y(:,i); % residuals of the model
end

disp('Inverting stochastic DCM... OK')
disp('-----------------------------')
disp(' ')

return

function [u] = resampleU(uu,dt,nt,nu,ny,options)
microDT = options.inF.deltat;
grid1 = 0:dt:dt*(nt-1);
grid2 = 0:microDT:microDT*options.decim*ny;
u = zeros(nu,ny);
for i=1:length(grid2)-1
    [tmp,ind1] = min(abs(grid2(i)-grid1));
    [tmp,ind2] = min(abs(grid2(i+1)-grid1));
    u(:,i) = mean(uu(:,ind1:ind2),2);
end
% [u,alpha] = spm_resample(full(uu),dt/microDT);

function [priors] = getPriors(nreg,dim,alpha,n_t)
priors.muX0 = 0.*ones(nreg,1);
priors.SigmaX0 = 0e-3*eye(nreg);
priors.muTheta = 0*ones(dim.n_theta,1);
priors.SigmaTheta = 1e-2*eye(dim.n_theta);
priors.muPhi = [];
priors.SigmaPhi = [];
% priors.SigmaTheta(options.inF.indself,options.inF.indself) = 0;

% NB on hyperpriors:
%   - fix state noise precision using high scale param
%   - use non-informative priors on the residual precision, with high
%   expectation.
% This is because of the first iteration of the hidden states posterior
% update, which has to deviate from the its prior predictive density (as
% derived from the deterministic inversion).
% The following iterations will then work with a realistic (expected)
% residual precision, and adapt.
SC = 1e0;
priors.a_alpha = SC*alpha;
priors.b_alpha = SC;
priors.a_sigma = 1e0;
priors.b_sigma = 1e-4;
for t = 1:n_t
    priors.iQx{t} = eye(dim.n);
    priors.iQy{t} = eye(dim.p);
end

function options = getOptions(options,priors)
options.priors = priors;
options.DisplayWin = 1;
options.GnFigs = 0;
options.gradF = 0;
options.updateHP = 1;
options.backwardLag = 2;
options.Laplace = 0;
% options.noSXi = 1;
% options.init0 = 0;
% options.embed = 0;


%% dihedralAngles
% Below is a demonstration of the features of the |dihedralAngles| function

%%
clear; close all; clc;

%% Syntax
% |[A]=dihedralAngles(varargin);|

%% Description
% UNDOCUMENTED
%% Examples
%

%%
% Plot settings

fontSize=20;
faceAlpha1=0.8;

%% CREATING A MESHED BOX
boxDim=2*ones(1,3);
boxEl=3*ones(1,3);
[meshStruct]=hexMeshBox(boxDim,boxEl);
E=meshStruct.E;
V=meshStruct.V;

%%
d=eye(3,3);
d(1,2)=1;

V=V*d;

%%

[A,EE,AE]=dihedralAngles(E,V,'hex8');
A=180*(A./pi);
AE=180*(AE./pi);

A_max=max(A,[],2);
A_min=min(A,[],2);

[F,A_max_F]=element2patch(E,A_max);
[~,A_min_F]=element2patch(E,A_min);
%%

cFigure;
subplot(1,2,1); hold on;
title(['Max dihedral angle ',num2str(max(A_max_F))])
gpatch(F,V,A_max_F,'k',1,1);
axisGeom; camlight headlight;
colormap(gca,gjet(25)); colorbar;
clim([min(A(:)) max(A(:))]);

subplot(1,2,2); hold on;
title(['Min dihedral angle ',num2str(min(A_min_F))])
gpatch(F,V,A_min_F,'k',1,1);
axisGeom; camlight headlight;
colormap(gca,gjet(25)); colorbar;
clim([min(A(:)) max(A(:))]);
gdrawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>
%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [ gx, gy ] = g05_xy ( gn )

%*****************************************************************************80
%
%% G05_XY returns the grid points for grid 5.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 August 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer GN, the grid size.
%
%    Output, real GX(GN,1), GY(GN,1), the grid coordinates.
%
  gx = [ ...
    0.000,  0.000,  0.000,  0.000,  0.000,  0.000,  0.000,  0.000,  0.000, ...
    0.125,  0.125,  0.125,  0.125,  0.125,  0.125,  0.125,  0.125,  0.125, ...
    0.250,  0.250,  0.250,  0.250,  0.250,  0.250,  0.250,  0.250,  0.250, ...
    0.375,  0.375,  0.375,  0.375,  0.375,  0.375,  0.375,  0.375,  0.375, ...
    0.500,  0.500,  0.500,  0.500,  0.500,  0.500,  0.500,  0.500,  0.500, ...
    0.625,  0.625,  0.625,  0.625,  0.625,  0.625,  0.625,  0.625,  0.625, ...
    0.750,  0.750,  0.750,  0.750,  0.750,  0.750,  0.750,  0.750,  0.750, ...
    0.875,  0.875,  0.875,  0.875,  0.875,  0.875,  0.875,  0.875,  0.875, ...
    1.000,  1.000,  1.000,  1.000,  1.000,  1.000,  1.000,  1.000,  1.000 ]';

  gy = [ ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000 ]';

  return
end


  function [y, eo_fun] = embed_out(y, Mdim)
%|function [y, eo_fun] = embed_out(y, Mdim)
%|
%| Dual or adjoint of embed_in().  See embed_in() for details.
%| To be used around _back() routines that expect [(M) *L]
%| input and produce [(N) *L] output that then must be reshaped
%| to be either [(N) (L)] or [np *L]
%|
%| in
%|	y	[*M (L)] or [(M) (L)]	input data array(s), possibly as columns
%|	Mdim				(M)
%| out
%|	y	[(M) *L]		output data arrays
%|	eo_fun	strum object with methods:
%|	x = eo_fun.shape(x, mask, np)	reshape x from [(N) *L] to be either
%|					[np (L)] or [(N) (L)], depending on y
%|
%| Copyright 2006-12-9, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if nargin == 1 && streq(y, 'test'), embed_out_test, clear y, return, end
if nargin < 2, Mdim = size(y,1); end % trick: try to guess...

if size(y,1) == prod(Mdim) % convert [*M (L)] to [(M) *L]
	state.column = true;
	dimy = size(y);
	state.diml = dimy(2:end); % [(L)]
	y = reshape(y, [Mdim prod(state.diml)]); % [(M) *L]

else % convert [(M) (L)] to [(M) *L]
	state.column = false;
	dimi = [size(y) 1];
	jf_equal(dimi(1:length(Mdim)), Mdim)
	state.diml = dimi(length(Mdim)+1:end); % (L) (possibly empty)
	y = reshape(y, [Mdim prod(state.diml) 1]); % [(N) *L]
end

eo_fun = strum(state, {'shape', @embed_out_shape});

%
% embed_out_shape()
%
function x = embed_out_shape(state, x, mask, np)

if nargin < 4, np = sum(mask(:)); end

diml = state.diml;

if state.column % column in yields column out, i.e., [(N) *L] to [np (L)]
	if any(diml > 1)
		x = reshape(x, numel(mask), []); % [*N *L]
		x = x(mask,:); % [np *L]
		x = reshape(x, [np diml]); % [np (L)]
	else
		x = x(mask); % [np,1]
	end

else % [(N) *L] to [(N) (L)]
	if any(diml > 1)
		x = reshape(x, [size(mask) diml]); % [(N) (L)]
	end
end

%
% embed_out_test
%
function embed_out_test
ig = image_geom('nx', 10, 'ny', 8, 'dx', 1);
ig.mask = ig.circ > 0;

Mdim = [6 7];
y0 = ones(Mdim); % [(M)]
dl = [2 3];

[y2 eo] = embed_out(y0(:), Mdim); % single column
x2 = eo.shape(ig.ones, ig.mask);
jf_equal(size(x2), [ig.np 1])

[y2 eo] = embed_out(y0, Mdim); % usual 2d
x2 = eo.shape(ig.ones, ig.mask);
jf_equal(size(x2), [ig.nx ig.ny])

yr = repmat(y0, [1 1 dl]); % [(M) (L)]
[y2 eo] = embed_out(yr, Mdim);
x2 = eo.shape(ones([ig.nx ig.ny prod(dl)]), ig.mask);
jf_equal(size(x2), [ig.nx ig.ny dl])

yr = repmat(y0(:), [1 dl]); % [*M (L)]
[y2 eo] = embed_out(yr, Mdim);
x2 = eo.shape(ones([ig.nx ig.ny prod(dl)]), ig.mask);
jf_equal(size(x2), [ig.np dl])


function I = reconstruction(temp_vector, unit, t1,t2, count1)
I_d_temp = zeros(t1,t2);
for ii=1:t1-unit+1
    for jj=1:t2-unit+1
        temp = temp_vector(:,(ii-1)*(t2-unit+1)+jj);
        I_d_temp(ii:(unit+ii-1), jj:(unit+jj-1)) = I_d_temp(ii:(unit+ii-1), jj:(unit+jj-1)) + reshape(temp, [unit unit]);
    end
end
I = I_d_temp./count1;
end


function value = p12_exact ( dim_num )

%*****************************************************************************80
%
%% P12_EXACT returns the exact integral for problem 12.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Output, real VALUE, the exact value of the integral.
%
  value = 1.0;
  for i = 1 : dim_num
    value = value * sin ( i );
  end

  return
end


function [cost,costs] = rcrossvalidate(model, L, wfun, estfct,combinefct)

% Estimate the model performance of a model with robust [$ l$] -fold crossvalidation

% CAUTION!! Use this function only to obtain the value of the rcrossvalidation score
% function given the tuning parameters. Do not use this function together with
% 'tunelssvm', but use 'rcrossvalidatelssvm' instead. The latter is a faster
% implementation which uses previously computed results.
%
%
% >> cost = rcrossvalidate({Xtrain,Ytrain,type,gam,sig2})
% >> cost = rcrossvalidate( model)
%
% Robustness in the $l$-fold crossvalidation score function is obtained by
% iteratively reweighting schemes.
%
% This routine is computational intensive.
%
%
% Some commonly used criteria are:
%
% >> cost = rcrossvalidate(model, 10, 'whuber', 'mae')
% >> cost = rcrossvalidate(model, 10, 'whampel', 'mae')
% >> cost = rcrossvalidate(model, 10, 'wlogistic', 'mae')
% >> cost = rcrossvalidate(model, 10, 'wmyriad', 'mae')
%
% Full syntax
%
%     1. Using LS-SVMlab with the functional interface:
%
% >> [cost, costs] = rcrossvalidate({X,Y,type,gam,sig2,kernel,preprocess}, L, wfun, estfct, combinefct)
%
%       Outputs
%         cost          : Cost estimation of the L-fold cross validation
%         costs(*)      : L x 1 vector with costs estimated on the L different folds
%       Inputs
%         X             : Training input data used for defining the LS-SVM and the preprocessing
%         Y             : Training output data used for defining the LS-SVM and the preprocessing
%         type          : 'function estimation' ('f') or 'classifier' ('c')
%         gam           : Regularization parameter
%         sig2          : Kernel parameter (bandwidth in the case of the 'RBF_kernel')
%         kernel(*)     : Kernel type (by default 'RBF_kernel')
%         preprocess(*) : 'preprocess'(*) or 'original'
%         L(*)          : Number of folds (by default 10)
%         wfun(*)       : weighting scheme (by default: whuber)
%         estfct(*)     : Function estimating the cost based on the residuals (by default mse)
%         combinefct(*) : Function combining the estimated costs on the different folds (by default mean)
%
%
%     2. Using the object oriented interface:
%
% >> [cost, costs] = crossvalidate(model, L, wfun, estfct, combinefct)
%
%       Outputs
%         cost          : Cost estimation of the L-fold cross validation
%         costs(*)      : L x 1 vector with costs estimated on the L different folds
%         ec(*)         : N x 1 vector with residuals of all data
%       Inputs
%         model         : Object oriented representation of the LS-SVM model
%         Xval          : Nt x d matrix with the inputs of the validation points used in the procedure
%         Yval          : Nt x m matrix with the outputs of the validation points used in the procedure
%         L(*)          : Number of folds (by default 10)
%         wfun(*)       : weighting scheme (by default: whuber)
%         estfct(*)     : Function estimating the cost based on the residuals (by default mse)
%         combinefct(*) : Function combining the estimated costs on the different folds (by default mean)
%
% See also:
% mae,whuber,wlogistic,whampel,wmyriad, crossvalidate, trainlssvm, robustlssvm

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

% LS-SVMlab
eval('model = initlssvm(model{:});',' ');
eval('L;','L=min(ceil(sqrt(model.nb_data)),10);');
eval('estfct;','estfct=''mae'';');
eval('combinefct;','combinefct=''mean'';');
eval('wfun;','wfun=''whuber'';');
%
% initialisation and defaults
%
nb_data = size(model.ytrain,1);

if L==nb_data, p = 1:nb_data; else p = randperm(nb_data); end
px = model.xtrain(p,:);
py = model.ytrain(p,:);

[~,Y] = postlssvm(model,[],py);

%initialize: no incremental  memory allocation
costs = zeros(L,length(model.gam));
block_size = floor(nb_data/L);

S = ones(nb_data,1);
Atot = kernel_matrix(px,model.kernel_type,model.kernel_pars)+eye(nb_data)./model.gam;
%
%
% start loop over l validations
%
for l = 1:L,

    % divide in data and validation set, trainings data set is a copy
    % of permutated_data, validation set is just a logical index
    if l==L,
        train = 1:block_size*(l-1);
        validation = block_size*(l-1)+1:nb_data;
    else
        train = [1:block_size*(l-1) block_size*l+1:nb_data];
        validation = block_size*(l-1)+1:block_size*l;
    end

    A = [0 S(train)';S(train) Atot(train,train)];
    b = [0;py(train)];

    % Solve linear system
    sol = linsolve(A,b,struct('SYM',true));

    % Determine residuals ek
    ek = sol(2:end)./model.gam;
    g = model.gam;
    %for i=2:size(A,1), A(i,i) = A(i,i) - 1/g; end
    A = A-eye(size(train,2)+1)./g; A(1,1)=0;
    Ah = A;
    %
    % robust estimation of the variance
    %
    for k = 1:20
        vare = 1.483*median(abs((ek)-median(ek)));
        alphaold = sol(2:end);
        %
        % robust re-estimation of the alpha's and the b
        %
        cases = reshape((ek./vare),1,size(ek,1));
        W = g*weightingscheme(cases,wfun);

        for t=1:size(train,2), A(t+1,t+1) = A(t+1,t+1)+1./W(t); end

        sol = linsolve(A,b,struct('SYM',true));

        ek = sol(2:end)./W';
        A = Ah;
        if norm(abs(alphaold-sol(2:end)),'fro')<=1e-4,
            %fprintf('\n Converged after %.0f iteration(s)', k);
            k = inf;
        end
        model.status = 'changed';
    end

    % regression
    % Simulate system on validation data
    yh = Atot(train,validation)'*sol(2:end) + ones(numel(validation),1)*sol(1);
    [~,yh] = postlssvm(model,[],yh);
    costs(l,1) = feval(estfct,yh - Y(validation,:));
end
cost = feval(combinefct, costs);


function [res] = tt_dot(tt1,tt2)
%Scalar product of two TT-tensors in the TT1.0 format
%   [RES]=TT_DOT(TT1,TT2) Scalar product of two TT-tensors in the TT1.0
%   format. Please avoid its usage, use dot function from the
%   object-oriented version
%
%
% 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
%---------------------------
d=size(tt1,1);
g0=(tt1{1})'*(tt2{1});
gd=(tt1{d})'*(tt2{d});
r1=size(gd,1); r2=size(gd,2);
gd=reshape(gd,1,r1*r2);
for i=2:d-1
    core1=tt1{i};
    core2=tt2{i};
    core1=ten_conv(core1,2,g0);
    ncur=size(core1,1); r21=size(core1,2); r31=size(core1,3);
    r22=size(core2,2); r32=size(core2,3);
    core1=reshape(core1,[ncur*r21,r31]);
    core2=reshape(core2,[ncur*r22,r32]);
    g0=core1'*core2;
end
r1=size(g0,1); r2=size(g0,2);
g0=reshape(g0,1,r1*r2);
res=g0*gd';
return

end


function [cum_ret, cumprod_ret, daily_ret, daily_portfolio] ...
    = pamr_2_run(fid, data, epsilon, C, tc, opts)
% This program simulates the PAMR algorithm
%
% function [cum_ret, cumprod_ret, daily_ret, daily_portfolio, exp_ret] ...
%    = pamr_run(fid, data, K, L, c, tc, opts)
%
% cum_ret: a number representing the final cumulative wealth.
% cumprod_ret: cumulative return until each trading period
% daily_ret: individual returns for each trading period
% daily_portfolio: individual portfolio for each trading period
%
% data: market sequence vectors
% fid: handle for write log file
% epsilon: mean reversion threshold
% C: aggressive parameter
% tc: transaction cost rate parameter
% opts: option parameter for behvaioral control
%
% Example: [cum_ret, cumprod_ret, daily_ret, daily_portfolio, exp_ret] ...
%            = pamr_run(fid, data, K, L, c, tc, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of OLPS: http://OLPS.stevenhoi.org/
% Original authors: Bin LI, Steven C.H. Hoi
% Contributors:
% Change log:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[n, m] = size(data);

% Variables for return, start with uniform weight
cum_ret = 1;
cumprod_ret = ones(n, 1);
daily_ret = ones(n, 1);
ret_m = ones(n, 2);  % total_ret + day_ret
day_weight = ones(m, 1)/m;  %#ok<*NASGU>
day_weight_o = zeros(m, 1);
daily_portfolio = zeros(n, m);

% print file head
fprintf(fid, '-------------------------------------\n');
fprintf(fid, 'Parameters [epsilon:%f, C:%f, tc:%f]\n', epsilon, C, tc);
fprintf(fid, 'day\t Daily Return\t Total return\n');

fprintf(1, '-------------------------------------\n');
if(~opts.quiet_mode)
    fprintf(1, 'Parameters [epsilon:%f, C:%f, tc:%f]\n', epsilon, C, tc);
    fprintf(1, 'day\t Daily Return\t Total return\n');
end

if (opts.progress)
	progress = waitbar(0,'Executing Algorithm...');
end
for t = 1:1:n,
    % Calculate t's portfolio at the beginning of t-th trading day
    if (t >= 2)
        [day_weight] = pamr_kernel(data(1:t-1, :), day_weight, eta);
    end

    % Normalize the constraint, always useless
    day_weight = day_weight./sum(day_weight);
    daily_portfolio(t, :) = day_weight';

    % Cal t's return and total return
    daily_ret(t, 1) = (data(t, :)*day_weight)*(1-tc/2*sum(abs(day_weight-day_weight_o)));
    cum_ret = cum_ret * daily_ret(t, 1);
    cumprod_ret(t, 1) = cum_ret;

    % Adjust weight(t, :) for the transaction cost issue
    day_weight_o = day_weight.*data(t, :)'/daily_ret(t, 1);

%     denominator = data(t, :)*data(t, :)'/(day_ret(t, 1)^2)-1/nStocks*sum(data(t, :))^2/(day_ret(t, 1)^2)+0.5/C_t;
    denominator = (data(t, :)-1/m*sum(data(t, :)))*(data(t, :)-1/m*sum(data(t, :)))'+0.5/C;
    if (~eq(denominator, 0.0))
        eta = (daily_ret(t, 1)-epsilon)/denominator;
    end
    eta = max(0, eta); % MRPA

    % Debug information
    % Time consuming part, other way?
    fprintf(fid, '%d\t%f\t%f\n', t, daily_ret(t, 1), cumprod_ret(t, 1));
    if (~opts.quiet_mode)
        if (~mod(t, opts.display_interval)),
            fprintf(1, '%d\t%f\t%f\n', t, daily_ret(t, 1), cumprod_ret(t, 1));
        end
    end
    if (opts.progress)
		if mod(t, 50) == 0
			waitbar((t/n));
		end
	end
end

% Debug Information
fprintf(fid, 'PAMR-2(epsilon:%.2f, C:%d, tc:%.4f), Final return: %.2f\n', ...
    epsilon, C, tc, cum_ret);
fprintf(fid, '-------------------------------------\n');
fprintf(1, 'PAMR-2(epsilon:%.2f, C:%d, tc:%.4f), Final return: %.2f\n', ...
    epsilon, C, tc, cum_ret);
fprintf(1, '-------------------------------------\n');
    if (opts.progress)
		close(progress);
	end

end


function [KB] = PB2KB(PB)
% Convert computery things from petabytes to kilobytes.
% Chad A. Greene 2012
KB = PB*1099511627776;


function Y = linsolve_cov(C,Y,type)
% X = linsolve_cov(C,Y)
%
% Solves C*X = Y, where C is symmetric positive definite matrix.

Y = linsolve_chol(chol(C),Y);


function [Population,FrontNo,CrowdDis] = DominationSelection(Global,Population)
% The dominant relationship and crowding based environmental selection

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Shufen Qin
% E-mail: shufen.qin@stu.tyust.edu.cn

    %% Non-dominated sorting
    [FrontNo,MaxFNo] = NDSort(Population.objs,Global.N);
    Next = false(1,length(FrontNo));
    Next(FrontNo<MaxFNo) = true;

    %% Calculate the crowding distance of each solution
    CrowdDis = CrowdingDistance(Population.objs,FrontNo);

    %% Select the solutions in the last front based on their crowding distances
    Last     = find(FrontNo==MaxFNo);
    [~,Rank] = sort(CrowdDis(Last),'descend');
    Next(Last(Rank(1:Global.N-sum(Next)))) = true;

    %% Population for next generation
    FrontNo    = FrontNo(Next);
    CrowdDis   = CrowdDis(Next);
    Population = Population(Next);
end


function gf = gradspars(Hkt_r, varargin)
% function gf = gradddivH(Hkt_r, varargin)
% gf is 1 x k*t row vector
% Hkt_r = reshape(Hkt,1,k*t) -> row vector
% Vxt = varargin{1};  %data
% Wxk = varargin{2};  %W matrix
% Wxk_fix = varargin{3}; %fixed part of the Wxk matrix (e.g. background) ->rows
% Hkt_fix = varargin{4}; %fixed part (lines) of the H matrix (e.g. background)

alphaH=1; %for now....
Vxt = varargin{1};  %data
Wxk_tmp = varargin{2};  %W matrix
Wxk_fix = varargin{3}; %fixed part of the Wxk matrix (e.g. background) ->rows
Hkt_fix = varargin{4}; %fixed part (lines) of the H matrix (e.g. background)
peval = varargin{5}; %parameters

if ~isfield(peval, 'w_lambda') peval.w_lambda=0; end

t=size(Vxt,2);
k=length(peval.h_dovec);

Hkt_tmp = exp(reshape(Hkt_r,k,t));

Wxk = zeros(peval.numpix, peval.ncomp+1);
Hkt = zeros(peval.ncomp+1, peval.nt);

Wxk(:,peval.w_dovec)=Wxk_tmp;
Hkt(peval.h_dovec,:)=Hkt_tmp;

Wxk(:,peval.w_fixvec)=Wxk_fix;
Hkt(peval.h_fixvec,:)=Hkt_fix;

deltasum=sum(sum(Wxk_tmp,1))-k;
if and(~isempty(Wxk_tmp), abs(deltasum)>10^-6)
    error('Wxk is not correctly normalized! (sum(Wxk_tmp,1)<>1)\n sum(Wxk_tmp,1)=%f',deltasum)
end

% ap = peval.alphapen;
% sumH_t = sum(Hkt(peval.h_dovec,:),2);
sumH_t = sum(Hkt,2);
sumH_t_sq = sum(sumH_t.^2);
sumH = sum(sumH_t);
nh = length(sumH_t);

sparsness = (sqrt(nh)-sumH/(sqrt(sumH_t_sq)))/(sqrt(nh)-1);
% gradspars=(1/(sqrt(nh)-1))*(sumH/(sumH_t_sq)^1.5*sumH_t - 1/(sqrt(sumH_t_sq)));
gradspars= (1/(sqrt(nh)-1)*(sumH_t*(sumH)-sumH_t_sq)/(sumH_t_sq^1.5));
gradspars_kt = repmat(gradspars, 1, t);

% fkt = ddivergence(Vxt,Wxk*Hkt);
% gfkt = (1-Wxk'*(Vxt./(Wxk*Hkt)))*alphaH.*Hkt; %d/dh(d-divergence)
% gfkt = (1-Wxk'*(Vxt./(Wxk*Hkt))).*Hkt; %d/dh(d-divergence)
%(fg)'=(f'g-fg')/g^2;
% gfkt_all=(gfkt.*sparsness-fkt.*gradspars_kt)./(sparsness).^2;
% gfkt_all = gradspars_kt.*Hkt(peval.h_dovec, :); %because of exp...
gfkt_all = gradspars_kt.*Hkt; %because of exp...
% one is tehre because Wxt is normalized: sum(Wxt,1)=1
gf=reshape(gfkt_all(peval.h_dovec,:),1,k*t); %making row vector


% Inference on a conditional Gaussian model

% Make the following polytree, where all arcs point down

% 1   2
%  \ /
%   3
%  / \
% 4   5

N = 5;
dag = zeros(N,N);
dag(1,3) = 1;
dag(2,3) = 1;
dag(3, [4 5]) = 1;

ns = [2 1 2 1 2];

dnodes = 1;
%onodes = [1 5];
bnet = mk_bnet(dag, ns, 'discrete', dnodes, 'observed', dnodes);

bnet.CPD{1} = tabular_CPD(bnet, 1);
for i=2:N
  bnet.CPD{i} = gaussian_CPD(bnet, i);
end

engine = {};
engine{end+1} = jtree_inf_engine(bnet);
engine{end+1} = pearl_inf_engine(bnet, 'protocol', 'parallel');

[time, engine] = cmp_inference_static(bnet, engine, 'maximize', 0, 'check_ll', 0, ...
				      'singletons_only', 0, 'observed', [1 3]);


function result = CDPLSA_enterFunc(numCluster,numIter,inputPath)
    %% ======================================================================
    %%STEP 1: load the data
    fprintf('start load the data...\n');
     [TrainData, TestData, TrainLabel, TestLabel, numSource, numTrain, numTarget, numTest] = CDPLSA_loadData(inputPath);
%     [TrainData, TestData, TrainLabel, TestLabel, numSource, numTrain, numTarget, numTest] = original_loadData(inputPath);
     TrainData = double(TrainData);
      TestData = double(TestData);
      TrainLabel = double(TrainLabel);
      TestLabel = double(TestLabel);
    TrainData = sparse(TrainData);
     TestData = sparse(TestData);
    fprintf('training the model and testing...\n');
    [Results, pz_d] = CD_PLSA(TrainData, TestData, TrainLabel, TestLabel, numCluster, numIter, numSource, numTrain, numTarget, numTest);

    result = Results(end);
end


function [rfAmp,rfPhase,rfFreq,rfCoil,rfTime]=rfSLR(p)
%create a SLR rf pulse starting from tStart
%  Inputs for dzrf are:
%    np -- number of points.         (required)
%    tb -- time-bandwidth product    (required)
%    ptype -- pulse type.  Options are:
%      st  -- small tip angle         (default)
%      ex  -- pi/2 excitation pulse
%      se  -- pi spin-echo pulse
%      sat -- pi/2 saturation pulse
%      inv -- inversion pulse
%    ftype -- filter design method.  Options are:
%      ms  -- Hamming windowed sinc (an msinc)
%      pm  -- Parks-McClellan equal ripple
%      ls  -- Least Squares           (default)
%      min -- Minimum phase (factored pm)
%      max -- Maximum phase (reversed min)
%    d1 -- Passband ripple        (default = 0.01)
%    d2 -- Stopband ripple        (default = 0.01)
%    pclsfrac -- pcls tolerance   (default = 1.5)

tStart=p.tStart;
tEnd=p.tEnd;
dt=p.dt;
FA=p.FA;
TBP=p.TBP;
rfFreq=p.rfFreq;
rfPhase=p.rfPhase;
PRipple=p.PRipple;
SRipple=p.SRipple;
PulseType=p.PulseType;
FilterType=p.FilterType;
rfCoil=p.CoilID;
Duplicates=max(1,p.Duplicates);
DupSpacing=max(0,p.DupSpacing);

rfTime=linspace(tStart,tEnd,ceil((tEnd-tStart)/dt)+1);
rf = dzrf(ceil((tEnd-tStart)/dt)+1,TBP,PulseType,FilterType,PRipple,SRipple,1.5);
rfAmp = real(rf);
rfAmp = DoB1Scaling(rfAmp,dt,FA)*rfAmp; %B1 Scaling

rfPhase=(rfPhase)*ones(size(rfTime));
rfFreq=(rfFreq)*ones(size(rfTime));
rfCoil=(rfCoil)*ones(size(rfTime));
rfAmp(1)=0;
rfAmp(end)=0;
rfFreq(1)=0;
rfFreq(end)=0;
rfPhase(1)=0;
rfPhase(end)=0;

% Create Duplicates
if Duplicates~=1 & DupSpacing ~=0
    rfAmp=repmat(rfAmp,[1 Duplicates]);
    rfFreq=repmat(rfFreq,[1 Duplicates]);
    rfPhase=repmat(rfPhase,[1 Duplicates]);
    rfCoil=repmat(rfCoil,[1 Duplicates]);
    TimeOffset = repmat(0:DupSpacing:(Duplicates-1)*DupSpacing,[length(rfTime) 1]);
    rfTime=repmat(rfTime,[1 Duplicates]) + (TimeOffset(:))';
end

end


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

%%begin

% Our robot will operate within a grid world that contains free
% space where it can drive and obstacles.  We load a map of the world
load map1
% which loads a variable called map
about map
% and the cells contain zero if it is free space (driveable) and one if it is an
% obstacle.

% Now we create an instance of a robot with the PRM navigation algorithm
prm = PRM(map);

% and because PRM is a probabilistic method we will reset the random number
% generator to a known state
randinit

% Now we define the goal and start coordinates
goal = [50,30];
start = [20, 10];

% then ask the robot to plan a path to goal (it will take few seconds)
prm.plan();

% The roadmap planner does not need to know, yet, the goal or start positions, it
% chooses random points in the world and tries to find obstacle-free paths between
% them (like railway lines or freeways)

% Now we can display the obstacles and the cost to reach the goal from every
% point in the world
prm.plot();

% Now we can find a path from start to goal using the roadmap
p = prm.query(start, goal);

% and now display it
prm.plot(p);


function p1 = gibbs(p1, x)
%GIBBS      Gibbs sampling
%
%           Description
%           P1 = GIBBS(P1, X) Samples one sample for parameters
%           contained in structure P1 at level n and and gets a
%           parameter structure X from level n-1 in hierarchy.
%           Returns a structure P1 containing new samples for
%           parameters in structure P1.
%
%           If P1 contains higher level parameters, e.g P1 =
%           net.p.w{1} and though there is a field P1.p, the
%           function calls itself to sample for those higher level
%           parameters also. So function takes care of the whole
%           hierarchical structure of ARD if called for the lowest
%           level parameters.
%
% Copyright (c) 1999-2003 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

%Matlab 6.5
%if any(strcmp(fieldnames(p1),'p')) & ~isempty(p1.p) %isfield(p1,'p')
%  for a = fieldnames(p1.a)';
%    a=a{:};
%    if any(strcmp(fieldnames(p1.p),a)) %isfield(p1.p,a);
%      p2=p1.p.(a);
%      % call the spesified distribution sampler
%      p1.a.(a)=feval(['cond_' p1.f '_' p2.f],a,p1.a,p2.a,x);
%      % sample the upper level parameters
%      p1.p.(a)=gibbs(p2, p1.a.(a));
%    end
%  end
%end

% Pre matlab 6.5
S.type='.';
if any(strcmp(fieldnames(p1),'p')) & ~isempty(p1.p) %isfield(p1,'p')
  for a = fieldnames(p1.a)';
    a=a{:};
    S.subs=a;
    if any(strcmp(fieldnames(p1.p),a)) %isfield(p1.p,a);
      p2=subsref(p1.p,S);
      p1.a=subsasgn(p1.a, S, feval(['cond_' p1.f '_' p2.f],a,p1.a,p2.a,x));
      p1.p=subsasgn(p1.p, S, gibbs(p2, subsref(p1.a,S)));
    end
  end
end


function [] = generate_undistort_map_radtan(cinfo, sequence_name, batch_size)
%GENERATE_UNDISTORT_MAP_RADTAN generates a mapping from distorted to
%undistorted pixels in a given image with radtan camera distortion, given
%the camera intrinsics.
%
% Syntax: GENERATE_UNDISTORT_MAP_RADTAN(cinfo, sequence_name, batch_size)
%
% Inputs:
%    cinfo         - camera info struct, generated by extract_ros_data.
%    sequence_name - string to be concatenated to the output name.
%    batch_size    - OPTIONAL number of points to undistort at once.
%                    Unclear if increasing this will speed up the program.
%
% See also EXTRACT_ROS_DATA
%
% Author: Alex Zihao Zhu, University of Pennsylvania
% Email: alexzhu(at)seas.upenn.edu
% Copyright 2018 University of Pennsylvania
% Alex Zihao Zhu, Nikolay Atanasov, Kostas Daniilidis
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS, CONTRIBUTORS, AND THE
% TRUSTEES OF THE UNIVERSITY OF PENNSYLVANIA "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, CONTRIBUTORS OR THE TRUSTEES OF
% THE UNIVERSITY OF PENNSYLVANIA 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.

output_name = ['undistort_map_', sequence_name];
fprintf('Generating undistort maps and saving in %s. This may take some time.\n', ...
    output_name)

if nargin < 3
    batch_size = 1;
end

K = reshape(cinfo.K, 3, 3);
D = cinfo.D;
rows = cinfo.height;
cols = cinfo.width;
cameraParams = cameraParameters(...
    'IntrinsicMatrix', K, ...
    'RadialDistortion', D(1:2), ...
    'TangentialDistortion', D(3:4));

undistort_map_x = zeros(rows, cols);
undistort_map_y = zeros(rows, cols);

[X, Y] = meshgrid(0:cols-1, 0:rows-1);
X = X(:);
Y = Y(:);

n_pts = length(X);
undistorted_points = zeros(n_pts, 2);

last_iter = 1;
fprintf('Processing %d points\n', n_pts);
for i=1:batch_size:n_pts
    end_val = min(n_pts, i+batch_size-1);
    undistorted_points(i:end_val, :) = undistortPoints([X(i:end_val) Y(i:end_val)], cameraParams);
    if i - last_iter >= 1000
        fprintf('Processed %d points\n', i);
        last_iter = i;
    end
end

% undistortedPoints = undistortPoints([X Y], cameraParams);
addpath('../EventFeatureTracking/Tracker/')
inds = sub2indc(X+1, Y+1, [rows, cols]);
undistort_map_x(inds) = undistorted_points(:, 1);
undistort_map_y(inds) = undistorted_points(:, 2);

save(output_name, 'undistort_map_x', 'undistort_map_y', '-v7.3');
end


function write_input_data(off_path, data_path, classes, volume_size, pad_size, angle_inc)
% Put the mesh object in a volume grid and save the volumetric
% represenation file.
% This is the input volumetric data for 3D ShapeNets.
% off_path: root off data folder
% data_path: destination volumetric data folder

phases = {'train', 'test'};

data_size = pad_size * 2 + volume_size;
for c = 1 : length(classes)
    fprintf('writing the %s category\n', classes{c});
    category_path = [off_path '/' classes{c}];
    dest_path = [data_path '/' classes{c} '/' num2str(data_size)];
    if ~exist(dest_path, 'dir')
        mkdir(dest_path);
    end
    % for train and test phases
    for t = 1 : numel(phases)
        phase = phases{t};
        off_list = [category_path '/' phase];
        dest_tsdf_path = [dest_path '/' phase];
        if ~exist(dest_tsdf_path, 'dir')
            mkdir(dest_tsdf_path);
        end
        files = dir(off_list);
        for i = 1 : length(files)
            if strcmp(files(i).name, '.') || strcmp(files(i).name, '..') || files(i).isdir == 1 || ~strcmp(files(i).name(end-2:end), 'off')
                continue;
            end
            filename = [off_list '/' files(i).name];
            for viewpoint = 1 : 360/angle_inc
                destname = [dest_tsdf_path '/' files(i).name(1:end-4) '_' num2str(viewpoint) '.mat'];
                off_data = off_loader(filename, (viewpoint-1)*angle_inc);
                instance = polygon2voxel(off_data, [volume_size, volume_size, volume_size], 'auto');
                instance = padarray(instance, [pad_size, pad_size, pad_size]);
                instance = int8(instance);
                save(destname, 'instance');
            end
        end
    end
end

function offobj = off_loader(filename, theta, axis, stretch)

offobj = struct();
fid = fopen(filename, 'rb');
OFF_sign = fscanf(fid, '%c', 3);
assert(strcmp(OFF_sign, 'OFF') == 1);

info = fscanf(fid, '%d', 3);
offobj.vertices = reshape(fscanf(fid, '%f', info(1)*3), 3, info(1))';
offobj.faces = reshape(fscanf(fid, '%d', info(2)*4), 4, info(2))';

% do some translation and rotation
center = (max(offobj.vertices) + min(offobj.vertices)) / 2;
offobj.vertices = bsxfun(@minus, offobj.vertices, center);
if exist('axis', 'var')
    switch axis
        case 'x',
            offobj.vertices(:,1) = offobj.vertices(:,1) * stretch;
        case 'y',
            offobj.vertices(:,2) = offobj.vertices(:,2) * stretch;
        case 'z',
            offobj.vertices(:,3) = offobj.vertices(:,3) * stretch;
        otherwise,
            error('off_loader axis set wrong');
    end
end
theta = theta * pi / 180;
R = [cos(theta), -sin(theta), 0;
     sin(theta), cos(theta) , 0;
        0      ,    0       , 1];

offobj.vertices = offobj.vertices * R;

% These vertices to define faces should be offset by one to follow the matlab convention.
offobj.faces = offobj.faces(:,2:end) + 1;

fclose(fid);


function [ detectedPts, scale ] = dog(  vertex, faces, num_octaves, params )
%vertex, faces
%num_octaves - number of octaves to use (20 works fine)
%params.sigscale - dog scaling method. default is good
%params.ExcludeBoundery - exclude boundary vertices from detection
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~exist('params','var')
    params = [];
end

if ~isfield(params,'sigscale')
    params.sigscale = 0;
end
if ~isfield(params,'ExcludeBoundery')
    params.ExcludeBoundery = 1;
end

%dog find geometry difference of gaussians features

    ss = size(vertex);
    if (ss(1) == 3)
        vertex = vertex';
    end

%create ocataves
    %W = my_euclidean_distance(triangulation2adjacency(faces),vertex);
    W = triangulation2adjacency(faces);
    W = W + speye(length(vertex));

    W = spdiags(1./sum(W,2),0,length(vertex),length(vertex))*W;
    vertex2=vertex;
    for ind = 1:num_octaves
        vertex2 = (W*vertex2);
        octaves {ind} = vertex2;
        if (ind > 1)
            diff{ind} =  sum((octaves{ind - 1} - vertex2)'.^2)';
        else
            diff{ind} =  sum((vertex - vertex2)'.^2)';
        end
        ds(ind) = sum(diff{ind});
        if (params.sigscale == 1)
            diff{ind} = diff{ind}*ind;
        elseif (ds(ind) > 0)
            diff{ind} = diff{ind}/ds(ind);
        end
    end

    detectedPts = find_local_maxima(faces,vertex2,diff);

    D = my_euclidean_distance(triangulation2adjacency(faces),vertex);
    d = sum(D);
    w = sum(triangulation2adjacency(faces));
   scale = full((d./w)');
   if (params.ExcludeBoundery)
       boundary = find_boundary_vertex(faces);
       for k = 1:length(detectedPts)
           detectedPts{k} = setdiff(detectedPts{k}, boundary);
       end
   end
  %detectedPts{1} = {};
   %detectedPts{2} = {};
%    a = sum(abs(vertex - repmat([5.92 -63.36 -596.2],length(vertex),1)),2)<0.1;
% b = sum(abs(vertex - repmat([-111.4 -35.49 -580.7],length(vertex),1)),2)<0.1;
% detectedPts = {};
% detectedPts{1}  = [find(a) find(b)];
 end

function W = my_euclidean_distance(A,vertex)

if size(vertex,1)<size(vertex,2)
    vertex = vertex';
end

[i,j,s] = find(sparse(A));
d = sqrt(sum( (vertex(i,:) - vertex(j,:)).^2, 2));
W = sparse(i,j,d);

end


function out=acosh(x)

out=log(x+sqrt(x+1).*sqrt(x-1));

%%%precision=x(1).precision;
%%%out=mp(zeros(size(x)));
%%%
%%%mpPi=mppi(precision);
%%%for ii=1:numel(x)
%%% out(ii)=log(x(ii)+sqrt(x(ii)+1)*sqrt(x(ii)-1));
%%%end % for ii=1:max(ex,


function x = r8vec_index_order ( n, x, indx )

%*****************************************************************************80
%
%% R8VEC_INDEX_ORDER sorts an integer vector using an index vector.
%
%  Discussion:
%
%    The index vector itself is not modified.  Therefore, the pair
%    (X,INDX) no longer represents an index sorted vector.  If this
%    relationship is to be preserved, then simply set INDX(1:N)=(1:N).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 October 2000
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the size of the current list.
%
%    Input, real X(N), the list.
%
%    Input, integer INDX(N), the sort index of the list.
%
%    Output, real X(N), the list has been sorted.
%
  y(1:n) = x(indx(1:n));
  x(1:n) = y(1:n);

  return
end


function obj = add_matrix(obj, mat3d, mat, imgno)
% Add 3d matrix image vol to slice overlay
% FORMAT obj = add_matrix(obj, mat3d, mat, imgno)
%
% Inputs
% obj          - object
% mat3d        - 3D matrix to add as img
% mat          - optional 4x4 voxel->world translation
% imgno        - optional img no to add to (defaults to last in object)
%
% Ouputs
% obj          - modified object
%__________________________________________________________________________

% Matthew Brett
% $Id: add_matrix.m 6623 2015-12-03 18:38:08Z guillaume $

if nargin < 2
    return
end
if nargin < 3
    mat = [];
end
if nargin < 4
    imgno = [];
end
if isempty(imgno)
    imgno = length(obj.img);
end
if ~isempty(mat3d)
    obj.img(imgno).vol = pr_matrix2vol(mat3d, mat);
end


function data = rawsignal2power(data,T)
% Gets *square of* the power time series using the Hilbert transform,
% as done in Baker et al (2014), eLife.
%
% Diego Vidaurre, OHBA, University of Oxford (2017)

if isstruct(data), ndim = size(data.X,2);
else, ndim = size(data,2);
end

for n = 1:length(T)
    ind = sum(T(1:n-1))+1:sum(T(1:n));
    if isstruct(data)
        for j = 1:ndim
            data.X(ind,j) = abs(hilbert(data.X(ind,j)));
        end
    else
        for j = 1:ndim
            data(ind,j) = abs(hilbert(data(ind,j)));
        end
    end
end

end


function bitsHitV = cumbitor(bitsHitV)
%"cumbitor"
%   Find cumulative bitor of all values in bitsHitV, using a "vectorized"
%   binary tree algorithm to reduce calls to bitor.
%
%JRA 12/15/04
%
%Usage:
%   function bitsHitV = cumbitor(bitsHitV)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
% without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
% See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with CERR.  If not, see <http://www.gnu.org/licenses/>.

while length(bitsHitV) >= 2
    len = length(bitsHitV);
    if mod(len, 2) == 1
        last = bitsHitV(end);
        bitsHitV = bitor(bitsHitV(1:(len-1)/2), bitsHitV((len-1)/2+1:end-1));
        bitsHitV(1) = bitor(bitsHitV(1), last);
    else
        bitsHitV = bitor(bitsHitV(1:len/2), bitsHitV(len/2+1:end));
    end
end


function [pointst,pointsf]=ridges(tfr,hat,t,method,trace);
%RIDGES	Extraction of ridges.
%	[POINTST,POINTSF]=RIDGES(TFR,HAT,T,METHOD,TRACE) extracts the
%	ridges of a time-frequency distribution. These ridges are some
%	particular sets of curves deduced from the stationary points of
%	their  reassignment operators.
%
%	TFR    : time-frequency representation
%	HAT    : complex matrix of the reassignment vectors.
%	T      : the time instant(s).
%	METHOD : the chosen representation (default: 'tfrrsp').
%	TRACE  : if nonzero, the progression of the algorithm is shown
%					   (default : 0).
%
%	POINTST,POINTSF are two vectors for the time and frequency
%	coordinates of the stationary points of the reassignment.
%	Therefore, PLOT(POINTST,POINTSF,'.') shows the squeleton of the
%	representation.
%
%	Example :
%	 sig=fmlin(128,0.1,0.4); g=tftb_window(21,'kaiser');
%	 h=tftb_window(47,'Kaiser'); t=1:2:127;
%	 figure(1), [tfr,rtfr,hat]=tfrrspwv(sig,t,128,g,h);
%	 ridges(tfr,hat,t,'tfrrspwv',1);
%	 figure(2), [tfr,rtfr,hat]=  tfrrsp(sig,t,128,h);
%	 ridges(tfr,hat,t,'tfrrsp',1);
%
%	See also : FRIEDMAN.

%	F. Auger, August 1994, December 1995.
%	Copyright (c) 1996 by CNRS (France).
%
%  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
%  (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, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin<2),
 error('at least 2 parameters required');
end;

[tfrrow,tfrcol]=size(tfr);
[hatrow,hatcol]=size(hat);

if (nargin==2),
 t=1:tfrcol; method='tfrrsp'; trace=0;
elseif (nargin==3),
 method='tfrrsp'; trace=0;
elseif (nargin==4),
 trace=0;
end;

[trow,tcol] = size(t);
if (trow~=1),
 error('T must only have one row');
elseif (tfrrow~=hatrow)|(tfrcol~=hatcol),
 error('TFR and HAT must have the same size');
end;

Nf=tfrrow; frequencies=(1:Nf)';
threshold=sum(sum(tfr))*0.5/(tfrrow*tfrcol);

pointst=[]; pointsf=[];

if trace, fprintf ('\nRidge extraction: \n'); end;

method=upper(method);
if strcmp(method,'TFRRPWV') | strcmp(method,'TFRRPMH'),
 for icol=1:tfrcol, ti=t(icol);
  if trace, disprog(icol,tfrcol,10); end;
  indices=find((tfr(:,icol)>threshold)&(hat(:,icol)-frequencies==0));
  nbindices=length(indices);
  if (nbindices>0),
   pointst=[pointst;ones(nbindices,1)*ti];
   pointsf=[pointsf;indices/(2.0*Nf)];
  end;
 end;
elseif strcmp(method,'TFRRSPWV'),
 for icol=1:tfrcol, ti=t(icol);
  if trace, disprog(icol,tfrcol,10); end;
  indices=find((real(hat(:,icol))-frequencies==0)&...
               (imag(hat(:,icol))-icol==0)&...
               (tfr(:,icol)>threshold));
  nbindices=length(indices);
  if (nbindices>0),
   pointst=[pointst;ones(nbindices,1)*ti];
   pointsf=[pointsf;indices/(2.0*Nf)];
  end;
 end;
elseif strcmp(method,'TFRRSP')|strcmp(method,'TYPE1')
 for icol=1:tfrcol, ti=t(icol);
  if trace, disprog(icol,tfrcol,10); end;
  indices=find((real(hat(:,icol))-frequencies==0)&...
               (imag(hat(:,icol))-icol==0)&...
               (tfr(:,icol)>threshold));
  nbindices=length(indices);
  if (nbindices>0),
   pointst=[pointst;ones(nbindices,1)*ti];
   pointsf=[pointsf;indices/Nf];
  end;
 end;
else
 error('unknown representation');
end;

if (nargout==0),
 clf;
 plot(pointst,pointsf,'.')
 axis([min(t) max(t) 0 0.5]);
end;