%--------------------------------------------------------------------------
%
% interpolate_slice_response.m
%
% Michael P Owen, 8/09/2007, MERS Lab, Brigham Young University
%
% Description:
% This functions returns the interpolated slice spatial response function
% for one pulse at a time. The response function for all 8 slices is 
% returned simultanesouly. A bi-linear interpolation scheme is used first
% interpolating in orbit time and then in antenna azimuth. More complete
% documentation of this code exists in "Ultra-High-Resolution Near-Coastal
% Wind Retrieval for QuikSCAT."
%
% Usage:
% [pulse_del_lat, pulse_del_lon, resp] =  interpolate_slice_response(antenna_angle, orbit_time, polarization, c_lat);
%
% Inputs:
% antenna_angle: the antenna azimuth angle of the desired response function (0 - 360);
% orbit_time: orbit time of the desired response function (0 - 6061);
% polarization: 1 = Vpol, 0 = Hpol;
% c_lat: center latitude of desired response function (used in interpolation);
%
% Outputs: 
% pulse_del_lat: (m x n) array consisting of latitude perturbations
% pulse_del_lon: (m x n) array consisting of longitude perturbations
% resp:(m x n x 8) array consisting of one (m x n) array of the response
%           function for each slice
%
%--------------------------------------------------------------------------

function [pulse_del_lat, pulse_del_lon, resp] =  interpolate_slice_response(antenna_angle, orbit_time, polarization, c_lat);


% lat/lon to rectilinear transform constants
deg2rad = pi/180;
rad2deg = 180/pi;
Kflat = 1/298.257;
Ra = 6378.1363;
Re = (1 - Kflat*sin(c_lat)^2)*Ra;

%verify that angle is between - and 360
if(antenna_angle < 0)
    antenna_angle= antenna_angle+360;
end
if(antenna_angle > 360)
    antenna_angle = mod(antenna_angle,360);
end

% extract two nearest antenna angle indices
%   azimuth entries are spaced every 10 degrees starting with 0;
azimuth_index=floor(antenna_angle/10)+1;
azimuth_index_up = azimuth_index+1;
    % check for wrap-around
if(azimuth_index == 36)
    azimuth_index = 36;
    azimuth_index_up = 1;
end



% extract the nearest orbit time indices
%    orbit time entries are spaced according to orbit_time_table
 orbit_time_table= [0, 515, 745, 945, 1115, 1255, 1365, 1445, 1495, 1515, ...
     1535, 1585, 1665, 1775, 1915, 2085, 2285, 2515, 3030, 3545, 3775, ...
     3975, 4145, 4285, 4395, 4475, 4525, 4545, 4565, 4615, 4695, 4805, ...
     4945, 5115, 5315, 5545] ;
 orbit_time_max = 6061;
 
 for i = 1:length(orbit_time_table)
    if(orbit_time_table(i) <= orbit_time)
        orbit_index = i;
        orbit_index_up = i+1;
    end     
 end
    % check for wrap-around
 if(orbit_index_up > length(orbit_time_table))
     orbit_index_up = 1;
 end
 
%load the four responses 
% the latitude and longitude arrays and the sig_n and sig_m arrays are
% required for each

[ m, n, pos1_lat, pos1_lon, pos1_resp, pos1_m1, pos1_n1, pos1_sig_m, pos1_sig_n] =  load_response_wsig_points(azimuth_index, orbit_index, polarization);
[ m, n, pos1_2lat, pos1_2lon, pos1_2resp, pos1_2m1, pos1_2n1, pos1_2sig_m, pos1_2sig_n] =  load_response_wsig_points(azimuth_index_up, orbit_index, polarization);
[ m, n, pos2_lat, pos2_lon, pos2_resp, pos2_m1, pos2_n1, pos2_sig_m, pos2_sig_n] =  load_response_wsig_points(azimuth_index, orbit_index_up, polarization);
[ m, n, pos2_2lat, pos2_2lon, pos2_2resp, pos2_2m1, pos2_2n1, pos2_2sig_m, pos2_2sig_n] =  load_response_wsig_points(azimuth_index_up, orbit_index_up, polarization);


% Interpolate the vectors for the lower azimuth angle for both orbit times
    %lower orbit position, lower azimuth angle
    %do not rotate any points
    la = pos1_lat(pos1_sig_m(1),pos1_sig_n(1));
    lo = pos1_lon(pos1_sig_m(1),pos1_sig_n(1));
    Rphi = Re*cos((c_lat+ la )*deg2rad);
    lx1 = Rphi*sin(lo*deg2rad);
    ly1 = Re * sin(la*deg2rad) + Rphi*(1-cos(lo *deg2rad))*sin(c_lat*deg2rad);
    
    la = pos1_lat(pos1_sig_m(2),pos1_sig_n(2));
    lo = pos1_lon(pos1_sig_m(2),pos1_sig_n(2));
    Rphi = Re*cos((c_lat+ la )*deg2rad);
    lx2 = Rphi*sin(lo*deg2rad);
    ly2 = Re * sin(la*deg2rad) + Rphi*(1-cos(lo *deg2rad))*sin(c_lat*deg2rad);
    
    la = pos1_lat(pos1_sig_m(3),pos1_sig_n(3));
    lo = pos1_lon(pos1_sig_m(3),pos1_sig_n(3));
    Rphi = Re*cos((c_lat+ la )*deg2rad);    
    lx3 = Rphi*sin(lo*deg2rad);
    ly3 = Re * sin(la*deg2rad) + Rphi*(1-cos(lo *deg2rad))*sin(c_lat*deg2rad);
        
    %upper orbit position, lower azimuth angle
    % do not rotate points
    la = pos2_lat(pos2_sig_m(1),pos2_sig_n(1));
    lo = pos2_lon(pos2_sig_m(1),pos2_sig_n(1));
    Rphi = Re*cos((c_lat+ la )*deg2rad);
    ux1 = Rphi*sin(lo*deg2rad);
    uy1 = Re * sin(la*deg2rad) + Rphi*(1-cos(lo *deg2rad))*sin(c_lat*deg2rad);
    
    la = pos2_lat(pos2_sig_m(2),pos2_sig_n(2));
    lo = pos2_lon(pos2_sig_m(2),pos2_sig_n(2));
    Rphi = Re*cos((c_lat+ la )*deg2rad);
    ux2 = Rphi*sin(lo*deg2rad);
    uy2 = Re * sin(la*deg2rad) + Rphi*(1-cos(lo *deg2rad))*sin(c_lat*deg2rad);
    
    la = pos2_lat(pos2_sig_m(3),pos2_sig_n(3));
    lo = pos2_lon(pos2_sig_m(3),pos2_sig_n(3));
    Rphi = Re*cos((c_lat+ la )*deg2rad);    
    ux3 = Rphi*sin(lo*deg2rad);
    uy3 = Re * sin(la*deg2rad) + Rphi*(1-cos(lo *deg2rad))*sin(c_lat*deg2rad);
    
    % make set of points into two vectors for each position 
    Lx1 = lx2 - lx1;
    Ly1 = ly2 - ly1;
    Lx2 = lx3 - lx1;
    Ly2 = ly3 - ly1;
    Ux1 = ux2 - ux1;
    Uy1 = uy2 - uy1;
    Ux2 = ux3 - ux1;
    Uy2 = uy3 - uy1;
    
    if(orbit_index_up == 1)
        orbit_scale = (orbit_time - orbit_time_table(orbit_index))/(orbit_time_max - orbit_time_table(orbit_index));
    else
        orbit_scale = (orbit_time - orbit_time_table(orbit_index))/(orbit_time_table(orbit_index_up) - orbit_time_table(orbit_index));
    end

    % calculate the interpolated vector components between orbit_positions
    % for the lower antenna angle (linear interpolation)
    lowx1 = (Ux1-Lx1)*orbit_scale + Lx1;
    lowy1 = (Uy1-Ly1)*orbit_scale + Ly1;
    lowx2 = (Ux2-Lx2)*orbit_scale + Lx2;
    lowy2 = (Uy2-Ly2)*orbit_scale + Ly2;
    
    
 % Interpolate the vectors for the lower azimuth angle for both orbit times  
    % lower orbit position, upper azimuth angle
    % rotate all points CCW 10deg
    rotate_deg = 10;
    la = pos1_2lat(pos1_2sig_m(1),pos1_2sig_n(1));
    lo = pos1_2lon(pos1_2sig_m(1),pos1_2sig_n(1));
    Rphi = Re*cos((c_lat+ la )*deg2rad);
    x1 = Rphi*sin(lo*deg2rad);
    y1 = Re * sin(la*deg2rad) + Rphi*(1-cos(lo *deg2rad))*sin(c_lat*deg2rad);
    lx1 = x1 * cos( rotate_deg*deg2rad) + y1 *-sin(rotate_deg*deg2rad);
    ly1 = x1 * sin( rotate_deg*deg2rad) + y1 * cos(rotate_deg*deg2rad); 

    la = pos1_2lat(pos1_2sig_m(2),pos1_2sig_n(2));
    lo = pos1_2lon(pos1_2sig_m(2),pos1_2sig_n(2));
    Rphi = Re*cos((c_lat+ la )*deg2rad);
    x2 = Rphi*sin(lo*deg2rad);
    y2 = Re * sin(la*deg2rad) + Rphi*(1-cos(lo *deg2rad))*sin(c_lat*deg2rad);
    lx2 = x2 * cos( rotate_deg*deg2rad) + y2 *-sin(rotate_deg*deg2rad);
    ly2 = x2 * sin( rotate_deg*deg2rad) + y2 * cos(rotate_deg*deg2rad);

    la = pos1_2lat(pos1_2sig_m(3),pos1_2sig_n(3));
    lo = pos1_2lon(pos1_2sig_m(3),pos1_2sig_n(3));
    Rphi = Re*cos((c_lat+ la )*deg2rad);
    x3 = Rphi*sin(lo*deg2rad);
    y3 = Re * sin(la*deg2rad) + Rphi*(1-cos(lo *deg2rad))*sin(c_lat*deg2rad);
    lx3 = x3 * cos( rotate_deg*deg2rad) + y3 *-sin(rotate_deg*deg2rad);
    ly3 = x3 * sin( rotate_deg*deg2rad) + y3 * cos(rotate_deg*deg2rad);   

    %upper orbit position, upper azimuth angle
    % rotate all points CCW 10deg
    rotate_deg = 10;
    la = pos2_2lat(pos2_2sig_m(1),pos2_2sig_n(1));
    lo = pos2_2lon(pos2_2sig_m(1),pos2_2sig_n(1));
    Rphi = Re*cos((c_lat+ la )*deg2rad);
    x1 = Rphi*sin(lo*deg2rad);
    y1 = Re * sin(la*deg2rad) + Rphi*(1-cos(lo *deg2rad))*sin(c_lat*deg2rad);
    ux1 = x1 * cos( rotate_deg*deg2rad) + y1 *-sin(rotate_deg*deg2rad);
    uy1 = x1 * sin( rotate_deg*deg2rad) + y1 * cos(rotate_deg*deg2rad); 

    la = pos2_2lat(pos2_2sig_m(2),pos2_2sig_n(2));
    lo = pos2_2lon(pos2_2sig_m(2),pos2_2sig_n(2));
    Rphi = Re*cos((c_lat+ la )*deg2rad);
    x2 = Rphi*sin(lo*deg2rad);
    y2 = Re * sin(la*deg2rad) + Rphi*(1-cos(lo *deg2rad))*sin(c_lat*deg2rad);
    ux2 = x2 * cos( rotate_deg*deg2rad) + y2 *-sin(rotate_deg*deg2rad);
    uy2 = x2 * sin( rotate_deg*deg2rad) + y2 * cos(rotate_deg*deg2rad);

    la = pos2_2lat(pos2_2sig_m(3),pos2_2sig_n(3));
    lo = pos2_2lon(pos2_2sig_m(3),pos2_2sig_n(3));
    Rphi = Re*cos((c_lat+ la )*deg2rad);
    x3 = Rphi*sin(lo*deg2rad);
    y3 = Re * sin(la*deg2rad) + Rphi*(1-cos(lo *deg2rad))*sin(c_lat*deg2rad);
    ux3 = x3 * cos( rotate_deg*deg2rad) + y3 *-sin(rotate_deg*deg2rad);
    uy3 = x3 * sin( rotate_deg*deg2rad) + y3 * cos(rotate_deg*deg2rad); 
        
    % create vectors from significant points
    Lrx1 = lx2 - lx1;
    Lry1 = ly2 - ly1;
    Lrx2 = lx3 - lx1;
    Lry2 = ly3 - ly1;
    Urx1 = ux2 - ux1;
    Ury1 = uy2 - uy1;
    Urx2 = ux3 - ux1;
    Ury2 = uy3 - uy1;
    
    % calculate the interpolated vector components between orbit_positions
    % for the upper antenna angle (linear interpolation)
    upx1 = (Urx1-Lrx1)*orbit_scale + Lrx1;
    upy1 = (Ury1-Lry1)*orbit_scale + Lry1;
    upx2 = (Urx2-Lrx2)*orbit_scale + Lrx2;
    upy2 = (Ury2-Lry2)*orbit_scale + Lry2;    

    % Interpolate between the azimuth angles of the two orbit positions 

    % calculate the azimuth scale factor 
    azimuth_scale = (antenna_angle - floor((azimuth_index-1)*10))/10.0 ;
    
    % linearly interpolate for antenna azimuth between the two interpolated positions 
    upx1 = (upx1 - lowx1)*azimuth_scale + lowx1;
    upy1 = (upy1 - lowy1)*azimuth_scale + lowy1;
    upx2 = (upx2 - lowx2)*azimuth_scale + lowx2;
    upy2 = (upy2 - lowy2)*azimuth_scale + lowy2;
    
 % determine which table input is closest in both orbit position and
 % azimuth angle, and calculate the transform matrix and set the response
 % function to be returned
    if(orbit_scale < 0.5) %use the lower orbit position
      if(azimuth_scale < 0.5) %use the lower azimuth angle

        % calculate the transform matrix coefficients between the lower_deg lower-orbit position vectors and the interpolated vectors 
        d = (Lx2*upy1 - Lx1*upy2)/(Lx2*Ly1 - Lx1*Ly2);
        b = (Lx2*upx1 - Lx1*upx2)/(Lx2*Ly1 - Lx1*Ly2);
        c = (upy1 - d*Ly1)/Lx1;
        a = (upx1 - b*Ly1)/Lx1;

        % calculate the needed rotation ammount 
        rotate_deg = (antenna_angle - floor(((azimuth_index-1)*10)));

        % get the lower set of del_lat and del_lon 
        lower_lat = pos1_lat;
        lower_lon = pos1_lon;
        resp = pos1_resp;      
      else % use the upper azimuth position

        % calculate the transform matrix coefficients between the lower_deg lower-orbit position vectors and the interpolated vectors 
        d = (Lrx2*upy1 - Lrx1*upy2)/(Lrx2*Lry1 - Lrx1*Lry2);
        b = (Lrx2*upx1 - Lrx1*upx2)/(Lrx2*Lry1 - Lrx1*Lry2);
        c = (upy1 - d*Lry1)/Lrx1;
        a = (upx1 - b*Lry1)/Lrx1;

        % calculate the needed rotation ammount 
        rotate_deg = (antenna_angle - floor(((azimuth_index_up-1)*10)));

        % get the lower set of del_lat and del_lon 
        lower_lat = pos1_2lat;
        lower_lon = pos1_2lon;
        resp = pos1_2resp;
      end
    
    else  %use the upper orbit position 
      if(azimuth_scale < 0.5) %use the lower azimuth angle

        % calculate the transform matrix coefficients between the lower_deg lower-orbit position vectors and the interpolated vectors 
        d = (Ux2*upy1 - Ux1*upy2)/(Ux2*Uy1 - Ux1*Uy2);
        b = (Ux2*upx1 - Ux1*upx2)/(Ux2*Uy1 - Ux1*Uy2);
        c = (upy1 - d*Uy1)/Ux1;
        a = (upx1 - b*Uy1)/Ux1;

        %calculate the needed rotation ammount 
        rotate_deg = (antenna_angle - floor(((azimuth_index-1)*10)));

        % get the lower set of del_lat and del_lon 
        lower_lat = pos2_lat;
        lower_lon = pos2_lon;
        resp = pos2_resp;
      
      else % use the upper azimuth position

        % calculate the transform matrix coefficients between the lower_deg lower-orbit position vectors and the interpolated vectors 
        d = (Urx2*upy1 - Urx1*upy2)/(Urx2*Ury1 - Urx1*Ury2);
        b = (Urx2*upx1 - Urx1*upx2)/(Urx2*Ury1 - Urx1*Ury2);
        c = (upy1 - d*Ury1)/Urx1;
        a = (upx1 - b*Ury1)/Urx1;

        % calculate the needed rotation ammount 
        rotate_deg = (antenna_azimuth - floor(((azimuth_index_up-1)*10)));

        % get the lower set of del_lat and del_lon 
        lower_lat = pos2_2lat;
        lower_lon = pos2_2lon;
        resp = pos2_2resp;
        
      end
    end
    
    %create empty output arrays
    pulse_del_lon = zeros(m,n);
    pulse_del_lat = zeros(m,n);
  
        
    % Direct Transform using the calculated transform matrix        
     for i =1:m
         for j = 1:n
            Rphi = Re*cos((c_lat+ lower_lat(i,j) )*deg2rad);
            x = Rphi*sin(lower_lon(i,j)*deg2rad);
            y = Re * sin(lower_lat(i,j)*deg2rad) + Rphi*(1-cos(pos1_lon(i,j)*deg2rad))*sin(c_lat*deg2rad);
            % direct transform
            s_x = a*x + b*y;
            s_y = c*x + d*y;
            %rotate CW to desired azimuth
            r_x = s_x * cos( rotate_deg*deg2rad) + (s_y) * sin(rotate_deg*deg2rad);
            r_y = s_x *-sin( rotate_deg*deg2rad) + (s_y) * cos(rotate_deg*deg2rad); 
            pulse_del_lon(i,j) = asin(r_x/Rphi)*180/pi;
            pulse_del_lat(i,j) = asin( (r_y-(1-cos(pulse_del_lon(i,j)*deg2rad))*sin(c_lat*deg2rad)*Rphi)/Re)*180/pi;

          end    
     end