startup
%SEG=load('ColdIOP.txt');
%ASC=load('ColdASC.txt');
%steprsp=load('step.txt');
stim=steprsp(:,1);
sval=interp1(stim,steprsp(:,2),SEG(:,1));
cr=(1-sval);



dt=1.99968;
dt=dt-2;
t1=0;
t2=30;
tt=ASC(:,1);
ind=find(and(tt>=t1-dt,tt<t2-dt));
ta=ASC(ind,1)+dt;
a=ASC(ind,2)*1000+0.04;



tt=SEG(:,1);
ind=find(and(tt>=t1,tt<t2));
ts=SEG(ind,1);
ssa=interp1(ta,a,ts);

g=-0.06666;
s1=g*SEG(ind,2);
s2=g*SEG(ind,3);
s3=g*SEG(ind,4);
s4=g*SEG(ind,5);

m1=mean(s1(end/4:end));
m2=mean(s2(end/4:end));
m3=mean(s3(end/4:end));
m4=mean(s4(end/4:end));
mm=m1+m2+m3+m4;

v=45.28;
s1=s1+v*cr *m1/mm;
s2=s2+v*cr *m2/mm;
s3=s3+v*cr *m3/mm;
s4=s4+v*cr *m4/mm;

ss=s1+s2+s3+s4;



t1=7.0;
t2=7.6;
ind=find(and(ts>=t1,tt<t2));
ts=ts(ind);
s1=s1(ind);
s2=s2(ind);
s3=s3(ind);
s4=s4(ind);
ss=ss(ind);
ssa=ssa(ind);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% find peaks of 00 using the quadrant that sees most 00 (s3)
if 1
    [pk,vpk,pk2,vpk2]=pkFind(ts,s3,0,0,0.8);
end
plot(ts,ss,ts,s1,ts,s2,ts,s3,ts,s4);
legend('sum','1','2','3','4');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% find peaks of 10 in ss
[pkss]=pkFind(ts,ss,0,0,0.4);
[pk6]=pkFind(ts,ss,0,0,0.8);
pkss=setdiff(pkss,pk6);
plot(ts,ss,'-b',pkss,interp1(ts,ss,pkss),'dr',pk,interp1(ts,ss,pk),'dm')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% find peaks of 10 in s1
[pks1]=pkFind(ts,s1,0,0,0.38);
pks1=pks1([2:2:end]);
plot(ts,ss,'-b',pkss,interp1(ts,ss,pkss),'dk',pk,interp1(ts,ss,pk),'dm',...
     ts,s1,'-r',pks1,interp1(ts,s1,pks1),'dk',pk,interp1(ts,s1,pk),'dm')
grid on
xlabel('time (sec)')
ylabel('power (cts)')
legend('AS\_C\_SUM','10','00','AS\_C\_SEG1')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% find peaks of 10 in s2
[pks2]=pkFind(ts,s2,0,0,0.3);
pks2=pks2([2:2:end]);
plot(ts,ss,'-b',pkss,interp1(ts,ss,pkss),'dk',pk,interp1(ts,ss,pk),'dm',...
     ts,s2,'-r',pks2,interp1(ts,s2,pks2),'dk',pk,interp1(ts,s2,pk),'dm',...
     ts,s1,'-k',pks1,interp1(ts,s1,pks1),'dk',pk,interp1(ts,s1,pk),'dm')
grid on
xlabel('time (sec)')
ylabel('power (cts)')
legend('AS\_C\_SUM','10','00','AS\_C\_SEG2')




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fit amplitude shape (not used)
val=spline(pk,interp1(ts,ss,pk),ts);
plot(ts,ss,ts,s1,ts,s2,ts,s3,ts,s4,ts,val);
legend('sum','1','2','3','4','val');
plot(ts,ss./val,ts,s1./val,ts,s2./val,ts,s3./val,ts,s4./val);
legend('sum','1','2','3','4');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fit the SRM motiopn with a spline and cast in degree
ct=1:length(pk);
cct=spline(pk,ct,ts);
ind=find(and(ts>=t1,ts<t2));
deg=360*mod(cct,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% statistics on 10 peaks
pkssdeg=360*mod(spline(pk,ct,pkss),1);
pks2deg=360*mod(spline(pk,ct,pks2),1);
pks1deg=360*mod(spline(pk,ct,pks1),1);
pkssdeg_m=mean(pkssdeg);
pks2deg_m=mean(pks2deg);
pks1deg_m=mean(pks1deg);

pkssdeg_std=std(pkssdeg);
pks2deg_std=std(pks2deg);
pks1deg_std=std(pks1deg);
pkssdeg_stdm=pkssdeg_std/sqrt(length(pkssdeg));
pks2deg_stdm=pks2deg_std/sqrt(length(pks2deg));
pks1deg_stdm=pks1deg_std/sqrt(length(pks1deg));

rtgp=mean([pks1deg_m]);
rtgp_state=max([pkssdeg_stdm,pks2deg_stdm,pks1deg_stdm]);
rtgp_syste=max([abs(pkssdeg_m-pks2deg_m),abs(pks1deg_m-pkssdeg_m),abs(pks2deg_m-pks1deg_m)]);

plot(deg(ind),ss(ind),'.b',deg(ind),s2(ind),'.r',deg(ind),s1(ind),'.k',...
    [pks1deg_m,pks1deg_m],[0,55],'-m',[pks2deg_m,pks2deg_m],[0,55],'-m',2*[pks1deg_m,pks1deg_m],[0,55],'-m',2*[pks2deg_m,pks2deg_m],[0,55],'-m');
legend('AS\_C\_SUM','AS\_C\_SEG2')
xlabel('Gouy phase (deg, round trip)')
ylabel('power (cts)')
title(['SRC one-way Gouy phase measurement: ',num2str(rtgp/2,4),'deg +-',num2str(rtgp_state/2,2),'deg (statistical error) +-',num2str(rtgp_syste/2,2),'deg (systematic error)']);
grid on
axis([0,180,0,36])


if 0
end

