function [fhat, fola] = blocksyn(F, c , Lb, fola)
%BLOCKSYN Blockwise synthesis interface
%   Usage: blocksyn(F, c, Lb)
%
%   Input parameters:
%      F    : Synthesis frame object.
%      c    : Coefficients of a block.
%      Lb   : Length of the block.
%      fola : Explicitly defined overlap.
%   Output parameters:
%      fhat : Reconstructed block of signal.
%      fola : Stored overlap.
%
%   `fhat=blocksyn(F,c,Lb)` reconstructs the signal block *fhat* from the
%   coefficients *c* using the frame defined by *F*. If some overlap is used,
%   it is stored internally using block_interface. Note that the function is
%   capable of handling overlaps internally only for a single call
%   to the function in the blockproc. loop.%
%
%   `[fhat,fola]=blocksyn(F,c,Lb,fola)` does the same, but the block algorithm
%   uses `fola` to read and store overlap explicitly. `fola` can be empty.
%
%   **Note:** To get perfect reconstruction, the synthesis frame *F* must
%   be a dual frame of the analysis frame used in |blockana|.
%
%   See also: block, blockana, framedual
%
%   References: dogrhove12 ltfatnote026

complainif_notenoughargs(nargin,3,'BLOCKSYN');
complainif_notvalidframeobj(F,'BLOCKSYN');

    if ~isfield(F,'blockalg')
        F.blockalg = 'naive';
    end

    if ~isfield(F,'L')
         error(['%s: The frame object was not accelerated. See ',...
                'BLOCKFRAMEACCEL.'],upper(mfilename));
    end

    if nargin<4
       fola = [];
    end

    % Next block index start (from a global point of view, starting with zero)
    nextSb = block_interface('getPos');
    % Block index start (from a global point of view, starting with zero)
    Sb = nextSb-Lb;

    if strcmp(F.blockalg,'naive')
       if F.L < Lb
          error(['%s: The frame object was accelerated with incompatible ',...
                 'length. The block length is %i but the accelerated ',...
                 'length is %i.'],upper(mfilename),Lb,F.L);
       end
       % Most general. Should work for anything.
       % Produces awful block artifacts when coefficients are altered.
       fhat = F.frsyn(c);
       fhat = fhat(1:Lb,:);
    elseif strcmp(F.blockalg,'sliced')
        if F.L < 2*Lb
          error(['%s: The frame object was accelerated with incompatible ',...
                 'length. The block length is %i but the accelerated ',...
                 'length is %i.'],upper(mfilename),Lb,F.L);
        end
        % General processing
        % Equal block length assumption
        % Reconstruct
        f = F.frsyn(c);
        % If the transform length differs from the 2*Lb,
        % Pick the correct part from the result
        pad = size(f,1) - 2*Lb;
        sIdx = floor(pad/2);
        % Result should not be longer than 2*Lb
        f = f(1+sIdx:sIdx+2*Lb,:);
        % Multiply by a slicing window
        f = bsxfun(@times,F.sliwin,f);
        % Load and add overlap (first half)
        ol = loadOverlap(Lb,size(f,2),fola);
        olLen = size(ol,1);
        f(1:olLen,:) = f(1:olLen,:) + ol;
        % Store overlap (second half)
        if nargout>1
           fola=storeOverlap(f,Lb);
        else
           storeOverlap(f,Lb);
        end
        % Return first half
        fhat = f(1:Lb,:);
    elseif strcmp(F.blockalg,'segola')
       if ~isfield(F,'winLen')
          error('%s: Frame does not have FIR windows.',upper(mfilename));
       end
       Lw = F.winLen;
       switch(F.type)
         case 'fwt'
           % The SegDWT algorithm
           J = F.J;
           w = F.g;
           m = numel(w.g{1}.h);
           a = w.a(1);
           blocksize = a^J;
           r = (a^J-1)/(a-1)*(m-1);
           Lbrec = (floor(nextSb/blocksize) - floor(Sb/blocksize))*blocksize;
           rSb = (a^J-1)/(a-1)*(m-a) + mod(Sb,a^J);
           over = r - rSb;
           f = block_ifwt(c,w,J,Lbrec);
           ol = loadOverlap(r-mod(Sb, a^J),size(c,2),fola);
           olLen = size(ol,1);
           f(1:olLen-over,:) = f(1:olLen-over,:) + ol(1+over:end,:);
           f = [ol(1:over,:);f];
           if nargout>1
              fola=storeOverlap(f,r-mod(nextSb, a^J));
           else
              storeOverlap(f,r-mod(nextSb, a^J));
           end
           fhat = f(1:Lb,:);
          case {'dgt','dgtreal','dwilt','wmdct'}
           % Time step
           a = F.a;
           % Length of the left half of the window
           Lwl = floor(Lw/2);

           Sbonelmax =  ceil((Lw-1)/a)*a + a-1;
           Sbolen = ceil((Lw-1)/a)*a + mod(Sb,a);
           % Next block overlap length
           nextSbolen = ceil((Lw-1)/a)*a + mod(nextSb,a);
           Lext = Sbolen + Lb - mod(nextSb,a);
           Lextc = Sbolen + Lb - nextSbolen + Lwl;

           startc = ceil(Lwl/a)+1;
           endc = ceil((Lextc)/a);

           cc = F.coef2native(c,size(c));
           chat = zeros(size(cc,1),ceil(F.L/a),size(cc,3),class(cc));
           chat(:,startc:endc,:) = cc;
           chat = F.native2coef(chat);
           f = F.frsyn(chat);
           f = f(1:Lext,:);
           over = Sbonelmax - Sbolen;

           ol = loadOverlap(Sbonelmax-mod(Sb,a),size(c,2),fola);
           olLen = size(ol,1);
           f(1:olLen-over,:) = f(1:olLen-over,:) + ol(1+over:end,:);
           f = [ol(1:over,:);f];
           if nargout>1
              fola=storeOverlap(f,Sbonelmax-mod(nextSb,a));
           else
              storeOverlap(f,Sbonelmax-mod(nextSb,a));
           end
           fhat = f(1:Lb,:);
          case {'filterbank','filterbankreal'}
           lcma = F.lcma;
           % Time step
           a = F.a(:,1);
           % Length of the left half of the window
           Lwl = max(-F.g_info.offset);

                if Lw-1 < a
           Sbonelmax =   lcma-1;
           Sbolen =  mod(Sb,lcma);
           nextSbolen = mod(nextSb,lcma);
                else
           Sbonelmax =  ceil((Lw-1)/lcma)*lcma + lcma-1;
           Sbolen = ceil((Lw-1)/lcma)*lcma + mod(Sb,lcma);
           nextSbolen = ceil((Lw-1)/lcma)*lcma + mod(nextSb,lcma);
                end

           Lext = Sbolen + Lb - mod(nextSb,lcma);
           Lextc = Sbolen + Lb - nextSbolen + Lwl;

           startc = ceil(Lwl./a)+1;
           endc = ceil((Lextc)./a);

           cc = F.coef2native(c,size(c));

           chat = cell(numel(cc),1);
           for ii=1:numel(cc)
              chat{ii} = zeros(ceil(F.L./a(ii)),size(cc{ii},2));
              chat{ii}(startc(ii):endc(ii),:) = cc{ii};
           end

           chat = F.native2coef(chat);
           f = F.frsyn(chat);
           f = f(1:Lext,:);
           over = Sbonelmax - Sbolen;

           ol = loadOverlap(Sbonelmax-mod(Sb,lcma),size(c,2),fola);
           olLen = size(ol,1);
           f(1:olLen-over,:) = f(1:olLen-over,:) + ol(1+over:end,:);
           f = [ol(1:over,:);f];
           if nargout>1
              fola=storeOverlap(f,Sbonelmax-mod(nextSb,lcma));
           else
              storeOverlap(f,Sbonelmax-mod(nextSb,lcma));
           end
           fhat = f(1:Lb,:);
          otherwise
           error('%s: Unsupported frame.',upper(mfilename));
        end

    else
       error('%s: Frame was not created with blockaccel.',upper(mfilename));
    end

end

function overlap = loadOverlap(L,chan,overlap)
%LOADOVERLAP Loads overlap
%
%
   if isempty(overlap)
      overlap = block_interface('getSynOverlap');
   end

   if isempty(overlap)
     overlap = zeros(L,chan,block_interface('getClassId'));
   end
   Lo = size(overlap,1);
   if nargin<1
      L = Lo;
   end
   if L>Lo
      % Required more samples than stored
      if 0
         error('%s: Required more samples than stored.',upper(mfilename));
      else
         % pad with zeros
         overlapTmp = zeros(L,chan,block_interface('getClassId'));
         overlapTmp(end-Lo+1:end,:) = overlap;
         overlap = overlapTmp;
      end
   end
   overlap = overlap(end-L+1:end,:);
end

function overlap = storeOverlap(fext,L)
%STOREOVERLAP Stores overlap
%
%
    if L>size(fext,1)
        error('%s: Storing more samples than passed.',upper(mfilename));
    end
    overlap = fext(end-L+1:end,:);

    if nargout<1
       block_interface('setSynOverlap',overlap);
    end
end % STOREOVERLAP


function RoPSs = RoPSFunc(mesh,neighborSize,binSize, rotaSize, LocalFrames)

%  Author: Yulan Guo {yulan.guo@nudt.edu.cn}
%  NUDT, China & CSSE, UWA, Australia
%
% This function takes a mesh and local reference frames (LRFs) for a set of
% keypoints as an input, and generate RoPS feature descriptors for the
% keypoints as an output. An illustration is shown in Fig4 in Ref.[1].
%
% Arguments : mesh - with vertices and faces
%                       neighborSize - the size of neighborhood to define a
%                                                  local surface for a selected keypoint
%                       binSize - number of bins for 2D plane partition
%                       rotaSize - number of rotations around each coordinate axis
%                       LocalFrames - local reference frames corresponding to all
%                                                keypoints of the input mesh

% Return :         RoPS - RoPS feature descriptors corresponding to all
%                                     keypoints
%
% Copyright : This code is written by Yulan Guo {yulan.guo@nudt.edu.cn}, NUDT.
%               The code may be used, modified and distributed for research purposes with
%              acknowledgement of the author and inclusion this copyright information.
%References:
%           [1] Yulan Guo, Ferdous Sohel, Mohammed Bennamoun, Min Lu, Jianwei Wan.
%           Rotational Projection Statistics for 3D Local Surface Description and Object Recognition.
%           Internation Journal of Computer Vision. 2013, 105 (1), 63-86
%           [2] Yulan Guo, Mohammed Bennamoun, Ferdous A Sohel, Min Lu, Jianwei Wan.
%           3D Object Recognition in Cluttered Scenes with Local Surface Features: A Survey.
%           IEEE Transactions on Pattern Analysis and Machine Intelligence,2014
%
% Disclaimer: This code is provided as is without any warranty.

%%%%%%%%%%%% parallel computing to accelerate.
p = gcp('nocreate'); % If no pool, do not create new one.
if isempty(p)   % if exists no parallel computing handle, create one.
    poolsize = parpool;
    % disp('Create a Parallel Computing Pool to accelarate process' );
else
    poolsize = p.NumWorkers;
    % disp(sprintf( 'Parpool has %d workers', poolsize) );
end

keypntIdx = mesh.keypntIdx;
% BucketSize = floor(length(mesh.vertices)/100);
% kdtreeVertices = KDTreeSearcher(mesh.vertices,'Distance','euclidean','BucketSize',BucketSize);
% BucketSize = floor(length(mesh.vertices)/100);
kdtreeVertices = KDTreeSearcher(mesh.vertices,'Distance','euclidean');

interval = pi/2/rotaSize;
parfor i = 1:length(keypntIdx)
    %obtain the neighboring point of a keypoint
    keypnt = mesh.vertices(keypntIdx(i),:);
    [neighborIdx2,neighborDis]= rangesearch(kdtreeVertices,keypnt,neighborSize);
    neighborIdx2 = cell2mat(neighborIdx2);
    neighborIdx = neighborIdx2(2:end);
    neighbNum = length(neighborIdx);
    if neighbNum<=1
        RoPSs{i,1} = ones(rotaSize*45,1)/sum(ones(rotaSize*45,1));
        continue;
    end

    %transfom the neighboring point to the local reference frame (LRF)
    rotation = LocalFrames{i};
    neighbor = [];
    for j=1:neighbNum
        neighbor(j,:) = (mesh.vertices(neighborIdx(j),:)-mesh.vertices(keypntIdx(i),:))*inv(rotation);
    end

    RoPS = [];
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %calculate the sub-feature of the keypoint along the Z axis
    for rotaIdx = 1:rotaSize
        rotaAngle = (rotaIdx-1)*interval + interval/2;
        R = [cos(rotaAngle) sin(rotaAngle) 0; -sin(rotaAngle) cos(rotaAngle) 0; 0 0 1]';
        rotaNeighbor = neighbor*R;
        %projection on the XY plane
        projNeighborXY = [rotaNeighbor(:,1),rotaNeighbor(:,2)];
        histTemp = subRoPSFunc(projNeighborXY,binSize);
        RoPS = [RoPS,histTemp];
        %projection on the XZ plane
        projNeighborXZ = [rotaNeighbor(:,1),rotaNeighbor(:,3)];
        histTemp = subRoPSFunc(projNeighborXZ,binSize);
        RoPS = [RoPS,histTemp];
        %projection on the YZ plane
        projNeighborYZ = [rotaNeighbor(:,2),rotaNeighbor(:,3)];
        histTemp = subRoPSFunc(projNeighborYZ,binSize);
        RoPS = [RoPS,histTemp];
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %calculate the sub-feature of the keypoint along the Y axis
    for rotaIdx = 1:rotaSize
        rotaAngle = (rotaIdx-1)*interval + interval/2;
        R = [cos(rotaAngle) 0 sin(rotaAngle); 0 1 0 ;-sin(rotaAngle) 0 cos(rotaAngle)]';
        rotaNeighbor = neighbor*R;
        %projection on the XY plane
        projNeighborXY = [rotaNeighbor(:,1),rotaNeighbor(:,2)];
        histTemp = subRoPSFunc(projNeighborXY,binSize);
        RoPS = [RoPS,histTemp];
        %projection on the XZ plane
        projNeighborXZ = [rotaNeighbor(:,1),rotaNeighbor(:,3)];
        histTemp = subRoPSFunc(projNeighborXZ,binSize);
        RoPS = [RoPS,histTemp];
        %projection on the YZ plane
        projNeighborYZ = [rotaNeighbor(:,2),rotaNeighbor(:,3)];
        histTemp = subRoPSFunc(projNeighborYZ,binSize);
        RoPS = [RoPS,histTemp];
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    end
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %calculate the sub-feature of the keypoint along the X axis
    for rotaIdx = 1:rotaSize
        rotaAngle = (rotaIdx-1)*interval + interval/2;
        R = [1,0,0; 0,cos(rotaAngle),sin(rotaAngle); 0, -sin(rotaAngle), cos(rotaAngle)]';
        rotaNeighbor = neighbor*R;
        %projection on the XY plane
        projNeighborXY = [rotaNeighbor(:,1),rotaNeighbor(:,2)];
        histTemp = subRoPSFunc(projNeighborXY,binSize);
        RoPS = [RoPS,histTemp];
        %projection on the XZ plane
        projNeighborXZ = [rotaNeighbor(:,1),rotaNeighbor(:,3)];
        histTemp = subRoPSFunc(projNeighborXZ,binSize);
        RoPS = [RoPS,histTemp];
        %projection on the YZ plane
        projNeighborYZ = [rotaNeighbor(:,2),rotaNeighbor(:,3)];
        histTemp = subRoPSFunc(projNeighborYZ,binSize);
        RoPS = [RoPS,histTemp];
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    RoPSs{i,1} = (RoPS)'/sum(RoPS);
end


function segment_length = p14_boundary_segment_length ( segment_index, h )

%*****************************************************************************80
%
%% P14_BOUNDARY_SEGMENT_LENGTH returns boundary segment lengths in problem 14.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer SEGMENT_INDEX, the index of one of the boundary segments.
%
%    Input, real H, the suggested spacing between points.
%
%    Output, integer SEGMENT_LENGTH, the number of points in the segment.
%
  n1 = 90;
  n2 = 11;

  v1 = [ ...
    316.43027, 404.47559; ...
    291.04946, 400.70917; ...
    265.16504, 409.77890; ...
    241.46794, 402.40310; ...
    216.55145, 396.52064; ...
    163.28492, 411.37102; ...
    142.81752, 391.16355; ...
    111.95404, 346.70264; ...
    100.03538, 325.72710; ...
    103.98723, 302.51587; ...
    128.72978, 285.72802; ...
    147.49111, 266.23345; ...
    196.65261, 242.24055; ...
    213.56835, 221.67192; ...
    226.49969, 198.09326; ...
    248.37126, 183.50473; ...
    262.21952, 165.39102; ...
    278.42330, 149.91715; ...
    300.71846, 145.82601; ...
    311.12698, 166.71094; ...
    326.66315, 184.58335; ...
    359.78574, 225.48049; ...
    357.08892, 252.88958; ...
    358.76685, 285.34403; ...
    361.50834, 303.71287; ...
    371.68926, 314.92452; ...
    380.49890, 324.58632; ...
    396.37634, 328.88990; ...
    412.59116, 327.25238; ...
    425.48394, 315.28623; ...
    435.84305, 302.44664; ...
    458.34025, 297.55121; ...
    479.66439, 288.99238; ...
    493.09812, 270.20636; ...
    518.87309, 264.56427; ...
    547.18014, 268.18846; ...
    600.49708, 240.62570; ...
    625.96183, 238.40347; ...
    633.90530, 260.70629; ...
    621.50451, 285.88914; ...
    576.87224, 322.14121; ...
    570.51915, 348.85423; ...
    567.16400, 378.24075; ...
    558.00668, 406.86552; ...
    565.19008, 435.75599; ...
    567.56437, 465.33407; ...
    550.87626, 490.96358; ...
    532.98174, 515.84491; ...
    500.66817, 551.89078; ...
    478.75120, 562.17222; ...
    430.03371, 583.94286; ...
    401.20454, 587.69910; ...
    368.32214, 581.10110; ...
    354.26303, 585.86085; ...
    346.75200, 601.10367; ...
    332.85137, 628.74602; ...
    308.02188, 645.84180; ...
    295.52344, 647.18525; ...
    286.51519, 651.60328; ...
    285.98846, 662.07339; ...
    298.93455, 665.66316; ...
    301.70226, 682.79570; ...
    278.65857, 689.63850; ...
    266.25737, 712.11005; ...
    287.28701, 732.77147; ...
    318.19548, 736.85151; ...
    343.83067, 753.60957; ...
    375.53164, 758.35231; ...
    405.73444, 768.98687; ...
    406.33873, 785.59001; ...
    378.35436, 789.44240; ...
    350.02151, 795.02238; ...
    338.68030, 788.87325; ...
    325.67930, 786.10177; ...
    319.05995, 798.04657; ...
    301.78158, 795.34254; ...
    280.69272, 773.86634; ...
    254.55844, 758.02898; ...
    234.07759, 737.42090; ...
    218.38337, 711.41500; ...
    220.99086, 682.17833; ...
    224.50640, 651.96297; ...
    240.25971, 631.36117; ...
    259.86174, 612.60253; ...
    291.85381, 556.70385; ...
    315.52139, 537.56387; ...
    341.63663, 520.12519; ...
    351.37130, 458.75372; ...
    349.33183, 431.31454; ...
    328.80465, 412.43055 ]';
  v2 = [ ...
    238.64853, 266.58978; ...
    235.14026, 287.95183; ...
    238.20736, 303.46785; ...
    250.13902, 303.71290; ...
    258.51675, 297.46973; ...
    274.55300, 291.27357; ...
    284.66230, 280.72063; ...
    279.73288, 267.83455; ...
    270.68478, 255.55440; ...
    255.73801, 249.16872; ...
    241.72690, 256.73448 ]';

  if ( h <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P14_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
    fprintf ( 1, '  Nonpositive H = %f\n', h );
    error ( 'P14_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
  end

  if ( segment_index == 1 )

    length = polyloop_length_nd ( 2, n1, v1 ) ;

  elseif ( segment_index == 2 )

    length = polyloop_length_nd ( 2, n2, v2 );

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'P14_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
    fprintf ( 1, '  Illegal SEGMENT_INDEX = %d\n', segment_index );
    error ( 'P14_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );

  end

  n = round ( length / h );

  segment_length = n;

  return
end


function [A, W] = fpica(X, whiteningMatrix, dewhiteningMatrix, approach, ...
    numOfIC, g, finetune, a1, a2, myy, stabilization, ...
    epsilon, maxNumIterations, maxFinetune, initState, ...
    guess, sampleSize, displayMode, displayInterval, ...
    s_verbose);
%FPICA - Fixed point ICA. Main algorithm of FASTICA.
%
% [A, W] = fpica(whitesig, whiteningMatrix, dewhiteningMatrix, approach,
%        numOfIC, g, finetune, a1, a2, mu, stabilization, epsilon,
%        maxNumIterations, maxFinetune, initState, guess, sampleSize,
%        displayMode, displayInterval, verbose);
%
% Perform independent component analysis using Hyvarinen's fixed point
% algorithm. Outputs an estimate of the mixing matrix A and its inverse W.
%
% whitesig                              :the whitened data as row vectors
% whiteningMatrix                       :can be obtained with function whitenv
% dewhiteningMatrix                     :can be obtained with function whitenv
% approach      [ 'symm' | 'defl' ]     :the approach used (deflation or symmetric)
% numOfIC       [ 0 - Dim of whitesig ] :number of independent components estimated
% g             [ 'pow3' | 'tanh' |     :the nonlinearity used
%                 'gaus' | 'skew' ]
% finetune      [same as g + 'off']     :the nonlinearity used in finetuning.
% a1                                    :parameter for tuning 'tanh'
% a2                                    :parameter for tuning 'gaus'
% mu                                    :step size in stabilized algorithm
% stabilization [ 'on' | 'off' ]        :if mu < 1 then automatically on
% epsilon                               :stopping criterion
% maxNumIterations                      :maximum number of iterations
% maxFinetune                           :maximum number of iteretions for finetuning
% initState     [ 'rand' | 'guess' ]    :initial guess or random initial state. See below
% guess                                 :initial guess for A. Ignored if initState = 'rand'
% sampleSize    [ 0 - 1 ]               :percentage of the samples used in one iteration
% displayMode   [ 'signals' | 'basis' | :plot running estimate
%                 'filters' | 'off' ]
% displayInterval                       :number of iterations we take between plots
% verbose       [ 'on' | 'off' ]        :report progress in text format
%
% EXAMPLE
%       [E, D] = pcamat(vectors);
%       [nv, wm, dwm] = whitenv(vectors, E, D);
%       [A, W] = fpica(nv, wm, dwm);
%
%
% This function is needed by FASTICA and FASTICAG
%
%   See also FASTICA, FASTICAG, WHITENV, PCAMAT

% @(#)$Id: fpica.m,v 1.7 2005/06/16 12:52:55 jarmo Exp $

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Global variable for stopping the ICA calculations from the GUI
global g_FastICA_interrupt;
if isempty(g_FastICA_interrupt)
    clear global g_FastICA_interrupt;
    interruptible = 0;
else
    interruptible = 1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Default values

if nargin < 3, error('Not enough arguments!'); end
[vectorSize, numSamples] = size(X);
if nargin < 20, s_verbose = 'on'; end
if nargin < 19, displayInterval = 1; end
if nargin < 18, displayMode = 'on'; end
if nargin < 17, sampleSize = 1; end
if nargin < 16, guess = 1; end
if nargin < 15, initState = 'rand'; end
if nargin < 14, maxFinetune = 100; end
if nargin < 13, maxNumIterations = 1000; end
if nargin < 12, epsilon = 0.0001; end
if nargin < 11, stabilization = 'on'; end
if nargin < 10, myy = 1; end
if nargin < 9, a2 = 1; end
if nargin < 8, a1 = 1; end
if nargin < 7, finetune = 'off'; end
if nargin < 6, g = 'pow3'; end
if nargin < 5, numOfIC = vectorSize; end     % vectorSize = Dim
if nargin < 4, approach = 'defl'; end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Checking the data

if ~isreal(X)
    error('Input has an imaginary part.');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Checking the value for verbose

switch lower(s_verbose)
    case 'on'
        b_verbose = 1;
    case 'off'
        b_verbose = 0;
    otherwise
        error(sprintf('Illegal value [ %s ] for parameter: ''verbose''\n', s_verbose));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Checking the value for approach

switch lower(approach)
    case 'symm'
        approachMode = 1;
    case 'defl'
        approachMode = 2;
    otherwise
        error(sprintf('Illegal value [ %s ] for parameter: ''approach''\n', approach));
end
if b_verbose, fprintf('Used approach [ %s ].\n', approach); end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Checking the value for numOfIC

if vectorSize < numOfIC
    error('Must have numOfIC <= Dimension!');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Checking the sampleSize
if sampleSize > 1
    sampleSize = 1;
    if b_verbose
        fprintf('Warning: Setting ''sampleSize'' to 1.\n');
    end
elseif sampleSize < 1
    if (sampleSize * numSamples) < 1000
        sampleSize = min(1000/numSamples, 1);
        if b_verbose
            fprintf('Warning: Setting ''sampleSize'' to %0.3f (%d samples).\n', ...
                sampleSize, floor(sampleSize * numSamples));
        end
    end
end
if b_verbose
    if  b_verbose & (sampleSize < 1)
        fprintf('Using about %0.0f%% of the samples in random order in every step.\n',sampleSize*100);
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Checking the value for nonlinearity.

switch lower(g)
    case 'pow3'
        gOrig = 10;
    case 'tanh'
        gOrig = 20;
    case {'gaus', 'gauss'}
        gOrig = 30;
    case 'skew'
        gOrig = 40;
    otherwise
        error(sprintf('Illegal value [ %s ] for parameter: ''g''\n', g));
end
if sampleSize ~= 1
    gOrig = gOrig + 2;
end
if myy ~= 1
    gOrig = gOrig + 1;
end

if b_verbose,
    fprintf('Used nonlinearity [ %s ].\n', g);
end

finetuningEnabled = 1;
switch lower(finetune)
    case 'pow3'
        gFine = 10 + 1;
    case 'tanh'
        gFine = 20 + 1;
    case {'gaus', 'gauss'}
        gFine = 30 + 1;
    case 'skew'
        gFine = 40 + 1;
    case 'off'
        if myy ~= 1
            gFine = gOrig;
        else
            gFine = gOrig + 1;
        end
        finetuningEnabled = 0;
    otherwise
        error(sprintf('Illegal value [ %s ] for parameter: ''finetune''\n', ...
            finetune));
end

if b_verbose & finetuningEnabled
    fprintf('Finetuning enabled (nonlinearity: [ %s ]).\n', finetune);
end

switch lower(stabilization)
    case 'on'
        stabilizationEnabled = 1;
    case 'off'
        if myy ~= 1
            stabilizationEnabled = 1;
        else
            stabilizationEnabled = 0;
        end
    otherwise
        error(sprintf('Illegal value [ %s ] for parameter: ''stabilization''\n', ...
            stabilization));
end

if b_verbose & stabilizationEnabled
    fprintf('Using stabilized algorithm.\n');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Some other parameters
myyOrig = myy;
% When we start fine-tuning we'll set myy = myyK * myy
myyK = 0.01;
% How many times do we try for convergence until we give up.
failureLimit = 5;

usedNlinearity = gOrig;
stroke = 0;
notFine = 1;
long = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Checking the value for initial state.

switch lower(initState)
    case 'rand'
        initialStateMode = 0;
    case 'guess'
        if size(guess,1) ~= size(whiteningMatrix,2)
            initialStateMode = 0;
            if b_verbose
                fprintf('Warning: size of initial guess is incorrect. Using random initial guess.\n');
            end
        else
            initialStateMode = 1;
            if size(guess,2) < numOfIC
                if b_verbose
                    fprintf('Warning: initial guess only for first %d components. Using random initial guess for others.\n', size(guess,2));
                end
                guess(:, size(guess, 2) + 1:numOfIC) = ...
                    rand(vectorSize,numOfIC-size(guess,2))-.5;
            elseif size(guess,2)>numOfIC
                guess=guess(:,1:numOfIC);
                fprintf('Warning: Initial guess too large. The excess column are dropped.\n');
            end
            if b_verbose, fprintf('Using initial guess.\n'); end
        end
    otherwise
        error(sprintf('Illegal value [ %s ] for parameter: ''initState''\n', initState));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Checking the value for display mode.

switch lower(displayMode)
    case {'off', 'none'}
        usedDisplay = 0;
    case {'on', 'signals'}
        usedDisplay = 1;
        if (b_verbose & (numSamples > 10000))
            fprintf('Warning: Data vectors are very long. Plotting may take long time.\n');
        end
        if (b_verbose & (numOfIC > 25))
            fprintf('Warning: There are too many signals to plot. Plot may not look good.\n');
        end
    case 'basis'
        usedDisplay = 2;
        if (b_verbose & (numOfIC > 25))
            fprintf('Warning: There are too many signals to plot. Plot may not look good.\n');
        end
    case 'filters'
        usedDisplay = 3;
        if (b_verbose & (vectorSize > 25))
            fprintf('Warning: There are too many signals to plot. Plot may not look good.\n');
        end
    otherwise
        error(sprintf('Illegal value [ %s ] for parameter: ''displayMode''\n', displayMode));
end

% The displayInterval can't be less than 1...
if displayInterval < 1
    displayInterval = 1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if b_verbose, fprintf('Starting ICA calculation...\n'); end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SYMMETRIC APPROACH
if approachMode == 1,

    % set some parameters more...
    usedNlinearity = gOrig;
    stroke = 0;
    notFine = 1;
    long = 0;

    A = zeros(vectorSize, numOfIC);  % Dewhitened basis vectors.
    if initialStateMode == 0
        % Take random orthonormal initial vectors.
        B = orth (randn (vectorSize, numOfIC));
    elseif initialStateMode == 1
        % Use the given initial vector as the initial state
        B = whiteningMatrix * guess;
    end

    BOld = zeros(size(B));
    BOld2 = zeros(size(B));

    % This is the actual fixed-point iteration loop.
    for round = 1:maxNumIterations + 1,
        if round == maxNumIterations + 1,
            fprintf('No convergence after %d steps\n', maxNumIterations);
            fprintf('Note that the plots are probably wrong.\n');
            if ~isempty(B)
                % Symmetric orthogonalization.
                B = B * real(inv(B' * B)^(1/2));

                W = B' * whiteningMatrix;
                A = dewhiteningMatrix * B;
            else
                W = [];
                A = [];
            end
            return;
        end

        if (interruptible & g_FastICA_interrupt)
            if b_verbose
                fprintf('\n\nCalculation interrupted by the user\n');
            end
            if ~isempty(B)
                W = B' * whiteningMatrix;
                A = dewhiteningMatrix * B;
            else
                W = [];
                A = [];
            end
            return;
        end

        % Symmetric orthogonalization.
        B = B * real(inv(B' * B)^(1/2));

        % Test for termination condition. Note that we consider opposite
        % directions here as well.
        minAbsCos = min(abs(diag(B' * BOld)));
        minAbsCos2 = min(abs(diag(B' * BOld2)));

        if (1 - minAbsCos < epsilon)
            if finetuningEnabled & notFine
                if b_verbose, fprintf('Initial convergence, fine-tuning: \n'); end;
                notFine = 0;
                usedNlinearity = gFine;
                myy = myyK * myyOrig;
                BOld = zeros(size(B));
                BOld2 = zeros(size(B));

            else
                if b_verbose, fprintf('Convergence after %d steps\n', round); end

                % Calculate the de-whitened vectors.
                A = dewhiteningMatrix * B;
                break;
            end
        elseif stabilizationEnabled
            if (~stroke) & (1 - minAbsCos2 < epsilon)
                if b_verbose, fprintf('Stroke!\n'); end;
                stroke = myy;
                myy = .5*myy;
                if mod(usedNlinearity,2) == 0
                    usedNlinearity = usedNlinearity + 1;
                end
            elseif stroke
                myy = stroke;
                stroke = 0;
                if (myy == 1) & (mod(usedNlinearity,2) ~= 0)
                    usedNlinearity = usedNlinearity - 1;
                end
            elseif (~long) & (round>maxNumIterations/2)
                if b_verbose, fprintf('Taking long (reducing step size)\n'); end;
                long = 1;
                myy = .5*myy;
                if mod(usedNlinearity,2) == 0
                    usedNlinearity = usedNlinearity + 1;
                end
            end
        end

        BOld2 = BOld;
        BOld = B;

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Show the progress...
        if b_verbose
            if round == 1
                fprintf('Step no. %d\n', round);
            else
                fprintf('Step no. %d, change in value of estimate: %.3g \n', round, 1 - minAbsCos);
            end
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Also plot the current state...
        switch usedDisplay
            case 1
                if rem(round, displayInterval) == 0,
                    % There was and may still be other displaymodes...
                    % 1D signals
                    icaplot('dispsig',(X'*B)');
                    drawnow;
                end
            case 2
                if rem(round, displayInterval) == 0,
                    % ... and now there are :-)
                    % 1D basis
                    A = dewhiteningMatrix * B;
                    icaplot('dispsig',A');
                    drawnow;
                end
            case 3
                if rem(round, displayInterval) == 0,
                    % ... and now there are :-)
                    % 1D filters
                    W = B' * whiteningMatrix;
                    icaplot('dispsig',W);
                    drawnow;
                end
            otherwise
        end

        switch usedNlinearity
            % pow3
            case 10
                B = (X * (( X' * B) .^ 3)) / numSamples - 3 * B;
            case 11
                % optimoitu - epsilonin kokoisia eroja
                % t�m� on optimoitu koodi, katso vanha koodi esim.
                % aikaisemmista versioista kuten 2.0 beta3
                Y = X' * B;
                Gpow3 = Y .^ 3;
                Beta = sum(Y .* Gpow3);
                D = diag(1 ./ (Beta - 3 * numSamples));
                B = B + myy * B * (Y' * Gpow3 - diag(Beta)) * D;
            case 12
                Xsub=X(:, getSamples(numSamples, sampleSize));
                B = (Xsub * (( Xsub' * B) .^ 3)) / size(Xsub,2) - 3 * B;
            case 13
                % Optimoitu
                Ysub=X(:, getSamples(numSamples, sampleSize))' * B;
                Gpow3 = Ysub .^ 3;
                Beta = sum(Ysub .* Gpow3);
                D = diag(1 ./ (Beta - 3 * size(Ysub', 2)));
                B = B + myy * B * (Ysub' * Gpow3 - diag(Beta)) * D;

                % tanh
            case 20
                hypTan = tanh(a1 * X' * B);
                B = X * hypTan / numSamples - ...
                    ones(size(B,1),1) * sum(1 - hypTan .^ 2) .* B / numSamples * ...
                    a1;
            case 21
                % optimoitu - epsilonin kokoisia
                Y = X' * B;
                hypTan = tanh(a1 * Y);
                Beta = sum(Y .* hypTan);
                D = diag(1 ./ (Beta - a1 * sum(1 - hypTan .^ 2)));
                B = B + myy * B * (Y' * hypTan - diag(Beta)) * D;
            case 22
                Xsub=X(:, getSamples(numSamples, sampleSize));
                hypTan = tanh(a1 * Xsub' * B);
                B = Xsub * hypTan / size(Xsub, 2) - ...
                    ones(size(B,1),1) * sum(1 - hypTan .^ 2) .* B / size(Xsub, 2) * a1;
            case 23
                % Optimoitu
                Y = X(:, getSamples(numSamples, sampleSize))' * B;
                hypTan = tanh(a1 * Y);
                Beta = sum(Y .* hypTan);
                D = diag(1 ./ (Beta - a1 * sum(1 - hypTan .^ 2)));
                B = B + myy * B * (Y' * hypTan - diag(Beta)) * D;

                % gauss
            case 30
                U = X' * B;
                Usquared=U .^ 2;
                ex = exp(-a2 * Usquared / 2);
                gauss =  U .* ex;
                dGauss = (1 - a2 * Usquared) .*ex;
                B = X * gauss / numSamples - ...
                    ones(size(B,1),1) * sum(dGauss)...
                    .* B / numSamples ;
            case 31
                % optimoitu
                Y = X' * B;
                ex = exp(-a2 * (Y .^ 2) / 2);
                gauss = Y .* ex;
                Beta = sum(Y .* gauss);
                D = diag(1 ./ (Beta - sum((1 - a2 * (Y .^ 2)) .* ex)));
                B = B + myy * B * (Y' * gauss - diag(Beta)) * D;
            case 32
                Xsub=X(:, getSamples(numSamples, sampleSize));
                U = Xsub' * B;
                Usquared=U .^ 2;
                ex = exp(-a2 * Usquared / 2);
                gauss =  U .* ex;
                dGauss = (1 - a2 * Usquared) .*ex;
                B = Xsub * gauss / size(Xsub,2) - ...
                    ones(size(B,1),1) * sum(dGauss)...
                    .* B / size(Xsub,2) ;
            case 33
                % Optimoitu
                Y = X(:, getSamples(numSamples, sampleSize))' * B;
                ex = exp(-a2 * (Y .^ 2) / 2);
                gauss = Y .* ex;
                Beta = sum(Y .* gauss);
                D = diag(1 ./ (Beta - sum((1 - a2 * (Y .^ 2)) .* ex)));
                B = B + myy * B * (Y' * gauss - diag(Beta)) * D;

                % skew
            case 40
                B = (X * ((X' * B) .^ 2)) / numSamples;
            case 41
                % Optimoitu
                Y = X' * B;
                Gskew = Y .^ 2;
                Beta = sum(Y .* Gskew);
                D = diag(1 ./ (Beta));
                B = B + myy * B * (Y' * Gskew - diag(Beta)) * D;
            case 42
                Xsub=X(:, getSamples(numSamples, sampleSize));
                B = (Xsub * ((Xsub' * B) .^ 2)) / size(Xsub,2);
            case 43
                % Uusi optimoitu
                Y = X(:, getSamples(numSamples, sampleSize))' * B;
                Gskew = Y .^ 2;
                Beta = sum(Y .* Gskew);
                D = diag(1 ./ (Beta));
                B = B + myy * B * (Y' * Gskew - diag(Beta)) * D;

            otherwise
                error('Code for desired nonlinearity not found!');
        end
    end

    % Calculate ICA filters.
    W = B' * whiteningMatrix;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Also plot the last one...
    switch usedDisplay
        case 1
            % There was and may still be other displaymodes...
            % 1D signals
            icaplot('dispsig',(X'*B)');
            drawnow;
        case 2
            % ... and now there are :-)
            % 1D basis
            icaplot('dispsig',A');
            drawnow;
        case 3
            % ... and now there are :-)
            % 1D filters
            icaplot('dispsig',W);
            drawnow;
        otherwise
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DEFLATION APPROACH
if approachMode == 2

    B = zeros(vectorSize);

    % The search for a basis vector is repeated numOfIC times.
    round = 1;

    numFailures = 0;

    while round <= numOfIC,
        myy = myyOrig;
        usedNlinearity = gOrig;
        stroke = 0;
        notFine = 1;
        long = 0;
        endFinetuning = 0;

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Show the progress...
        if b_verbose, fprintf('IC %d ', round); end

        % Take a random initial vector of lenght 1 and orthogonalize it
        % with respect to the other vectors.
        if initialStateMode == 0
            w = randn (vectorSize, 1);
        elseif initialStateMode == 1
            w=whiteningMatrix*guess(:,round);
        end
        w = w - B * B' * w; %orthogonalization - B is the previous w
        w = w / norm(w);

        wOld = zeros(size(w));
        wOld2 = zeros(size(w));

        % This is the actual fixed-point iteration loop.
        %    for i = 1 : maxNumIterations + 1
        i = 1;
        gabba = 1;
        while i <= maxNumIterations + gabba
            if (usedDisplay > 0)
                drawnow;
            end
            if (interruptible & g_FastICA_interrupt)
                if b_verbose
                    fprintf('\n\nCalculation interrupted by the user\n');
                end
                return;
            end

            % Project the vector into the space orthogonal to the space
            % spanned by the earlier found basis vectors. Note that we can do
            % the projection with matrix B, since the zero entries do not
            % contribute to the projection.
            w = w - B * B' * w;
            w = w / norm(w);

            if notFine
                if i == maxNumIterations + 1
                    if b_verbose
                        fprintf('\nComponent number %d did not converge in %d iterations.\n', round, maxNumIterations);
                    end
                    round = round - 1;
                    numFailures = numFailures + 1;
                    if numFailures > failureLimit
                        if b_verbose
                            fprintf('Too many failures to converge (%d). Giving up.\n', numFailures);
                        end
                        if round == 0
                            A=[];
                            W=[];
                        end
                        return;
                    end
                    % numFailures > failurelimit
                    break;
                end
                % i == maxNumIterations + 1
            else
                % if notFine
                if i >= endFinetuning
                    wOld = w; % So the algorithm will stop on the next test...
                end
            end
            % if notFine

            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Show the progress...
            if b_verbose, fprintf('.'); end;

            % Test for termination condition. Note that the algorithm has
            % converged if the direction of w and wOld is the same, this
            % is why we test the two cases.
            if norm(w - wOld) < epsilon | norm(w + wOld) < epsilon
                if finetuningEnabled & notFine
                    if b_verbose, fprintf('Initial convergence, fine-tuning: '); end;
                    notFine = 0;
                    gabba = maxFinetune;
                    wOld = zeros(size(w));
                    wOld2 = zeros(size(w));
                    usedNlinearity = gFine;
                    myy = myyK * myyOrig;

                    endFinetuning = maxFinetune + i;

                else
                    numFailures = 0;
                    % Save the vector
                    B(:, round) = w;

                    % Calculate the de-whitened vector.
                    A(:,round) = dewhiteningMatrix * w;
                    % Calculate ICA filter.
                    W(round,:) = w' * whiteningMatrix;

                    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                    % Show the progress...
                    if b_verbose, fprintf('computed ( %d steps ) \n', i); end

                    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                    % Also plot the current state...
                    switch usedDisplay
                        case 1
                            if rem(round, displayInterval) == 0,
                                % There was and may still be other displaymodes...
                                % 1D signals
                                temp = X'*B;
                                icaplot('dispsig',temp(:,1:numOfIC)');
                                drawnow;
                            end
                        case 2
                            if rem(round, displayInterval) == 0,
                                % ... and now there are :-)
                                % 1D basis
                                icaplot('dispsig',A');
                                drawnow;
                            end
                        case 3
                            if rem(round, displayInterval) == 0,
                                % ... and now there are :-)
                                % 1D filters
                                icaplot('dispsig',W);
                                drawnow;
                            end
                    end
                    % switch usedDisplay
                    break; % IC ready - next...
                end
                %if finetuningEnabled & notFine
            elseif stabilizationEnabled
                if (~stroke) & (norm(w - wOld2) < epsilon | norm(w + wOld2) < ...
                        epsilon)
                    stroke = myy;
                    if b_verbose, fprintf('Stroke!'); end;
                    myy = .5*myy;
                    if mod(usedNlinearity,2) == 0
                        usedNlinearity = usedNlinearity + 1;
                    end
                elseif stroke
                    myy = stroke;
                    stroke = 0;
                    if (myy == 1) & (mod(usedNlinearity,2) ~= 0)
                        usedNlinearity = usedNlinearity - 1;
                    end
                elseif (notFine) & (~long) & (i > maxNumIterations / 2)
                    if b_verbose, fprintf('Taking long (reducing step size) '); end;
                    long = 1;
                    myy = .5*myy;
                    if mod(usedNlinearity,2) == 0
                        usedNlinearity = usedNlinearity + 1;
                    end
                end
            end

            wOld2 = wOld;
            wOld = w;

            switch usedNlinearity
                % pow3
                case 10
                    w = (X * ((X' * w) .^ 3)) / numSamples - 3 * w;
                case 11
                    EXGpow3 = (X * ((X' * w) .^ 3)) / numSamples;
                    Beta = w' * EXGpow3;
                    w = w - myy * (EXGpow3 - Beta * w) / (3 - Beta);
                case 12
                    Xsub=X(:,getSamples(numSamples, sampleSize));
                    w = (Xsub * ((Xsub' * w) .^ 3)) / size(Xsub, 2) - 3 * w;
                case 13
                    Xsub=X(:,getSamples(numSamples, sampleSize));
                    EXGpow3 = (Xsub * ((Xsub' * w) .^ 3)) / size(Xsub, 2);
                    Beta = w' * EXGpow3;
                    w = w - myy * (EXGpow3 - Beta * w) / (3 - Beta);
                    % tanh
                case 20
                    hypTan = tanh(a1 * X' * w);
                    w = (X * hypTan - a1 * sum(1 - hypTan .^ 2)' * w) / numSamples;
                case 21
                    hypTan = tanh(a1 * X' * w);
                    Beta = w' * X * hypTan;
                    w = w - myy * ((X * hypTan - Beta * w) / ...
                        (a1 * sum((1-hypTan .^2)') - Beta));
                case 22
                    Xsub=X(:,getSamples(numSamples, sampleSize));
                    hypTan = tanh(a1 * Xsub' * w);
                    w = (Xsub * hypTan - a1 * sum(1 - hypTan .^ 2)' * w) / size(Xsub, 2);
                case 23
                    Xsub=X(:,getSamples(numSamples, sampleSize));
                    hypTan = tanh(a1 * Xsub' * w);
                    Beta = w' * Xsub * hypTan;
                    w = w - myy * ((Xsub * hypTan - Beta * w) / ...
                        (a1 * sum((1-hypTan .^2)') - Beta));
                    % gauss
                case 30
                    % This has been split for performance reasons.
                    u = X' * w;
                    u2=u.^2;
                    ex=exp(-a2 * u2/2);
                    gauss =  u.*ex;
                    dGauss = (1 - a2 * u2) .*ex;
                    w = (X * gauss - sum(dGauss)' * w) / numSamples;
                case 31
                    u = X' * w;
                    u2=u.^2;
                    ex=exp(-a2 * u2/2);
                    gauss =  u.*ex;
                    dGauss = (1 - a2 * u2) .*ex;
                    Beta = w' * X * gauss;
                    w = w - myy * ((X * gauss - Beta * w) / ...
                        (sum(dGauss)' - Beta));
                case 32
                    Xsub=X(:,getSamples(numSamples, sampleSize));
                    u = Xsub' * w;
                    u2=u.^2;
                    ex=exp(-a2 * u2/2);
                    gauss =  u.*ex;
                    dGauss = (1 - a2 * u2) .*ex;
                    w = (Xsub * gauss - sum(dGauss)' * w) / size(Xsub, 2);
                case 33
                    Xsub=X(:,getSamples(numSamples, sampleSize));
                    u = Xsub' * w;
                    u2=u.^2;
                    ex=exp(-a2 * u2/2);
                    gauss =  u.*ex;
                    dGauss = (1 - a2 * u2) .*ex;
                    Beta = w' * Xsub * gauss;
                    w = w - myy * ((Xsub * gauss - Beta * w) / ...
                        (sum(dGauss)' - Beta));
                    % skew
                case 40
                    w = (X * ((X' * w) .^ 2)) / numSamples;
                case 41
                    EXGskew = (X * ((X' * w) .^ 2)) / numSamples;
                    Beta = w' * EXGskew;
                    w = w - myy * (EXGskew - Beta*w)/(-Beta);
                case 42
                    Xsub=X(:,getSamples(numSamples, sampleSize));
                    w = (Xsub * ((Xsub' * w) .^ 2)) / size(Xsub, 2);
                case 43
                    Xsub=X(:,getSamples(numSamples, sampleSize));
                    EXGskew = (Xsub * ((Xsub' * w) .^ 2)) / size(Xsub, 2);
                    Beta = w' * EXGskew;
                    w = w - myy * (EXGskew - Beta*w)/(-Beta);

                otherwise
                    error('Code for desired nonlinearity not found!');
            end

% TEST % % % % % % % % % % % % % % % % % % % % % % % % %
interval = 1;
if mod(i,interval)==0
    modifyA =1; %for now...
    nx = 20;
    ny = 20;
    sigfix = 3.7/4; % sig of PSF...

    if exist ('modifyA','var')
        if modifyA == 1
            %                     fprintf ('Modifying matrix conevrgence of w...')
            Atemp = dewhiteningMatrix * w;
%             signum = 1;
%             if abs(min(Atemp))>max(Atemp) %or sum(Atemp)<0...?
%                 signum = -1;
%             end
%             if abs(min(Atemp))>max(Atemp) %or sum(Atemp)<0...?
%                 Atemp = -Atemp;
%             end
            absA = abs(Atemp);
            maxA = max(absA);

            gf = (gauss2d([nx ny],[nx/2,ny/2],sigfix,maxA));
            Aconv = (conv2(gf, reshape (Atemp, nx, ny),'same')).^5;
            Atemp = reshape(Aconv,ny*ny,1);

%             [x_mu, y_mu, sig] = fitgauss2d(reshape(absA, nx, ny),sigfix); %fit to abs...
%             Atemp = (Atemp + signum * reshape(gauss2d([nx ny],[x_mu,y_mu],sig,maxA),nx*ny,1))/2;
            % Atemp = reshape(gauss2d([nx ny],[x_mu,y_mu],sig,maxA),nx*ny,1);
        end
    end
    w = whiteningMatrix*Atemp;
end

% TEST % % % % % % % % % % % % % % % % % % % % % % % % %

% %%% TEST version old %%%%%%%%%
% if mod(i,5)==0
%     modifyA =1; %for now...
%     nx = 20;
%     ny = 20;
%     sigfix = 3.7/4; % sig of PSF...
%
%     if exist ('modifyA','var')
%         if modifyA == 1
%             %                     fprintf ('Modifying matrix conevrgence of w...')
%             Atemp = dewhiteningMatrix * w;
%             if abs(min(Atemp))>max(Atemp) %or sum(Atemp)<0...?
%                 Atemp = -Atemp;
%             end
%             [x_mu, y_mu, sig] = fitgauss2d(reshape(Atemp, nx, ny),sigfix);
%             maxA = max(abs(Atemp));
%             Atemp = (Atemp + reshape(gauss2d([nx ny],[x_mu,y_mu],sig,maxA),nx*ny,1))/2;
%             % Atemp = reshape(gauss2d([nx ny],[x_mu,y_mu],sig,maxA),nx*ny,1);
%         end
%     end
%     w = whiteningMatrix*Atemp;
%
% %%% TEST version old %%%%%%%%%

% Normalize the new w.
            w = w / norm(w);
            i = i + 1;
        end
        round = round + 1;
    end
    if b_verbose, fprintf('Done.\n'); end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Also plot the ones that may not have been plotted.
    if (usedDisplay > 0) & (rem(round-1, displayInterval) ~= 0)
        switch usedDisplay
            case 1
                % There was and may still be other displaymodes...
                % 1D signals
                temp = X'*B;
                icaplot('dispsig',temp(:,1:numOfIC)');
                drawnow;
            case 2
                % ... and now there are :-)
                % 1D basis
                icaplot('dispsig',A');
                drawnow;
            case 3
                % ... and now there are :-)
                % 1D filters
                icaplot('dispsig',W);
                drawnow;
            otherwise
        end
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% In the end let's check the data for some security
if ~isreal(A)
    if b_verbose, fprintf('Warning: removing the imaginary part from the result.\n'); end
    A = real(A);
    W = real(W);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Subfunction
% Calculates tanh simplier and faster than Matlab tanh.
function y=tanh(x)
y = 1 - 2 ./ (exp(2 * x) + 1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Samples = getSamples(max, percentage)
Samples = find(rand(1, max) < percentage);


function [UTC1,UTC2]=UT12UTC(UT11,UT12,deltaUTCUT1)
%%UT12UTC Convert from UT1, which is a nonuniform timescale based on the
%         rotation rate of the Earth, to Coordinated Universal Time (UTC),
%         which is the "standard" time that pretty much every country uses
%         (the time zone is that of the United Kingdom) including
%         leapseconds. Due to the use of leapseconds, UT1 is always within
%         0.9 seconds of UTC.
%
%INPUTS: Jul1,Jul2 Two parts of a Julian date given in UTC. The units of
%                  the date are days. The full date is the sum of both
%                  terms. The date is broken into two parts to provide
%                  more bits of precision. It does not matter how the date
%                  is split.
%      deltaUTCUT1 An optional parameter specifying the offset between UTC
%                  and UT1 in seconds. If this parameter is omitted, then
%                  the value of the function getEOP will be used and
%                  iterations will be performed, because the correct
%                  tabulated value is given in UTC, not UT1.
%
%OUTPUTS: Jul1,Jul2 Two parts of a pseudo-Julian date in UTC with units of
%                   Julian days.
%
%This essentially just adds deltaUTCUT1 from the given UTC date, or
%looks up deltaUTCUT1 using the getEOP function and then adds
%deltaUTCUT1. The "looking up" step involves an interation, since the value
%of deltaUTCUT1 is tabulated by UTC, which is unknown. To know the peroper
%amount to subtract from the pseudo-Julian day, the function leapSecsOnDay
%is used to determine whether the day contains a leapsecond. As that
%function is also tabulated by UTC, an iteration is necessary even if
%deltaUTCUT1 is given. Unlike the function iauUtcut1 in the International
%Astronomical Union's Standards of Fundamental Astronomy library, this does
%not try to deal with deltaUTCUT1 being off by a second near a leapsecond
%as it was noticed that the "corrections" made in the library make the pair
%of UTC2UT1 and UT12UTC inconsistent by a second when it is near a
%leapsecond.
%
%Many temporal coordinate systems standards are compared in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "An Overview of Major Terrestrial, Celestial, and
%    Temporal Coordinate Systems for Target Tracking," Formal Report, Naval
%    Research Laboratory, no. NRL/FR/5344--16-10,279, 10 Aug. 2016, 173
%    pages.
%
%April 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%The initial estimate of UTC is just UT1. This will be within 0.9 seconds
%of the correct value.
UTC1=UT11;
UTC2=UT12;

for curIter=1:2
    if(nargin<3)
        [~,~,deltaUTCUT1]=getEOP(UTC1,UTC2);
    end

    %Find the number of seconds in the day.
    [year,month,day]=UTC2Cal(UTC1,UTC2,true);
    numSecInDay=24*60*(60+leapSecsOnDay(year,month,day));

    %Add preserving precision.
    if(UTC1<UTC2)
        UTC2=UT11+deltaUTCUT1/numSecInDay;
        UTC1=UT12;
    else
        UTC2=UT12+deltaUTCUT1/numSecInDay;
        UTC1=UT11;
    end

    %Put the fractional part into UTC2 and the integer part into UTC1.
    frac1=UTC1-fix(UTC1);
    frac2=UTC2-fix(UTC2);
    sumVal=frac1+frac2;
    sumInt=fix(sumVal);
    sumFrac=sumVal-sumInt;
    UTC1=fix(UTC2)+fix(UTC1)+sumInt;
    UTC2=sumFrac;
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [altitude,gasTable,t,d]=NRLMSISE00Alt4Pres(dayOfYear,secondOfTheDay,pressure,latLon,AP,F107,F107A)
%%NRLMSISE00ALT4PRES Given the pressure at a particular latitude and
%                   longitude and time, obtain the approximate ellipsoidal
%                   height using the NRLMSISE-00 atmospheric model. This
%                   function also returns a standard temperature and the
%                   constitutents of the atmosphere, assuming dry air. The
%                   pressure returned is consistent with the function
%                   NRLMSISE00GasTemp, but will not be perfectly
%                   consistent with the function standardAtmosParam.
%
%INPUTS: dayOfYear The integer day of the year in the Gregorian calendar
%                  in universal coordinated time (UTC). Counting starts at
%                  1. the resolution of the model is not sufficient for it
%                  to matter whether 365 or 366 is given at the day if it
%                  isn't/is a leap year.
%   secondOfTheDay The second of the day. This starts at zero. The
%                  resolution of the model is not high enough for leap
%                  seconds to matter, so values above 86400.0 are just
%                  clipped to 86400.0.
%         pressure The measured atmospheirc pressure in Pascals.
%           latLon WGS-84 latitude and longitude of the point where the
%                  pressure is measured.
%               Ap An optional parameter specifying the average daily
%                  geomagnetic index. If omitted, the value 4.0, which is
%                  suitable for models below 90km is used. The index at a
%                  particular time can be obtained from a service of the
%                  International Service of Geomagnetic Indices at
%                  http://www-app3.gfz-potsdam.de/kp_index/
%                  A scalar value of AP indicates standard mode. If a 7X1
%                  vector is passed, then it is assumed that storm mode is
%                  desired. In storm mode, Ap(1) is the daily Ap index.
%                  Ap(2) through Ap(5) are 3 hours Ap indices respectively
%                  for the current time and 3, 6, and 9 hours before the
%                  current time. Ap(6) is the average of 8 3-hour Ap
%                  indices from 12 to 33 hours prior to the current time
%                  and Ap(7) is the average of 8 3-hour Ap indices from 36
%                  to 59 hours prior to the current time.
%             F107 An optional parameter specifying the 10.7cm solar radio
%                  flux for the previous day. If omitted, a value of 150
%                  is used, which should be suitable for models below
%                  90km. Values of the index can be obtained from
%                  http://www.swpc.noaa.gov/ftpdir/indices/quar_DSD.txt
%            F107A An optional parameter specifying the 81 day average of
%                  the 10.7cm radio solar flux. If omitted, a value of 150
%                  is used, which should be suitable for models below 90km.
%
%OUTPUTS: altitude The approximate ellipsoidal altitude associated with
%                  the pressure given on the input in meters.
%         gasTable A cell array of constituent elements of the atmosphere
%                  and their number densities. gasTable{i,1} is a string
%                  listing the name of the ith constituent element of the
%                  atmosphere. This can take the values
%                  'He' Helium
%                  'O'  Elemental Oxygen
%                  'N2' Nitrogen
%                  'O2' Oxygen
%                  'Ar' Argon
%                  'H'  Elemental Hydrogen
%                  'N'  Elemental Nitrogen
%                  'O*' Anomalous Oxygen
%                  gasTable{i,2} is the corresponding number density of
%                  the ith constituent element in units of atoms per cubic
%                  meter.
%                t Temperature variables. t(1) is the exospheric
%                  temperature and t(2) is the temperature at altitude.
%                  The temperature is in units of Kelvin.
%                d A 9X1 vector containing the same information as
%                  gasTable but without labels. The entries in d are
%                  d(1) - HE NUMBER DENSITY
%                  d(2) - O NUMBER DENSITY
%                  d(3) - N2 NUMBER DENSITY
%                  d(4) - O2 NUMBER DENSITY
%                  d(5) - AR NUMBER DENSITY
%                  d(6) - TOTAL MASS DENSITY without d(9)
%                  d(7) - H NUMBER DENSITY
%                  d(8) - N NUMBER DENSITY
%                  d(9) - Anomalous oxygen NUMBER DENSITY(M-3)
%                  where all number densities are in units of atoms per
%                  cubic meter and the units of d(6) are kilograms per
%                  cubic meter. Note that d(6) is computed using a
%                  definition of the atomic mass unit and definitions of
%                  the atomic masses that are not necessarily the most up
%                  to date.
%
%This function is essentially a wrapper for the C-code implementation of
%the  NRLMSISE-00 atmospheric model using appropriate functions to put the
%time and location in the correct format. The model is described in [1].
%
%The algorithm can be compiled for use in Matlab  using the
%CompileCLibraries function.
%
%The algorithm is run in Matlab using the command format
%[altitude,gasTable,t,d]=NRLMSISE00Alt4Pres(dayOfYear,secondOfTheDay,pressure,latLon);
%or
%[altitude,gasTable,t,d]=NRLMSISE00Alt4Pres(dayOfYear,secondOfTheDay,pressure,latLon,AP);
%or
%[altitude,gasTable,t,d]=NRLMSISE00Alt4Pres(dayOfYear,secondOfTheDay,pressure,latLon,AP,F107);
%or
%[altitude,gasTable,t,d]=NRLMSISE00Alt4Pres(dayOfYear,secondOfTheDay,pressure,latLon,AP,F107,F107A);
%
%EXAMPLE:
% dayOfYear=172;
% secondOfTheDay=29000;
% latLon=[60;-70]*(pi/180);
% F107=150;
% F107A=150;
% AP=4;
% pressure=1;%1 Pascal pressure.
% [altitude,gasTable,t,d]=NRLMSISE00Alt4Pres(dayOfYear,secondOfTheDay,pressure,latLon,AP,F107,F107A)
% %One can verify that the altitude is consistent with the standard model:
% latLonAlt=[latLon;altitude];
% [gasTable1,t1,d1]=NRLMSISE00GasTemp(dayOfYear,secondOfTheDay,latLonAlt,AP,F107,F107A)
% %One will see that t1 and d1 match t and d.
%
%REFERENCES:
%[1] J. M. Picone, A. E. Hedin, D. P. Drob, and A. C. Aikin, "NRLMSISE-00
%    empirical model of the atmosphere: Statistical comparisons and
%    scientific issues," Journal of Geophysical Research: Space Physics,
%    vol. 107, no. A12, Dec. 2002.
%
%June 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

error('This function is only implemented as a mexed C or C++ function. Please run CompileCLibraries.m to compile the function for use.')

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


%kmeigen 'Compute eigenvectors and eigenvalues of a square matrix '
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros meigen.pane file
%
% Parameters:
% InputFile: i 'Input Matrix', required: 'Input Matrix'
% OutputFile: o1 'Eigenvectors', optional: 'Eigenvector output matrix'
% OutputFile: o2 'Eigenvalues ', optional: 'Eigenvalue output matrix'
%
% Example: [o1, o2] = kmeigen(i, {'i','';'o1','';'o2',''})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% meigen - Compute Eigenvectors and Eigenvalues of a Square Matrix
%
%  DESCRIPTION
% .I meigen
% computes the eigenvalues and right eigenvectors of a square matrix.
% The input matrix can be nonsymmetric.
% The input matrix is caste to KDCOMPLEX for reading, and all output
% matrices are of type KDCOMPLEX.
%
% The eigenvectors are stored as columns of the eigenvector output matrix.
% The eigenvectors are all normalized to have unit 2-norm and a purely real
% largest component.
%
% The eigenvalues are stored as a diagonal matrix with the i'th diagonal
% being the eigenvalue corresponding the the eigenvector in column i of the
% eigenvector output matrix.
%
% The eigenvalues (and thier associated eigenvectors) appear in decreasing
% order. They are explicitly sorted (while keeping track of the associated
% eigenvectors).
%
% "meigen\fR uses the LAPACK routine ZGEEV to perform the calculations.
%
%
%
%  EXAMPLES
%
%  "SEE ALSO"
% MATRIX:zlapack:zgeev
%
%  RESTRICTIONS
% It has been observed that
% LAPACK generally gets the eigenvalues sorted in decending order, but
% occasionally will return with some of the smaller eigenvalues out of
% order. This is not a bug in "meigen\fR, although a sort may be inserted
% at some future date to make sure that the eigenvalues and thier associated
% eigenvectors are sorted. The 1992 version of the LAPACK USERS' GUIDE doesn't
% specify an ordering for the eigenvalues.
%
%  REFERENCES
% LAPACK Users' Guide, E. Anderson et. al., SIAM 1992. ISBN 0-89871-294-7
%
%  COPYRIGHT
% Copyright (C) 1993 - 1997, Khoral Research, Inc. ("KRI")  All rights reserved.
%

function varargout = kmeigen(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,..] = kmeigen(Inputs,arglist).');
end
if size(arglist,2)~=2
  error('arglist must be of form {''ParameterTag1'',value1;''ParameterTag2'',value2}')
 end
narglist={'i', '__input';'o1', '__output';'o2', '__output'};
maxval={0,1,1};
minval={0,1,1};
istoggle=[0,1,1];
was_set=istoggle * 0;
paramtype={'InputFile','OutputFile','OutputFile'};
% identify the input arrays and assign them to the arguments as stated by the user
if ~iscell(Inputs)
Inputs = {Inputs};
end
NumReqOutputs=0; 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 'meigen"  '],Inputs,narglist);


function [text,AIC,BIC]=igarch_display(parameters,ll,vcv,data,p,q,errorType,igarchType,constant)
% Display parameters, tstats, pvals, log-likelihood and AIC/BIC
% from estimates of a IGARCH(P,O,Q) produced using igarch
%
% USAGE:
%   [TEXT] = igarch_display(PARAMETERS,LL,VCV,DATA,P,Q)
%   [TEXT,AIC,BIC] = igarch_display(PARAMETERS,LL,VCV,DATA,P,Q,ERRORTYPE,IGARCHTYE,CONSTANT)
%
% INPUTS:
%   PARAMETERS    - A CONSTANT+p+q-1 column vector of parameters with
%                   [omega alpha(1) ... alpha(p) beta(1) ... beta(q-1) [nu lambda]]'.
%   LL            - The log likelihood at the optimum
%   VCV           - Non-robust standard errors (inverse Hessian)
%   DATA          - A column of mean zero data
%   P             - Positive, scalar integer representing the number of
%                   symmetric innovations
%   Q             - Non-negative, scalar integer representing the number
%                   of lags of conditional variance (0 for ARCH)
%   ERRORTYPE     - [OPTIONAL] The error distribution used, valid types are:
%                     'NORMAL'    - Gaussian Innovations [DEFAULT]
%                     'STUDENTST' - T distributed errors
%                     'GED'       - Generalized Error Distribution
%                     'SKEWT'     - Skewed T distribution
%   IGARCHTYPE     - [OPTIONAL] The type of variance process, either
%                     1 - Model evolves in absolute values
%                     2 - Model evolves in squares [DEFAULT]
%   CONSTANT      - [OPTIONAL] Logical value indicating whether model
%                     should include a constant.  Default is true
%                     (include)
%
% OUTPUTS:
%   TEXT          - Character matrix with the formatted parameters of the model
%   AIC           - Aikake Information Criteria computed from the LL
%   BIC           - Schwartz/Bayesian Information Criteria computed from the LL
%
%  See also IGARCH

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 9/1/2005

%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETER CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 6
        igarchType=2;
        errorType='NORMAL';
        constant = 1;
    case 7
        igarchType=2;
        constant = 1;
    case 8
        constant = 1;
    case 9
        % Nothing
    otherwise
        error('5 to 8 inputs required')
end
if isempty(igarchType)
    igarchType=2;
end
if isempty(errorType)
    errorType='NORMAL';
end
if isempty(constant)
    constant = 1;
end
% parameters, N by 1, real
if any(~isreal(parameters)) || size(parameters,2)~=1
    error('PARAMETERS must be a column vector.')
end
% LL
if ~isscalar(ll) || ~isreal(ll)
    error('LL must be a scalar.')
end
% VCV
if size(vcv,2)~=size(vcv,1) || any(min(eig(vcv))<=0) || size(vcv,1)~=length(parameters)
    error('VCV must be a square positive definite matrix compatible with PARAMETERS.')
end
% data
if any(~isreal(data)) || size(data,2)~=1
    error('DATA must be a T by 1 column vector.')
end
% p
if ~isscalar(p) || p<1 || floor(p)~=p
    error('P must be a postitive scalar integer')
end
% q
if ~isscalar(q) || q<0 || floor(q)~=q
    error('Q must be a positive scalar integer')
end
% errorType
if ~ischar(errorType)
    errorType=[];
end
switch errorType
    case 'NORMAL'
        errorType=1;
        extraP=0;
    case 'STUDENTST'
        errorType=2;
        extraP=1;
    case 'GED'
        errorType=3;
        extraP=1;
    case 'SKEWT'
        errorType=4;
        extraP=2;
    otherwise
        error('ERRORTYPE is not one of the supported distributions')
end
% igarchType
if ~ismember(igarchType,[1 2])
    error('IGARCHTYPE must be either 1 or 2')
end
if length(parameters)~=(constant+p+q-1+extraP)
    error('Size of PARAMETERS is not compatible with input P, O, Q')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETER CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%

if igarchType==1
    model_name = ['IAVARCH(' num2str(p) ',' num2str(q) ')'];
else
    model_name = ['IGARCH(' num2str(p) ',' num2str(q) ')'];
end

parameters_text=num2str(parameters,'%4.4f');
stderr=sqrt(diag(vcv));
stderr_text=num2str(stderr,'%4.4f');
tstats=parameters./stderr;
tstats_text=num2str(tstats,'%4.4f');
pvals=2-2*normcdf(abs(tstats));
pvals_text=num2str(pvals,'%4.4f');

% Add in the extra beta
finalBetaPos = constant+p+q;
archParameters = parameters(constant+1:constant+p+q-1);
finalBeta = 1 - sum(archParameters);

n = length(parameters);

parameters_text=[parameters_text(1:finalBetaPos-1,:);
    num2str(finalBeta,'%4.4f');
    parameters_text(finalBetaPos+1:n,:)];
stderr_text=[stderr_text(1:finalBetaPos-1,:);
    repmat('-',1,size(stderr_text,2));
    stderr_text(finalBetaPos+1:n,:)];
tstats_text=[tstats_text(1:finalBetaPos-1,:);
    repmat('-',1,size(tstats_text,2));
    tstats_text(finalBetaPos+1:n,:)];
pvals_text=[pvals_text(1:finalBetaPos-1,:);
    repmat('-',1,size(pvals_text,2));
    pvals_text(finalBetaPos+1:n,:)];

T=length(data);
AIC = -ll/T+2*length(parameters)/T;
BIC = -ll/T+log(T)*length(parameters)/T;

% Format the output
text=[];
text{1,1}= ' ';
text{2,1}= ' ';
text{3,1}=repmat('-',1,50);
text{4,1}=model_name;
text{5,1}=repmat('-',1,50);
text{6,1}=' ';
text{7,1}=['Loglikelihood: ' sprintf('%1.2f',ll)];
text{8,1}=['AIC: ' sprintf('%1.4f',AIC)];
text{9,1}=['BIC: ' sprintf('%1.4f',BIC)];
text{10,1}=' ';

for i=1:size(text,1);
    disp(text{i,1})
end

% Format the parameter table, need to right align everything
K=size(parameters_text,1);
for i=1:K
    N=size(parameters_text,2);
    if any(parameters_text(i,:)==' ')
        numSpace=sum(parameters_text(i,:)==' ');
        parameters_text(i,:)=[repmat(' ',1,numSpace) parameters_text(i,1:N-numSpace)];
    end
    N=size(stderr_text,2);
    if any(stderr_text(i,:)==' ')
        numSpace=sum(stderr_text(i,:)==' ');
        stderr_text(i,:)=[repmat(' ',1,numSpace) stderr_text(i,1:N-numSpace)];
    end
    N=size(tstats_text,2);
    if any(tstats_text(i,:)==' ')
        numSpace=sum(tstats_text(i,:)==' ');
        tstats_text(i,:)=[repmat(' ',1,numSpace) tstats_text(i,1:N-numSpace)];
    end
    N=size(pvals_text,2);
    if any(pvals_text(i,:)==' ')
        numSpace=sum(pvals_text(i,:)==' ');
        pvals_text(i,:)=[repmat(' ',1,numSpace) pvals_text(i,1:N-numSpace)];
    end
end
% Append column labels
labels={' Parameters','   Std. Err.','     T-stat','      P-val'};
cols = {parameters_text,stderr_text,tstats_text,pvals_text};
for i=1:length(labels);
    text1=labels{i};
    text2=cols{i};
    maxcols=max(size(text1,2),size(text2,2));
    if size(text1,2)<maxcols
        labels{i} = [repmat(' ',1,maxcols-size(text1,2)) text1 ];
    end
    if size(text2,2)<maxcols
        cols{i} = [repmat(' ',size(text2,1),maxcols-size(text2,2)) text2 ];
    end
end

% Finally variable names
variable_names=cell(length(parameters),1);
variable_names{1}='omega';
index=2;
for i=1:p
    variable_names{index}=['alpha(' num2str(i) ')'];
    index=index+1;
end
for i=1:q
    variable_names{index}=['beta(' num2str(i) ')'];
    index=index+1;
end
if errorType>1
    variable_names{index}='nu';
    index=index+1;
end
if errorType==4
    variable_names{index}='lambda';
end

maxVarNameLength=max(cellfun('length',variable_names));
for i=1:length(variable_names)
    variable_names{i} = [repmat(' ',1,maxVarNameLength-length(variable_names{i})) variable_names{i}];
end
variable_names=cell2mat(variable_names(:));

% Output the parameters
outmat=strvcat(' ',variable_names); %#ok<*VCAT>
for i=1:4
    outmat=[outmat repmat('  ',size(outmat,1),1) strvcat(labels{i},cols{i})]; %#ok<AGROW>
end

text2=[];
for i=1:length(text)
    text2=strvcat(text2,text{i});
end
text=strvcat(text2,outmat);

disp(outmat)


function mpc = t_case9_opfv2
%T_CASE9_OPFV2   Power flow data for 9 bus, 3 generator case, with OPF data.
%   Please see CASEFORMAT for details on the case file format.

%   MATPOWER

%% MATPOWER Case Format : Version 2
mpc.version = '2';

%%-----  Power Flow Data  -----%%
%% system MVA base
mpc.baseMVA = 100;

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
mpc.bus = [
	1	3	0	0	0	0	1	1	0	345	1	1.1	0.9;
	2	2	0	0	0	0	1	1	0	345	1	1.1	0.9;
	30	2	0	0	0	0	1	1	0	345	1	1.1	0.9;
	4	1	0	0	0	0	1	1	0	345	1	1.1	0.9;
	5	1	90	30	0	0	1	1	0	345	1	1.1	0.9;
	6	1	0	0	0	0	1	1	0	345	1	1.1	0.9;
	7	1	100	35	0	0	1	1	0	345	1	1.1	0.9;
	8	1	0	0	0	0	1	1	0	345	1	1.1	0.9;
	9	1	125	50	0	0	1	1	0	345	1	1.1	0.9;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin	Pc1	Pc2	Qc1min	Qc1max	Qc2min	Qc2max	ramp_agc	ramp_10	ramp_30	ramp_q	apf
mpc.gen = [
	1	0	0	300	-300	1	100	1	250	90	0	0	0	0	0	0	0	0	0	0	0;
	30	85	0	300	-300	1	100	1	270	10	0	200	-30	30	-15	15	0	0	0	0	0;
	2	163	0	300	-300	1	100	1	300	10	0	200	-20	20	-10	10	0	0	0	0	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status	angmin	angmax
mpc.branch = [
	1	4	0	0.0576	0	0	250	250	0	0	1	-360	2.48;
	4	5	0.017	0.092	0.158	0	250	250	0	0	1	-360	360;
	5	6	0.039	0.17	0.358	150	150	150	0	0	1	-360	360;
	30	6	0	0.0586	0	0	300	300	0	0	1	-360	360;
	6	7	0.0119	0.1008	0.209	40	150	150	0	0	1	-360	360;
	7	8	0.0085	0.072	0.149	250	250	250	0	0	1	-360	360;
	8	2	0	0.0625	0	250	250	250	0	0	1	-360	360;
	8	9	0.032	0.161	0.306	250	250	250	0	0	1	-360	360;
	9	4	0.01	0.085	0.176	250	250	250	0	0	1	-2	360;
];

%%-----  OPF Data  -----%%
%% generator cost data
%	1	startup	shutdown	n	x1	y1	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
mpc.gencost = [
	1	0	0	4	0	0	100	2500	200	5500	250	7250;
	1	0	0	3	0	0	200	3000	300	5000	0	0;
	2	0	0	2	24.035	-403.5	0	0	0	0	0	0;
];


function [dStates, contactForces] = dynamics_doubleStance(States, Actuators, Parameters)
% function [dStates, Actuators] = dynamics_doubleStance(States, Actuators, Parameters)
%
% Computer Generated File -- DO NOT EDIT
%
% This function was created by the function Write_ContinuousDynamics()
% 10-Dec-2013 19:40:47
%
% Dymanics Model: retractable double pendulum biped
% Motion Phase: Double Stance
%
% Matthew Kelly
% Cornell University
%

x = States(1,:); % (m) Foot One horizontal position
y = States(2,:); % (m) Foot One vertical position
th1 = States(3,:); % (rad) Leg One absolute angle
th2 = States(4,:); % (rad) Leg Two absolute angle
L1 = States(5,:); % (m) Leg One length
L2 = States(6,:); % (m) Leg Two length
dx = States(7,:); % (m/s) Foot One horizontal velocity
dy = States(8,:); % (m/s) Foot One vertical velocity
dth1 = States(9,:); % (rad/s) Leg One absolute angular rate
dth2 = States(10,:); % (rad/s) Leg Two absolute angular rate
dL1 = States(11,:); % (m/s) Leg One extension rate
dL2 = States(12,:); % (m/s) Leg Two extensioin rate

F1 = Actuators(1,:); % (N) Compresive axial force in Leg One
F2 = Actuators(2,:); % (N) Compresive axial force in Leg Two
T1 = Actuators(3,:); % (Nm) External torque applied to Leg One
T2 = Actuators(4,:); % (Nm) External torque applied to Leg Two
Thip = Actuators(5,:); % (Nm) Torque acting on Leg Two from Leg One

m1 = Parameters.m1; % (kg) Foot One mass
m2 = Parameters.m2; % (kg) Foot Two mass
M = Parameters.M; % (kg) Hip mass
g = Parameters.g; % (m/s^2) Gravity

% Constraints for this phase:
ddx = 0; %(m) Foot One horizontal position
ddy = 0; %(m) Foot One vertical position

dStates = zeros(size(States));
dStates(1:6,:) = States((1+6):(6+6),:);
dStates(7,:) = zeros(1,size(States,2));
dStates(8,:) = zeros(1,size(States,2));
dStates(9,:) = -(L2.*Thip - L2.*T1 + L1.*T2.*cos(th1 - th2) + L1.*Thip.*cos(th1 - th2) - F2.*L1.*L2.*sin(th1 - th2) + 2.*L1.*L2.*M.*dL1.*dth1 + L1.*L2.*M.*ddy.*cos(th1) + L1.*L2.*M.*g.*cos(th1) - L1.*L2.*M.*ddx.*sin(th1))./(L1.^2.*L2.*M);
dStates(10,:) = (L2.*Thip.*cos(th1).*cos(th2) - L2.*T1.*cos(th1).*cos(th2) - L2.*T1.*sin(th1).*sin(th2) + L2.*Thip.*sin(th1).*sin(th2) - L1.*L2.*M.*ddy.*cos(th2) + L1.*L2.*M.*ddx.*sin(th2) + L1.*T2.*cos(th1 - th2).*cos(th1).*cos(th2) + L1.*Thip.*cos(th1 - th2).*cos(th1).*cos(th2) + L1.*T2.*cos(th1 - th2).*sin(th1).*sin(th2) - L1.*T2.*sin(th1 - th2).*cos(th1).*sin(th2) + L1.*T2.*sin(th1 - th2).*cos(th2).*sin(th1) + L1.*Thip.*cos(th1 - th2).*sin(th1).*sin(th2) - L1.*Thip.*sin(th1 - th2).*cos(th1).*sin(th2) + L1.*Thip.*sin(th1 - th2).*cos(th2).*sin(th1) + F1.*L1.*L2.*cos(th1).*sin(th2) - F1.*L1.*L2.*cos(th2).*sin(th1) - F2.*L1.*L2.*cos(th1 - th2).*cos(th1).*sin(th2) + F2.*L1.*L2.*cos(th1 - th2).*cos(th2).*sin(th1) - F2.*L1.*L2.*sin(th1 - th2).*cos(th1).*cos(th2) - F2.*L1.*L2.*sin(th1 - th2).*sin(th1).*sin(th2) - 2.*L1.*L2.*M.*dL2.*dth2.*cos(th2).^2 - 2.*L1.*L2.*M.*dL2.*dth2.*sin(th2).^2 + L1.*L2.*M.*ddy.*cos(th1).^2.*cos(th2) + L1.*L2.*M.*g.*cos(th1).^2.*cos(th2) - L1.*L2.*M.*ddx.*cos(th1).^2.*sin(th2) + L1.*L2.*M.*ddy.*cos(th2).*sin(th1).^2 + L1.*L2.*M.*g.*cos(th2).*sin(th1).^2 - L1.*L2.*M.*ddx.*sin(th1).^2.*sin(th2))./(L1.*L2.^2.*M.*(cos(th2).^2 + sin(th2).^2));
dStates(11,:) = -(T2.*sin(th1 - th2) + Thip.*sin(th1 - th2) - F1.*L2 + F2.*L2.*cos(th1 - th2) - L1.*L2.*M.*dth1.^2 + L2.*M.*ddx.*cos(th1) + L2.*M.*ddy.*sin(th1) + L2.*M.*g.*sin(th1))./(L2.*M);
dStates(12,:) = (L2.*T1.*cos(th2).*sin(th1) - L2.*T1.*cos(th1).*sin(th2) + L2.*Thip.*cos(th1).*sin(th2) - L2.*Thip.*cos(th2).*sin(th1) - F1.*L1.*L2.*sin(th1).*sin(th2) - L1.*L2.*M.*ddx.*cos(th2) - L1.*L2.*M.*ddy.*sin(th2) + L1.*L2.^2.*M.*dth2.^2.*cos(th2).^2 + L1.*L2.^2.*M.*dth2.^2.*sin(th2).^2 + L1.*T2.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*T2.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*T2.*sin(th1 - th2).*cos(th1).*cos(th2) + L1.*Thip.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*Thip.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*Thip.*sin(th1 - th2).*cos(th1).*cos(th2) + L1.*T2.*sin(th1 - th2).*sin(th1).*sin(th2) + L1.*Thip.*sin(th1 - th2).*sin(th1).*sin(th2) - F1.*L1.*L2.*cos(th1).*cos(th2) + F2.*L1.*L2.*cos(th1 - th2).*sin(th1).*sin(th2) - F2.*L1.*L2.*sin(th1 - th2).*cos(th1).*sin(th2) + F2.*L1.*L2.*sin(th1 - th2).*cos(th2).*sin(th1) + L1.*L2.*M.*ddx.*cos(th1).^2.*cos(th2) + L1.*L2.*M.*ddx.*cos(th2).*sin(th1).^2 + L1.*L2.*M.*ddy.*cos(th1).^2.*sin(th2) + L1.*L2.*M.*g.*cos(th1).^2.*sin(th2) + L1.*L2.*M.*ddy.*sin(th1).^2.*sin(th2) + L1.*L2.*M.*g.*sin(th1).^2.*sin(th2) + F2.*L1.*L2.*cos(th1 - th2).*cos(th1).*cos(th2))./(L1.*L2.*M.*(cos(th2).^2 + sin(th2).^2));

% contactForces(1,:) == H1 == (N) Foot One, horizontal contact force
% contactForces(2,:) == V1 == (N) Foot One, vertical contact force
% contactForces(3,:) == H2 == (N) Foot Two, horizontal contact force
% contactForces(4,:) == V2 == (N) Foot Two, vertical contact force
contactForces = zeros(4,size(States,2));
contactForces(1,:) = (Thip.*sin(th1) - T1.*sin(th1) + L1.*ddx.*m1 + F1.*L1.*cos(th1))./L1;
contactForces(2,:) = (T1.*cos(th1) - Thip.*cos(th1) + L1.*ddy.*m1 + L1.*g.*m1 + F1.*L1.*sin(th1))./L1;
contactForces(3,:) = (L1.*M.*T2.*sin(th2) + L1.*M.*Thip.*sin(th2) + L2.*T1.*m2.*sin(th1) - L2.*Thip.*m2.*sin(th1) - L1.*L2.*M.*ddx.*m2 - L1.*T2.*m2.*cos(th1 - th2).*sin(th1) + L1.*T2.*m2.*sin(th1 - th2).*cos(th1) - L2.*T1.*m2.*cos(th1 - th2).*sin(th2) - L2.*T1.*m2.*sin(th1 - th2).*cos(th2) - L1.*Thip.*m2.*cos(th1 - th2).*sin(th1) + L1.*Thip.*m2.*sin(th1 - th2).*cos(th1) + L2.*Thip.*m2.*cos(th1 - th2).*sin(th2) + L2.*Thip.*m2.*sin(th1 - th2).*cos(th2) - F2.*L1.*L2.*M.*cos(th2) - F1.*L1.*L2.*m2.*cos(th1) + L1.*T2.*m2.*cos(th1 - th2).^2.*sin(th2) + L1.*Thip.*m2.*cos(th1 - th2).^2.*sin(th2) + L1.*T2.*m2.*sin(th1 - th2).^2.*sin(th2) + L1.*Thip.*m2.*sin(th1 - th2).^2.*sin(th2) + L1.*L2.*M.*ddx.*m2.*cos(th1).^2 + L1.*L2.*M.*ddx.*m2.*cos(th2).^2 + L1.*L2.*M.*ddx.*m2.*sin(th1).^2 + L1.*L2.*M.*ddx.*m2.*sin(th2).^2 + F1.*L1.*L2.*m2.*cos(th1 - th2).*cos(th2) + F2.*L1.*L2.*m2.*cos(th1 - th2).*cos(th1) - F1.*L1.*L2.*m2.*sin(th1 - th2).*sin(th2) + F2.*L1.*L2.*m2.*sin(th1 - th2).*sin(th1) - F2.*L1.*L2.*m2.*cos(th1 - th2).^2.*cos(th2) - F2.*L1.*L2.*m2.*sin(th1 - th2).^2.*cos(th2) + L1.*L2.*M.*ddy.*m2.*sin(th1 - th2).*sin(th1).*sin(th2) + L1.*L2.*M.*g.*m2.*sin(th1 - th2).*sin(th1).*sin(th2) - L1.*L2.*M.*ddx.*m2.*cos(th1 - th2).*cos(th1).*cos(th2) + L1.*L2.*M.*ddy.*m2.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*L2.*M.*ddy.*m2.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*L2.*M.*ddy.*m2.*sin(th1 - th2).*cos(th1).*cos(th2) + L1.*L2.*M.*g.*m2.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*L2.*M.*g.*m2.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*L2.*M.*g.*m2.*sin(th1 - th2).*cos(th1).*cos(th2) - L1.*L2.*M.*ddx.*m2.*cos(th1 - th2).*sin(th1).*sin(th2) + L1.*L2.*M.*ddx.*m2.*sin(th1 - th2).*cos(th1).*sin(th2) - L1.*L2.*M.*ddx.*m2.*sin(th1 - th2).*cos(th2).*sin(th1))./(L1.*L2.*M.*(cos(th2).^2 + sin(th2).^2));
contactForces(4,:) = -(L1.*M.*T2.*cos(th2) + L1.*M.*Thip.*cos(th2) + L2.*T1.*m2.*cos(th1) - L2.*Thip.*m2.*cos(th1) + L1.*L2.*M.*ddy.*m2 - L1.*T2.*m2.*cos(th1 - th2).*cos(th1) - L2.*T1.*m2.*cos(th1 - th2).*cos(th2) - L1.*Thip.*m2.*cos(th1 - th2).*cos(th1) + L2.*Thip.*m2.*cos(th1 - th2).*cos(th2) - L1.*T2.*m2.*sin(th1 - th2).*sin(th1) + L2.*T1.*m2.*sin(th1 - th2).*sin(th2) - L1.*Thip.*m2.*sin(th1 - th2).*sin(th1) - L2.*Thip.*m2.*sin(th1 - th2).*sin(th2) + F2.*L1.*L2.*M.*sin(th2) + F1.*L1.*L2.*m2.*sin(th1) + L1.*T2.*m2.*cos(th1 - th2).^2.*cos(th2) + L1.*Thip.*m2.*cos(th1 - th2).^2.*cos(th2) + L1.*T2.*m2.*sin(th1 - th2).^2.*cos(th2) + L1.*Thip.*m2.*sin(th1 - th2).^2.*cos(th2) + F2.*L1.*L2.*m2.*sin(th1 - th2).^2.*sin(th2) - L1.*L2.*M.*ddy.*m2.*cos(th1).^2 - L1.*L2.*M.*ddy.*m2.*cos(th2).^2 - L1.*L2.*M.*g.*m2.*cos(th1).^2 - L1.*L2.*M.*g.*m2.*cos(th2).^2 - L1.*L2.*M.*ddy.*m2.*sin(th1).^2 - L1.*L2.*M.*ddy.*m2.*sin(th2).^2 - L1.*L2.*M.*g.*m2.*sin(th1).^2 - L1.*L2.*M.*g.*m2.*sin(th2).^2 - F1.*L1.*L2.*m2.*cos(th1 - th2).*sin(th2) - F1.*L1.*L2.*m2.*sin(th1 - th2).*cos(th2) - F2.*L1.*L2.*m2.*cos(th1 - th2).*sin(th1) + F2.*L1.*L2.*m2.*sin(th1 - th2).*cos(th1) + F2.*L1.*L2.*m2.*cos(th1 - th2).^2.*sin(th2) + L1.*L2.*M.*ddx.*m2.*sin(th1 - th2).*sin(th1).*sin(th2) + L1.*L2.*M.*ddy.*m2.*cos(th1 - th2).*cos(th1).*cos(th2) + L1.*L2.*M.*g.*m2.*cos(th1 - th2).*cos(th1).*cos(th2) + L1.*L2.*M.*ddx.*m2.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*L2.*M.*ddx.*m2.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*L2.*M.*ddx.*m2.*sin(th1 - th2).*cos(th1).*cos(th2) + L1.*L2.*M.*ddy.*m2.*cos(th1 - th2).*sin(th1).*sin(th2) - L1.*L2.*M.*ddy.*m2.*sin(th1 - th2).*cos(th1).*sin(th2) + L1.*L2.*M.*ddy.*m2.*sin(th1 - th2).*cos(th2).*sin(th1) + L1.*L2.*M.*g.*m2.*cos(th1 - th2).*sin(th1).*sin(th2) - L1.*L2.*M.*g.*m2.*sin(th1 - th2).*cos(th1).*sin(th2) + L1.*L2.*M.*g.*m2.*sin(th1 - th2).*cos(th2).*sin(th1))./(L1.*L2.*M.*(cos(th2).^2 + sin(th2).^2));

end