function info = ctSent_locate(info,rectangle)
%CTSENT_RADARCODE Summary of this function goes here
%   Detailed explanation goes here
parm = load('parms');
orbit_Degree = parm.orbit_degree;
for i = 1:3
    for j = 1:info.IW(i).subSwath.numOfBursts
morbit = orbit(info.IW(i),orbit_Degree);
lat = linspace(rectangle(1)-3,rectangle(2)+3,80);
lon = linspace(rectangle(3)-3,rectangle(4)+3,80);
[DEM_ex.lat,DEM_ex.lon] = meshgrid(lat,lon);
DEM_ex.z = zeros(size(DEM_ex.lat));
Ellipse = BLH2XYZ(DEM_ex.lat,DEM_ex.lon,DEM_ex.z);
[time_m] = xyz2t(Ellipse,info.IW(i),morbit);
[mline,mpixel] = ta2lp( info.IW(i),time_m,j);
ratio = info.IW(i).sensor_para.rangePixelSpacing/info.IW(i).sensor_para.azimuthPixelSpacing;
[mline_grid,mpixel_grid] = gridlinepixel(info.IW(i));
Lat = trinterp(mline,mpixel,DEM_ex.lat,mline_grid,mpixel_grid,ratio);
Lon = trinterp(mline,mpixel,DEM_ex.lon,mline_grid,mpixel_grid,ratio);
info.IW(i).burst_lat(:,j) = Lat(:);
info.IW(i).burst_lon(:,j) = Lon(:);
    end
end
end
function Inc = Theta(M, P)
[m,n] = size(M.X);
Master=[M.X(:),M.Y(:),M.Z(:)];
Point=[P.X(:),P.Y(:),P.Z(:)];
r1 = Master - Point;
Inc = vec_angle(Point, r1);
Inc = reshape(Inc,[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]);
% [ti,bc] = pointLocation(TRI,xq(:),yq(:));
% bc(isnan(ti),:)=[];ti(isnan(ti))=[];
% triVals = v(TRI(ti,:));
% vq = dot(bc',triVals')';
% vq = reshape(vq,size(xq));
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 [ dopplertime,rangetime] = getZeroDopplerTime( info ,earthPoint)
  [numOrbitVec,~] = size(info.orbit_para.orbit_points);
  lineTimeInterval=info.sensor_para.line_time_interval;
  wavelength=info.sensor_para.Wavelength;
  firstVecTime = 0.0;
  secondVecTime = 0.0;
  firstVecFreq = 0.0;
  secondVecFreq = 0.0;
        for i = 0:numOrbitVec-1
            sensorPosition.x=info.orbit_para.orbit_points(i+1,2);
            sensorPosition.y=info.orbit_para.orbit_points(i+1,3);
            sensorPosition.z=info.orbit_para.orbit_points(i+1,4);
            sensorVelocity.x=info.orbit_para.orbit_points(i+1,5);
            sensorVelocity.y=info.orbit_para.orbit_points(i+1,6);
            sensorVelocity.z=info.orbit_para.orbit_points(i+1,7);
            currentFreq = getDopplerFrequency(earthPoint, sensorPosition, sensorVelocity, wavelength);
            if (i == 0 || firstVecFreq * currentFreq > 0) 
                firstVecTime = info.orbit_para.orbit_points(i+1,1);
                firstVecFreq = currentFreq;
             else 
                secondVecTime = info.orbit_para.orbit_points(i+1,1);
                secondVecFreq = currentFreq;
                break;
            end
        end

        if (firstVecFreq * secondVecFreq >= 0.0) 
            disp( 'NonValidZeroDopplerTime');
        end
        lowerBoundTime = firstVecTime;
        upperBoundTime = secondVecTime;
        lowerBoundFreq = firstVecFreq;
        upperBoundFreq = secondVecFreq;
        diffTime = abs(upperBoundTime - lowerBoundTime);
        absLineTimeInterval = abs(lineTimeInterval);%abs(lineTimeInterval)/86400;
        totalIterations = floor(diffTime/ absLineTimeInterval) + 1;
        numIterations = 0;
        while (diffTime > absLineTimeInterval && numIterations <= totalIterations) 

            midTime = (upperBoundTime + lowerBoundTime) / 2.0;
            [sensorPosition, sensorVelocity] = getPositionVelocity(midTime,info);
            midFreq = getDopplerFrequency(earthPoint, sensorPosition, sensorVelocity, wavelength);

            if (midFreq * lowerBoundFreq > 0.0) 
                lowerBoundTime = midTime;
                lowerBoundFreq = midFreq;
            elseif (midFreq * upperBoundFreq > 0.0) 
                upperBoundTime = midTime;
                upperBoundFreq = midFreq;
            elseif (midFreq == 0) 
                dopplertime= midTime;
                break;
            end

            diffTime = abs(upperBoundTime - lowerBoundTime);
            numIterations=numIterations+1;
        end


        dopplertime= lowerBoundTime - lowerBoundFreq * (upperBoundTime - lowerBoundTime) / (upperBoundFreq - lowerBoundFreq);
        rangetime = computeSlantRange(earthPoint,dopplertime,info);
end
function Dop=getDopplerFrequency(earthPoint, sensorPosition, sensorVelocity, wavelength)
xDiff = earthPoint.X - sensorPosition.x;
yDiff = earthPoint.Y - sensorPosition.y;
zDiff = earthPoint.Z - sensorPosition.z;
distance = sqrt(xDiff * xDiff + yDiff * yDiff + zDiff * zDiff);
Dop=2.0 * (sensorVelocity.x * xDiff + sensorVelocity.y * yDiff + sensorVelocity.z * zDiff) / (distance * wavelength);
end
function rangetime = computeSlantRange(earthPoint,dopplerTime,info)
[sensorPos, ~]=getPositionVelocity(dopplerTime,info);
xDiff = sensorPos.x - earthPoint.X;
yDiff = sensorPos.y - earthPoint.Y;
zDiff = sensorPos.z - earthPoint.Z;
slantRange=sqrt(xDiff * xDiff + yDiff * yDiff + zDiff * zDiff);
rangetime = slantRange/299792458;
end
function [line,pixel] = determinZeroDoppler(info,Ellipse,burstIndex)
[m,n] = size(Ellipse.X);
line = zeros(m,n);
pixel = line;
for i = 1:m
    for j = 1:n
        earthPoint.X = Ellipse.X(i,j);earthPoint.Y = Ellipse.Y(i,j);earthPoint.Z = Ellipse.Z(i,j);
        [ dopplertime,rangetime ] = getZeroDopplerTime( info ,earthPoint);
        line(i,j) = (dopplertime - info.subSwath.burstFirstLineTime(burstIndex)) / info.subSwath.azimuthTimeInterval+1;
        pixel(i,j) = 1+(rangetime-info.sensor_para.t_Range1)*299792458/info.subSwath.rangePixelSpacing;
    end
end
end
function [line,pixel] = gridlinepixel(info)
azimuthline = [1,info.sensor_para.linesPerBurst];
rangepixel = [1,info.sensor_para.samplesPerBurst];
[line,pixel] = meshgrid(azimuthline,rangepixel);
end
function [DEM_ex,Ellipse] = computeDEMbox(info,burstIndex,dem_read)
longitude = info.subSwath.longitude(burstIndex:burstIndex+1,:);
lonMin = min(longitude(:));lonMax = max(longitude(:));
latitude = info.subSwath.latitude(burstIndex:burstIndex+1,:);
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 - 200*deltaDegree;latMax = latMax + 200*deltaDegree;
lonMin = lonMin - 200*deltaDegree;lonMax = lonMax + 200*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 phase = computeDerampDemodPhase(info,burstIndex)
[ dopplerRate] = computeDopplerRate( info,burstIndex );
RangeDependDopplerRate=computeRangeDependentDopplerRate(info.subSwath,burstIndex);
dopplerCentroid  = computeDopplerCentroid( info.subSwath,burstIndex);
referenceTime= computeReferenceTime( dopplerCentroid,RangeDependDopplerRate,info.subSwath );
temp =  ((0:(info.subSwath.linesPerBurst-1)).*info.subSwath.azimuthTimeInterval)';
ta = repmat(temp,1,info.sensor_para.samplesPerBurst);
deramp = -pi .* dopplerRate .* (ta - referenceTime).^2;
demod = -2 .* pi .* dopplerCentroid .* ta;
phase = deramp + demod;
end
function derampDemodSLC = performDerampDemod(slcData, derampDemodPhase)
phaseCplx = complex(cos(derampDemodPhase),sin(derampDemodPhase));
derampDemodSLC = slcData.*conj(phaseCplx);
end
function [ dopplerRate] = computeDopplerRate( info,burstIndex )
%morbit=orbit(info,3);
RangeDependentDopplerRate=computeRangeDependentDopplerRate(info.subSwath,burstIndex);
azTime = (info.sensor_para.first_line_time + info.sensor_para.last_line_time)/2.0;
[~,velocity] = getPositionVelocity(azTime,info);
v = getVelocity(velocity); 
steeringRate = info.subSwath.azimuthSteeringRate * (pi / 180.0);
krot = 2*v*steeringRate/info.sensor_para.Wavelength;
dopplerRate= RangeDependentDopplerRate * krot...
                            ./ (RangeDependentDopplerRate - krot);
end
function RangeDependentDopplerRate = computeRangeDependentDopplerRate(subSwath,burstIndex)
azFmRateList = subSwath.azFmRateList;
x = 1:subSwath.samplesPerBurst;
slrt = getSlantRangeTime(x,subSwath)*2;
dt = slrt - azFmRateList(burstIndex).t0;
RangeDependentDopplerRate =...
   azFmRateList(burstIndex).c0 + azFmRateList(burstIndex).c1.*dt + azFmRateList(burstIndex).c2.*dt.^2;
end
function [ dopplerCentroid ] = computeDopplerCentroid( subSwath,burstIndex )
dcBurstList  = subSwath.DCEstimate;
x = 1:subSwath.samplesPerBurst;
slrt = getSlantRangeTime(x,subSwath)*2;
dt = slrt - dcBurstList(burstIndex).t0;
dopplerCentroid = dcBurstList(burstIndex).c0 + dcBurstList(burstIndex).c1.*dt + dcBurstList(burstIndex).c2*dt.^2;
end
function [ referenceTime ] = computeReferenceTime( dopplerCentroid,rangeDependDopplerRate,subSwath )

tmp1 = subSwath.linesPerBurst * subSwath.azimuthTimeInterval / 2.0;
tmp2 = tmp1 + dopplerCentroid(subSwath.firstValidPixel+1) /...
       rangeDependDopplerRate(subSwath.firstValidPixel+1);
referenceTime = tmp2 - dopplerCentroid ./ rangeDependDopplerRate;

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