function [inc_iwrap_t, az_N]=aircraft2track( inc_iwrap, az_a, pitch, roll, drift, trackp)
% az should be relative to +xa axis, 0 at xa , +90 deg at ya 
%inc should be relative to +za. 0 at +za, +90 in the xa-ya plan
%inc from iwrap is given from z down

inc_a=180- inc_iwrap;

sinpitch = sin(deg2rad(pitch));
cospitch = cos(deg2rad(pitch));
sinroll  = sin(deg2rad(roll));
cosroll  = cos(deg2rad(roll));
sindrift = sin(deg2rad(drift));
cosdrift = cos(deg2rad(drift));
cosinc = cos(deg2rad(inc_a));
sininc = sin(deg2rad(inc_a));
cosAzimuth = cos(deg2rad(az_a));
sinAzimuth = sin(deg2rad(az_a));

for azi=1:length(az_a)
    rg=1;
    M = [(cosdrift(rg,azi)*cosroll(rg,azi)-sindrift(rg,azi)*sinpitch(rg,azi)*sinroll(rg,azi)) (-sindrift(rg,azi)*cospitch(rg,azi)) (cosdrift(rg,azi)*sinroll(rg,azi)+...
            sindrift(rg,azi)*sinpitch(rg,azi)*cosroll(rg,azi)) ;...
            (sindrift(rg,azi)*cosroll(rg,azi)+cosdrift(rg,azi)*sinpitch(rg,azi)*sinroll(rg,azi)) (cosdrift(rg,azi)*cospitch(rg,azi)) (sinroll(rg,azi)*sindrift(rg,azi)...
            -cosdrift(rg,azi)*sinpitch(rg,azi)*cosroll(rg,azi));(-sinroll(rg,azi)*cospitch(rg,azi)) (sinpitch(rg,azi)) (cospitch(rg,azi)*cosroll(rg,azi))];
    
    Xa_over_r = [sininc(rg,azi)*cosAzimuth(rg,azi);sininc(rg,azi)*sinAzimuth(rg,azi);cosinc(rg,azi)];
    
    
    
    Xt_over_r = M*Xa_over_r;
    
    inc_t(rg,azi)=acos((Xt_over_r(3)));
    inc_t(rg,azi)=rad2deg(inc_t(rg,azi));
    inc_iwrap_t=180-inc_t;
    az_t(rg,azi)=atan2(Xt_over_r(2),Xt_over_r(1));
    az_t(rg,azi)=rad2deg(az_t(rg,azi));
    az_tt(rg,azi)=90 - az_t(rg,azi);
    az_N(rg,azi)=az_tt(rg,azi) + trackp(rg,azi);
end

    u_neg=find(az_N <0);
    az_N(u_neg)=az_N(u_neg)+360;
    u_360=find(az_N>=360);
    az_N(u_360)=az_N(u_360)-360;

% Peter version
% el=90 - inc_a
% sinel=sin(deg2rad(el));
% cosel=cos(deg2rad(el));
% 
% az=90-az_a
% sinaz=sin(deg2rad(az));
% cosaz=cos(deg2rad(az));
% xa2 = cosel * cosaz;
% ya2 = cosel * sinaz;;
% za2 = -sinel;
% 
% xe = xa2 * cospitch * cosdrift +...
%                ya2 * (sinroll * sinpitch * cosdrift + cosroll * sindrift) +...
%                za2 * (cosroll * sinpitch * cosdrift - sinroll * sindrift);
%        ye = -xa2 * cospitch * sindrift +...
%                ya2 * (cosroll * cosdrift - sinroll * sinpitch * sindrift) +...
%                za2 * ( -cosroll * sinpitch * sindrift - sinroll * cosdrift);
%        ze = -xa2 * sinpitch + ya2 * sinroll * cospitch + za2 * cosroll * cospitch;
% 
% 
% el=90+rad2deg(asin(-ze))
% az_t=rad2deg(atan2(ye,xe))
