function [y,err]=golaycodec(x,enc,ext)
% GOLAYCODEC - encode/decode a binary array using the Golay code with error correction.
%
% golaycodec encodes or decodes a binary array using the Golay code.
% Words of length 12 are encoded as codewords of length 23 (or 24 for the
% extended Golay code).  The Golay code is tolerant of up to 3 errors,
% i.e. for any word of length 23 there is a unique codeword that differs
% in at most 3 positions.
%
% The Golay code can be constructed as a polynomial code using one of the
% degree 11 factors of z^23+1 (mod 2).  Here we use
%   g = 1 + z^2 + z^4 + z^5 + z^6 + z^10 + z^11
% For more details see Wikipedia etc.
%
% Usage:
%   y = golaycodec(x)
%   [y,err] = golaycodec(x)
%   ... = golaycodec(x,enc)
%   ... = golaycodec(x,enc,ext)
%
% Inputs:
%   x can be an Mx12, Mx23 or Mx24 binary (logical) array or an Mx1 integer array.
%     If x is Mx1, the binary representation of each integer is used.
%   enc is a logical; if true x is to be encoded, otherwise x is to be decoded.
%     If not specified, enc is inferred from the size of x or set to true.
%   ext is a logical; if true use the extended Golay code (length 24),
%     otherwise use the non-extended Golay code (length 23).
%     If not specified, enc is inferred from the size of x or set to false.
%     NOTE: the algorithm has so far only been implemented for ext=false
%
% Outputs:
%   y is an array consisting of the coded or decoded rows of x
%     If x is Mx12 and ext=false then y is Mx23
%     If x is Mx12 and ext=true then y is Mx24
%     If x is Mx23 or Mx24 then y is Mx12
%     If x is Mx1 then y is Mx1
%   err (only available if decoding is done, e.g enc=false) is an array
%     with the errors that differ each word in x from its closest codeword)
%
% Example: encode some messages, add transmission errors and decode
%   n=10;
%   x=zeros(n,12);for i=1:n,x(i,ceil(12*rand(1,8)))=1;end; % random message
%   y=golaycodec(x);
%   err=zeros(n,23);for i=1:n,err(i,ceil(23*rand(1,3)))=1;end; % 3 random errors per message
%   y1=xor(y,err); % add transmission error
%   [x1,err1]=golaycodec(y1); % decode: should have x1==x and err1==err
%

% Author: Ben Petschel 18/3/2009
%
% Change history:
%  18/3/2009 - created
%  30/4/2009 - updated to implement extended Golay codes
%

encdef = true; % default value of "enc" if not specified for integer input
extdef = false; % default value of "ext" if not specified

% input checking: x
nx=size(x,2);
if any(~isfinite(x(:))) || any(~isreal(x(:)))
  error('golaycodec: input array must be finite real values');
end;
if nx==1
  if any(x~=round(x))
    error('golaycodec: column input must be integers');
  end;
  if any(x<0)
    % check later that integers are small enough
    error('golaycodec: column input must be non-negative integers');
  end;
  intout = true;

elseif nx>1
  if any((x(:)~=0)&(x(:)~=1))
    error('golaycodec: entries of row or matrix input must be 0 or 1');
  end;
  if ~any(nx==[12,23,24]),
    error('golaycodec: number of columns of input must be 1, 12, 23 or 24');
  end;
  intout = false; % binary output

else
  error('golaycodec: empty input');
end; % if nx==1 elseif ... else ... end;

% input checking: enc, ext must be booleans
if nargin>=2,
  if isnumeric(enc),
    enc = enc>0;
  end;
  if ~islogical(enc) || ~isscalar(enc)
    error('golaycodec: enc must be 1x1 logical value');
  end;
end;
if nargin>=3,
  if isnumeric(ext),
    ext = ext>0;
  end;
  if ~islogical(ext) || ~isscalar(ext)
    error('golaycodec: enc must be 1x1 logical value');
  end;
end;

% input checking: consistency of nx with enc and ext if specified
switch nx
  case 1
    if nargin == 1,
      enc = encdef; % set to default value (usually true)
    end;
    if nargin <= 2,
      ext = extdef; % set to default value (usually false)
    end;
  case 12
    if nargin == 1,
      enc = true; % encode 12-digit word
    elseif ~enc,
      error('golaycodec: cannot decode 12-digit words');
    end;
    if nargin <=2,
      ext = extdef; % set to default value (usually false)
    end;
  case 23
    if nargin == 1,
      enc = false; % decode 23-digit word
    elseif enc,
      error('golaycodec: cannot encode 23-digit words');
    end;
    if nargin <=2,
      ext = false; % not from the extended code
    elseif ext,
      error('golaycodec: 23 digit words are not in the extended code');
    end;
  case 24
    if nargin == 1,
      enc = false; % decode 24-digit word
    elseif enc,
      error('golaycodec: cannot encode 24-digit words');
    end;
    if nargin <=2,
      ext = true; % use the extended code
    elseif ~ext,
      error('golaycodec: 24-digit words are not in the non-extended code');
    end;
  otherwise
    error('golaycodec: input must have 1, 12, 23 or 24 columns');
end; % switch nx

% for integer inputs, check that the integers are not too large
if intout
  if enc
    if any(x(:)>=2^12),
      error('golaycodec: integers for coding must be less than 2^12');
    end;
  elseif any(x(:)>=2^23),
    error('golaycodec: integers for decoding must be less than 2^23');
  end;
end;

% output checking: nargout>1 only for decoding
if (nargout>2) || ((nargout>1) && enc)
  error('golaycodec: can only specify two outputs when decoding');
end;

if nx==1,
  % convert integers to the appropriate length
  if enc
    nx=12;
  elseif ext
    % decode extended code
    nx=24;
  else
    % decode non-extended code
    nx=23;
  end;
  x = dec2logi(x,nx);
end; % if nx==1

% retain generating polynomial, parity check and error parity table
persistent g h errtab

if isempty(g)
  % generating polynomial: g = 1+x^2+x^4+x^5+x^6+x^10+x^11
  g=zeros(1,12);g([0,2,4,5,6,10,11]+1)=1;
end;

if isempty(h)
  % parity check polynomial is the other factor of x^23-1 (mod 2)
  % h = (1+x)*(1+x+x^5+x^6+x^7+x^9+x^11)
  h=mod(deconv([1,zeros(1,22),1],g),2);
end;

if isempty(errtab),
  % set up the error table for decoding: determine all errors of up to 3 bits
  % the error will be determined by simple table lookup of the parity check
  % the table is a sparse array indexed by the integer representation of the parity bits
  errtab = geterrtab(h,3,23);
end;

% now encode or decode the array
if enc
  y = circprod(x,g,23);
  if ext
    % if using extended code, append a checksum bit (row sums mod 2)
    y = [y,mod(sum(y,2),2)];
  end;
else
  if ext
    % drop the checksum bit if using extended code
    c = x(:,24);
    x = x(:,1:23);
  end;
  % check parity
  p = circprod(x,h,23);
  % look up parity in error table
  err = dec2logi(errtab(logi2dec(p)+1),23);
  y = zeros(size(x,1),12);
  for i=1:size(x,1),
    y(i,:) = mod(deconv(x(i,:)-err(i,:),g),2);
  end;
  if ext
    % if using extended code, append the checksum bit error
    err = [err,mod(c-sum(y,2),2)];
  end;
end;

% convert back to integers, if necessary
if intout,
  y = logi2dec(y);
  if nargout == 2,
    err = logi2dec(err);
  end;
end;

end % main function golaycodec

function t=geterrtab(h,k,n)
% produces lookup table of parity checks of all errors with weight <=k on n digits
% y is a sparse matrix: index is 1 + (h*err converted to integer)
% and y(i) is (err) converted to binary
%
% h must be row vector of length <= n
% also 0 <= k <= n.

m=length(h);
if (2^n-1)>intmax,
  error('golaycodec:geterrtab: too many digits to convert to integer');
end;
if m>n,
  error('golaycodec:geterrtab: parity check polynomial is too long');
end;

t = sparse(1,1,0,2^n,1); % initialize with 0*h+1 -> 0

for i=1:k,
  ind = nchoosek(1:n,i); % all possible combinations of i error digits
  nck = size(ind,1);
  errbin = zeros(nck,n); % error in binary format
  errbin(sub2ind([nck,n],repmat((1:nck)',i,1),ind(:)))=1; % in row j place 1's in columns ind(j,:)
  errint = logi2dec(errbin); % error in integer format
  errh = circprod(errbin,h,n); % do parity check using h
  t(logi2dec(errh)+1)=errint;
end;

end % helper function geterrtab(...)

function z=circprod(x,h,n)
% does parity check of rows of x using polynomial h
m=size(x,1);

% do circular convolution by fourier transform (fft acts on columns)
% (could also use conv and wrap the tail elements, but conv isn't vectorized)
z = mod(round(ifft(fft(x',n).*repmat(fft(h',n),1,m))'),2);

end % helper function paritycheck

function y=logi2dec(x)
% converts each row of logical array from binary to a decimal number
n=size(x,2);
y=x*(2.^(n-1:-1:0))';

end % helper function logi2dec

function y=dec2logi(x,n)
% converts vector of integers to array of logicals (binary representation)
y=rem(floor(x(:)*2.^(1-n:0)),2); % see dec2bin.m (base matlab) for details

end % helper function dec2logi


function pass = test_mean( pref )

% Grab some preferences
if ( nargin == 0 )
    pref = chebfunpref();
end
tol = pref.techPrefs.chebfuneps;

% Example 1:
f = ballfun(@(x,y,z)1);
I = mean(f,1);
g = spherefun(1);
pass(1) = ( norm(I-g) < tol );

% Example 2:
f = ballfun(@(x,y,z)2);
I = mean(f,2);
g = diskfun(2);
pass(2) = ( norm(I-g) < tol );

% Example 3:
f = ballfun(@(x,y,z)3);
I = mean(f,3);
g = diskfun(3);
pass(3) = ( norm(I-g) < tol );
end


function pobjs = APPfitGroundHough(x,y, imsize)
% pobjs = APPfitGroundHough(x,y, imsize)
% Fits the ground-vertical boundary with a set of polylines
%
% Input:
%   x, y: the x and y positions of ground-vertical boundaries
%   imsize: the image size
% Output:
%   pobjs(nplanes, [m b x1 x2 maxy disconnect])
%
% Copyright(C) Derek Hoiem, Carnegie Mellon University, 2005
% Permission granted to non-commercial enterprises for
% modification/redistribution under GNU GPL.
% Current Version: 1.0  09/30/2005

MAKE_PLOTS = 0; % whether to make displays

[x, inds] =  sort(x);
y = y(inds);

height = imsize(1);
width = imsize(2);
y = height-round(y*(height-1));
x = round(x*(width-1)+1);
%figure(1), hold off, plot(x, y, '.b'), axis equal, hold on

ty =y;
tx =x;
minpts = sqrt(width^2+height^2)/20;
distt = sqrt(width^2+height^2)/100;
mingapt = sqrt(width^2+height^2)/20;
count = 0;

pobjs = {};

% find initial line segments
while length(tx)>minpts

    % get the hough transform image for the points (tx, ty)
    gim = zeros(imsize);
    inds = round(ty+(tx-1)*height);
    gim(inds) = 1;
    gim = conv2(gim, fspecial('gaussian', 5, 1), 'same');
    theta = [0:179];
    [R, xp] = radon(gim, theta);
    theta = theta';

    % get the best line from the hough transform image
    [maxval, ind] = max(R(:));

    % get line parameters (ax + by = c)
    [rind,thind] = ind2sub(size(R),ind);
    t = -theta(thind)*pi/180;
    r = xp(rind);
    lines = [cos(t) sin(t) -r];
    cx = width/2-1;
    cy = height/2-1;
    lines(:,3) = lines(:,3) - lines(:,1)*cx - lines(:,2)*cy;
    a = lines(1, 1);
    b2 = lines(1, 2);
    c = lines(1, 3);

    % get line parameters in y = mx+b form
    m = -lines(:, 1)/lines(:, 2);
    b = -lines(:, 3)/lines(:, 2);

    % get the distance of each point from the line
    pdst = abs(a*tx+b2*ty+c)/sqrt(a^2+b2^2);
    inds = find(pdst < distt);

    % while there is a big gap take only the larger side of the gap
    sortx = sort(tx(inds));
    gapt = max((sortx(end)-sortx(1))/5, mingapt);
    endpoints(count+1, 1:2) = [sortx(1)  sortx(end)];
    %disp(['initlength: ' num2str(length(inds))])
    %disp(['gapt: ' num2str(gapt)])
    while (max(abs(sortx(1:end-1)-sortx(2:end))) > gapt)
        [maxval, maxind] = max(abs(sortx(1:end-1)-sortx(2:end)));
        if maxind > length(inds)/2
            inds = inds(1:maxind);
        else
            inds = inds((maxind+1):end);
        end
        sortx = sort(tx(inds));
        endpoints(count+1, 1:2) = [sortx(1)  sortx(end)];
        gapt = max((sortx(end)-sortx(1))/5, mingapt);
        %disp(['gapt: ' num2str(gapt)])
    end

    %disp(['length: ' num2str(length(inds))])
    % check that best line has enough points
    if length(inds) < minpts
        break;
    end

    count = count + 1;

    % plot the lines and points on each line
    if MAKE_PLOTS
        figure(1)
        color_chars = ['y'];
        ab = axis;
        col = color_chars(mod(count-1, length(color_chars))+1);
        if abs(m)<1
             %px = [ab(1:2)];
             px = endpoints(count, 1:2);
             plot(px, height-(m*px+b)+1, ['-' col], 'LineWidth', 5);
        else
             py = ab(3:4);
             plot((height-(py-b)+1)/m,  height-py+1,  ['-' col], 'LineWidth', 5);
        end
        %plot(tx(inds), height-ty(inds)+1, ['.' col])
        drawnow;
        pause(1)
    end

    % add assigned points to x and y
    % remove assigned points from tx and ty
    tx(inds) = [];
    ty(inds) = [];

    % store current line
    pobjs{count}(1, :) = [m b 0 0 a b2 c];

end

count = length(pobjs);

if ~isempty(pobjs)

    px = cell(count, 1);
    py = cell(count, 1);
    pmedx = zeros(count, 1);
    pmedy = zeros(count, 1);
    oldpinds = cell(count, 1);

    % reassign points to each object and re-estimate lines
    distt2 = distt;
    changed = 1;
    while changed
        changed = 0;
        pdsts = zeros(length(x), count);
        for p1 = 1:length(pobjs)
            a = pobjs{p1}(5);
            b = pobjs{p1}(6);
            c = pobjs{p1}(7);
            pdsts(:, p1) = abs(a*x+b*y+c)/sqrt(a^2+b^2);
            %pobjs{p1}(6:7) = [];
        end
        [mdists, inds] = min(pdsts, [], 2);
        inds2 = find(mdists<=distt2);
        for p1 = 1:length(pobjs)
            pinds = inds2(find(inds(inds2)==p1));
            pinds = pinds(find( (x(pinds)>=(endpoints(p1, 1)-mingapt)) & ...
                (x(pinds)<=(endpoints(p1, 2)+mingapt))));
            if length(setdiff(pinds, oldpinds{p1}))>2
                changed = 1;
                %disp('changed')
                oldpinds{p1} = pinds;
            end
            px{p1} = x(pinds);
            py{p1} = y(pinds);

            if length(pinds) > 0
                endpoints(p1, 1) = min(px{p1});
                endpoints(p1, 2) = max(px{p1});

                % re-estimate line params
                p = polyfit(px{p1},py{p1},1);
                pobjs{p1}(1:2) = p;

                pm = p(1);
                pb = p(2);

                a = 1;
                b = -1/pm;
                c = pb/pm;
                pobjs{p1}(5:7) = [a b c];

                pmedx(p1) = median(px{p1});
                pmedy(p1) = median(py{p1});

            else
                endpoints(p1, 1:2) = 0;
                px{p1} = [];
                py{p1} = [];
            end

        end
    end

    % remove smaller of overlapping segments
    reminds = [];
    for p1 = 1:length(pobjs)
        for p2 = (p1+1):length(pobjs)
            if (endpoints(p1, 1)-endpoints(p2, 1))*...
                    (endpoints(p1, 2)-endpoints(p2, 2)) < 0
                if MAKE_PLOTS, disp('removing overlapping segment'), end;
                if length(px{p1}) < length(px{p2})
                    reminds(end+1) = p1;
                else
                    reminds(end+1) = p2;
                end
            end
        end
        if isempty(px{p1})
            if MAKE_PLOTS, disp('removing empty segment'), end;
            reminds = union(reminds, p1);
        end
    end
    [pobjs, pmedx, pmedy, px, py] = removeIndices(reminds, pobjs, pmedx, pmedy, px, py);
    endpoints(reminds, :) = [];

    if MAKE_PLOTS
        figure(2)
        disp(length(pobjs))
        for p1 = 1:length(pobjs)
            color_chars = ['r' 'y' 'g' 'c' 'k'];
            ab = axis;
            col = color_chars(mod(p1-1, length(color_chars))+1);
            m = pobjs{p1}(1);
            b = pobjs{p1}(2);
            if abs(m)<1
                 tpx = [ab(1:2)];
                 plot(tpx, height-(m*tpx+b)+1, ['-' col], 'LineWidth', 3);
            else
                 tpy = ab(3:4);
                 plot((height-(tpy-b)+1)/m,  height-tpy+1,  ['-' col], 'LineWidth', 3);
            end
            plot(px{p1}, height-py{p1}+1, ['.' col])
            %plot(pmedx(p1), height-pmedy(p1)+1, '+k');
        end
        drawnow;
        pause(5)
    end

    count = length(pobjs);

    if length(pobjs) > 0

    % determine which objects should be merged
    domerge = zeros(count, count);
    intersectx = zeros(count, count);
    for p1 = 1:length(pobjs)
        for p2 = (p1+1):length(pobjs)
            % if slopes are not same
            if pobjs{p1}(1) ~= pobjs{p2}(1)
                xi = (pobjs{p2}(2)-pobjs{p1}(2))/(pobjs{p1}(1)-pobjs{p2}(1));
                yi = xi*pobjs{p1}(1) + pobjs{p1}(2);
                %plot(xi, height-yi+1, '*k')
                % if med x's appear on opposite sides of intersection
                if (sign(pmedx(p1)-xi)~=sign(pmedx(p2)-xi))
                    domerge(p1, p2) = 1;
                    domerge(p2, p1) = 1;
                    intersectx(p1, p2) = xi;
                    intersectx(p2, p1) = xi;
                end
            end
        end
    end
    intersectx = round(intersectx);
    if MAKE_PLOTS, disp(['num merges: ' num2str(sum(domerge(:))/2)]), end;

    % merge objects that should be merged
    reminds = [];
    for p1 = 1:length(pobjs)
        mergeinds = find(domerge(p1, :));
        if ~isempty(mergeinds)
            domerge(p1, mergeinds) = 0;
            domerge(mergeinds, p1) = 0;
            oldlength = 0;
            while length(mergeinds)~=oldlength
                oldlength = length(mergeinds);
                for p2 = mergeinds
                    mergeinds = union(mergeinds, find(domerge(p2, :)));
                    domerge(p2, mergeinds) = 0;
                    domerge(mergeinds, p2) = 0;
                end
            end
            mergeinds = union(mergeinds, p1);

            [tmp, sind] = sort(pmedx(mergeinds));

            % sort mergeinds by their x-medians
            mergeinds = mergeinds(sind);
            tpx = [];
            tpy = [];
            tobj = [];
            for i = 1:length(mergeinds)

                % if (i) and (i-1) should merge or or (i == 1)
                if (i == 1 || intersectx(mergeinds(i), mergeinds(i-1))~=0)

                    if MAKE_PLOTS, disp(['merge: ' num2str(i)]), end;
                    tobj(end+1, :) = pobjs{mergeinds(i)};

                    % set endpoints to intersection points
                    if size(tobj, 1) > 1

                        tobj(end-1, 4) = intersectx(mergeinds(i), mergeinds(i-1));
                        tobj(end, 3) = tobj(end-1, 4);

                    end

                    % add points from (i) to merged segment
                    tpx = [tpx ; px{mergeinds(i)}];
                    tpy = [tpy ; py{mergeinds(i)}];
                else
                    % effectively remove (i) from merginds
                    mergeinds(i) = mergeinds(i-1);
                end
            end
            pobjs{p1} = tobj;
            [px{p1}, inds] = sort(tpx);
            py{p1} = tpy(inds);
            mergeinds = setdiff(mergeinds, p1);

            % set indices to remove
            reminds = [reminds(:) ; mergeinds(:)];
        end
    end
    % remove merged segments
    [pobjs, px, py, pmedx, pmedy] = removeIndices(reminds, pobjs, px, py, pmedx, pmedy);
    endpoints(reminds, :) = [];

    % set endpoints (that were not found by merging)
    for i = 1:length(pobjs)
        for j = 1:size(pobjs{i}, 1)
            if pobjs{i}(j, 3) == 0
                pobjs{i}(j, 3) = min(px{i});
            end
            if pobjs{i}(j, 4) == 0
                pobjs{i}(j, 4) = max(px{i});
            end
        end
    end

    % find overlapping segments and resolve
    reminds = [];
    for p1 = 1:length(pobjs)
        for p2 = p1+1:length(pobjs)
            if isempty(intersect(reminds, [p1 p2]))
                overx1 = max([px{p1}(1) px{p2}(1)]);
                overx2 = min([px{p1}(end) px{p2}(end)]);
                if (overx2 > overx1) % then p1 and p2 overlap

                    if MAKE_PLOTS, disp('warning: removing segment'), end;
                    if length(px{p1}) > length(px{p2})
                        reminds(end+1) = p2;
                    else
                        reminds(end+1) = p1;
                    end

                end
            end
        end
    end

    [pobjs, px, py, pmedx, pmedy] = removeIndices(reminds, pobjs, px, py, pmedx, pmedy);

    % make sure all ground points belong to one segment
    xstart = zeros(length(pobjs), 1);
    xend = zeros(length(pobjs), 1);
    for p1 = 1:length(pobjs)
        xstart(p1) = pobjs{p1}(1, 3);
        xend(p1) = pobjs{p1}(end, 4);
    end
    [xstart, ind] = sort(xstart);
    xend = xend(ind);
    pobjs = pobjs(ind);
    if xstart(1) ~= x(1)
        pobjs{1}(1, 3) = x(1);
    end
    for p1 = 1:length(xstart)-1
        if xend(p1)<xstart(p1+1)
            if pmedy(p1) > pmedy(p1+1) % p1 lower than than p2
                pobjs{p1}(end, 4) = xstart(p1+1)-1;
            else
                pobjs{p1+1}(1, 3) = xend(p1)+1;
            end
        end
    end
    [xend, ind] = sort(xend);
    if xend(end) ~= x(end)
        pobjs{ind(end)}(end, 4) = x(end);
    end

    end % if at least one object

end

if isempty(pobjs)

    reminds = [];
    for i =2:length(x)
        if x(i) == x(i-1)
            reminds(end+1) = i;
        end
    end
    x(reminds) = [];
    y(reminds) = [];
    pobjs{1} = piecewise_linear_spline(x, y, 3);
    pobjs{1}(:, 5) = 0;
end

% reverse height
for i = 1:length(pobjs)
    ind = find(pobjs{i}(:, 5)~=0);
    pobjs{i}(ind, 5) = height - pobjs{i}(ind, 5) + 1;
    pobjs{i}(:, 1) = -pobjs{i}(:, 1);
    pobjs{i}(:, 2) = height- pobjs{i}(:, 2)+1;
end

%figure(1) %, hold off,
%hold on, plot(oldx, height-oldy+1, '.k');
if MAKE_PLOTS
    figure(3)
    color_chars = ['r' 'g' 'c' 'b' 'm'];
    count = 0;
    for p = 1:length(pobjs)
        rc = mod(p-1, 5)+1;
        for j = 1:size(pobjs{p}, 1)
            count = count + 1;
            m = pobjs{p}(j, 1);
            b = pobjs{p}(j, 2);
            ppx = pobjs{p}(j, 3:4);
            plot(ppx, ppx*m+b, ['-' color_chars(rc)], 'LineWidth', 5);
            if j > 1
                plot([ppx(1) ppx(1)], [ppx(1)*m+b ppx(1)*m+b-50], ...
                    ['-' color_chars(rc)], 'LineWidth', 5);
            end
        end
    end
    drawnow;
end

tp = pobjs;
pobjs = zeros(0, 6);
for i = 1:length(tp)
    for j = 1:size(tp{i},1)
        pobjs(end+1, 1:5) = tp{i}(j, 1:5);
    end
    pobjs(end, 6) = 1;
end

pobjs(:, 1) = pobjs(:, 1)*(width-1)/(height-1);
pobjs(:, 2) = (pobjs(:, 2)-1)/(height-1);
pobjs(:, 3:4) = (pobjs(:, 3:4)-1)/(width-1);
pobjs(:, 5) = (pobjs(:, 5)-1)/(height-1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function varargout = removeIndices(indices, varargin)

for i = 1:length(varargin)
    varargout(i) = varargin(i);
    varargout{i}(indices) = [];
end


function cout = intermutual(cin1, cin2, n)

%tstoolbox/@core/intermutual
%   Syntax:
%     * intermutual(cin1,cin2,n)
%
%   Input Arguments:
%     * cin1,cin2 - core objects
%
%   Calculates the mutual information of cin1 and cin2.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

N = dlens(cin1,1);
cout = zeros(n,1);
epsilon = 1e-9;
points1 = data(cin1);
points1 = points1 - min(points1);
points2 = data(cin2);
points2 = points2 - min(points2);
ma1 = max(points1);
ma2 = max(points2);
increment = 1/N;

for i=1:n
	partitionen = 2^i;
	pointsA = 1+floor(points1 / (ma1/(partitionen-epsilon)));
	pointsB = 1+floor(points2 / (ma2/(partitionen-epsilon)));
	% Aus den Werten sind jetzt fertige Indizes fuer die Partititonen geworden

	histA = sparse(pointsA, ones(N,1), increment);
	histB = sparse(ones(N,1), pointsB, increment);
	histAB = sparse(pointsA, pointsB, increment);
	[ind1, ind2, value] = find(histAB);
	amf = sum(value .* log2(value ./ (histA(ind1) .* histB(ind2)')));	% '

	cout(i) = amf;
end

%cout = core(amf);


function SE = functionComputeSE_DL_poweralloc(rho,signal,interference,prelogFactor)
%Compute the SE in Theorem 4.6 using the formulation in (7.1) for a given
%power allocation scheme.
%
%INPUT:
%rho          = K x L matrix where element (k,j) is the downlink transmit
%               power allocated to UE k in cell j
%signal       = K x L matrix where element (k,j,n) is a_jk in (7.2)
%interference = K x L x K x L matrix where (l,i,jk,n) is b_lijk in (7.3)
%prelogFactor = Prelog factor
%
%OUTPUT:
%SE = K x L matrix where element (k,j) is the downlink SE of UE k in cell j
%     using the power allocation given as input
%
%
%This Matlab function was developed to generate simulation results to:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Extract number of UEs
K = size(signal,1);

%Extract number of cells
L = size(signal,2);

%Prepare to save results
SE = zeros(K,L);

%% Go through all cells
for j = 1:L

    %Go through all UEs in cell j
    for k = 1:K

        %Compute the SE in Theorem 4.6 using the formulation in (7.1)
        SE(k,j) = prelogFactor*log2(1+(rho(k,j)*signal(k,j)) / (sum(sum(rho.*interference(:,:,k,j))) + 1));

    end

end


function bs = ISC(bs)
% Calculate brain-wide inter-subject correlations in patterns across parcel means
%
% :Usage:
% ::
%
%     bs = ISC(bs)
%
% For objects: Type methods(object_name) for a list of special commands
%              Type help object_name.method_name for help on specific
%              methods.
%
% ..
%     Author and copyright information:
%
%     Copyright (C)2020 Tor Wager
%
%     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/>.
% ..
%
% :Inputs:
%
%   **bs:**
%        A brainpathway_multisubject object
%
%
% :Optional Inputs:
%   **'nofactor':**
%        Omit factor analysis
%
%
% :Outputs:
%
%   **bs:**
%        A brainpathway_multisubject object
%        Output is added to relevant object properties:
%
%         bs.connectivity.regions.isc = intersubject correlation matrix;
%         bs.connectivity.regions.isc_subj_unusualness = 1/mean isc with others for each subject. 1/isc = [0 Inf] (bounded at zero);
%         bs.connectivity.regions.isc_outliers = subjects with isc_subj_unusualness > 2 SD above the mean
%         bs.connectivity.regions.isc_numfactors = estimated number of factors from elbow of PCA eigenvalues
%         bs.connectivity.regions.isc_Lambda = Subject factor loadings, can be correlated with other variables
%         bs.connectivity.regions.isc_Lambda_descrip = 'Lambdas are n_subjects x numfactors factor loadings, describing dimensions of subject similarity here';
%
%
% :Examples:
% ::
%
%    % None yet - example from HCP data
%
% :References:
%
% :See also:
%   - methods(brainpathway_multisubject)
%

% ..
%    Programmers' notes:
%    12/2020 Created by Tor Wager
% ..

fprintf('Flattening conn matrices. ')
mat = bs.flatten_conn_matrices('replacenans');

% Find NaNs and replace with subject mean (so regions will have min=zero influence on isc)
% Done now in flatten_conn_matrices
% fprintf('Imputing missing values. ')
%
% whnan = isnan(mat);
% subjmean = nanmean(mat');
%
% for i = 1:size(mat, 1)
% 	mat(i, whnan(i, :)) = subjmean(i);
% end

fprintf('Calculating ISC. ')
isc = corrcoef(double(mat'));

create_figure('region isc');
imagesc(isc); colorbar
drawnow

%whnan = all(isnan(isc), 1);
% anynans = sum(any(whnan, 2));

% Subject unusualness (potential outliers)
% --------------------------
meanisc = mean(isc) - (1./size(isc, 1)); % subtract the 1 on the diagonal
isc_subj_unusualness = 1 ./ meanisc;

isc_outliers = isc_subj_unusualness > (mean(isc_subj_unusualness) + 2 * std(isc_subj_unusualness));

figure; plot( 1 ./ meanisc, 'bo', 'MarkerFaceColor', [.3 .3 1]);

% Factor analysis
% --------------------------
fprintf('Factor analysis ')

% Choose # dimensions: where acceleration is greatest, i.e., derivative of
% eigenvalues changes fastest and the curve flattens at the "elbow"
[eigvec, eigval] = pcacov(isc);
gg = gradient(gradient(eigval));
[~, isc_numfactors] = max(gg);

fprintf('(%d) factors. ', isc_numfactors)

Lambda = factoran(isc, isc_numfactors, 'Xtype', 'cov');

if isc_numfactors > 1

    create_figure('region isc');
    scatterhist(Lambda(:, 1), Lambda(:, 2));
    xlabel('Factor 1')
    ylabel('Factor 2')

end

fprintf('\n');
disp('Added results to bs.connectivity.regions.isc');

bs.connectivity.regions.isc = isc;
bs.connectivity.regions.isc_subj_unusualness = isc_subj_unusualness;
bs.connectivity.regions.isc_outliers = isc_outliers;
bs.connectivity.regions.isc_numfactors = isc_numfactors;
bs.connectivity.regions.isc_Lambda = Lambda;
bs.connectivity.regions.isc_Lambda_descrip = 'Lambdas are n_subjects x numfactors factor loadings, describing dimensions of subject similarity here';

% bs.connectivity.regions.isc = intersubject correlation matrix;
% bs.connectivity.regions.isc_subj_unusualness = 1/mean isc with others for each subject. 1/isc = [0 Inf] (bounded at zero);
% bs.connectivity.regions.isc_outliers = subjects with isc_subj_unusualness > 2 SD above the mean
% bs.connectivity.regions.isc_numfactors = estimated number of factors from elbow of PCA eigenvalues
% bs.connectivity.regions.isc_Lambda = Subject factor loadings, can be correlated with other variables
% bs.connectivity.regions.isc_Lambda_descrip = 'Lambdas are n_subjects x numfactors factor loadings, describing dimensions of subject similarity here';

end


%ISHOMOG Test if SE(3) homogeneous transformation
%
% ISHOMOG(T) is true (1) if the argument T is of dimension 4x4 or 4x4xN, else
% false (0).
%
% ISHOMOG(T, 'valid') as above, but also checks the validity of the rotation
% sub-matrix.
%
% Notes::
% - The first form is a fast, but incomplete, test for a transform is SE(3).
% - Does not work for the SE(2) case.
%
% See also ISROT, ISVEC.

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

function h = ishomog(tr, rtest)
    d = size(tr);
    if ndims(tr) >= 2
        h =  all(d(1:2) == [4 4]);

        if h && nargin > 1
            h = abs(det(tr(1:3,1:3)) - 1) < eps;
        end

    else
        h = false;
    end


function polygon_grid_display ( n, nv, v, ng, xg )

%*****************************************************************************80
%
%% POLYGON_GRID_DISPLAY displays grid points inside a polygon.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of subintervals.
%
%    Input, integer NV, the number of vertices in the polygon.
%
%    Input, real V[NV,2], the coordinates of the vertices.
%
%    Input, integer NG, the number of grid points.
%
%    Input, real XG(NG,2), the grid points.
%
  clf

  hold on

  plot ( [v(nv,1),v(1,1)], [v(nv,2),v(1,2)], 'r-', 'LineWidth', 2 );
  for i = 1 : nv - 1
    plot ( v(i:i+1,1), v(i:i+1,2), 'r-', 'LineWidth', 2 );
  end
  plot ( v(:,1), v(:,2), 'r.', 'MarkerSize', 25 );

  vc(1,1) = sum ( v(1:nv,1) ) / nv;
  vc(1,2) = sum ( v(1:nv,2) ) / nv;
  for i = 1 : nv
    plot ( [v(i,1),vc(1,1)], [v(i,2),vc(1,2)], 'b-', 'LineWidth', 2 );
  end
  plot ( xg(:,1), xg(:,2), 'b.', 'MarkerSize', 20 );
  axis equal
  title ( sprintf ( 'Polygonal grid, N = %d, NG = %d', n, ng ) )
  grid on
  hold off

  return
end


function gmst=utc2gmst(t,ut1_utc)

ep2000=[2000 1 1 12 0 0];

tut=timeadd(t,ut1_utc);
[ut,tut0]=time2sec(tut);
t1=timediff(tut0,epoch2time(ep2000))/86400.0/36525.0;
t2=t1^2; t3=t1^2;
gmst0=24110.54841+8640184.812866*t1+0.093104*t2-6.2E-6*t3;
gmst=gmst0+1.002737909350795*ut;

if gmst<0
    gmst=-mod(abs(gmst),86400.0)*pi/43200.0;
else
    gmst=mod(abs(gmst),86400.0)*pi/43200.0;
end

return


% Slopes of temperature distribution with altitude.
m12=-6.5e-3;
m34=9.92e-4;
m45=2.78e-3;
m67=-1.96e-3;
m78=-3.94e-3;


function [ node_xy, element_node, element_neighbor ] = example2_q4_mesh ( ...
  node_num, element_num )

%*****************************************************************************80
%
%% EXAMPLE2_Q4_MESH sets up example #2 Q4 mesh.
%
%  Discussion:
%
%    The region is a semicircle.  This example includes degenerate elements
%    (the first layer of elements is touching the origin, and so has a side
%    of length zero).  The elements are not parallelograms.  And the elements
%    vary in size.
%
%    Because of the treatment of node 1, algorithms for counting boundary
%    edges may become "confused".
%
%    The appropriate values of NODE_NUM and ELEMENT_NUM can be found by
%    calling EXAMPLE1_Q4_MESH_SIZE first.
%
%   29---30---31---32---33---34---35---36---37
%    | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 |
%   20---21---22---23---24---25---26---27---28
%    | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 |
%   11---12---13---14---15---16---17---18---19
%    |  9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 |
%    2----3----4----5----6----7----8----9---10
%    |  1 |  2 |  3 |  4 |  5 |  6 |  7 |  8 |
%    1----1----1----1----1----1----1----1----1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Output, real NODE_XY(2,NODE_NUM), the coordinates of the
%    nodes.
%
%    Output, integer ELEMENT_NODE(4,ELEMENT_NUM), the nodes
%    that make up the elements.
%
%    Output, integer ELEMENT_NEIGHBOR(4,ELEMENT_NUM), the
%    element neighbors on each side.  Negative values indicate edges that
%    lie on the exterior.
%
  k = 1;
  node_xy(1,k) = 0.0;
  node_xy(2,k) = 0.0;

  for row = 1 : 4
    r = row;
    for col = 0 : 8
      a = ( 8 - col ) * pi / 8.0;
      k = k + 1;
      node_xy(1,k) = r * cos ( a );
      node_xy(2,k) = r * sin ( a );
    end
  end

  element = 0;
  for row = 0 : 3
    for col = 0 : 7
      element = element + 1;
      if ( row == 0 )
        element_node(1,element) = 1;
        element_node(2,element) = 1;
        element_node(3,element) = col + 3;
        element_node(4,element) = col + 2;
      else
        element_node(1,element) = element_node(4,element-8);
        element_node(2,element) = element_node(3,element-8);
        element_node(3,element) = element_node(2,element) + 9;
        element_node(4,element) = element_node(1,element) + 9;
      end
    end
  end

  element = 0;
  for row = 0 : 3
    for col = 0 : 7
      element = element + 1;
      if ( row == 0 )
        element_neighbor(1,element) = -1;
      else
        element_neighbor(1,element) = element - 8;
      end
      if ( col == 7 )
        element_neighbor(2,element) = -1;
      else
        element_neighbor(2,element) = element + 1;
      end
      if ( row == 3 )
        element_neighbor(3,element) = - 1;
      else
        element_neighbor(3,element) = element + 8;
      end
      if ( col == 0 )
        element_neighbor(4,element) = - 1;
      else
        element_neighbor(4,element) = element - 1;
      end
    end
  end

  return
end


function r = rank(f, tol)
%RANK      Rank of a SEPARABLEAPPROX.
%   RANK(F) produces an estimate of the rank of the approximant F.
%
%   RANK(F, TOL) is the number of singular values of F greater than TOL*N, where
%   N is the first singular value of F.
%
% See also LENGTH.

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

% Empty check:
if ( isempty(f) )
    r = [];
    return
end

% HARD tolerance.
if ( nargin == 1 )
    tol = 0;
end

% Compute the singular values of f.
s = svd( f );

% Check for zero function.
if ( max(s) == 0  )
    r = 0;
else
    % r = no. of s.v. above relative tol.
    r = find(s/s(1) > tol, 1, 'last');
end

end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
% function [fh,ph,th] = checkDerivative(fctn,x0);
%
% checks the implementation of a derivative by comparing the function
% with the Taylor-poly
%
%   \| f(x0 + h ) - TP_p(x0,h) \|   !=   O( h^{p+1} )
%
% Input:
%  fctn    function handle
%  x0      expanding point
%
% Output:
%  fh      figure handle to graphical output
%  ph      plot handle
%  th      text handle
% call checkDerivative for a minimal example.
%==============================================================================

function varargout = checkDerivative(fctn,x0,varargin)

if nargin == 0, % help and minimal example
  help(mfilename);
  fctn = @xSquare;
  x0   = 1;
  checkDerivative(fctn,x0);
  return;
end;

fig = [];
for k=1:2:length(varargin),     % overwrites default parameter
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

FAIRmessage(mfilename,'.')
fprintf('test derivative of function <%s>\n',func2str(fctn));

fprintf('T0 = |f0 - ft|, T1 = |f0+h*f0'' - ft|\n');
[f0,df] = feval(fctn,x0);
if ~isnumeric(df),
  try
    [f0,para,df] = feval(fctn,x0);
  catch
    if isfield(df,'Q'), df = df.Q;  end;
  end;
end;

h = logspace(-1,-10,10);
v = randn(size(x0));
if isnumeric(df),
  dvf = df*v;
elseif isa(df,'function_handle'),
  dvf = df(v);
else
  keyboard;
end;

for j=1:length(h),
  ft = feval(fctn,x0+h(j)*v);      % function value
  T0(j) = norm(f0-ft);             % TaylorPoly 0
  T1(j) = norm(f0+h(j)*dvf - ft);  % TaylorPoly 1
  fprintf('h=%12.4e     T0=%12.4e    T1=%12.4e\n',h(j),T0(j),T1(j));
end;

fh = FAIRfigure(fig);
ph = loglog(h,[T0;T1]); set(ph(2),'linestyle','--')
th = title(sprintf('%s: |f-f(h)|,|f+h*dvf -f(h)| vs. h',mfilename));

if nargout>0,
  varargout = {fh,ph,th};
end;
FAIRmessage('.');
%------------------------------------------------------------------------------
function [y,dy] = xSquare(x)
if nargin == 0, return; end;
y = x.^2; dy = reshape(2*x,1,[]);
%==============================================================================


function disp(tt,name,varargin)
%Command window display of a QTT_TUCKER
%
disp_mode_sizes=false;
for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'modes'
            disp_mode_sizes=varargin{i+1};
        otherwise
            error('Unrecognized option: %s\n',varargin{i});
    end
end

if ~exist('name','var')
    name = 'ans';
end
fprintf('%s is a QTT-tucker tensor: \n',name);
fprintf('Tucker ranks: \n');
sz=size(tt.core);
rk=rank(tt.core);
for i=1:tt.dphys-1
   fprintf('%d-',sz(i));
end
fprintf('%d\n',sz(tt.dphys));
fprintf('TT-ranks for the core: \n');
for i=1:tt.dphys
   fprintf('%d-',rk(i));
end
fprintf('%d\n',rk(tt.dphys+1));
tk=tt.tuck;
fprintf('TT-ranks/mode sizes for the factors: \n');
for i=1:tt.dphys
   di=ndims(tk{i});
   ri=rank(tk{i});
   ni=size(tk{i});
   fprintf('Factor %d: ',i);
   for j=1:di
      fprintf('%d-',ri(j));
   end
   fprintf('%d\n',ri(di+1));
   if ( disp_mode_sizes )
     fprintf('Modes   : ');
     for j=1:di-1
       fprintf('%d-',ni(j));
     end
     fprintf('%d\n',ni(di));
   end
end
% %fprintf('r(1)=%d \n', r(1));
% for i=1:d
%    fprintf('r(%d)=%d \t n(%d)=%d \n',i,r(i),i,n(i));
% end
% fprintf('r(%d)=%d \n',d+1,r(d+1));


function y = time_varying_estimate(meth,data,varargin)
% Performs correlation operation (default) or any function you pass in (as a function handle) on symmetric moving average of data
%
% :Usage:
% ::
%
%     y = time_varying_estimate(meth,data,[window width],[function handle])
%
% - Works on all columns of the data input matrix together, so function handles can
%   operate on multivariate data (e.g. cond())
% - For moving averages, use the Matlab built-in smoothdata.m
%
% :Inputs:
%
%   **meth:**
%   Window-type options, either:
%   - 'gaussian'
%   - 'tukey'
%
% :Optional Inputs:
%
%   **stepby:**
%
%   Sometimes input data is very high resolution, and it would take too much
%   time to work element by element across the inputs.  You can enter an
%   option here to compute estimates at every n-th lag.
%
% 	**Any function handle**
%   Any function handle to a function to evaluate on local rows of [data]
%
%   e.g., @corr
%
% :Examples:
% ::
%
%    y =  time_varying_estimate('gaussian',data,20);
%
%    % Generate sample data:
%    x = mvnrnd([0 0], [1 .6; .6 1], 100);
%
%    % Correlation between columns of x:
%    r = time_varying_estimate('tukey', x, 20);
%
%    % St. deviation of first column
%    mystd = time_varying_estimate('tukey', x(:, 1), 20, @(y) std(y));
%
% ..
%    By Tor Wager
%    Last updated: Dec 2008
% ..

nshift = 0;
stepby = 1;                 % step size for shift
center_local_data = true;   % mean-center local data: good for corr, bad for moving average

% set up the kernel
% -------------------------------------------
switch meth

    case 'gaussian'

         ntrials = varargin{1};
         kern = normpdf(-3:6/ntrials:3);
         kern = kern./max(kern);            % norm to max of 1

         mymax = find(kern == max(kern));
         nshift = mymax - 1;  % kernel shifts by n points; adjust

         kern = kern';  % column

    case 'tukey'
        % Window length is the zero-influence to zero-influence time
        kern = tukeywin(varargin{1});
        nshift = round(varargin{1} ./ 2);

        % kludgey adjust in case we need extra element
        %if length(i - shift : i + nshift) > length(kern)
            kern = [kern; 0];
        %end
    otherwise error('Unknown method.')

end

% set up function handle
% -------------------------------------------
if length(varargin) > 1
    fhandle = varargin{2};
else
    fhandle = @(y) my_corrcoef(y);
end

if length(varargin) > 2
    stepby = varargin{3};
end

% set up data
% -------------------------------------------
[nobs,ncols] = size(data);

% replicate kernel for each column
kern = repmat(kern,1,ncols);

if nobs < nshift, error('Not enough observations to support kernel.'); end
y = zeros(nobs,1);

% pad data at ends to avoid edge artifacts
% -------------------------------------------
paddat = data(end:-1:end-nshift,:);

data = [data; paddat];

paddat = data(nshift:-1:1,:);

data = [paddat; data];

% execute
% -------------------------------------------
for i = [(nshift+1):stepby:(nobs + nshift) (nobs + nshift)]
    % start at nshift to avoid ends; data is padded

    % set up windowed data

    dati = data(i - nshift : i + nshift,:);

    if center_local_data
        dati = scale(dati,1) .* kern;   % center and multiply by kern so data taper towards mean
    else
        dati = dati .* kern;
    end

    % execute: IN DEVELOPMENT: THIS is hard-coded for weighted correlation
    %r = weighted_corrcoef(dati,kern(:,1));
    %r = corrcoef(dati);

    y(i, :) = fhandle(dati);  %r(1,2);

    %y(:,i) = tmpy(nshift+1:nshift+nobs);

end

if stepby == 1
    y = y( (nshift+1):(nobs + nshift) );

else
    indx = [(nshift+1):stepby:(nobs + nshift) (nobs + nshift)];
    y = interp1(indx, y(indx), (nshift+1):(nobs + nshift));

end

end

function y = my_corrcoef(dati)
r = corrcoef(dati);
y = r(1,2);
end


function stirling2_test ( )

%*****************************************************************************80
%
%% STIRLING2_TEST tests STIRLING2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  m = 8;
  n = 8;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'STIRLING2_TEST\n' );
  fprintf ( 1, '  STIRLING2: Stirling numbers of second kind.\n' );
  fprintf ( 1, '  Get rows 1 through %d\n', m );
  fprintf ( 1, '\n' );

  s2 = stirling2 ( m, n );

  for i = 1 : m
    fprintf ( 1, '  %4d', i );
    for j = 1 : n
      fprintf ( 1, '  %6d', s2(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function [] = visBinaryCRBMLearning(RBM);
%------------------------------------------------------------------
%  visBinaryCRBMLearning(RBM);
%------------------------------------------------------------------
% DES

figure(99)
colormap gray
set(99,'name','Learning Convolutional RBM (Binary Visible)');
subplot(221);
imagesc(RBM.auxVars.batchX'); axis image; axis off;
title('Visible Data');

subplot(222);
imagesc(RBM.eVis'); axis image; axis off;
title('Reconstruction');

[c,r,k]=size(RBM.eHid);
subplot(223);
data = reshape(RBM.eHid,r*c,k);
visWeights(data,1);
title('Feature Maps');

[c,r,k]=size(RBM.W);
subplot(224);
data = reshape(RBM.W,r*c,k);
visWeights(data,1);
title('Learned Filters');
drawnow

figure(98)
colormap gray;
set(98,'name','Learning Convolutional RBM (Binary Visible)');
subplot(221);
bar(1:RBM.nFM,RBM.c);  axis square
xlim([1 RBM.nFM]);axis square;
xlabel('Feature Index')
title('Hidden Biases')

subplot(222);
data = squeeze(mean(mean(RBM.eHid0)));
bar(1:RBM.nFM,data);
xlim([1 RBM.nFM]);axis square;
xlabel('Feature Index')
title(sprintf('Mean Hidden Activation\nactual=%g \n target = %g\n',mean(data),RBM.sparsity))
drawnow

subplot(223);
data = reshape(RBM.dW,r*c,k);
visWeights(data); axis image; axis off
title('Weight Gradients')

subplot(224);
bar(1:RBM.nFM,RBM.auxVars.dcSparse); axis square;
xlim([1 RBM.nFM]);axis square;
xlabel('Hidden Feature Index')
title('Sparsenss Offset')
%  title('Hidden Bias Gradients')


function [ n_data, h, a, t ] = owen_values ( n_data )

%*****************************************************************************80
%
%% OWEN_VALUES returns some values of Owen's T function.
%
%  Discussion:
%
%    Owen's T function is useful for computation of the bivariate normal
%    distribution and the distribution of a skewed normal distribution.
%
%    Although it was originally formulated in terms of the bivariate
%    normal function, the function can be defined more directly as
%
%      T(H,A) = 1 / ( 2 * pi ) *
%        Integral ( 0 <= X <= A ) e^(H^2*(1+X^2)/2) / (1+X^2) dX
%
%    In Mathematica, the function can be evaluated by:
%
%      fx = 1/(2*Pi) * Integrate [ E^(-h^2*(1+x^2)/2)/(1+x^2), {x,0,a} ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 December 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real H, a parameter.
%
%    Output, real A, the upper limit of the integral.
%
%    Output, real T, the value of the function.
%
  n_max = 22;

  a_vec = [ ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.1000000000000000E+01, ...
    0.2000000000000000E+01, ...
    0.3000000000000000E+01, ...
    0.1000000000000000E+02, ...
    0.1000000000000000E+03 ];

  h_vec = [ ...
    0.1000000000000000E+01, ...
    0.1000000000000000E+01, ...
    0.1000000000000000E+01, ...
    0.1000000000000000E+01, ...
    0.5000000000000000E+00, ...
    0.5000000000000000E+00, ...
    0.5000000000000000E+00, ...
    0.5000000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.2500000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.1250000000000000E+00, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02, ...
    0.7812500000000000E-02 ];

  t_vec = [ ...
    0.4306469112078537E-01, ...
    0.6674188216570097E-01, ...
    0.7846818699308410E-01, ...
    0.7929950474887259E-01, ...
    0.6448860284750376E-01, ...
    0.1066710629614485E+00, ...
    0.1415806036539784E+00, ...
    0.1510840430760184E+00, ...
    0.7134663382271778E-01, ...
    0.1201285306350883E+00, ...
    0.1666128410939293E+00, ...
    0.1847501847929859E+00, ...
    0.7317273327500385E-01, ...
    0.1237630544953746E+00, ...
    0.1737438887583106E+00, ...
    0.1951190307092811E+00, ...
    0.7378938035365546E-01, ...
    0.1249951430754052E+00, ...
    0.1761984774738108E+00, ...
    0.1987772386442824E+00, ...
    0.2340886964802671E+00, ...
    0.2479460829231492E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    h = 0.0;
    a = 0.0;
    t = 0.0;
  else
    h = h_vec(n_data);
    a = a_vec(n_data);
    t = t_vec(n_data);
  end

  return
end


%*******************************************************
%
% DESCRIPTION:
% 		This script contains many useful constants for GPS
%       and related work.  It should be kept in only
%       one place so that updates are immediately available
%       to all other scripts/functions.
%
% ARGUMENTS:
% 		None, just call this script to place
% 		the constants in your workspace.
%
% OUTPUT:
% 		Variables in your current workspace.
%
% CALLED BY:
% 		Many other codes.
%
% FUNCTIONS CALLED:
% 		None.
%
% MODIFICATIONS:
%       XX-XX-02  :  Jan Weiss - Original
%       07-25-04  :  Jan Weiss - updated header.
%       10-19-04  :  Jan Weiss - Cleanup and added
%                                some conversion factors.
%                 :  See SVN log for further updates.
%
% Colorado Center for Astrodynamics Research
% Copyright 2005 University of Colorado, Boulder
%*******************************************************

% GENERAL CONSTANTS
% =========================================================================
c = 299792458;          %----> Speed of light (meters/s).
Re = 6378137 ;          %----> Earth Radius (meters)
% =========================================================================

% CONVERSION FACTORS
% =========================================================================
Hz2MHz = 1E-6;
MHz2Hz = 1E6;
s2ns = 1E9;
ns2s = 1E-9;
s2micros = 1E6;
micros2s = 1E-6;
s2ms = 1E3;
ms2s = 1E-3;
dtr = pi / 180;
rtd = 180 / pi;
m2cm = 100;
cm2m = 1 / 100;
m2mm = 1000;
mm2m = 1 / 1000;
ft2m = 0.3048;  % Source: http://www.nodc.noaa.gov/dsdt/ucg/
m2ft = 1 / 0.3048;
ns2m = c * ns2s;  % Converts time in nano-sec to distance,
                  % assuming the speed of light.
% =========================================================================

% GNSS SPECIFIC CONSTANTS
% =========================================================================
L1 = 1575.42e6;         %----> Freqs in Hz.
L2 = 1227.60e6;
L5 = 1176.45e6;

L1MHz = 1575.42;        %----> Freqs in MHz.
L2MHz = 1227.60;
L5MHz = 1176.45;

L1GHz = 1.57542;        %----> Freqs in GHz.
L2GHz = 1.22760;
L5GHz = 1.17645;

LAMBDA_L1 = c / L1;     %----> Wavelengths in meters.
LAMBDA_L2 = c / L2;
LAMBDA_L5 = c / L5;

CA_CODE_RATE = 1.023e6; %----> C/A and P code chipping rate in chips/s.
P_CODE_RATE = 10.23e6;

CA_CHIP_PERIOD = 1 / CA_CODE_RATE;   %----> C/A & P code chip periods in s.
P_CHIP_PERIOD = 1 / P_CODE_RATE;

CA_CHIP_LENGTH = c / CA_CODE_RATE;  %----> C/A & P code chip lengths in meters.
P_CHIP_LENGTH = c / P_CODE_RATE;

CA_CODE_LENGTH = 1023;  % chips
% =========================================================================


 function X = nufft2_bilinear(omega, x, K1, K2, n_shift)
%function X = nufft2_bilinear(omega, x, K1, K2, n_shift)
%	in:
%		omega	[M 2]		frequencies in radians
%		x	[N1 N2]		image dimensions
%		J			# of neighbors used
%		K1,K2			FFT sizes (should be > N1,N2)
%		n_shift [2]		n = 0-n_shift to N-1-n_shift
%	out:
%		X	[M 1]	DTFT2 at omega, approximated by bilinear interp
%
%	Like fft(), this expects the signals to be x(0,0), ...
%	Use n_shift = [N1/2 N2/2] for x(-N1/2,-N2/2), ...
%
% Copyright 2001-9-20, Jeff Fessler, University of Michigan

warn('This function is obsolete.  Use nufft_init with the "linear" option')

% if no arguments, then run a simple test
if nargin < 1
	N1 = 4;
	N2 = 8;
	n_shift = [2.7 3.3];
	x = [[1:N1]'*ones(1,3), ones(N1,N2-3)]; % test signal
%	x = zeros(N1,N2); x(1,1) = 1;
	if 0	% test with uniform frequency locations
		o1 = 2 * pi * [0:(N1-1)]' / N1;
		o2 = 2 * pi * [0:(N2-1)]' / N2;
		[o1, o2] = ndgrid(o1, o2);
		omega = [o1(:) o2(:)];
	else	% nonuniform frequencies
		o1 = [0 7.2 2.6 3.3]';
		o2 = [0 4.2 -1 5.5]';
		omega = [o1(:) o2(:)];
	end
	Xd = dtft2(x, omega, n_shift);
	Xb = nufft2_bilinear(omega, x, 2*N1, 2*N2, n_shift);
%	Xb = reshape(Xb, N1, N2)
%	disp([Xd Xb Xb-Xd])
	help(mfilename)
	disp(sprintf('max %% difference = %g', max_percent_diff(Xd,Xb)))
	return
end

if ~isvar('n_shift') || isempty(n_shift), n_shift = [0 0]; end
if ~isvar('useloop') || isempty(useloop), useloop = 0; end

M = size(omega,1);
[N1 N2] = size(x);

koff1 = bilinear_offset(omega(:,1), K1);	% [M 1] nearest
koff2 = bilinear_offset(omega(:,2), K2);	% [M 1] nearest

% indices into oversampled FFT components
k1 = mod(outer_sum(koff1, [0 1]), K1) + 1;	% [M 2] {1,...,K1}
k2 = mod(outer_sum(koff2, [0 1]), K2) + 1;	% [M 2] {1,...,K2}

% 1D interpolation coefficient vectors
[u1l u1r] = interp_coef(omega(:,1), koff1, K1, N1);	% [M 1]
[u2l u2r] = interp_coef(omega(:,2), koff2, K2, N2);	% [M 1]

% precorrect for bilinear interpolation filtering
if 0
	warning 'applying precorrection'
	[i1, i2] = ndgrid([0:(N1-1)]-n_shift(1), [0:(N2-1)]-n_shift(2));
	tmp = (nufft_sinc(i1 / K1) .* nufft_sinc(i2 / K2)).^2;
	printm('sinc correction min=%g', min(tmp(:)))
	x = x ./ tmp;
end

% FFT and bilinear interpolation
% can't use interp2 here because we want periodic end conditions!
% oh, maybe i could with a little padding of ends...
Xk = fft2(x, K1, K2);
kk11 = k1(:,1) + (k2(:,1) - 1) * K1;
kk21 = k1(:,2) + (k2(:,1) - 1) * K1;
kk12 = k1(:,1) + (k2(:,2) - 1) * K1;
kk22 = k1(:,2) + (k2(:,2) - 1) * K1;
t1 = u1l .* Xk(kk11) + u1r .* Xk(kk21);
t2 = u1l .* Xk(kk12) + u1r .* Xk(kk22);
X = u2l .* t1 + u2r .* t2;

% apply phase shift
phase = exp(i * (omega * n_shift(:)));	% [M 1]
X = X .* phase;

% index from 0 to K-1 (will modulo later)
function koff = bilinear_offset(om, K)
gam = 2*pi/K;
koff = floor(om / gam);

% make 1D interpolation coefficient vectors
function [ul, ur] = interp_coef(om, koff, K, N)
gam = 2*pi/K;
ul = 1 - (om/gam - koff);
ur = 1 - ul;
ul = ul .* kphase(om - koff * gam, N);
ur = ur .* kphase(om - (koff+1) * gam, N);

% natural phase function. trick: force it to be 2pi periodic
function ph = kphase(om, N)
ph = exp(-i * mod0(om,2*pi) * (N-1)/2);


% add the path of RBM code
addpath('..');
addpath('~/work/Algorithms/liblinear-1.7/matlab');

% load MNIST
load 'mnist_14x14.mat';

% shuffle the training data
X_labels = X_labels + 1;
X_test_labels = X_test_labels + 1;

perm_idx = randperm (size(X,1));

n_all = size(X, 1);
n_train = ceil(n_all * 3 / 4);
n_valid = floor(n_all /4);

X_valid = X(perm_idx(n_train+1:end), :);
X_valid_labels = X_labels(perm_idx(n_train+1:end));
X = X(perm_idx(1:n_train), :);
X_labels = X_labels(perm_idx(1:n_train));

layers = [size(X,2), 500, 500, 10];
n_layers = length(layers);
blayers = [1, 1, 1, 1];

use_tanh = 0;
do_pretrain = 1;

if do_pretrain
    Ds = cell(n_layers - 2, 1);
    H = X;
    H_valid = X_valid;

    for l = 1:n_layers-2
        % construct DAE and use default configurations
        D = default_dae (layers(l), layers(l+1));

        D.data.binary = blayers(l);
        D.hidden.binary = blayers(l+1);

        if use_tanh
            if l > 1
                D.visible.use_tanh = 1;
            end
            D.hidden.use_tanh = 1;
        else
            if D.data.binary
                mH = mean(H, 1)';
                D.vbias = min(max(log(mH./(1 - mH)), -4), 4);
            else
                D.vbias = mean(H, 1)';
            end
        end

        D.learning.lrate = 1e-1;
        D.learning.lrate0 = 5000;
        D.learning.minibatch_sz = 128;

        D.noise.drop = 0.2;
        D.noise.level = 0;

        %D.adagrad.use = 1;
        %D.adagrad.epsilon = 1e-8;
        D.adadelta.use = 1;
        D.adadelta.epsilon = 1e-8;
        D.adadelta.momentum = 0.99;

        D.valid_min_epochs = 10;

        if blayers(l+1)
            D.cae.cost = 0.01;
            %D.sparsity.target = 0.1;
            %D.sparsity.cost = 0.01;
        end

        D.iteration.n_epochs = 500;

        % save the intermediate data after every epoch
        D.hook.per_epoch = {@save_intermediate, {sprintf('dae_mnist_%d.mat', l)}};

        % print learining process
        D.verbose = 0;
        % display the progress
        D.debug.do_display = 0;

        % train RBM
        fprintf(1, 'Training DAE (%d)\n', l);
        tic;
        D = dae (D, H, H_valid, 0.1);
        fprintf(1, 'Training is done after %f seconds\n', toc);

        H = dae_get_hidden(H, D);
        H_valid = dae_get_hidden(H_valid, D);

        Ds{l} = D;
    end
end

M = default_mlp (layers);

M.output.binary = blayers(end);
M.hidden.use_tanh = use_tanh;

M.valid_min_epochs = 10;
M.dropout.use = 1;

M.hook.per_epoch = {@save_intermediate, {'mlp_mnist.mat'}};

M.learning.lrate = 1e-3;
M.learning.lrate0 = 5000;
M.learning.minibatch_sz = 128;

M.adadelta.use = 1;
M.adadelta.epsilon = 1e-8;
M.adadelta.momentum = 0.99;

M.noise.drop = 0;
M.noise.level = 0;

M.iteration.n_epochs = 100;

if do_pretrain
    for l = 1:n_layers-2
        M.biases{l+1} = Ds{l}.hbias;
        M.W{l} = Ds{l}.W;
    end
end

fprintf(1, 'Training MLP\n');
tic;
M = mlp (M, X, X_labels, X_valid, X_valid_labels, 0.1);
fprintf(1, 'Training is done after %f seconds\n', toc);

[pred] = mlp_classify (M, X_test);
n_correct = sum(X_test_labels == pred);

fprintf(2, 'Correctly classified test samples: %d/%d\n', n_correct, size(X_test, 1));


function theta = moment_estimation( M, default_beta, default_dev )

% Parameter estimates from moment equations
% Note that parameter estimation is bad for small data sets: The
% variances can be negative and the beta's are numerically too large.
%
% Syntax:
%   E = moment_estimation( M, n_dev[, def_b, max_v] )
%
% Input:
%   M       : Matrix with moments from compute_moments.
%
%   def_b   : If beta is estimated to be negative or too large it is
%             replaced by DEF_B
%			  Default: 1
%
%   def_std : If sigma and kappa are estimated to be negative they are
%             replaced by STD_VAL.
%             Default: 0.5
%
%
% Output:
%   theta : L-by-5-by-D matrix where L is the number of levels in the
%           wavelet transform, D is the number of directions and the
%           (l+1)'th row is estimates of
%           mu_l, sigma_l, alpha_l, beta_l, kappa_l
%
% See also: COMPUTE_MOMENTS

% Default values
if ~exist( 'default_beta', 'var' )
    default_beta = 1;
end

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

% Preallocate output
L = size(M, 1) - 1;
theta = zeros( L+1, 5, size(M,3) );

% Indices for beta's, alpha's and kappa's
idx = 1:L;

% sigma's
sigma2 = log(M(:,2,:)/3) - 2*log(M(:,1,:));
theta(:, 2, :) = sqrt( sigma2 );

% mu's
theta(:, 1, :) = log(M(:,1,:)) - sigma2/2;

% TODO: Is there a good way to test if the beta estimate is too large

% beta's
theta(1+idx, 4, :) = ( log(M(1+idx,3,:)) - 2*log(M(1+idx,1,:)) ) ./ ...
    ( log(M(idx,4,:)) - log(M(1+idx,1,:)) - log(M(idx,1,:)) );

% Restrict large beta's
beta_idx = false( size(theta) );
beta_idx(1+idx, 4, :) = true;

positive_idx = real(theta) > 2;
negative_idx = real(theta) < 0;

theta( beta_idx & (positive_idx | negative_idx) ) = default_beta;

% alpha's
theta(1+idx, 3, :) = theta(1+idx, 1, :) - theta(1+idx, 4, :).*theta(idx, 1, :);

% kappa's
kappa2 = sigma2(1+idx, :, :) - theta(1+idx, 4, :).^2 .* sigma2(idx, :, :);
theta(1+idx, 5, :) = sqrt( kappa2 );

% ----------------------------------------------------------------------
%
% If any of the variances are estimated to be negative, we estimate
% parameters in the homogeneous GLG model

ind = find( imag(theta) );
[~, ~, D] = ind2sub( size(theta), ind );
D = unique( D );

if isempty(D)
    return
end

mu    = theta(1,1,:);
sigma = theta(1,2,:);
alpha = theta(2,3,:);
beta  = theta(2,4,:);
kappa = theta(2,5,:);

% Default values
sigma( imag(sigma) ~= 0 ) = default_dev;
kappa( imag(kappa) ~= 0 ) = default_dev;
alpha( imag(alpha) ~= 0 ) = -default_beta;
beta( imag(beta) ~= 0 )   = default_beta;

homogeneous_theta = homogeneous_to_full_GLG( L+1, mu, sigma, alpha, beta, kappa );
theta(:,:,D) = homogeneous_theta(:,:,D);

end


function [winscores]=mvg_windowbec_fast(img, windows)

if size(img,3)==3
  if isinteger(img)
    grayimage=double(rgb2gray(img))/255.0;
    img=double(img)/255.0;
  else
    grayimage=0.2989*img(:,:,1)+0.5879*img(:,:,2)+0.1140*img(:,:,3);
  end
else
  grayimage=img;
end

gmagth=0.02;%0.025;
nsubbox=3;
nbin=4; % 4, 6 or 8
binids=1:nbin;
binbounds=[1:2:2*nbin]/(2*nbin)*180;
binbounds=binbounds/180*pi;
bincenters=[0:2:(2*nbin-1)]/(2*nbin)*180;
bincenters=bincenters/180*pi;
binvects=[cos(bincenters);sin(bincenters)];

nr=size(grayimage,1);
nc=size(grayimage,2);
%edgeim=edge(grayimage,'canny');
[X,Y]=meshgrid(1:nc,1:nr);

[gv,Ngv,gvx,gvy]=gaussianderiv1D_(1);
Gx=zeros(nr,nc);
Gy=zeros(nr,nc);
if size(img,3)>1
  for cdim=1:size(img,3)
    Gxc=conv2(gv,gvx,img(:,:,cdim),'same');
    Gindicator=logical(abs(Gxc)>abs(Gx));
    Gx=Gindicator.*Gxc+~Gindicator.*Gx;
    Gyc=conv2(gvy,gv,img(:,:,cdim),'same');
    Gindicator=logical(abs(Gyc)>abs(Gy));
    Gy=Gindicator.*Gyc+~Gindicator.*Gy;
  end
else
  Gx=conv2(gv,gvx,grayimage,'same');%.*edgeim;
  Gy=conv2(gvy,gv,grayimage,'same');%.*edgeim;
end

Gx(1:Ngv,:)=0;Gx((nr-Ngv+1):nr,:)=0;
Gx(:,1:Ngv)=0;Gx(:,(nc-Ngv+1):nc)=0;
Gy(1:Ngv,:)=0;Gy((nr-Ngv+1):nr,:)=0;
Gy(:,1:Ngv)=0;Gy(:,(nc-Ngv+1):nc)=0;

Gmag=sqrt(Gx.^2+Gy.^2);
Gxu=Gx./(Gmag+eps);
Gyu=Gy./(Gmag+eps);

[cedge,cannyth]=edge(grayimage,'canny');
[cedge,cannyth]=edge(grayimage,'canny',0.5*cannyth);

cedgeint=integralimage(double(cedge));

Gvalid=conv2(gv,gv,double(cedge).*(Gmag>gmagth),'same');

Gxvalid=Gx.*Gvalid;
Gyvalid=Gy.*Gvalid;

Gs(:,:,1)=Gxvalid;
Gs(:,:,2)=Gyvalid;
%keyboard
%figure;sc(Gs,'flow');
%keyboard

oribins=zeros(nr,nc,nbin);
tmp=zeros(nr,nc);
gvs=gaussianderiv1D_(1.5);
oribinintim=zeros(nr,nc,nbin);
for i=1:nbin
  tmp=Gmag.*Gvalid.*reshape(abs([Gxu(:) Gyu(:)]*binvects(:,i)),nr,nc);
  tmp=conv2(gvs,gvs,tmp,'same');
  oribins(:,:,i)=tmp;
  %oribinintim(:,:,i)=integralimage(tmp);
  %figure;sc(tmp);
end
oribinsum=sum(oribins,3);
oribinsummax=max(oribinsum(:));
oribins=oribins/oribinsummax;
oribinsum=oribinsum/oribinsummax;
for i=1:nbin
  oribinintim(:,:,i)=integralimage(oribins(:,:,i));
  %figure;sc(tmp);
end

wincx=0.5*(windows(:,1)+windows(:,3));
wincy=0.5*(windows(:,2)+windows(:,4));
nw=size(windows,1);

%for i=1:4
  winhists=zeros(2*nsubbox,nbin,2*nsubbox);
  winhistsums=zeros(2*nsubbox,2*nsubbox);
%end

winscores=zeros(nw,1);
winareas=zeros(nw,1);
nedgepixels=zeros(nw,1);

onesv=ones(2*nsubbox+1,1);
onesh=ones(1,2*nsubbox+1);

subboxind=1:(2*nsubbox);
intersection=0;
suma=0;sumb=0;

BoxWeights=[ones(1,6); 1 0.5*ones(1,4) 1; 1 0.5 0 0 0.5 1];
BoxWeights=[BoxWeights;flipud(BoxWeights)];
BoxIndices=[2 3*ones(1,4) 4; 1 2 3 3 4 1; 1 1 2 4 1 1;1 1 4 2 1 1; ...
	    1 4 3 3 2 1; 4 3*ones(1,4) 2];

winscores=bescores(windows,oribinintim,nbin,nsubbox,BoxWeights,BoxIndices);
%fprintf('BE scores computed in C \n');
if 0
for i=1:nw
  %fprintf('%d / %d \n',i,nw);
  xa=windows(i,1);ya=windows(i,2);
  xb=windows(i,3);yb=windows(i,4);
  xaa=max(1,xa-1);yaa=max(1,ya-1);

  wi=xb-xa+1;
  hi=yb-ya+1;
  winareas(i,1)=wi*hi;
  if wi<(2*nsubbox) | hi<(2*nsubbox)
    continue;
  end

  xs=[-0.5:1/(2*nsubbox):0.5]*wi;
  ys=[-0.5:1/(2*nsubbox):0.5]*hi;
  xsr=ceil(xs+wincx(i));
  ysr=ceil(ys+wincy(i))';

  Xstart=max(1,onesv*xsr-1);
  Ystart=max(1,ysr*onesh-1);

  for j=subboxind
    for k=subboxind
      for l=binids
	winhists(j,l,k)=oribinintim(Ystart(j+1,k+1),Xstart(j+1,k+1),l)+oribinintim(Ystart(j,k),Xstart(j,k),l)-oribinintim(Ystart(j+1,k),Xstart(j+1,k),l)-oribinintim(Ystart(j,k+1),Xstart(j,k+1),l);
      end
      winhistsum(j,k)=sum(winhists(j,:,k));
    end
  end

  for j=1:(2*nsubbox)
    for k=1:(2*nsubbox)
      winscores(i,1)=winscores(i,1)+BoxWeights(j,k)*winhists(j,BoxIndices(j,k),k);
    end
  end

end
end
winscoremax=max(winscores);
winscores=(winscores./winscoremax);
%winscoremaxtmp=max(winscorestmp);
%winscorestmp=(winscorestmp./winscoremaxtmp);

%keyboard
%%%%%%%%%%%%%%%%%%%%%%%%
% Additional functions %
%%%%%%%%%%%%%%%%%%%%%%%%

function [g,N,gx,gy]=gaussianderiv1D_(sigma,N)

if nargin<2
  N=4*sigma+1;
end

% make sure that N is odd
N=2*floor(N/2)+1;
sigma2=sigma^2;

t=1:N;
mu=(N-1)/2+1;
g=1/(sqrt(2*pi)*sigma)*exp(-0.5*(t-mu).^2/sigma2);
gx=-(t-mu)/sigma2.*g;
gy=gx';


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% function B = getElasticMatrixStg(omega,m,mu,lambda)
%
% Builds the elasticity matrix B for a domain defined by omega
% and a staggered grid discretization defined by m:
%
%     | a\nabla       0        0 |
% B = |       0 a\nabla        0 |
%     |       0       0  a\nabla |
%     |           b \div         |,
%
% where \nabla and \div are weighted by a=\sqrt(mu) and b=\sqrt(\lambda+\mu),
% respectively; defaults: \mu = 1, \lambda = 0.
% Note: the matrices are simple, it is size that matters.
%
% =============================================================================

function B = getElasticMatrixStg(omega,m,mu,lambda)
if nargin == 0,
  help(mfilename);
  runMinimalExample;
  B = 'endOfMinimalExample';
  return;
end;

                                        % set defautls
if ~exist('mu','var'),     mu     = [];  end;
if ~exist('lambda','var'), lambda = [];  end;
if isempty(mu),
  warning('mu has not yet been set! set mu=1');
  mu = 1;
end;
if isempty(lambda),
  warning('lambda has not yet been set! lambda=0');
  lambda = 0;
end;

dim = length(omega)/2;
h   = (omega(2:2:end)-omega(1:2:end))./m; % voxel size for integration
a   = sqrt(mu);
b   = sqrt(mu+lambda);

% for the dimension ms(i) of the i-th staggered grid, the i-th component
% of of m=[m(1),m(2),m(3)] has to be increased by one; the i-th staggered
% grid contains ns(i) points

ms  = @(i)   m + ((1:dim)==i);
ns  = @(i)   prod(ms(i));
% Example: m=(2,3), ms(1) = (3,3); ms(2)=(2,4), ns(1) = 9, ns(2) = 8.

% the dimension of \partial^{h,i}_k is p(i,k)-by-ns(i),
% where q(i)=prod(ms(i)) is the length of the i-th staggered grid
% and p(i,k) is the product of a modified m:
%   m(i) has to be increase by one and
%   m(k) has to be decrase  by one
p = @(i,k) prod(m + ((1:dim)==i) - ((1:dim==k)));

% Example:
% p(1,1) = 2*3         = 6,   p(1,2) = (2+1)*(3-1) = 6, ns(1) = 9
% p(2,1) = (2-1)*(3+1) = 4,   p(2,2) = 2*3         = 6, ns(2) = 8

% E is a q-by-q identity matrix, where q=m(k)+1 for i==k and q=m(k) (i~=k)
E = @(i,k) speye(m(k)+(i==k));

% Example:
% E(1,1) = speye(m(1)+1) = speye(3)
% E(1,2) = speye(m(2))   = speye(3)
% E(2,1) = speye(m(1))   = speye(2)
% E(2,2) = speye(m(2)+1) = speye(4)

% D is the 1D derivative matrix of size p-by-(p+1)
%        |-1 1     |
% D = |  .  .   |/h(k), where p=m(k) (i==k) and p=m(k)-1 (i~=k)
%        |     -1 1|

D = @(i,k) spdiags(ones(m(k)-(i~=k),1)*[-1,1],...
  [0,1],m(k)-(i~=k),m(k)+(i==k))/h(k);

% Example:
% D(1,1) is m(1)-by-(m(1)+1) = 2-by-3
% D(1,2) is (m(2)-1)-by-m(2) = 2-by-3
% D(2,1) is (m(1)-1)-by-m(1) = 1-by-2
% D(2,2) is m(2)-by-(m(2)+1) = 3-by-4

switch dim
  case 2
    % build the 2D elasticity operator
    %
    %      | a\nabla 0      |   |a D11  0     |
    %  B = |                | = |a D12  0     |
    %      | 0      a\nabla |   |0      a D21 |
    %      |                |   |0      a D22 |
    %      | b Div1 b Div2  |   |b D11  b D22 |

    D11 = kron(E(1,2),D(1,1));  D12 = kron(D(1,2),E(1,1));
    D21 = kron(E(2,2),D(2,1));  D22 = kron(D(2,2),E(2,1));

    % Example:
    % D11 = 3-by-3 \otimes 2-by-3 = 6-by-9
    % D12 = 2-by-3 \otimes 3-by-3 = 6-by-9
    % D21 = 4-by-4 \otimes 1-by-2 = 4-by-8
    % D22 = 3-by-4 \otimes 2-by-2 = 6-by-8

    B = [  a*D11,sparse(p(1,1),ns(2));
           a*D12,sparse(p(1,2),ns(2));
           sparse(p(2,1),ns(1)),a*D21;
           sparse(p(2,2),ns(1)),a*D22;
           b*D11,b*D22];
    % Example: B = 28-by-17
  case 3
    % build the 3D elasticity operator
    %
    %      |a D11  0      0    |
    %  B = |a D12  0      0    |
    %      |a D13  0      0    |
    %      |0      a D21  0    |
    %      |0      a D22  0    |
    %      |0      a D23  0    |
    %      |0      0      a D31|
    %      |0      0      a D32|
    %      |0      0      a D33|
    %      |b D11  b D22  b D33|

    % Example: m=[2,3,4] ns = (36,32,30)
    D11 = kron(kron(E(1,3),E(1,2)),D(1,1));
    D12 = kron(kron(E(1,3),D(1,2)),E(1,1));
    D13 = kron(kron(D(1,3),E(1,2)),E(1,1));
    D21 = kron(kron(E(2,3),E(2,2)),D(2,1));
    D22 = kron(kron(E(2,3),D(2,2)),E(2,1));
    D23 = kron(kron(D(2,3),E(2,2)),E(2,1));
    D31 = kron(kron(E(3,3),E(3,2)),D(3,1));
    D32 = kron(kron(E(3,3),D(3,2)),E(3,1));
    D33 = kron(kron(D(3,3),E(3,2)),E(3,1));

    % Example:
    % D11 = kron(kron(4-by-4,3-by-3),2-by-3) = 24-by-36
    % D12 = kron(kron(4-by-4,2-by-3),3-by-3) = 24-by-36
    % D13 = kron(kron(3-by-4,3-by-3),3-by-3) = 27-by-36
    % D21 = kron(kron(4-by-4,4-by-4),1-by-2) = 16-by-32
    % D22 = kron(kron(4-by-4,3-by-4),2-by-2) = 24-by-32
    % D23 = kron(kron(3-by-4,4-by-4),2-by-2) = 24-by-32
    % D31 = kron(kron(5-by-5,3-by-3),1-by-2) = 15-by-30
    % D32 = kron(kron(5-by-5,2-by-3),2-by-2) = 20-by-30
    % D32 = kron(kron(4-by-5,3-by-3),2-by-2) = 24-by-30

    B = [
      a * D11, sparse(p(1,1),ns(2)), sparse(p(1,1),ns(3)); ...
      a * D12, sparse(p(1,2),ns(2)), sparse(p(1,2),ns(3)); ...
      a * D13, sparse(p(1,3),ns(2)), sparse(p(1,3),ns(3)); ...
      sparse(p(2,1),ns(1)), a * D21, sparse(p(2,1),ns(3)); ...
      sparse(p(2,2),ns(1)), a * D22, sparse(p(2,2),ns(3)); ...
      sparse(p(2,3),ns(1)), a * D23, sparse(p(2,3),ns(3)); ...
      sparse(p(3,1),ns(1)), sparse(p(3,1),ns(2)), a * D31; ...
      sparse(p(3,2),ns(1)), sparse(p(3,2),ns(2)), a * D32; ...
      sparse(p(3,3),ns(1)), sparse(p(3,3),ns(2)), a * D33; ...
      b * D11,              b * D22,              b * D33
      ];
    % Example:
    % B = [75-by-36    0      0
    %         0     64-by-32  0
    %         0        0     59-by-30
    %      24-by-36 24-by-32 24-by-30 ]
    %   = 228-by-98

  otherwise,  error('nyi');
end;
%------------------------------------------------------------------------------
function runMinimalExample

omega = [0,1,0,2,0,3]; m = [4,5,6];
B2 = getElasticMatrixStg(omega(1:4),m(1:2),1,0);
B3 = getElasticMatrixStg(omega,m,1,0);
figure(1); clf;
subplot(1,2,1); spy(B2); title(sprintf('%s: B2',mfilename));
subplot(1,2,2); spy(B3); title(sprintf('%s: B3',mfilename));
%==============================================================================


function [rho,rhou,rhov,Ener] = ChannelIC2D(x, y, time);

%  function [Q] = ChannelIC2D(x, y, time)
%  Purpose: Impose uniform plane flow

mu = 1e-2; pbar = 10; gamma = 1.5;

rho  = 1;
rhou = y.^2;
rhov = 0;
Ener = (2*mu*x + pbar)/(gamma-1) + .5*(y.^4);
return


%isiscalar - Test if parameter is a scalar (integer) satisfying an optional list of tests.
%
%  USAGE
%
%    test = isiscalar(x,test1,test2,...)
%
%    x              parameter to test
%    test1...       optional list of additional tests
%
%  EXAMPLES
%
%    % Test if x is a scalar (double)
%    isiscalar(x)
%
%    % Test if x is a strictly positive scalar (double)
%    isiscalar(x,'>0')
%
%    % Test if x is a scalar (double) included in [2,3]
%    isiscalar(x,'>=2','<=3')
%
%  NOTE
%
%    The tests ignore NaN, e.g. isiscalar(nan), isiscalar(nan,'>0') and isiscalar(nan,'<=0')
%    all return 1.
%
%  SEE ALSO
%
%    See also isdmatrix, isdvector, isdscalar, isimatrix, isivector, isstring,
%    islscalar, islvector, islmatrix.
%

% Copyright (C) 2010 by Michaël Zugaro
%
% 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.

function test = isiscalar(x,varargin)

% Check number of parameters
if nargin < 1,
  error('Incorrect number of parameters (type ''help isiscalar'' for details).');
end

% Test: double, scalar
test = isa(x,'double') & isscalar(x);
if ~test, return; end

% Test: integers?
test = test & round(x)==x;

% Optional tests
for i = 1:length(varargin),
	try
		if ~eval(['x' varargin{i} ';']), test = false; return; end
	catch err
		error(['Incorrect test ''' varargin{i} ''' (type ''help isiscalar'' for details).']);
	end
end


function tec_io_test02 ( )

%*****************************************************************************80
%
%% TEC_IO_TEST02 tests TEC_WRITE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2006
%
%  Author:
%
%    John Burkardt
%
  tec_file_name = 'tiny.dat';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEC_IO_TEST02\n' );
  fprintf ( 1, '  TEC_WRITE can write finite element data to a TECPLOT ASCII file.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  In this example, we will write data to "%s".\n', tec_file_name );

  dim_num = 2;
  node_num = 5;
  element_num = 3;
  element_order = 3;
  node_data_num = 2;
  node_coord = [ ...
    0.0, 0.0; ...
    1.0, 0.0; ...
    2.0, 0.0; ...
    0.0, 1.0; ...
    1.0, 1.0 ]';
  element_node = [ ...
    1, 2, 4;
    5, 4, 2;
    2, 3, 5 ]';
  node_data = [ ...
    1.0, 0.0; ...
    0.8, 0.2; ...
    0.6, 0.4; ...
    0.9, 0.1; ...
    0.5, 0.5 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension         = %d\n', dim_num );
  fprintf ( 1, '  Number of nodes           = %d\n', node_num );
  fprintf ( 1, '  Number of elements        = %d\n', element_num );
  fprintf ( 1, '  Element order             = %d\n', element_order );
  fprintf ( 1, '  Number of node data items = %d\n', node_data_num );

  r8mat_transpose_print ( dim_num, node_num, node_coord, ...
    '  Coordinates of nodes:' );

  i4mat_transpose_print ( element_order, element_num, element_node, ...
    '  Nodes of elements:' );

  r8mat_transpose_print ( node_data_num, node_num, node_data, ...
    '  Node data for nodes:' );

  tec_write ( tec_file_name, dim_num, node_num, element_num, ...
    element_order, node_data_num, node_coord, element_node, node_data );

  return
end


function varargout = tucker(f)
%TUCKER   SLICE-TUCKER expansion of a CHEBFUN3 object.
%   [CORE, COLS, ROWS, TUBES] = TUCKER(F) returns the core tensor CORE and
%   the three factor quasimatrices COLS, ROWS, and TUBES in the low-rank
%   representation of a CHEBFUN3 object F. The factor quasimatrices are of
%   size Inf-by-length(F, 1), Inf-by-length(F, 2) and Inf-by-length(F, 3),
%   respectively and we have
%
%   F(x,y,z) = CORE x_1 COLS(x,:,:) x_2 ROWS(:,y,:) x_3 TUBES(:,:,z).
%
%   CORE = TUCKER(F) returns the core tensor used in the construction of F.
%
% See also CHEBFUN2/CDR.

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

if ( isempty(f) )
    varargout = cell(1, nargout);
    return
end

% Get the low rank representation for f.
fCore = f.core;
fCols = f.cols;
fRows = f.rows;
fTubes = f.tubes;

% Output:
if ( nargout <= 1 )
    varargout = {fCore};
else
    % ST decomposition
    varargout = {fCore, fCols, fRows, fTubes};
end

end


% Test file for ADCHEBFUN CUMSUM, DIFF, MEAN and SUM

function pass = test_cumsumDiffSumMean

% List of trigonometric functions to test.
funcList = {@diff, @(u)diff(u, 2), @(u)diff(u, 4), ...
                 @sum, @(u) sum(u, -.25, .8), ...
                 @cumsum, @(u)cumsum(u,2), ...
                 @mean, ...
                 @(u) deriv(u, 0:0.01:0.1, 2)};

% Tolerance for Taylor testing
tolOrder = 1e-2;
tolDiff = 1e-12;
% Initialise vector with pass information
pass = zeros(2, numel(funcList));

% Do the tests.
for k = 1:numel(funcList)
    % Call the valueTesting method, which also returns linearity information
    [err, lin] = adchebfun.valueTesting(funcList{k});

    % First, check that the computed function values match what we expect
    pass(1, k) = ( err == 0 );

    % Call the taylorTesting method
    [order1, order2, nDiff2] = adchebfun.taylorTesting(funcList{k});

    % We expect all elements of ORDER1 to be close to 1. Since the methods being
    % tested in this case are all linear, ORDER2 will be noise. However, since
    % the methods are indeed linear, we should expect nDiff2 to have values all
    % close to machine epsilon, which we can use to check for the correctness of
    % the derivative computed.
    pass(2,k) = ( (max(abs(order1 - 1)) < tolOrder) && ...
        (max(abs(nDiff2)) < tolDiff) );

    % Check that we received the correct linearity information
    pass(3, k) = ( lin == 1 );
end

end


function [s] = gsp_jtv_filter_inverse(G, filter, filtertype,c, param)
%GSP_JTV_FILTER_INVERSE Inverse operator of a joint timve_vertex filterbank
%   Usage:  s = gsp_jtv_filter_inverse(G, filter, c);
%           s = gsp_jtv_filter_inverse(G, filter, c, param);
%
%   Input parameters:
%         G          : Time-vertex Graph structure.
%         filter     : Cell array of time-vertex filters.
%         filtertype : Filter domain (ts,js,ts-array,js-array)
%         c          : Transform coefficients
%         param      : Optional parameter
%   Output parameters:
%         signal     : sythesis signal
%

% Author: Francesco Grassi
% Testing: test_jtv_filter
% Date: September 2016

if nargin<5
    param = struct;
end

[dual_filter,filtertype] = gsp_jtv_design_can_dual(filter,filtertype);

s = gsp_jtv_filter_synthesis(G,dual_filter,filtertype,c,param);

end


function c=projkern(c,p2,p3,p4,p5);
%PROJKERN  Projection onto generating kernel space
%   Usage:  cout=projkern(cin,a);
%           cout=projkern(cin,g,a);
%           cout=projkern(cin,ga,gs,a);
%
%   Input parameters:
%         cin   : Input coefficients
%         g     : analysis/synthesis window
%         ga    : analysis window
%         gs    : synthesis window
%         a     : Length of time shift.
%   Output parameters:
%         cout  : Output coefficients
%
%   `cout=projkern(cin,a)` projects a set of Gabor coefficients *c* onto the
%   space of possible Gabor coefficients. This means that *cin* and *cout*
%   synthesize to the same signal. A tight window generated from a Gaussian
%   will be used for both analysis and synthesis.
%
%   The rationale for this function is a follows: Because the coefficient
%   space of a Gabor frame is larger than the signal space (since the frame
%   is redundant) then there are many coefficients that correspond to the
%   same signal.
%
%   Therefore, you might desire to work with the coefficients *cin*, but you
%   are in reality working with *cout*.
%
%   `cout=projkern(cin,g,a)` does the same, using the window *g* for analysis
%   and synthesis.
%
%   `cout=projkern(cin,ga,gs,a)` does the same, but for different analysis
%   *ga* and synthesis *gs* windows.
%
%   See also: dgt, idgt

complainif_argnonotinrange(nargin,2,4,mfilename);

M=size(c,1);
N=size(c,2);

if nargin==2
  a=p2;
  L=a*N;
  ga=gabtight(a,M,L);
  gs=ga;
end;

if nargin==3;
  ga=p2;
  gs=p2;
  a=p3;
  L=a*N;
end;

if nargin==4;
  ga=p2;
  gs=p3;
  a=p4;
  L=a*N;
end;

assert_squarelat(a,M,1,'PROJKERN');

c=dgt(idgt(c,gs,a),ga,a,M);


function cc=lpczz2cc(zz,np)
%LPCZZ2CC Convert poles to "complex" cepstrum CC=(ZZ,NP)

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: lpczz2cc.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p]=size(zz);
if (nargin < 2) np=p; end
cc=zeros(nf,np);
yy=zz.';
if p<2			% special case 'cos sum() is weird
  cc(:,1)=real(zz);
  for k=2:np
    yy=yy.*zz.';
    cc(:,k)=real(yy).'/k;
  end
else
  cc(:,1)=sum(real(yy)).';
  for k=2:np
    yy=yy.*zz.';
    cc(:,k)=sum(real(yy)).'/k;
  end
end


figure;

%% Cartesian
% subplot(1,2,1);
% hold on;
% truth_100 = truth.*100;
% %h1 = plot(ax(1),measurement(1,k),measurement(2,k),'k*','MarkerSize', 10);
% meas = 100*obs.heval_inv([measurement(1,:);measurement(2,:)]);
% h1 = plot(meas(obs.Mapping(1),:),meas(obs.Mapping(2),:),'r+','MarkerSize', 15);
% h2 = plot(truth_100(1,1:end),truth_100(2,1:end),'k--','LineWidth',2);
% h3 = plot(truth_100(1,1),truth_100(2,1),'ko','MarkerSize', 20, 'MarkerFaceColor','Green');
% h4 = plot(truth_100(1,end),truth_100(2,end),'ko','MarkerSize', 20, 'MarkerFaceColor','Red');
% h5 = plot(0,0,'rd','MarkerSize', 20, 'MarkerFaceColor','Blue');
% legend([h1,h2,h3,h4,h5],'Measurements','Trajectory', 'Start', 'End','Radar','Location','southeast')
% str = sprintf('Vessel trajectory');
% title(str)
% xlabel('X (m)')
% ylabel('Y (m)')
% axis([0,2500,0,1500])
% box on

%% Polar
% subplot(1,2,2);
% truth2meas = obs.heval(truth_100(1:2,:));
% % [a,b]=obs.heval(truth_100(1:2,:));
% %h1 = polarplot(a(x_start),b(y_start),'ko','LineWidth',2,'MarkerSize',15,'MarkerFaceColor','w');hold on;
% polarplot(truth2meas(1,:),truth2meas(2,:),'k-','LineWidth',2,'MarkerSize',10,'MarkerFaceColor','w');hold on;
% thetalim([0 90])
% %h2 = polarplot(a(x_end),b(y_end),'k^','LineWidth',2,'MarkerSize',15,'MarkerFaceColor','w');hold on;
% h3 = polarplot(measurement(1,:),100*measurement(2,:),'+r','MarkerSize',15);
% pax = gca;
% pax.ThetaAxisUnits = 'radians';
% %rlabel('Range (m)');
% %thetalabel('Bearing (rad)');


function G = curl(F)
%CURL Curl of a BALLFUNV.
%   G = CURL(F) returns the BALLFUNV of the curl of F.
%
% See also DIV.

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

if isempty( F )
    G = ballfunv();
    return
end

% Extract the components of a BALLFUNV:
Fc = F.comp;

% Formula for 3D curl
G = [ diff(Fc{3},2) - diff(Fc{2},3); ...
      diff(Fc{1},3) - diff(Fc{3},1); ...
      diff(Fc{2},1) - diff(Fc{1},2) ];
end


function merged_sample = merge_samples(sample1, sample2, w1, w2, sample_merge_type)
% Merge sample1 and sample2 using weights w1 and w2. The type of merging is
% decided by sample_merge_type.
% The sample_merge_type can be
% 1) Merge: The output is the weighted sum of the input samples
% 2) Replace: The output is the first sample. ie w2 is assumed to be 0

% Normalise the weights so that they sum to one
alpha1 = w1/(w1+w2);
alpha2 = 1 - alpha1;

% Build the merged sample
if strcmpi(sample_merge_type, 'replace')
    merged_sample = sample1;
elseif strcmpi(sample_merge_type, 'merge')
    num_feature_blocks = numel(sample1);

    merged_sample = cell(1, 1, num_feature_blocks);
    for k = 1:num_feature_blocks
        merged_sample{k} = alpha1*sample1{k} + alpha2*sample2{k};
    end
else
    error('Invalid sample merge type');
end


function Sp = querytp(S, U, domain, p)
%QUERYTP Query TP model at a given parameter point
%	Sp = QUERYTP(S, U, domain, p)
%
%	S        - core tensor of the TP model
%	U        - (discrete) weight function data of the TP model
%	domain   - intervals for each dimension
%	p        - parameter point where we query the model
%
%	Sp       - system at p (linear interpollated weights)

% TODO: method param to interp1 (spline,..)

W = queryw1(U, domain, p);
Sp = squeeze(tprod(S, W));


function [X,Y,U] = CollectData(SimPar,IC1,IC2,f1,f2,umin,umax,Ntraj,SimLength)
% Collect data for Burgers equation
disp('Starting data collection ...')

% random input
Ubig= rand(2,SimLength,Ntraj)* (umax-umin)+umin;

% Transition mapping of the controleld dynamical system
% control is a linear combination of u1 and u2
f = @(x,u)(BurgerSolver(x,u(1)*f1+u(2)*f2,SimPar));

% initialize
X = []; Y = []; U=[];

% loop pver trajectories
for i = 1:Ntraj
    xx = [];
    % Intial state is a random convex combination of IniitialConditions
    b = rand;
    a = [b,1-b];
    xx =b*IC1 + (1-b)*IC2;
    tic
    fprintf('Trajectory %d out of %d \n',i,Ntraj)
    % loop over each time step
    for j = 1:SimLength
        xx = [xx f(xx(:,end),Ubig(:,j,i))];
        U  = [U,Ubig(:,j,i)];
        % if the solution diverges, go to the next trajectory
        if ~isempty(find(isnan(xx(:,end)),1))
            break
        end

    end
    toc
    % Store
    X = [X xx(:,1:end-1)];
    Y = [Y xx(:,2:end)];

end

save('BurgersTrajectoryData','X','Y','U','SimLength','Ntraj')
end


function [ sindex ] = tournament(pop, index1, index2)
%   Apply tournament selection to row indexed by index1 and index2
%   Vectorization on this function may not give better speed, so beter
%   to keep as it is.

global nreal ;
global nobj ;
global ncon ;

obj_col = nreal+1:nreal+nobj;
cv_col = nreal+nobj+ncon+1;

% check the dominance
flag = check_dominance([pop(index1,obj_col), pop(index1,cv_col)], ...
                       [pop(index2,obj_col), pop(index2,cv_col)]);

cdist1 = pop(index1, end);
cdist2 = pop(index2, end);
if (flag == 1)
    sindex = index1 ;
    return ;
end
if(flag == -1)
    sindex = index2 ;
    return ;
end
if(cdist1 > cdist2)
    sindex = index1 ;
    return ;
end
if(cdist2 > cdist1)
    sindex = index2 ;
    return ;
end
if(rand(1) < 0.5)
% if(randomperc() < 0.5) % SLOW !!!
    sindex = index1 ;
    return ;
else
    sindex = index2 ;
    return ;
end


function f = feature_extract( imdist ,scale)
    f=[];
    imdist=rgb2gray(imdist);
    weight=[0.2 0.8];
    for i=1:scale
        im=imdist;
        fun0=@(x)secal(x);
        emat=blkproc(im,[8 8] ,fun0);

        sort_t = sort(emat(:),'ascend');
        len = length(sort_t);
        t=sort_t(ceil(len*weight(1)):ceil(len*weight(2)));
        mu= mean(t);
        ske=skewness(sort_t);

        f1=[ mu  ske];

        im=imdist;
        fun1=@(x)fecal(x);
        im=double(im);
        femat=blkproc(im,[8 8],fun1);

        sort_t = sort(femat(:),'ascend');
        len = length(sort_t);
        t=sort_t(ceil(len*weight(1)):ceil(len*weight(2)));
        mu= mean(t);
        ske=skewness(sort_t);

        f2=[ mu  ske];

        f=[f f1 f2] ;

        imdist = imresize(imdist,0.5);
    end
end


classdef FollowWall < simiam.controller.Controller

% Copyright (C) 2013, Georgia Tech Research Corporation
% see the LICENSE file included with this software

    properties

        % memory banks
        E_k
        e_k_1

        % gains
        Kp
        Ki
        Kd

        % plot support
        p

        % sensor geometry
        calibrated
        sensor_placement

    end

    properties (Constant)
        inputs = struct('v', 0, 'direction', 'right');
        outputs = struct('v', 0, 'w', 0)
    end

    methods

        function obj = FollowWall()
            obj = obj@simiam.controller.Controller('follow_wall');
            obj.calibrated = false;

            obj.Kp = 2;
            obj.Ki = 0;
            obj.Kd = 0;

            obj.E_k = 0;
            obj.e_k_1 = 0;

%             obj.p = simiam.util.Plotter();
        end

        function outputs = execute(obj, robot, state_estimate, inputs, dt)

            % Compute the placement of the sensors
            if(~obj.calibrated)
                obj.set_sensor_geometry(robot);
            end

            % Unpack state estimate
            [x, y, theta] = state_estimate.unpack();

            % Poll the current IR sensor values 1-5
            ir_distances = robot.get_ir_distances();

            % Interpret the IR sensor measurements geometrically
            ir_distances_wf = obj.apply_sensor_geometry(ir_distances, state_estimate);

            % Compute the heading vector
            d_fw = inputs.d_fw;

            % 1. Select p_2 and p_1, then compute u_fw_t
            if(strcmp(inputs.direction,'right'))
                % Pick two of the right sensors based on ir_distances
                S = [1:3 ; ir_distances(5:-1:3)'];
                [Y,i] = sort(S(2,:));
                S = S(1,i);

                Sp = 5:-1:3;

                S1 = Sp(S(1));
                S2 = Sp(S(2));

                if(S1 < S2)
                    p_1 = ir_distances_wf(:,S2);
                    p_2 = ir_distances_wf(:,S1);
                else
                    p_1 = ir_distances_wf(:,S1);
                    p_2 = ir_distances_wf(:,S2);
                end

            else
                % Pick two of the left sensors based on ir_distances
                S = [1:3 ; ir_distances(1:3)'];
                [Y,i] = sort(S(2,:));
                S = S(1,i);

                if(S(1) > S(2))
                    p_1 = ir_distances_wf(:,S(2));
                    p_2 = ir_distances_wf(:,S(1));
                else
                    p_1 = ir_distances_wf(:,S(1));
                    p_2 = ir_distances_wf(:,S(2));
                end
            end

            u_fw_t = p_2-p_1;

            % 2. Compute u_a, u_p, and u_fw_tp to compute u_fw_p

            u_fw_tp = u_fw_t/norm(u_fw_t);
            u_a = p_1;
            u_p = [x;y];

            u_fw_p = ((u_a-u_p)-((u_a-u_p)'*u_fw_tp)*u_fw_tp);

            % 3. Combine u_fw_tp and u_fw_pp into u_fw;
            u_fw_pp = u_fw_p/norm(u_fw_p);
            u_fw = d_fw*u_fw_tp+(u_fw_p-d_fw*u_fw_pp);

            % Compute the heading and error for the PID controller
            theta_fw = atan2(u_fw(2),u_fw(1));
            e_k = theta_fw-theta;
            e_k = atan2(sin(e_k),cos(e_k));

            e_P = e_k;
            e_I = obj.E_k + e_k*dt;
            e_D = (e_k-obj.e_k_1)/dt;

            % PID control on w
            v = inputs.v;
            w = obj.Kp*e_P + obj.Ki*e_I + obj.Kd*e_D;

            % Save errors for next time step
            obj.E_k = e_I;
            obj.e_k_1 = e_k;

            % plot
%             obj.p.plot_2d_ref(dt, atan2(sin(theta),cos(theta)), theta_fw, 'c');

%             fprintf('(v,w) = (%0.4g,%0.4g)\n', v,w);

            outputs.v = v;
            outputs.w = w;
        end

        % Helper functions

        function ir_distances_wf = apply_sensor_geometry(obj, ir_distances, state_estimate)

            % Apply the transformation to robot frame.

            ir_distances_rf = zeros(3,5);
            for i=1:5
                x_s = obj.sensor_placement(1,i);
                y_s = obj.sensor_placement(2,i);
                theta_s = obj.sensor_placement(3,i);

                R = obj.get_transformation_matrix(x_s,y_s,theta_s);
                ir_distances_rf(:,i) = R*[ir_distances(i); 0; 1];
            end

            % Apply the transformation to world frame.

            [x,y,theta] = state_estimate.unpack();

            R = obj.get_transformation_matrix(x,y,theta);
            ir_distances_wf = R*ir_distances_rf;

            ir_distances_wf = ir_distances_wf(1:2,:);
        end

        function set_sensor_geometry(obj, robot)
            obj.sensor_placement = zeros(3,5);
            for i=1:5
                [x, y, theta] = robot.ir_array(i).location.unpack();
                obj.sensor_placement(:,i) = [x; y; theta];
            end
            obj.calibrated = true;
        end

        function R = get_transformation_matrix(obj, x, y, theta)
            R = [cos(theta) -sin(theta) x; sin(theta) cos(theta) y; 0 0 1];
        end

        function reset(obj)
            % Reset accumulated and previous error
            obj.E_k = 0;
            obj.e_k_1 = 0;
        end

    end

end


clear all; close all;
I=imread('tire.tif');
J=histeq(I);
figure;
subplot(121);
imshow(uint8(I));
subplot(122);
imshow(uint8(J));
figure;
subplot(121);
imhist(I, 64);
subplot(122);
imhist(J, 64);


function x = rot2quat (R, tol)
% Use:  q = rot2quat(R)
%
% Computes the unit quaternion corresponding to the 3x3 rotation matrix R
% tol controls the tolerance to check is R == identity(3)
% Note: quaternion is q = [qw qx qy qz], where qw is the scalar part of q
%
% From Luca Carlone's
% https://bitbucket.org/lucacarlone/pgo3d-duality-opencode/annotate/ebb6e1b8cebaad7f2aaf581b1d0c0bad737faebb/lib/rot2quat.m?at=master&fileviewer=file-view-default

if nargin < 2
  tol =  1.0e-5;
end

if isrot(R, tol) == 0
  s(4) = .5 * sqrt( 1 + R(1,1) + R(2,2) + R(3,3) );
  if norm(s(4)) <= tol
    %disp ('rotation = 180 degrees')
    [u,teta]=rot2uth(R);
    s(1)=u(1);
    s(2)=u(2);
    s(3)=u(3);
  elseif norm(s(4)-1) <= tol
    % disp ('rotation = 0 degrees')
    s(1) = 0;
    s(2) = 0;
    s(3) = 0;
  else
    s(1) = (R(3,2) - R(2,3))/(4 * s(4));
    s(2) = (R(1,3) - R(3,1))/(4 * s(4));
    s(3) = (R(2,1) - R(1,2))/(4 * s(4));
  end
  x(1) = s(4);
  x(2) = s(1);
  x(3) = s(2);
  x(4) = s(3);
else
  disp ('Error in input matrix')
  x=[1 0 0 0]';
end


function [e, g] = gp_eg(w, gp, x, y, varargin)
%GP_EG  Evaluate the energy function (un-normalized negative marginal
%       log posterior) and its gradient
%
%  Description
%    [E, G] = GP_EG(W, GP, X, Y, OPTIONS) takes a Gaussian process
%    structure GP together with a matrix X of input vectors and a
%    matrix Y of targets, and evaluates the energy function E and
%    its gradient G. Each row of X corresponds to one input vector
%    and each row of Y corresponds to one target vector.
%
%    The energy is minus log posterior cost function:
%        E = EDATA + EPRIOR
%          = - log p(Y|X, th) - log p(th),
%    where th represents the parameters (lengthScale,
%    magnSigma2...), X is inputs and Y is observations (regression)
%    or latent values (non-Gaussian likelihood).
%
%    OPTIONS is optional parameter-value pair
%      z - optional observed quantity in triplet (x_i,y_i,z_i)
%          Some likelihoods may use this. For example, in case of
%          Poisson likelihood we have z_i=E_i, that is, expected
%          value for ith case.
%
%  See also
%    GP_E, GP_G
%

% Copyright (c) 2010 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.

% Single function for some optimization routines, no need for mydeal...
e=gp_e(w, gp, x, y, varargin{:});
if nargout>1
  if isnan(e)
    g=NaN;
  else
    g=gp_g(w, gp, x, y, varargin{:});
  end
end


function warped_img = mdlt_warping(height, width, img, Hmdlt, offset, X, Y)

warped_img = zeros(height,width,3);
[imgh,imgw,~] = size(img);

for xidx= 1:width
    for yidx = 1:height

        x_grididx = min(find(X >= xidx,1));
        y_grididx = min(find(Y >= yidx,1));

        grididx = (x_grididx-1) * size(Y,2) + y_grididx;
        h = reshape(Hmdlt(grididx,:),3,3);

        x = xidx - offset(1) + 1;
        y = yidx - offset(2) + 1;

        % [_x;_y;1] = H * [x;y;1]
        posz =  h(3,1) * x + h(3,2) * y + h(3,3) ;
        posx = (h(1,1) * x + h(1,2) * y + h(1,3)) ./ posz;
        posy = (h(2,1) * x + h(2,2) * y + h(2,3)) ./ posz;

        if (posx>=1)&&(posx<=imgw)&&(posy>=1)&&(posy<=imgh)
%             warped_img(yidx,xidx,:) = img(posy,posx,:);

            pix = [posy posx];
            float_Y=pix(1)-floor(pix(1));
            float_X=pix(2)-floor(pix(2));

            pix_up_left=[floor(pix(1)) floor(pix(2))];
            pix_up_right=[floor(pix(1)) ceil(pix(2))];
            pix_down_left=[ceil(pix(1)) floor(pix(2))];
            pix_down_right=[ceil(pix(1)) ceil(pix(2))];

            value_up_left   = (1-float_X) * (1-float_Y);
            value_up_right  =     float_X * (1-float_Y);
            value_down_left = (1-float_X) * float_Y;
            value_down_right=     float_X * float_Y;

            warped_img(yidx,xidx,:) = ...
               value_up_left*img(   pix_up_left(1),   pix_up_left(2),:)+ ...
              value_up_right*img(  pix_up_right(1),  pix_up_right(2),:)+ ...
             value_down_left*img( pix_down_left(1), pix_down_left(2),:)+ ...
            value_down_right*img(pix_down_right(1),pix_down_right(2),:);

        else
            warped_img(yidx,xidx,:) = [0,0,0];
        end
    end
end

end


function [out] = recharge_1(p1,S,Smax,flux)
%recharge_1

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Recharge as scaled fraction of incoming flux
% Constraints:  -
% @(Inputs):    p1   - fraction of flux that is recharge [-]
%               S    - current storage [mm]
%               Smax - maximum contributing storage [mm]
%               flux - incoming flux [mm/d]

out = p1*S/Smax*flux;

end