function [Bperpmat,refPhasemat] = ctSent_computeBnormal(info_m,info_s,mburstIndex,dem_read)
%CTSENT_GEOCOREG Summary of this function goes here
%   Detailed explanation goes here
Min4piCDivLam = -(4 *3.14159265358979323846264338327950288 * 299792458.0) /info_s.sensor_para.Wavelength;
parm = load('parms');
orbit_Degree = parm.orbit_degree;
morbit = orbit(info_m,orbit_Degree);
sorbit = orbit(info_s,orbit_Degree);
[DEM_ex,Ellipse] = computeDEMbox(info_m,mburstIndex,dem_read);
[time_m] = xyz2t(Ellipse,info_m,morbit);
Msat = getXYZ(time_m.timeAzimuth,morbit);
[time_s] = xyz2t(Ellipse,info_s,sorbit);
Ssat = getXYZ(time_s.timeAzimuth,sorbit);
referenceTime = time_m.timeRange - time_s.timeRange;
refPhase = Min4piCDivLam * referenceTime;
[Bperp] = BBparBperpTheta(Msat,Ellipse, Ssat);
[mline,mpixel] = ta2lp(info_m,time_m,mburstIndex);
ratio = info_m.sensor_para.rangePixelSpacing/info_m.sensor_para.azimuthPixelSpacing;
[mline_grid,mpixel_grid] = gridlinepixel(info_m);
% [Bpar] = trinterp(mline,mpixel,Bpar,mline_grid,mpixel_grid,ratio);
[Bperpmat] = trinterp(mline,mpixel,Bperp,mline_grid,mpixel_grid,ratio);
% [Incmat] = trinterp(refline,refpixel,Inc,refline_grid,refpixel_grid,ratio);
[refPhasemat] = trinterp(mline,mpixel,refPhase,mline_grid,mpixel_grid,ratio);
end
function [Bperp] = BBparBperpTheta(M, P, S)
[m,n] = size(M.X);
Master=[M.X(:),M.Y(:),M.Z(:)];
Point=[P.X(:),P.Y(:),P.Z(:)];
Slave=[S.X(:),S.Y(:),S.Z(:)];
B = normalvec(Master - Slave); 
range1 = normalvec(Master - Point);
range2 = normalvec(Slave - Point);
Bpar = range1 - range2;
r1 = Master - Point;
r2 = Slave - Point;
Inc = vec_angle(Point, r1);
Bperp = B.^2 - Bpar.^2;
Bperp(Bperp < 0.0) = 0;
Bperp(Inc > vec_angle(Point, r2)) = sqrt(Bperp(Inc > vec_angle(Point, r2)));
Bperp(Inc <= vec_angle(Point, r2)) = 0 - sqrt(Bperp(Inc <= vec_angle(Point, r2)));
Bperp = reshape(Bperp,[m,n]);
end
function normvec = normalvec(PointVec)
normvec = sqrt(PointVec(:,1).^2+PointVec(:,2).^2+PointVec(:,3).^2);
end
function angle_of_vec = vec_angle(vec1, vec2)
   in_vec = sum(vec1 .* vec2,2);
   angle_of_vec = acos(in_vec ./ (normalvec(vec1).*normalvec(vec2)));
end
function [vq] = trinterp(x,y,v,xq,yq,ratio)
[m,n] = size(xq);
X = x(:);Y = y(:)*ratio;Z = v(:);
xq = xq(:);yq = yq(:);
TRI = delaunayTriangulation(X,Y);
ID = pointLocation(TRI,xq,yq.*ratio);
Vx0 = X(TRI(ID,1));Vx1 = X(TRI(ID,2));Vx2 = X(TRI(ID,3));
Vy0 = Y(TRI(ID,1))./ratio;Vy1 = Y(TRI(ID,2))./ratio;Vy2 = Y(TRI(ID,3))./ratio;
Vz0 = Z(TRI(ID,1));Vz1 = Z(TRI(ID,2));Vz2 = Z(TRI(ID,3));
xkj = Vx1 - Vx0;
ykj = Vy1 - Vy0;
xlj = Vx2 - Vx0;
ylj = Vy2 - Vy0;
f = 1 ./ (xkj.*ylj - ykj.*xlj);
zj = Vz0;zk = Vz1;zl = Vz2;
zkj = zk - zj;
zlj = zl - zj;
a = -f.*(ykj.*zlj - zkj.*ylj);
b = -f.*(zkj.*xlj - xkj.*zlj);
c = -a.*Vx1 - b.*Vy1 + zk;
zout = a.* xq + b.* yq + c;
vq = reshape(zout,[m,n]);
end
function [ line,pixel ] = ta2lp( info_m,t,burstIndex)
%PRF=1.717128973878037e+03;		
line=1+(t.timeAzimuth-info_m.subSwath.burstFirstLineTime(burstIndex))/info_m.sensor_para.line_time_interval;
pixel=1+(t.timeRange-info_m.sensor_para.t_Range1)*299792458/info_m.subSwath.rangePixelSpacing;
% pixel=1+(t.timeRange-info_m.sensor_para.t_Range1)*info_m.sensor_para.RSR2x;
end
function [line,pixel] = gridlinepixel(info)
azimuthline = (1:info.sensor_para.linesPerBurst)';
rangepixel = 1:info.sensor_para.samplesPerBurst;
line = repmat(azimuthline,1,length(rangepixel));
pixel = repmat(rangepixel,length(azimuthline),1);
end
function [DEM_ex,Ellipse] = computeDEMbox(info,burstIndex,dem_read)
longitude = info.subSwath.longitude(:,burstIndex);
lonMin = min(longitude(:));lonMax = max(longitude(:));
latitude = info.subSwath.latitude(:,burstIndex);
latMin = min(latitude(:));latMax = max(latitude(:));
LatitudeLimitLo = min(dem_read.lat(:));LatitudeLimitHi = max(dem_read.lat(:));
LongitudeLimitLo = min(dem_read.lon(:));LongitudeLimitHi = max(dem_read.lon(:));
deltaDegree = abs(dem_read.lat(1,1)-dem_read.lat(2,1));
latMin = latMin - 50*deltaDegree;latMax = latMax + 50*deltaDegree;
lonMin = lonMin - 50*deltaDegree;lonMax = lonMax + 50*deltaDegree;
[RasterLine,RasterWidth] = size(dem_read.lat);
upperLeft.y = (latMin-LatitudeLimitLo)*RasterLine/(LatitudeLimitHi-LatitudeLimitLo)+1;
upperLeft.x = (lonMin-LongitudeLimitLo)*RasterWidth/(LongitudeLimitHi-LongitudeLimitLo)+1;
lowerRight.y = (latMax-LatitudeLimitLo)*RasterLine/(LatitudeLimitHi-LatitudeLimitLo)+1;
lowerRight.x = (lonMax-LongitudeLimitLo)*RasterWidth/(LongitudeLimitHi-LongitudeLimitLo)+1;
latMinIdx = floor(upperLeft.y);
latMaxIdx = floor(lowerRight.y);
lonMinIdx = floor(upperLeft.x);
lonMaxIdx = floor(lowerRight.x);
if latMinIdx <= 0
    latMinIdx = 1;
    fprintf('Warning: DEM cannot cover the whole data region !!! \n');
end
if lonMinIdx <= 0
    lonMinIdx = 1;
    fprintf('Warning: DEM cannot cover the whole data region !!! \n');
end
if latMaxIdx > RasterLine
    latMaxIdx = RasterLine;
    fprintf('Warning: DEM cannot cover the whole data region !!! \n');
end
if lonMaxIdx > RasterWidth
    lonMaxIdx = RasterWidth;
    fprintf('Warning: DEM cannot cover the whole data region !!! \n');
end
GeoBoxlat = floor(latMin):0.1:ceil(latMax);
GeoBoxlon = floor(lonMin):0.1:ceil(lonMax);
Lat_n = length(GeoBoxlat);
Lon_n = length(GeoBoxlon);
lat =repmat(GeoBoxlat',1,Lon_n);
lon =repmat(GeoBoxlon,Lat_n,1);
lon(lon<0) = abs(lon(lon<0))+180;
heightCorrection = geoidheight(lat,lon);
heightCorrection = reshape(heightCorrection,[Lat_n Lon_n]);
lon =repmat(GeoBoxlon,Lat_n,1);
DEM_ex.lat = dem_read.lat(latMinIdx:latMaxIdx,lonMinIdx:lonMaxIdx);
DEM_ex.lon = dem_read.lon(latMinIdx:latMaxIdx,lonMinIdx:lonMaxIdx);
DEM_ex.z = dem_read.z(latMinIdx:latMaxIdx,lonMinIdx:lonMaxIdx);
EGM96correction = griddata(lat,lon,heightCorrection,DEM_ex.lat,DEM_ex.lon);
DEM_ex.z = double(DEM_ex.z)+EGM96correction;
% [m,n] = size(DEM_ex.z);
% [pixel,line] = meshgrid(1:0.1:n,1:0.1:m);
% z = ba_interp2(DEM_ex.z,pixel,line,'linear');
% lat = ba_interp2(DEM_ex.lat,pixel,line,'linear');
% lon = ba_interp2(DEM_ex.lon,pixel,line,'linear');
% DEM_ex.lat = lat;DEM_ex.lon = lon;DEM_ex.z = z;
Ellipse = BLH2XYZ(DEM_ex.lat,DEM_ex.lon,DEM_ex.z);
end
function slrt=getSlantRangeTime(x,subSwath)
 slrt=subSwath.slrTimeToFirstPixel +(x-1) * subSwath.rangePixelSpacing / 299792458;
end
function [Velocity]=getVelocity(POS)
Velocity=sqrt(POS.x*POS.x+POS.y*POS.y+POS.z*POS.z);
end
function [position,velocity] = getPositionVelocity(time,info)
nv = 8;
orbitStateVectors=info.orbit_para.orbit_points;
[m,~]=size(orbitStateVectors);
dt = (orbitStateVectors(m,1)-orbitStateVectors(1,1)) / (m-1);
            if (m <= nv) 
                i0 = 0;
                iN = m - 1;
            else 
                i0 = max(floor(((time - orbitStateVectors(1,1) )/ dt)) - nv/2 + 1, 0);
                iN = min(i0 + nv - 1, m - 1);
                if iN < m - 1
                i0 = i0;
                else
                i0=iN - nv + 1;
                end
            end
                position.x = 0;
                position.y = 0;
                position.z = 0;
                velocity.x = 0;
                velocity.y = 0;
                velocity.z = 0;
            for i = i0:iN
                orbI.time_mjd = orbitStateVectors(i+1,1);
                orbI.x_pos = orbitStateVectors(i+1,2);
                orbI.y_pos = orbitStateVectors(i+1,3);
                orbI.z_pos = orbitStateVectors(i+1,4);
                orbI.x_vel = orbitStateVectors(i+1,5);
                orbI.y_vel = orbitStateVectors(i+1,6);
                orbI.z_vel = orbitStateVectors(i+1,7);
                weight = 1;
                for j = i0:iN
                    if (j ~= i) 
                        time2 = orbitStateVectors(j+1,1);
                        weight = weight * (time - time2) / (orbI.time_mjd - time2);
                    end
                end
                    position.x = position.x + weight * orbI.x_pos;
                    position.y = position.y + weight * orbI.y_pos;
                    position.z = position.z + weight * orbI.z_pos;
                    velocity.x = velocity.x + weight * orbI.x_vel;
                    velocity.y = velocity.y + weight * orbI.y_vel;
                    velocity.z = velocity.z + weight * orbI.z_vel;
            end
end
function index=computeCornerBasedIndex(x,y,sourceRectangle)  
x=x + 0.5; y=y + 0.5;
index.x = x;
index.y = y;
index.x0=sourceRectangle(1);
index.y0=sourceRectangle(2);
index.width = sourceRectangle(3);
index.height = sourceRectangle(4);
i0 = floor(x);
j0 = floor(y);
di = x - (i0 + 0.5);
dj = y - (j0 + 0.5);
index.i0 = i0;
index.j0 = j0;
iMax = index.width;
        if (di >= 0) 
            i1 = i0 + 1;
            if (i0 < 0)
            index.i(1) =  0 ;
            elseif(i0 > iMax) 
            index.i(1) =iMax ;
            else index.i(1) =i0;
            end
            if (i1 < 0)
            index.i(2) =  0 ;
            elseif(i1 > iMax) 
            index.i(2) =iMax ;
            else index.i(2) =i1;
            end
            index.ki(1) = di;
         else 
            i1 = i0 - 1;
            if (i1 < 0)
            index.i(1) =  0 ;
            elseif(i1 > iMax) 
            index.i(1) =iMax ;
            else index.i(1) =i1;
            end
            if (i0 < 0)
            index.i(2) =  0 ;
            elseif(i0 > iMax) 
            index.i(2) =iMax ;
            else index.i(2) =i0;
            end
            index.ki(1) = di + 1;
        end

        jMax = index.height;
        if (dj >= 0) 
            j1 = j0 + 1;
            if (j0 < 0)
            index.j(1) =  0 ;
            elseif(j0 > jMax) 
            index.j(1) =jMax ;
            else index.j(1) =j0;
            end
            if (j1 < 0)
            index.j(2) =  0 ;
            elseif(j1 > jMax) 
            index.j(2) =jMax; 
            else index.j(2) =j1;
            end
            index.kj(1) = dj;
         else 
            j1 = j0 - 1;
            if (j0 < 0)
            index.j(1) =  0 ;
            elseif(j1 > jMax) 
            index.j(1) =jMax ;
            else index.j(1) =j1;
            end
            if (j0 < 0)
            index.j(2) =  0 ;
            elseif(j0 > jMax) 
            index.j(2) =jMax ;
            else index.j(2) =j0;
            end
            index.kj(1) = dj + 1;
        end
end
function value=resample(raster,index)
x = [ floor(index.i(1)), floor( index.i(2))];
y = [ floor(index.j(1)), floor( index.j(2))];
samples(1,1) = raster(y(1),x(1));
samples(1,2) = raster(y(1),x(2));
samples(2,1) = raster(y(2),x(1));
samples(2,2) = raster(y(2),x(2));
if (samples(1,1)~=0&&samples(1,2)~=0&&samples(2,1)~=0&&samples(2,2)~=0)
ki = index.ki(1);
kj = index.kj(1);
value= samples(1,1)  * (1 - ki) * (1 - kj) +...
       samples(1,2) * ki * (1 - kj) +...
       samples(2,1)  * (1 - ki) * kj +...
       samples(2,2) * ki * kj;
else
    value=samples(1,1);
end
end
function [time] = xyz2t(pointOnEllips,info,coeff)
if nargin < 5
    CRITERTIM = 10e-11;
end
SOL=299792458;
if nargin < 4
    MAXITER = 50;
end
[m,n]=size(pointOnEllips.X);
timeAzimuth =ones(m,n).*line2ta(info.sensor_para.current_window.bottom/2,info.sensor_para.t_Azi1,info.sensor_para.line_time_interval); 
%solution = zeros(m,n,MAXITER);
for iter = 1:MAXITER
            satellitePosition = getXYZ(timeAzimuth,coeff);
            satelliteVelocity = getXYZDot(timeAzimuth,coeff);
            satelliteAcceleration = getXYZDotDot(timeAzimuth,coeff);
            delta.X = pointOnEllips.X-satellitePosition.X;
            delta.Y = pointOnEllips.Y-satellitePosition.Y;
            delta.Z = pointOnEllips.Z-satellitePosition.Z;
            solution = -eq1_Doppler(satelliteVelocity, delta) ./ eq1_Doppler_dt(delta, satelliteVelocity, satelliteAcceleration);
            timeAzimuth =timeAzimuth + solution ;
            if max(max(abs(solution))) < CRITERTIM
                break;
            end
            if (iter >= MAXITER) 
            disp('Iteration Numer exceeds 50...');
                break;
            end
end
        
satellitePosition = getXYZ(timeAzimuth,coeff);
delta.X = pointOnEllips.X-satellitePosition.X;
delta.Y = pointOnEllips.Y-satellitePosition.Y;
delta.Z = pointOnEllips.Z-satellitePosition.Z;
timeRange = sqrt(delta.X.^2 +delta.Y.^2+ delta.Z.^2)./ SOL;
time.timeAzimuth=timeAzimuth;
time.timeRange=timeRange;
end
function [POS]=getXYZ(azTime,coeff)
azTimeNormal = (azTime - coeff.time(floor(length(coeff.time) / 2)+1))./10;
% norm=(t-(t(floor(length(t)/2)+1)))./10;
%azTimeNormal = azTime;
POS.X=polyVal1D(azTimeNormal, coeff.X);
POS.Y=polyVal1D(azTimeNormal, coeff.Y);
POS.Z=polyVal1D(azTimeNormal, coeff.Z);
end
function [sum]=polyVal1D(aziTime, coeffs)
sum = 0.0;
d=length(coeffs);
        while d > 0
            sum =sum.* aziTime;
            sum =sum+ coeffs(d);
            d=d-1;
        end
end
function [POS]=getXYZDot(azTime,coeff)
azTimeNormal = (azTime - coeff.time(floor(length(coeff.time) / 2)+1))./10 ;
%azTimeNormal = azTime;
DEGREE = length(coeff.X)-1;
x = coeff.X(2);
y = coeff.Y(2);
z = coeff.Z(2);
        for  i = 2:DEGREE
            powT = i .*power(azTimeNormal, i - 1);
            x = x+coeff.X(i+1) .* powT;
            y = y+coeff.Y(i+1) .* powT;
            z = z+coeff.Z(i+1) .* powT;
        end
POS.X=x/10;
POS.Y=y/10; 
POS.Z=z/10;
end
function  [POS]=getXYZDotDot(azTime,coeff)
poly_degree = length(coeff.X)-1;
azTimeNormal = (azTime - coeff.time(floor(length(coeff.time) / 2))+1)./10 ;
%azTimeNormal = azTime;
x=0; y=0; z=0;
        for  i = 2: poly_degree
            powT = ((i - 1) * i) .* power(azTimeNormal, i - 2);
            x =x+ coeff.X(i+1) .* powT;
            y =y+ coeff.Y(i+1) .* powT;
            z =z+ coeff.Z(i+1) .* powT;
        end
POS.X=x/100;
POS.Y=y/100;
POS.Z=z/100;
end
function [POS]=eq1_Doppler(satVelocity,pointOnEllips)
POS=satVelocity.X.*pointOnEllips.X+satVelocity.Y.*pointOnEllips.Y+satVelocity.Z.*pointOnEllips.Z;
end
function [POS]=eq1_Doppler_dt(pointEllipsSat,satVelocity,satAcceleration)
     POS=  satAcceleration.X.*pointEllipsSat.X+satAcceleration.Y.*pointEllipsSat.Y+satAcceleration.Z.*pointEllipsSat.Z ...
       - satVelocity.X.*satVelocity.X - satVelocity.Y.*satVelocity.Y - satVelocity.Z.*satVelocity.Z;
end
function [Coeff] = orbit(info,degree)
orbitStateVectors = info.orbit_para.orbit_points;
numStateVectors = length(orbitStateVectors);
data.time= orbitStateVectors(:,1);
%data.Time=orbitStateVectors(11:37,1);
data.X = orbitStateVectors(:,2);
data.Y = orbitStateVectors(:,3);
data.Z= orbitStateVectors(:,4);
poly_degree = degree;
Coeff=computeCoefficients(data,poly_degree);
end
function norm=normalize( t) 
%t.sub(t.get(t.length / 2)).div(10.0);
norm=(t-(t(floor(length(t)/2)+1)))./10;
end
function coeff=computeCoefficients(data,poly_degree)
coeff.X = polyFitNormalized(data.time, data.X, poly_degree);
coeff.Y = polyFitNormalized(data.time, data.Y, poly_degree);
coeff.Z = polyFitNormalized(data.time, data.Z, poly_degree);
coeff.time=data.time;
end
function coeff=polyFitNormalized(time, data, poly_degree)
time=normalize(time);
numOfPoints=length(time);
numOfUnkowns=poly_degree+1;
if(numOfPoints<numOfUnkowns)
    disp('para too cheap');
end
%Amat=[time.^3, time.^2, time ,ones(numOfPoints,1)];
Amat=[ones(numOfPoints,1),  time, time.^2 ,time.^3];
N=Amat'*Amat;
rhs=Amat'*data;
coeff=SymmetricBandedCholesky(N,rhs); 
%coeff=N\rhs;
end
function [x]=SymmetricBandedCholesky(A,q)

[m,n]=size(A);
if (m~=n)
    error('Coefficient Matrix A Must be Square')
end
C=compactstorage(A);
B= compactchol(C);
L=unpack(B);L=L';
y=bandforward(L,q);%solves Ly=b
x=bandback(L',y);%solves L'x=y
end
%q=[5,3.55,2.81428571428571,2.34642857142857,2.01746031746032];
function B = compactstorage(A)

dim=size(A);
if ~(dim(1)==dim(2))
    error('A must be square')
end
if (all((all(A)~=all(A'))))
    error('A must be symmetric')
end
if ~(all(eig(A))> 0)
    error('Matrix is at least not positive definite')
end
c=find(A(1,1:dim(1))~=0);
B=zeros(dim(1),c(end));
n=dim(1);p=c(end)-1;
for i=1:n
if i<=n-p
for j=i:p+i
B(i,j-i+1)=A(i,j);
end
else 
for j=i:n
B(i,j-i+1)=A(i,j);
end
end
end
end
function C = compactchol(B)
[m,p] = size(B);  p = p - 1;
C = B;
for k = 1:m
    last = min(k+p,m) - k + 1;
    for j = 2:last
        i = k + j - 1;
        C(i,1:last-j+1) = C(i,1:last-j+1) - ...
            ((C(k,j))/C(k,1))*C(k,j:last); 
    end
    C(k,:) = C(k,:)/sqrt(C(k,1));
end
C(end-(p-1):end,end) = 0;
end
function L=unpack(B)
[m,n]=size(B);
L=zeros(m,n);
for i=1:m
for j=1:n
ind=i+j-1;
if ind<=m
L(i,ind)=B(i,j);
end
end
end
end
function y= bandforward(R,b)
dim=size(R);c=find(R(1:dim(1),1)~=0);d=c(end)-1;
n= length(b); y=b(:);
y(1)=y(1)/R(1,1);
for k=2: n
km= max (1,k-d);
y(k )=(y(k)-dot(R(k,km:k -1),y(km:k -1)))/ R(k,k);
end
end
function x= bandback(R,y)
dim=size(R);c=find(R(1,1:dim(1))~=0);d=c(end)-1;
n=length(y); x=y (:);
x(n)=x(n)/R(n,n);
for k=n-1: -1:1
kp= min(n,k+d);
x(k )=(x(k)-dot(R(k,k +1: kp ),x(k +1: kp )))/ R(k,k);
end
end
function ta = line2ta(line, ta1, deltat)
ta = ta1+(line - 1) *deltat;
end

