% use:
%
% dE_E_par = ...    % as done in FAST_detect_locate.m
%
% ws.dE_E_fun = @dE_E_fun;
%
% ws = run_scg_fast( ws );
%
% note:
% This is a backport from onearray_run_scg_fast_logsph_c.c
%
% Guillaume Lathoud, 2011, and Kaustubh Kale
% glathoud@yahoo.fr

function [dE, E] = dE_E_fun.m.save-2011-12-20-19h50.GL11b.linearized( logsph )

% ---------- inputs

% N: most likely nLocations * 3 (for each location: azimuth, elevation, logradius)
N = length( logsph(:) );
                
% Determine whether we need to output the
% cost E as well.
compute_E = nargout > 1;

%%KK00 added this
global dE_E_par;

geometry_in = dE_E_par.geometry;
pairs_in    = dE_E_par.pairs;   
%%KK00 changed fs_in and c_in to fs and c.
fs_in       = dE_E_par.fs;
c_in        = dE_E_par.c;

nPairs      = size( pairs_in, 1 );

%%GL01 added a check. Meaning: for each row (=one pair), the first
% column  contains the index of the first microphone , and the 2nd
% column contains the index of the 2nd microphone.
if size( pairs_in, 2 ) ~= 2
  error(['dE_E_fun (onearray_logsph): size(pairs, 2) must be 2']);
end

nDimensions = size( geometry_in, 1 );
nChannels   = size( geometry_in, 2 );

w_i_in      = dE_E_par.w_i(:);

nFreq       = size( w_i_in(:), 1 );

theta_in    = dE_E_par.theta;

nLocations  = N / nDimensions;

% ---------- inners

% where to store the time delays
t = zeros( nPairs * nLocations, 1 );

%%GL07 linearized the computation of `d1` and `d2`: removed unnecessary init
%%GL07 d1 = zeros( nPairs * nLocations, 1 );
%%GL07 d2 = zeros( nPairs * nLocations, 1 );

%%GL09 linearized the computation of `dxyz1` and `dxyz2`:
%%GL09 removed unnecessary init of `dxyz1` and `dxyz2`.
%%GL09 %%GL07 linearized the computation of `d1` and `d2`: modified the
%%GL09 %%GL07 two lines below.
%%GL09 %%GL07 dxyz1 = zeros( nPairs * nLocations * nDimensions, 1 );
%%GL09 %%GL07 dxyz2 = zeros( nPairs * nLocations * nDimensions, 1 );
%%GL09 dxyz1 = zeros( nPairs, nLocations, nDimensions );
%%GL09 dxyz2 = zeros( nPairs, nLocations, nDimensions );

%%GL11 linearized the computation of `D`
%%GL11 removed unnecessary init of `D`
%%GL11 D = zeros( nFreq * nPairs * nLocations, 1 );

%%GL09b removed unused tmp_int_array

% --------------------------------------------------
% Check dimensionality
% Normalize weights
% Convert coordinates from logspherical to Euclidean

if mod( N, nDimensions ) ~= 0
  error( ['dE_E_fun (onearray_logsph): inconsistent dimensions (`w`' ...
          ' and `dE_E_par.geometry`)' ])
end

weight_in = dE_E_par.weight(:);
if length( weight_in(:)) ~= nLocations
    %%KK00 added the required ]
  error( ['dE_E_fun (onearray_logsph): inconsistent dimensions' ...
          ' (`dE_E_par.weight` must have nLocations elements).' ])
end

weight_sum = sum( weight_in );

% convert logspherical coordinates to Euclidean coordinates

%%GL08 linearized conversion logsph->xyz (Euclidean)
%%GL08 removed the one line below (unnecessary init)
%%GL08 xyz = zeros( N, 1 );

%%GL08 linearized conversion logsph->xyz (Euclidean)
%%GL08 removed the `for` loop
%%GL08 added the lines below
logsph = reshape( logsph, nDimensions, nLocations );
az     = logsph( 1, : );
el     = logsph( 2, : );
% This is the difference with classical spherical coordinates
radius = exp( logsph( 3, : ) );   % radius, note the "exp"
%
radius_cos_el = radius .* cos( el );
xyz = [ 
    radius_cos_el .* cos( az );
    radius_cos_el .* sin( az );
    radius .* sin( el );
    ];
% size( xyz ): (nDimensions, nLocations)


% ( 5 ) Compute dxyz and time-delays

nPairs_nLocations = nPairs * nLocations;

%%GL09 linearized the computation of `dxyz1` and `dxyz2`:
%%GL09 - removed the 3 imbricated `for` loops (c,b,a),
%%GL09 - added the lines below.
%%GL09b removed `tmp_int_array`, replaced with `pairs_in`
tmp_xyz = repmat( permute( xyz, [3, 2, 1] ), [nPairs, 1, 1] );
% size(tmp_xyz): (nPairs, nLocations, nDimensions)
%
dxyz1   = tmp_xyz - repmat( permute( geometry_in( :, pairs_in( : , 1) ), [2, 3, 1] ) , [1, nLocations, 1] );
% size(dxyz1): (nPairs, nLocations, nDimensions)
%
dxyz2   = tmp_xyz - repmat( permute( geometry_in( :, pairs_in( : , 2) ), [2, 3, 1] ) , [1, nLocations, 1] );
% size(dxyz2): (nPairs, nLocations, nDimensions)



%%GL07 linearized the computation of `d1` and `d2`: added the
%%GL07 two lines below.
%%GL10 square-root right away 
d1 = sqrt( sum( dxyz1 .^ 2, 3 ) );    % size(d1): (nPairs, nLocations)
d2 = sqrt( sum( dxyz2 .^ 2, 3 ) );    % size(d2): (nPairs, nLocations)

%%GL01 replaced `_val` with `_in`
cc = fs_in / c_in;

%%GL10 linearized the computation of the time delays (in samples)
t = (d2 - d1) * cc;   % size(t): (nPairs, nLocations)

% ----------------------------------------------------------------------
% ( 6 ) Compute the gradient dE:
% we do it in Euclidean coordinates, then move back to logspherical coordinates

dE = zeros( size( logsph ) );

% Difference between measured and theoretical phase
% ( this is also used in the cost computation ).

%%GL11 linearized the computation of `D`
%%GL11 - removed the 3 imbricated for loops (n,p,i)
%%GL11 - added the code below
%
% size(theta_in): (nFreq, nPairs)
% size(w_i_in)  : (nFreq, 1)
% size(t)       : (nPairs, nLocations)
%
D = repmat( theta_in, [1,1,nLocations] ) - repmat( w_i_in(:), [1, nPairs, nLocations ] ) .* repmat( permute( t, [3,1,2] ), [ nFreq, 1, 1 ] );
% size(D): (nFreq, nPairs, nLocations)

% ----------

% Now compute each component of the gradient dE
%%GL01 replaced `_val` with `_in`
aa = - fs_in / ( 2 * c_in * nPairs * nFreq * weight_sum );

% index in 'xyz'
%%GL01 fixed this: indices start at 1 in MATLAB  (at 0 in C)
a = 1; % x
b = 2; % y
c = 3; % z

%%GL01 fixed this: indices start at 1 in MATLAB  (at 0 in C)
t = 1;   % index in 'd1' and 'd2', as well as x in 'dxyz1' and 'dxyz2'
u = 1 +     nPairs_nLocations;       % index of y in 'dxyz1' and 'dxyz2'
v = 1 + 2 * nPairs_nLocations;       % index of z in 'dxyz1' and 'dxyz2'

% index in D
%%GL01 fixed this: indices start at 1 in MATLAB  (at 0 in C)
d = 1;

% xxx `for` loops are bad in Matlab - but for now we want something
% that works like the MEX C does. We can optimize later.
for k = 1:nLocations
 
  % Where we'll temporarily store the gradient
  % in Euclidean coordinates, for location #k.
 
  dE_x = 0;
  dE_y = 0;
  dE_z = 0;
 
  for p = 1:nPairs
 
    % Compute the factor in sine, which is common to x,y,z
 
    cc = 0;
    for i = 1:nFreq
    %%KK00 changed this from += and ++
    %%KK02 changed to w_i_in
      cc = cc + w_i_in( i ) * sin( D( d ) );
  d= d+1;
    end
 
    % Update the sum over pairs, for each coordinate x,y,z
 
    dd = cc / d2( t );
    ee = cc / d1( t );
  %%KK00 changed this from +=
    dE_x = dE_x + dxyz2( t ) * dd - dxyz1( t ) * ee;        % x
    dE_y = dE_y + dxyz2( u ) * dd - dxyz1( u ) * ee;        % y
    dE_z = dE_z + dxyz2( v ) * dd - dxyz1( v ) * ee;        % z
 
    % move to next element in 'd1', 'd2', 'dxyz1', 'dxyz2'
    %%KK00 changed this from ++
    t=t+1;
    u=u+1;
    v=v+1;
 
  end % for p

  % Scaling factor
 %%KK02 changed to weight_in
  bb = aa * weight_in( k );
 %%KK00 changed this from *=
  dE_x = dE_x*bb;
  dE_y = dE_y*bb;
  dE_z = dE_z*bb;
 
  % Finally translate back to logspherical coordinates
 
  xx = xyz( a ); % x
  yy = xyz( b ); % y
  zz = xyz( c ); % z
 
  tt = logsph( a ); % azimuth 
  uu = logsph( b ); % elevation
  vv = logsph( c ); % log radius
 
 
  dE( a ) = -yy               * dE_x + xx        * dE_y;
  dE( b ) = -zz * ( cos( tt ) * dE_x + sin( tt ) * dE_y ) + vv * cos( uu ) * dE_z;
 
  % Note: compared to classical spherical coordinates,
  % only this line differs
  % It is the definition of ( dE / ds ) where s = log( radius )
  dE( c ) = xx                * dE_x + yy        * dE_y     + zz           * dE_z;
 
 
  % Move to the next location in 'xyz', 'logsph' and 'dE'
  %%KK00 changed this from ++
  %%GL01 fixed the a,b,c update
  a = a + 3;
  b = b + 3;
  c = c + 3;
 
end % for k


% ----------------------------------------------------------------------
% ( 7 ) If relevant, compute the cost E

if compute_E
 
  % Where we'll store the partial sum
  aa = 0;
 
  % Index in 'D'
  %%GL01 fixed this: indices start at 1 in MATLAB  (at 0 in C)
  a = 1;
 
  for n = 1:nLocations

    bb = 0;
 
    q = nPairs * nFreq;
    for p = 1:q
        %%KK00 changed this from += and ++ for a and [ ] for cos.
        bb = bb + cos( D(a) );
            a=a+1;
    end

    % scale with the weight of this location,
    % and update the partial sum.
    %%KK00 changed this from +=
    %%KK02 changed to weight_in
    aa = aa + bb * weight_in( n );
 
  end
 
  E = 0.5 * ( 1 - aa / ( nFreq * nPairs * weight_sum ) );

end
