clear all; 
close all;


write_u=0;%if you want to write the velocities (true and background) to YAO DAT file
write_coefs=0;%if you want to write the filters coefs
%------------GRID-------------------------------------------------------%

%for plotting

Nlat=58; %Phi
Nlong=87; %Lambda
%The grid in degrees 
load 'meshgrid2.dat';

GR_long=reshape(meshgrid2(:,1),Nlong,Nlat); %will give a Nlong by Nlat matrix 
%containing the long of the points 

GR_lat=reshape(meshgrid2(:,2),Nlong,Nlat); %will give a Nlong by Nlat matrix 
%containing the lat of the points 






%-----------The velocities U and V---------------------------------------%



%Load directly from AVISO folder where the u s for the whole mediterranean:



%UTRUE=the first week

U1=ncread('../obs_aviso/dt_ref_med_merged_madt_uv_20080102_20080102_20100503.nc','Grid_0001');
uaviso1=U1(1:58,end-86:end)./100; %choose the eatern med and make it in m/s

V1=ncread('../obs_aviso/dt_ref_med_merged_madt_uv_20080102_20080102_20100503.nc','Grid_0002');
vaviso1=V1(1:58,end-86:end)./100; %choose the eatern med and make it in m/s



%U the second week

U2=ncread('../obs_aviso/dt_ref_med_merged_madt_uv_20080109_20080109_20100503.nc','Grid_0001');
uaviso2=U2(1:58,end-86:end)./100;

V2=ncread('../obs_aviso/dt_ref_med_merged_madt_uv_20080109_20080109_20100503.nc','Grid_0002');
vaviso2=V2(1:58,end-86:end)./100;


%do the mask manually:
II1=find(isfinite(uaviso1)==0);
uaviso1(II1)=0;
II2=find(isfinite(uaviso2)==0);
uaviso2(II2)=0;

JJ1=find(isfinite(vaviso1)==0);
vaviso1(JJ1)=0;
JJ2=find(isfinite(vaviso2)==0);
vaviso2(JJ2)=0;


%interpolate to get the field every 2 h during the first week, in case need velocity in between:
Nt=7*24/2; %one week in units of delta t
for jt=1:Nt+1
    umod(:,:,jt)=(uaviso1*(Nt+1-jt)./Nt)+ (uaviso2*(jt-1)/Nt);
    vmod(:,:,jt)=(vaviso1*(Nt+1-jt)./Nt)+ (vaviso2*(jt-1)/Nt);
end
    





%change grid from degrees to uniteless

R_earth=6371229; %in meters
delta_x=zeros(Nlong,Nlat);
delta_y=zeros(Nlong,Nlat);

for ilon=1:Nlong-1;
    for jlat=1:Nlat-1;
        
    delta_x(ilon,jlat)=R_earth*(2*pi/360)*(GR_long(ilon+1,jlat)-GR_long(ilon,jlat))...
        .*cos(2*pi*GR_lat(ilon,jlat)/360); 
    delta_y(ilon,jlat)=R_earth*(2*pi/360)*(GR_lat(ilon,jlat+1)-GR_lat(ilon,jlat));  
    
    
end 
end



delta_x(Nlong,1:Nlat)=delta_x(Nlong-1,1:Nlat);
delta_y(Nlong,1:Nlat)=delta_y(Nlong-1,1:Nlat);


delta_x(1:Nlong,Nlat)=delta_x(1:Nlong,Nlat-1);
delta_y(1:Nlong,Nlat)=delta_y(1:Nlong,Nlat-1);



%choose UTRUE=Ufirst week

uu=umod(:,:,1)./delta_x.'; %u has units of pers second
vv=vmod(:,:,1)./delta_y.';

%Choose Uback=Uafter 3days

uu_pert=umod(:,:,35)./delta_x.'; 
vv_pert=vmod(:,:,35)./delta_y.';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%PUT U in YAO format%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%create a Nlat*Nlong by 5 matrix first 
u_yao1=reshape(uu, Nlong*Nlat,1);
v_yao1=reshape(vv, Nlong*Nlat,1);

up_yao1=reshape(uu_pert, Nlong*Nlat,1);
vp_yao1=reshape(vv_pert, Nlong*Nlat,1);
%create the vectors rehsaped
cd1=[]; for k=1:Nlong; cd1=[cd1; k*ones(Nlat,1)]; end
cd2=repmat([1:1:Nlat].',Nlong,1);

Yao_u=[ ones(Nlong*Nlat,1)  cd1 cd2 u_yao1];
Yao_v=[ ones(Nlong*Nlat,1)  cd1 cd2 v_yao1];

Yao_up=[ ones(Nlong*Nlat,1)  cd1 cd2 up_yao1];
Yao_vp=[ ones(Nlong*Nlat,1)  cd1 cd2 vp_yao1];

if (write_u==1)

dlmwrite('../obs_float/uzero.dat',Yao_u,'\t');
dlmwrite('../obs_float/vzero.dat',Yao_v,'\t');
%save UV_0.mat uu vv;
dlmwrite('../obs_float/unext3d.dat',Yao_up,'\t');
dlmwrite('../obs_float/vnext3d.dat',Yao_vp,'\t');
%save UVnext3d.mat uu_pert vv_pert cc1 cc2;
end;







%---------------------FILTER--------------------------------------------%

%%%%%%%%%%%Get filter coeff%%%%%%%%%


%length of filter in m, you can change this to 25 000 or 15 000, etc..
%WHEN you change this, 2 important numbers for the YAO code change, the
%filter order and the filter's time step.
lfil=20000;



load 'mask.dat';
mask2=reshape(mask,87,58);

umask=zeros(Nlong,Nlat); vmask=umask; 
for i=1:Nlong-1
    for j=1:Nlat-1
        umask(i,j)=mask2(i,j).*mask2(i+1,j);
        vmask(i,j)=mask2(i,j).*mask2(i,j+1);
    end
end

[minx gg1]=min(min(delta_x));
[miny gg2]=min(min(delta_y));
itxmin=floor(2*lfil*lfil./minx.^2);
itymin=floor(2*lfil*lfil./miny.^2);


filter_order=ceil((max(itxmin,itymin)+1)./2)
%THIS IS AN IMPORTANT NUMBER, when you change the filterlength it will
%change. IN YAO, this variable is  
%defval OFIL 4 //order of the filter
%in the floater_delta.d file. SO just copy th evalue from here








nbitfil=filter_order*2;%nb of filter passes
%nb of filter passes



dtfil=0.5*lfil*lfil./nbitfil %filter time step
%%%%%%THIS NUMBER IS ALSO IMPORTANT AS IT GOES IN THE floater.h code in the
%%%%%%variable YREAL dtfil=...; As you change the filter length it will
%%%%%%change


%%%%%%filter normalization coefs%%%%%%%%%%%%%%%%&
fileu=umask./sqrt(delta_x.*delta_y);
filev=vmask./sqrt(delta_x.*delta_y);


dx=delta_x;
dy=delta_y;
c1=zeros(Nlong,Nlat); c2=c1; c3=c1; c4=c1; c5=c1; c6=c1; c7=c1; c8=c1; c9=c1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%ucoefs
for i=2:Nlong-1
    for j=2:Nlat-1;
c1(i,j) = -1/dx(i,j)^2-dy(i,j)/(dx(i,j)*dx(i+1,j)*dy(i+1,j))-mask2(i,j-1)*mask2(i,j)*mask2(i+1,j-1)*mask2(i+1,j)*dx(i,j)/(dy(i,j)*dx(i,j-1)*dy(i,j-1))-mask2(i,j)*mask2(i,j+1)*mask2(i+1,j)*mask2(i+1,j+1)/dy(i,j)^2;
c2(i,j) = 1/(dx(i,j)*dx(i+1,j));
c3(i,j) = mask2(i,j)*mask2(i,j+1)*mask2(i+1,j)*mask2(i+1,j+1)*dx(i,j+1)/(dy(i,j)^2*dx(i,j));
c4(i,j) = mask2(i,j-1)*mask2(i,j)*mask2(i+1,j-1)*mask2(i+1,j)/(dy(i,j)*dy(i,j-1));
c5(i,j) = dy(i-1,j)/(dx(i,j)^2*dy(i,j));
c6(i,j) = -1/(dx(i,j)*dy(i,j))+mask2(i,j)*mask2(i,j+1)*mask2(i+1,j)*mask2(i+1,j+1)/(dx(i,j)*dy(i,j));
c7(i,j) = mask2(i,j-1)*mask2(i,j)*mask2(i+1,j-1)*mask2(i+1,j)*dy(i+1,j-1)/(dy(i,j)*dx(i,j-1)*dy(i,j-1))-dx(i+1,j-1)/(dx(i,j)*dx(i+1,j)*dy(i+1,j));
c8(i,j) = -mask2(i,j-1)*mask2(i,j)*mask2(i+1,j-1)*mask2(i+1,j)/(dy(i,j)*dx(i,j-1))+dx(i,j-1)/(dx(i,j)^2*dy(i,j));
c9(i,j) = -mask2(i,j)*mask2(i,j+1)*mask2(i+1,j)*mask2(i+1,j+1)*dy(i+1,j)/(dy(i,j)^2*dx(i,j))+1/(dx(i,j)*dy(i+1,j));
    end
end
c1=c1.*umask; c2=c2.*umask; c3=c3.*umask; 
c4=c4.*umask; c5=c5.*umask; c6=c6.*umask; 
c7=c7.*umask; c8=c8.*umask; c9=c9.*umask; 







%vcoefs
d1=zeros(Nlong,Nlat); d2=d1; d3=d1; d4=d1; d5=d1; d6=d1; d7=d1; d8=d1; d9=d1;
for i=2:Nlong-1
    for j=2:Nlat-1;
d1(i,j) = -mask2(i-1,j)*mask2(i-1,j+1)*mask2(i,j)*mask2(i,j+1)*dy(i,j)/(dx(i,j)*dx(i-1,j)*dy(i-1,j))-mask2(i,j)*mask2(i,j+1)*mask2(i+1,j)*mask2(i+1,j+1)/dx(i,j)^2-1/dy(i,j)^2-dx(i,j)/(dy(i,j)*dx(i,j+1)*dy(i,j+1));
d2(i,j) = mask2(i,j)*mask2(i,j+1)*mask2(i+1,j)*mask2(i+1,j+1)*dy(i+1,j)/(dx(i,j)^2*dy(i,j));
d3(i,j) = 1/(dy(i,j)*dy(i,j+1));
d4(i,j) = dx(i,j-1)/(dy(i,j)^2*dx(i,j));
d5(i,j) = mask2(i-1,j)*mask2(i-1,j+1)*mask2(i,j)*mask2(i,j+1)/(dx(i,j)*dx(i-1,j));
d6(i,j) = -1/(dx(i,j)*dy(i,j))+mask2(i,j)*mask2(i,j+1)*mask2(i+1,j)*mask2(i+1,j+1)/(dx(i,j)*dy(i,j));
d7(i,j) = -mask2(i,j)*mask2(i,j+1)*mask2(i+1,j)*mask2(i+1,j+1)*dx(i,j+1)/(dx(i,j)^2*dy(i,j))+1/(dy(i,j)*dx(i,j+1));
d8(i,j) = dy(i-1,j)/(dy(i,j)^2*dx(i,j))-mask2(i-1,j)*mask2(i-1,j+1)*mask2(i,j)*mask2(i,j+1)/(dx(i,j)*dy(i-1,j));
d9(i,j) = -dy(i-1,j+1)/(dy(i,j)*dx(i,j+1)*dy(i,j+1))+mask2(i-1,j)*mask2(i-1,j+1)*mask2(i,j)*mask2(i,j+1)*dx(i-1,j+1)/(dx(i,j)*dx(i-1,j)*dy(i-1,j));
    end
end


d1=d1.*vmask; d2=d2.*vmask; d3=d3.*vmask; 
d4=d4.*vmask; d5=d5.*vmask; d6=d6.*vmask; 
d7=d7.*vmask; d8=d8.*vmask; d9=d9.*vmask; 


%save into yao format
%get dt value and order of filter
%put mask as ones in yao put a fake mask
%load the fileu /v as the norm 


%%%%%%%%%%%%%%%%%SAVE THESE COEF INTO YAO FORMAT%%%%%%%%%%%%%%%%%%

%1------------------PUT U in YAO format-------------------------------------%
%1-CC
c1_yao=reshape(c1.', Nlong*Nlat,1);
c2_yao=reshape(c2.', Nlong*Nlat,1);
c3_yao=reshape(c3.', Nlong*Nlat,1);
c4_yao=reshape(c4.', Nlong*Nlat,1);
c5_yao=reshape(c5.', Nlong*Nlat,1);
c6_yao=reshape(c6.', Nlong*Nlat,1);
c7_yao=reshape(c7.', Nlong*Nlat,1);
c8_yao=reshape(c8.', Nlong*Nlat,1);
c9_yao=reshape(c9.', Nlong*Nlat,1);

%create the i j indices
cd1=[]; for k=1:Nlong; cd1=[cd1; k*ones(Nlat,1)]; end
cd2=repmat([1:1:Nlat].',Nlong,1);

Yao_CC=[ ones(Nlong*Nlat,1)  cd1 cd2 c1_yao;...
    2*ones(Nlong*Nlat,1)  cd1 cd2 c2_yao;...
    3*ones(Nlong*Nlat,1)  cd1 cd2 c3_yao; ...
    4*ones(Nlong*Nlat,1)  cd1 cd2 c4_yao;...
    5*ones(Nlong*Nlat,1)  cd1 cd2 c5_yao;...
    6*ones(Nlong*Nlat,1)  cd1 cd2 c6_yao;...
    7*ones(Nlong*Nlat,1)  cd1 cd2 c7_yao;...
    8*ones(Nlong*Nlat,1)  cd1 cd2 c8_yao;...
    9*ones(Nlong*Nlat,1)  cd1 cd2 c9_yao;];
    
%2-DD
d1_yao=reshape(d1.', Nlong*Nlat,1);
d2_yao=reshape(d2.', Nlong*Nlat,1);
d3_yao=reshape(d3.', Nlong*Nlat,1);
d4_yao=reshape(d4.', Nlong*Nlat,1);
d5_yao=reshape(d5.', Nlong*Nlat,1);
d6_yao=reshape(d6.', Nlong*Nlat,1);
d7_yao=reshape(d7.', Nlong*Nlat,1);
d8_yao=reshape(d8.', Nlong*Nlat,1);
d9_yao=reshape(d9.', Nlong*Nlat,1);



Yao_DD=[ ones(Nlong*Nlat,1)  cd1 cd2 d1_yao;...
    2*ones(Nlong*Nlat,1)  cd1 cd2 d2_yao;...
    3*ones(Nlong*Nlat,1)  cd1 cd2 d3_yao; ...
    4*ones(Nlong*Nlat,1)  cd1 cd2 d4_yao;...
    5*ones(Nlong*Nlat,1)  cd1 cd2 d5_yao;...
    6*ones(Nlong*Nlat,1)  cd1 cd2 d6_yao;...
    7*ones(Nlong*Nlat,1)  cd1 cd2 d7_yao;...
    8*ones(Nlong*Nlat,1)  cd1 cd2 d8_yao;...
    9*ones(Nlong*Nlat,1)  cd1 cd2 d9_yao;];

%3- Fake mask of ones, to remove from yao later!
maskk=ones(Nlat,Nlong);
mask_yao=reshape(maskk, Nlong*Nlat,1);
Yao_mask=[ ones(Nlong*Nlat,1)  cd1 cd2 mask_yao];

%4- The normalization after finishing the filter
norm_u=fileu.';
nu_yao=reshape(norm_u, Nlong*Nlat,1);
Yao_nu=[ ones(Nlong*Nlat,1)  cd1 cd2 nu_yao];

norm_v=filev.';
nv_yao=reshape(norm_v, Nlong*Nlat,1);
Yao_nv=[ ones(Nlong*Nlat,1)  cd1 cd2 nv_yao];

%2-Save
 
 if (write_coefs==1)
 
dlmwrite('../obs_float/CC.dat',Yao_CC,'\t');
dlmwrite('../obs_float/DD.dat',Yao_DD,'\t');
dlmwrite('../obs_float/Mask.dat',Yao_mask,'\t');
dlmwrite('../obs_float/NU.dat',Yao_nu,'\t');
dlmwrite('../obs_float/NV.dat',Yao_nv,'\t');

 end;
 
%%%%%TO load these in YAO, you need to add these lines, in the ini file (if
%there is a filter option_
%loadstate cfil 0 ij 0 A 1 ../obs_float/CC.dat D
%loadstate dfil 0 ij 0 A 1 ../obs_float/DD.dat D
%loadstate mask 0 ij 0 A 1 ../obs_float/Mask.dat D
%loadstate u_norm 0 ij 0 A 1 ../obs_float/NU.dat D
%loadstate v_norm 0 ij 0 A 1 ../obs_float/NV.dat D