% some functions to calculate statistics
% and some helper functions
%

%----------------------------------------
function x=cutrandnv(len,sigma)
% return vector of normal distributed random numbers up to +- sigma
%
x=randn(6,len);
for j=1:6
  for i=1:len
    while (x(j,i) > sigma)|(x(j,i)<-sigma)
        x(j,i)=randn(1);
    end;
  end;
end;

endfunction;

%----------------------------------------
% calculates the mean of weighted values
% the weight could be, e.g., the charge of a particle
function m=mean(x,w)
sx=size(x);
sw=size(w);

if ((sw(1)~=1)&&(sw(2)~=1))||((sx(1)~=1)&&(sx(2)~=1))
  m=0.0;
  return;
end;
if ((sx(1)==1)&&(sx(2)==1))
  m=x;
  return;
end;


if (sx(1)<sx(2))
  x=x.';
end;

if ((sw(1)==1)&&(sw(2)==1))
  w=ones(sx(1),sx(2));
  sw=size(w);
end;

if (sw(1)>sw(2))
  w=w.';
end;

m=w*x/sum(w);
endfunction;
%----------------------------------------


%----------------------------------------
% calculates the median of weighted values
% the weight could be, e.g., the charge of a particle
% has to be called with correctly sized and sorted arrays!
function m=medianw(x,w)
sx=size(x);
sw=size(w);

if ((sw(1)~=1)&&(sw(2)~=1))||((sx(1)~=1)&&(sx(2)~=1))
  m=0.0;
  return;
end;
if ((sx(1)==1)&&(sx(2)==1))
  m=x;
  return;
end;


w05=sum(w)/2;
imax=length(w);
i=round(imax/2);
wt=sum(w(1:i));
while wt > w05
  wt=wt-w(i);
  i--;
end;
while wt < w05
  i++;
  wt=wt+w(i);
end;

if wt==w05
  if round(i/2)==i/2
    m=(x(i)+x(i+1))/2;
   else
    m=x(i);
  end;
 else
  wt=wt-w(i);
  dx=(x(i)-x(i-1))/w(i)*(w05-wt);
  m=x(i-1)+dx;
end;
  
endfunction;
%----------------------------------------


%----------------------------------------
%calculates the rms of weighted values
%the weight could be, e.g., the charge of a particle
function r=rms(x,w)
sx=size(x);
sw=size(w);

if ((sw(1)~=1)&&(sw(2)~=1))||((sx(1)~=1)&&(sx(2)~=1))||((sx(1)==1)&&(sx(2)==1))
  r=0.0;
  return;
end;

if (sx(1)<sx(2))
  x=x.';
end;

if ((sw(1)==1)&&(sw(2)==1))
  w=ones(sx(1),sx(2));
  sw=size(w);
end;

if (sw(1)>sw(2))
  w=w.';
end;

xm=mean(x,w);
r=sqrt(w*((x-xm).^2)/sum(w));
endfunction;
%----------------------------------------


%----------------------------------------
% calculates the non-normalized projected emittance
function e=emitproj(x,xp,w)
sw=size(w);
sx=size(x);
sxp=size(xp);
if ((sx(1)!=sxp(1))||(sx(2)!=sxp(2)))
  e=0;
  return;
end;
if (sx(1)>sx(2))
  x=x.';
  xp=xp.';
end;
if ((sw(1)==1)&&(sw(2)==1))
  w=w*ones(sx(2),sx(1));
end;
sw=size(w);
sx=size(x);
if (sw==sx)
  w=w.';
end;

vxc=x-mean(x,w);
vyc=xp-mean(xp,w);

e=sqrt(((vxc.*vxc)*w/sum(w))*...
        ((vyc.*vyc)*w/sum(w))-...
        ((vxc.*vyc)*w/sum(w))^2);
endfunction;
%----------------------------------------


%----------------------------------------
% calculates beta function and alpha
function [b,a]=twiss(x,xp,w)
ex=emitproj(x,xp,w);
b=rms(x,w)^2/ex;

n=length(x);
sumxp=0;
for i=1:n
  if x(i)>0
    sumxp=sumxp+xp(i);
  end;
end;

if sumxp>0
  a=-sqrt(rms(x,w)^2*rms(xp,w)^2/ex^2-1);
 else
  a=sqrt(rms(x,w)^2*rms(xp,w)^2/ex^2-1);
end; 
endfunction;
%----------------------------------------


%----------------------------------------
function [xn,xpn,wn]=clear_outliers(x,xp,w)
% delete particle, whose coordinates are far from the center of the distribution.
no=length(x);
sizexo=size(x);
sizexpo=size(xp);
sizewo=size(w);

% we track the original position to be able to put the particles back in the original order
pos=1:no;

% first, delete Inf, NaN, NA and ridiculously far particles
% to speed up we do some tricks to avoid cycling through the complete array
ts=finite(x)+finite(xp)+(abs(x)<1e15)+(abs(xp)<1e15);
[tstmp,idtmp]=sort(ts);
xtmp=x(idtmp);
xptmp=xp(idtmp);
wtmp=w(idtmp);
postmp=pos(idtmp);

i=1;
while (tstmp(i)~=4)&&(i<no)
  i++;
end;
n1=no-i+1;

% for further calculations it is practical to have
% row vectors for x and xp and a column vector for w
if sizexo(1)==1
  xn1=xtmp(i:no).';
 else
  xn1=xtmp(i:no);
end;
if sizexpo(1)==1
  xpn1=xptmp(i:no).';
 else
  xpn1=xptmp(i:no);
end;
if sizewo(1)==1
  wn1=wtmp(i:no);
 else
  wn1=wtmp(i:no).';
end;
posn1=postmp(i:no);

% now gradually cut away particles until rms does not change significantly
% we calculate around median instead of mean
% median is more stable in case there are far outlying particles

% we cut a maximum of 1% of the particles
nmax=round(0.01*no);
% this is a criterium for the stability of the rms at which we stop cutting
cut=10/no+1;

%first we cut in one plane
mx=medianw(xn1,wn1);
[dtmp1,idtmp1]=sort(-abs(xn1-mx));
xtmp1=xn1(idtmp1);
xptmp1=xpn1(idtmp1);
wtmp1=wn1(idtmp1);
postmp1=posn1(idtmp1);

% these steps are required for a trick to speed up calculations in medianw!
[xtmp1s,idx1s]=sort(xtmp1);
wtmp1s=wtmp1(idx1s);

tmx=medianw(xtmp1s,wtmp1s);
rmsxold=sqrt(wtmp1*((xtmp1-tmx).^2)/sum(wtmp1));
wtmp1t=wtmp1;
wtmp1t(1)=0;
wtmp1s(idx1s(1))=0;
tmx=medianw(xtmp1s,wtmp1s);
rmsx=sqrt(wtmp1t*((xtmp1-tmx).^2)/sum(wtmp1t));

i=2;
while (rmsxold/rmsx>cut)&&(i<nmax)
  rmsxold=rmsx;
  wtmp1(i-1)=0;
  wtmp1t=wtmp1;
  wtmp1t(i)=0;
  wtmp1s(idx1s(i))=0;
  tmx=medianw(xtmp1s,wtmp1s);
  rmsx=sqrt(wtmp1t*((xtmp1-tmx).^2)/sum(wtmp1t));
  i++;
end;
xn2=xtmp1(i-1:n1);
xpn2=xptmp1(i-1:n1);
wn2=wtmp1(i-1:n1);
posn2=postmp1(i-1:n1);
n2=n1-i+2;

%then we cut in the other plane
mxp=medianw(xpn2,wn2);
[dtmp2,idtmp2]=sort(-abs(xpn2-mxp));
xtmp2=xn2(idtmp2);
xptmp2=xpn2(idtmp2);
wtmp2=wn2(idtmp2);
postmp2=posn2(idtmp2);

% these steps are required for a trick to speed up calculations in medianw!
[xptmp2s,idxp2s]=sort(xptmp2);
wtmp2s=wtmp2(idxp2s);

tmxp=medianw(xptmp2s,wtmp2s);
rmsxpold=sqrt(wtmp2*((xptmp2-tmxp).^2)/sum(wtmp2));
wtmp2t=wtmp2;
wtmp2t(1)=0;
wtmp2s(idxp2s(1))=0;
tmxp=medianw(xptmp2s,wtmp2s);
rmsxp=sqrt(wtmp2t*((xptmp2-tmxp).^2)/sum(wtmp2t));

i=2;
while (rmsxpold/rmsxp>cut)&&(i<nmax)
  rmsxpold=rmsxp;
  wtmp2(i-1)=0;
  wtmp2t=wtmp2;
  wtmp2t(i)=0;
  wtmp2s(idxp2s(i))=0;
  tmxp=medianw(xptmp2s,wtmp2s);
  rmsxp=sqrt(wtmp2t*((xptmp2-tmxp).^2)/sum(wtmp2t));
  i++;
end;
xn3=xtmp2(i-1:n2);
xpn3=xptmp2(i-1:n2);
wn3=wtmp2(i-1:n2);
posn3=postmp2(i-1:n2);
n3=n2-i+2;


% we bring the remaining particles back in the original order
% and we want to preserve the original orientation of the arrays
[possort,idsort]=sort(posn3);

if sizexo(1)==1
  xn=xn3(idsort).';
 else
  xn=xn3(idsort);
end;

if sizexpo(1)==1
  xpn=xpn3(idsort).';
 else
  xpn=xpn3(idsort);
end;

if sizewo(1)==1
  wn=wn3(idsort);
 else
  wn=wn3(idsort).';
end;

if (no-n1~=0)||(n1-n2~=0)||(n2-n3~=0)
  printf("%d + %d + %d particles excluded.\n",no-n1,n1-n2,n2-n3);
end;
endfunction;
%----------------------------------------


%----------------------------------------
function [xn,xpn,yn,ypn,sn,den,wn]=clear_outliers_6d(ps,w)
% delete particle, whose coordinates are far from the center of the distribution.

no=length(ps(:,1));

sizew=size(w);
if sizew(1)~=1
  w=w';
end;
if length(w)~=no
  w=ones(1,no);
end;

% we track the original position to be able to put the particles back in the original order
pos=1:no;

% first, delete Inf, NaN, NA and ridiculously far particles
% to speed up we do some tricks to avoid cycling through the complete array
ts=sum(finite(ps)+(abs(ps)<1e15),2);
[tstmp,pstmp,wtmp,postmp,idtmp]=sortmatrix(ts,ps,w,pos);
i=1;
while (tstmp(i)~=12)&&(i<no)
  i++;
end;
n1=no-i+1;
posn1=postmp(i:no);
wn1=wtmp(i:no);
psn1=pstmp(i:no,:);

% now gradually cut away particles until rms does not change significantly
% we perform the cut in each plane separately
% we calculate around median instead of mean
% median is more stable in case there are far outlying particles

% we cut a maximum of 1% of the particles
nmax=round(0.01*no);
% this is a criterium for the stability of the rms at which we stop cutting
cut=10/no+1;

c1=psn1(:,1);
c2=psn1(:,2);
c3=psn1(:,3);
c4=psn1(:,4);
c5=psn1(:,5);
c6=psn1(:,6);

wt=wn1;
post=posn1;
nt=n1;

for p=1:12
  switch(p)
    case 1
      v=c1;
    case 2
      v=c2;
    case 3
      v=c3;
    case 4
      v=c4;
    case 5
      v=c5;
    case 6
      v=c6;
    case 7
      v=c1;
    case 8
      v=c2;
    case 9
      v=c3;
    case 10
      v=c4;
    case 11
      v=c5;
    case 12
      v=c6;
  endswitch;
  
  mx=medianw(v,wt);
  [dtmp,idtmp]=sort(-abs(v-mx));
  vtmp=v(idtmp);
  wtmp=wt(idtmp);

  % the first steps are required for a trick to speed up calculations in medianw!
  [vtmps,ids]=sort(vtmp);
  wtmps=wtmp(ids);

  tmv=medianw(vtmps,wtmps);
  rmsvold=sqrt(wtmp*((vtmp-tmv).^2)/sum(wtmp));
  wtmpt=wtmp;
  wtmpt(1)=0;
  wtmps=wtmpt(ids);
  tmv=medianw(vtmps,wtmps);
  rmsv=sqrt(wtmpt*((vtmp-tmv).^2)/sum(wtmpt));
  i=2;
  while (rmsvold/rmsv>cut)&&(i<nmax)
    rmsvold=rmsv;
    wtmp(i-1)=0;
    wtmpt=wtmp;
    wtmpt(i)=0;
    wtmps=wtmpt(ids);
    tmv=medianw(vtmps,wtmps);
    rmsv=sqrt(wtmpt*((vtmp-tmv).^2)/sum(wtmpt));
    i++;
%    p=p
  end;

  c1t=c1(idtmp);
  c2t=c2(idtmp);
  c3t=c3(idtmp);
  c4t=c4(idtmp);
  c5t=c5(idtmp);
  c6t=c6(idtmp);
  postt=post(idtmp);

  c1=c1t(i-1:nt);
  c2=c2t(i-1:nt);
  c3=c3t(i-1:nt);
  c4=c4t(i-1:nt);
  c5=c5t(i-1:nt);
  c6=c6t(i-1:nt);
  wt=wtmp(i-1:nt);
  post=postt(i-1:nt);

  [possort,idsort]=sort(post);
  c1=c1(idsort);
  c2=c2(idsort);
  c3=c3(idsort);
  c4=c4(idsort);
  c5=c5(idsort);
  c6=c6(idsort);
  wt=wt(idsort);
  post=possort;
  
  nt=nt-i+2;
end;


% we bring the remaining particles back in the original order
[possort,idsort]=sort(post);

xn =c2(idsort);
xpn=c5(idsort);
yn =c3(idsort);
ypn=c6(idsort);
sn =c4(idsort);
den=c1(idsort);
wn =wt(idsort);

if (no-n1~=0)||(n1-nt~=0)
  printf("%d + %d particles excluded.\n",no-n1,n1-nt);
end;

endfunction;
%----------------------------------------



%----------------------------------------
function [tsn,psn,wn,posn,idn]=sortmatrix(ts,ps,w,pos)
c1=ps(:,1);
c2=ps(:,2);
c3=ps(:,3);
c4=ps(:,4);
c5=ps(:,5);
c6=ps(:,6);

[tstmp,idtmp]=sort(ts);

c1n=c1(idtmp);
c2n=c2(idtmp);
c3n=c3(idtmp);
c4n=c4(idtmp);
c5n=c5(idtmp);
c6n=c6(idtmp);

tsn=tstmp;
psn=[c1n,c2n,c3n,c4n,c5n,c6n];
wn=w(idtmp);
posn=pos(idtmp);
idn=idtmp;

endfunction;
%----------------------------------------


%----------------------------------------
function bn=units_std_to_placet(bo)
bn(:,1)=bo(:,1)/1e9;
bn(:,2)=bo(:,2)/1e-6;
bn(:,3)=bo(:,3)/1e-6;
bn(:,4)=bo(:,4)/1e-6;
bn(:,5)=bo(:,5)/1e-6;
bn(:,6)=bo(:,6)/1e-6;
endfunction;
%----------------------------------------

%----------------------------------------
function bn=units_placet_to_std(bo)
bn(:,1)=bo(:,1)*1e9;
bn(:,2)=bo(:,2)*1e-6;
bn(:,3)=bo(:,3)*1e-6;
bn(:,4)=bo(:,4)*1e-6;
bn(:,5)=bo(:,5)*1e-6;
bn(:,6)=bo(:,6)*1e-6;
endfunction;
%----------------------------------------

%----------------------------------------
function print_mean_rms(beam)
w=ones(length(beam),1);
[xn,xpn,yn,ypn,sn,en,wn]=clear_outliers_6d(beam,w);
beamcore=[en,xn,yn,sn,xpn,ypn];

gamma=mean(beam(:,1),1)/(0.5109989*1e6);
gammacore=mean(beamcore(:,1),1)/(0.5109989*1e6);

printf("---------------------------------------------------------------------\n");
printf("mean e =%3.15e / %3.15e\n",mean(beam(:,1),1),mean(beamcore(:,1),1));
printf("mean x =%3.15e / %3.15e\n",mean(beam(:,2),1),mean(beamcore(:,2),1));
printf("mean y =%3.15e / %3.15e\n",mean(beam(:,3),1),mean(beamcore(:,3),1));
printf("mean z =%3.15e / %3.15e\n",mean(beam(:,4),1),mean(beamcore(:,4),1));
printf("mean xp=%3.15e / %3.15e\n",mean(beam(:,5),1),mean(beamcore(:,5),1));
printf("mean yp=%3.15e / %3.15e\n",mean(beam(:,6),1),mean(beamcore(:,6),1));
printf("rms e =%3.15e / %3.15e\n", rms(beam(:,1),1), rms(beamcore(:,1),1));
printf("rms e/mean e=%3.15e / %3.15e\n",rms(beam(:,1),1)/mean(beam(:,1),1),rms(beamcore(:,1),1)/mean(beamcore(:,1),1));
printf("rms x =%3.15e / %3.15e\n",rms(beam(:,2),1),rms(beamcore(:,2),1));
printf("rms y =%3.15e / %3.15e\n",rms(beam(:,3),1),rms(beamcore(:,3),1));
printf("rms z =%3.15e / %3.15e\n",rms(beam(:,4),1),rms(beamcore(:,4),1));
printf("rms xp=%3.15e / %3.15e\n",rms(beam(:,5),1),rms(beamcore(:,5),1));
printf("rms yp=%3.15e / %3.15e\n",rms(beam(:,6),1),rms(beamcore(:,6),1));
printf("emit x=%3.15e / %3.15e\n",gamma*emitproj(beam(:,2),beam(:,5),1),gammacore*emitproj(beamcore(:,2),beamcore(:,5),1));
printf("emit y=%3.15e / %3.15e\n",gamma*emitproj(beam(:,3),beam(:,6),1),gammacore*emitproj(beamcore(:,3),beamcore(:,6),1));

[bx,ax]=twiss(beam(:,2),beam(:,5),1);
[bxcore,axcore]=twiss(beamcore(:,2),beamcore(:,5),1);
printf("beta x=%3.15e / %3.15e\n",bx,bxcore);
printf("alpha x=%3.15e / %3.15e\n",ax,axcore);

[by,ay]=twiss(beam(:,3),beam(:,6),1);
[bycore,aycore]=twiss(beamcore(:,3),beamcore(:,6),1);
printf("beta y=%3.15e / %3.15e\n",by,bycore);
printf("alpha y=%3.15e / %3.15e\n",ay,aycore);
printf("---------------------------------------------------------------------\n");

endfunction;
%----------------------------------------

%----------------------------------------
function save_mean_rms(beam,file)
w=ones(length(beam),1);
[xn,xpn,yn,ypn,sn,en,wn]=clear_outliers_6d(beam,w);
beamcore=[en,xn,yn,sn,xpn,ypn];

gamma=mean(beam(:,1),1)/(0.5109989*1e6);
gammacore=mean(beamcore(:,1),1)/(0.5109989*1e6);

fid=fopen(file,"at");

fprintf(fid,"---------------------------------------------------------------------\n");
fprintf(fid,"mean e =%3.15e / %3.15e\n",mean(beam(:,1),1),mean(beamcore(:,1),1));
fprintf(fid,"mean x =%3.15e / %3.15e\n",mean(beam(:,2),1),mean(beamcore(:,2),1));
fprintf(fid,"mean y =%3.15e / %3.15e\n",mean(beam(:,3),1),mean(beamcore(:,3),1));
fprintf(fid,"mean z =%3.15e / %3.15e\n",mean(beam(:,4),1),mean(beamcore(:,4),1));
fprintf(fid,"mean xp=%3.15e / %3.15e\n",mean(beam(:,5),1),mean(beamcore(:,5),1));
fprintf(fid,"mean yp=%3.15e / %3.15e\n",mean(beam(:,6),1),mean(beamcore(:,6),1));
fprintf(fid,"rms e =%3.15e / %3.15e\n", rms(beam(:,1),1), rms(beamcore(:,1),1));
fprintf(fid,"rms e/mean e=%3.15e / %3.15e\n",rms(beam(:,1),1)/mean(beam(:,1),1),rms(beamcore(:,1),1)/mean(beamcore(:,1),1));
fprintf(fid,"rms x =%3.15e / %3.15e\n",rms(beam(:,2),1),rms(beamcore(:,2),1));
fprintf(fid,"rms y =%3.15e / %3.15e\n",rms(beam(:,3),1),rms(beamcore(:,3),1));
fprintf(fid,"rms z =%3.15e / %3.15e\n",rms(beam(:,4),1),rms(beamcore(:,4),1));
fprintf(fid,"rms xp=%3.15e / %3.15e\n",rms(beam(:,5),1),rms(beamcore(:,5),1));
fprintf(fid,"rms yp=%3.15e / %3.15e\n",rms(beam(:,6),1),rms(beamcore(:,6),1));
fprintf(fid,"emit x=%3.15e / %3.15e\n",gamma*emitproj(beam(:,2),beam(:,5),1),gammacore*emitproj(beamcore(:,2),beamcore(:,5),1));
fprintf(fid,"emit y=%3.15e / %3.15e\n",gamma*emitproj(beam(:,3),beam(:,6),1),gammacore*emitproj(beamcore(:,3),beamcore(:,6),1));

[bx,ax]=twiss(beam(:,2),beam(:,5),1);
[bxcore,axcore]=twiss(beamcore(:,2),beamcore(:,5),1);
fprintf(fid,"beta x=%3.15e / %3.15e\n",bx,bxcore);
fprintf(fid,"alpha x=%3.15e / %3.15e\n",ax,axcore);

[by,ay]=twiss(beam(:,3),beam(:,6),1);
[bycore,aycore]=twiss(beamcore(:,3),beamcore(:,6),1);
fprintf(fid,"beta y=%3.15e / %3.15e\n",by,bycore);
fprintf(fid,"alpha y=%3.15e / %3.15e\n",ay,aycore);
fprintf(fid,"---------------------------------------------------------------------\n");

fclose(fid);
endfunction;
%----------------------------------------


%----------------------------------------
function save_beam_params(beam,file)

%w=ones(length(beam),1);
%[xn,xpn,yn,ypn,sn,en,wn]=clear_outliers_6d(beam,w);
%beam=[en,xn,yn,sn,xpn,ypn];

rmss=rms(beam(:,4),1);
meanenergy=mean(beam(:,1),1);
gamma=meanenergy/(0.5109989*1e6);

espread=rms(beam(:,1),1)/meanenergy;
lemit=emitproj(beam(:,4),beam(:,1)-meanenergy,1);
uncespread=lemit/(rmss*meanenergy);
echirp=sqrt((espread^2-uncespread^2)/rmss^2);

[betax,alphax]=twiss(beam(:,2),beam(:,5),1);
emitnx=gamma*emitproj(beam(:,2),beam(:,5),1);
[betay,alphay]=twiss(beam(:,3),beam(:,6),1);
emitny=gamma*emitproj(beam(:,3),beam(:,6),1);

outdata=[rmss;meanenergy;uncespread;echirp;betax;alphax;emitnx;betay;alphay;emitny];

fid=fopen(file,"wt");
fprintf(fid,"%24.14e\n",outdata);
fclose(fid);
endfunction;
%----------------------------------------


%----------------------------------------
function [xn,xpn,yn,ypn]=delete_residual_dispersion(beam)

x=beam(:,2);
xp=beam(:,5);
y=beam(:,3);
yp=beam(:,6);
s=beam(:,4);
de=beam(:,1);

meanx=mean(x,1);
rmsx=rms(x,1)
meanxp=mean(xp,1);
rmsxp=rms(xp,1)
meany=mean(y,1);
rmsy=rms(y,1)
meanyp=mean(yp,1);
rmsyp=rms(yp,1)
meande=mean(de,1);
rmsde=rms(de,1);

exde=emitproj(x,de,1);
si=0;
for i=1:length(de)
  if (de(i)-meande)>0
    si=si+(x(i)-meanx);
  end;
end;
si=si/abs(si);
yy=-sqrt(rmsx*rmsx-exde*exde/(rmsde*rmsde));
xn=x+si*yy/rmsde*(de-meande);
rms(xn,1)

expde=emitproj(xp,de,1);
si=0;
for i=1:length(de)
  if (de(i)-meande)>0
    si=si+(xp(i)-meanxp);
  end;
end;
si=si/abs(si);
yy=-sqrt(rmsxp*rmsxp-expde*expde/(rmsde*rmsde));
xpn=xp+si*yy/rmsde*(de-meande);
rms(xpn,1)

eyde=emitproj(y,de,1);
si=0;
for i=1:length(de)
  if (de(i)-meande)>0
    si=si+(y(i)-meany);
  end;
end;
si=si/abs(si);
yy=-sqrt(rmsy*rmsy-eyde*eyde/(rmsde*rmsde));
yn=y+si*yy/rmsde*(de-meande);
rms(yn,1)

eypde=emitproj(yp,de,1);
si=0;
for i=1:length(de)
  if (de(i)-meande)>0
    si=si+(yp(i)-meanyp);
  end;
end;
si=si/abs(si);
yy=-sqrt(rmsyp*rmsyp-eypde*eypde/(rmsde*rmsde));
ypn=yp+si*yy/rmsde*(de-meande);
rms(ypn,1)

endfunction;

%----------------------------------------
