%ITERATIVE_SOLVER_alpha_uo_CB.m
%
% Charles Brunette 2020/05/15
%
% Fitting Alpha and Uo parameters in the free drift problem.
%
% We take an iterative approach:
%
% 1) Evaluate set of alpha parameters (globally) using Uo initial guess.
% 2) Evaluate Uo (locally) using alpha fitted values.
% 3) Re-evaluate set of alpha (globally) with new Uo estimates.
% 4) Repeat until convergence or tolerance threshold is reached.
%
%% Common

% Paths etc.
%addpath('/aos/home/charles/Desktop/Matlab/') % -> SIKU
addpath('/Users/charlesbrunette/Dropbox/FREEDRIFT2020/Code/Matlab/') % -> MACBOOK
FAITPaths_LOCAL_CB
Make_EASE_grid_CB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Number of iterations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nit = 5; % 5 works well!

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Wind sourcec
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
WindType = 'ERA5'; %-> ERA5
                   %-> ERAi

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Make Cell structure
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CompileCELLstruct = 'no';
SaveCELLstruct = 'no';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Functionnal form choice
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FFchoice = 'ThetaH'; 
% 'un' -> alpha = (alpha_fd - (alpha_fd-alpha_ci)*exp(-c(1-A))
% 'deux' -> u_fd - u_ci; u_ci = sqrt((alpha_fd*ua)^2 - beta*h*exp(-c(1-A)))
% 'trois' -> u_fd - u_ci; u_ci = alpha_fd*ua - beta*h*exp(-c(1-A))
% 'vanilla' -> ui = alpha_fd*ua + u_w
% 'thicknessXP' -> alpha only function of thickness such that alpha = alpha_fd - beta*h
% 'ComboAh' -> alpha function of both thickness and concentration
% 'UoSubstitute' -> replacing Uo by beta*Ui
% 'ThetaH' -> adding a dependence of the turning angle on thickness
% 'AtanH' -> An arctangent functional with dependence on h!
% 'AlphaCNoUw' -> basic basic basic: just a fitted alpha and theta without
% ocean currents

%%%%% Others
if isequal(FFchoice,'AlphaCNoUw'); nit=1; end

%% Import XDATA & YDATA

%The input data can be accessed in two formats:
%Matrix format: all value pairs are listed one after the other -> for alpha
%parameters fit
%Cell format: each cell has the value corresponding to one grid cell -> for
%Uo fitting

%%%%%%%%%
        %Here instead of  just loading the cell structures; we should be processing
        %them in-script so we are sure that they corerspond to the exact same data
        %that is being used as a table in the solver. We can work out a switch -
        %process the table is optional but for matters of saving time it could be
        %turned off and downloaded from a file!

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

%%%% LIST / MATRIX for alpha estimate

TablePath = '/Users/charlesbrunette/Dropbox/FREEDRIFT2020/Analysis/Freedrift/Data/';
TableName = 'DATA_TABLE_DRIFT.mat';
datapath = [TablePath TableName];
load(datapath);

%Time crunch: only for time constraints %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% indt = find(ismember(DATA_TABLE_DRIFT.year, [1979:2015])); 
% DATA_TABLE_DRIFT = DATA_TABLE_DRIFT(indt,:);
%  DATA_TABLE_DRIFT.year <= 2000 );                                   %REMOVE THIS AFTERRERERERERE
%             ismember(DATA_TABLE_DRIFT.month,[1 2 3 4 5 6])); %-> 'Winter'
%             ismember(DATA_TABLE_DRIFT.month,[7 8 9 10 11 12])); %-> 'Summer'
%             DATA_TABLE_DRIFT.year < 2000 ); % last line: adding a temporal constrain

% Discard Nan values and select data where ice concentration is nonzero:
ind = find(~isnan(DATA_TABLE_DRIFT.u_buoy) & ...
           ~isnan(DATA_TABLE_DRIFT.v_buoy) & ...
           ~isnan(DATA_TABLE_DRIFT.u_ERA5) & ...  %%%% CHANGE TO ERA5/ERAi
           ~isnan(DATA_TABLE_DRIFT.v_ERA5) & ...  %%%% CHANGE TO ERA5/ERAi
           ~isnan(DATA_TABLE_DRIFT.SIC)    & ...
            DATA_TABLE_DRIFT.SIC > 0       & ...
           ~isnan(DATA_TABLE_DRIFT.h_piomas) & ...
            DATA_TABLE_DRIFT.h_piomas > 0);
                    
% POLE HOLE FIX -> since we use a parameterization based on PIOMAS thickness, 
% we can safely use the data in the polehole region % Essentially any
% experiment not limited by a dependence on concentration!
if sum(strcmp(FFchoice,{'vanilla';'thicknessXP';'AtanH'; 'AlphaCNoUw'; 'ThetaH' })) ~=0 %i.e. is the functional form choice a function of thickness... 
    %First step make polygons around pole hole for each period 1979-1987;
    %1988-2007; 2008-now;
    gridhole =zeros(361,361); gridhole(EASElat>=83.5)=1; B = bwboundaries(gridhole); b7987 = B{1};
    gridhole =zeros(361,361); gridhole(EASElat>=86.2)=1; B = bwboundaries(gridhole); b8807 = B{1};
    gridhole =zeros(361,361); gridhole(EASElat>=88.5)=1; B = bwboundaries(gridhole); b0821 = B{1};
    
    i7987 = find(DATA_TABLE_DRIFT.year>=1979 & DATA_TABLE_DRIFT.year<=1987);
        isin7987 = inpolygon(DATA_TABLE_DRIFT.EASE_x(i7987),DATA_TABLE_DRIFT.EASE_y(i7987),b7987(:,2),b7987(:,1));
        i7987(isin7987==0)=[];
    i8807 = find(DATA_TABLE_DRIFT.year>=1988 & DATA_TABLE_DRIFT.year<=2007);
        isin8807 = inpolygon(DATA_TABLE_DRIFT.EASE_x(i8807),DATA_TABLE_DRIFT.EASE_y(i8807),b8807(:,2),b8807(:,1));
        i8807(isin8807==0)=[];
    i0821 = find(DATA_TABLE_DRIFT.year>=2008 & DATA_TABLE_DRIFT.year<=2021);
        isin0821 = inpolygon(DATA_TABLE_DRIFT.EASE_x(i0821),DATA_TABLE_DRIFT.EASE_y(i0821),b0821(:,2),b0821(:,1));
        i0821(isin0821==0)=[]; 
  
        phfixlist = [i7987; i8807;i0821]; % final: list of points in the pole hole region that we can salvage for thickness experiments!      
end

% Take the union of selection index + pole hole fix!
if sum(strcmp(FFchoice,{'vanilla';'thicknessXP';'AtanH'; 'AlphaCNoUw'; 'ThetaH' })) ~=0
    masterind = union(ind, phfixlist);
else
    masterind = ind;
end
    
% Narrow down data table:
DATA = DATA_TABLE_DRIFT(masterind,:);

%For subset purposes: Select only part of the record:
%DATAt = DATA(1:5:end,:);
%DATA(1:5:end,:)=[]; %%%%%% NEED TO COMMENT THAT OUT FOR NORMAL RUNS!!!!


%%%%%%%
%%%%%%%%
% BUILD OR LOAD CELL structure for Uo estimates

if isequal(CompileCELLstruct,'yes')
    [XDATA_cell,YDATA_cell,PPFw_cell,TimeCell,Datapoint_numbers] = ...
        MAKE_CELL_STRUCTURE(DATA,SaveCELLstruct,WindType);
else
   load([TablePath 'XDATA_YDATA_cells.mat']);
end
%%%%%%%%
%%%%%%%

%Find index of grid cells where there are x-y data pairs:
Ind_isdata = find(Datapoint_numbers>0); 
Length_vector = length(Ind_isdata);

%Keep grid cells where data is present:
xdata_fd = XDATA_cell(Ind_isdata,:);    
ydata_fd = YDATA_cell(Ind_isdata,:);

Input_time_cell = TimeCell(Ind_isdata);

X_input_cell = xdata_fd;
Y_input_cell = ydata_fd;

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

%Find position
pos = round(DATA.EASE_x+1) + round(DATA.EASE_y+1-1).*361;

%Intersection between Individual buoy locations and Uo estimates:
Intersection_position = intersect(pos,Ind_isdata);

Ind_common = find(ismember(pos,Intersection_position));

%Create X & Y matrix with only the relevant data
Y_input_mat = [DATA.u_buoy(Ind_common) DATA.v_buoy(Ind_common)];
if isequal(WindType,'ERA5')
    X_input_mat = [DATA.u_ERA5(Ind_common) DATA.v_ERA5(Ind_common) DATA.SIC(Ind_common) DATA.h_piomas(Ind_common)]; %-> runing with ERA5
else
    X_input_mat = [DATA.u_ERAi(Ind_common) DATA.v_ERAi(Ind_common) DATA.SIC(Ind_common) DATA.h_piomas(Ind_common)]; % -> for testing with ERAi
end
Year_mat = DATA.year(Ind_common);

Positions_in_vector = pos(Ind_common);

               
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            %     % Synthetic dataset:
            %     % To test  the routine's performance, we create a dataset for which we know
            %     % the exact solution and see how well it retrieves the solution:
            % 
            %     % XDATA -> Ua, Va, SIC: we can use the xdata we have in our real problem:
            %     %X_input_mat
            %     %X_input_cell
            % 
            %     %YDATA create a set of Ui, Vi that corresponds to:
            %     % Ui = (alpha_fd - (alpha_fd-alpha_ci)*exp(-C(1-A)))*exp(i*theta)*Ua + Uo
            %      alpha_fd = 2.0;
            %      alpha_ci = 1.0;
            %      C = 5.0;
            %      Theta = 25; % degrees
            %     % Uo -> round climatological currents
            %      Noice_Mag = 1; % standard deviation of normally distributed error in cm/s
            %     
            %     disp('RUNNING SYNTHETIC DATASET EXPERIMENT')
            % 
            %     %Synthetic Uo - linear fit in each data cluster:
            %     uo_sy = NaN(length(xdata_fd),1); vo_sy = uo_sy;
            %     for j = 1:length(xdata_fd)
            %         p = polyfit(xdata_fd{j,1} + xdata_fd{j,2}*1i,ydata_fd{j,1} + ydata_fd{j,2}*1i,1);
            %         uo_sy(j)= real(p(2));
            %         vo_sy(j)= imag(p(2));
            %     end
            %     uo_sy = round(uo_sy);
            %     vo_sy = round(vo_sy);
            % 
            %     Y_synth = cell(size(ydata_fd,1),2);
            %     for k = 1:length(Y_synth)
            %     
            %         [ui_sy,vi_sy] = synthetic_ui(cell2mat(xdata_fd(k,1:3)),alpha_fd,alpha_ci,C,Theta,[uo_sy(k) vo_sy(k)],Noise_Mag);
            %         Y_synth{k,1} = ui_sy;
            %         Y_synth{k,2} = vi_sy;
            %     end
            % 
            %     Y_input_cell = Y_synth;    
            %     Y_input_mat = cell2mat(Y_synth);
            %     disp('Synth Data ready!')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

disp('data imported!')
%% Set Alpha & Uo Tables & initial values

%Initial guess for Uo - linear fit in each data cluster:
uo_i = NaN(length(xdata_fd),1); vo_i = uo_i;
for j = 1:length(xdata_fd)
    p = polyfit(xdata_fd{j,1} + xdata_fd{j,2}*1i,ydata_fd{j,1} + ydata_fd{j,2}*1i,1);
    uo_i(j)= real(p(2));
    vo_i(j)= imag(p(2));
end

%If testing different inital Uo guesses:
%uo_i = ones(length(xdata_fd),1).*50; vo_i = uo_i;

% Uo table
UO = NaN(length(xdata_fd),nit+1); VO = UO;
UO(:,1) = uo_i;
VO(:,1) = vo_i;

%Alpha table
ALPHA = NaN(5,nit+1);
%ALPHA(:,1) = zeros(5,1); %-> Initial guess zeros/
ALPHA(:,1) = ones(5,1);  %-> Initial guess ones
disp('tables ready!')
%% Iterative parameter fitting

for i = 1:nit
  disp(['Starting loop ' num2str(i)]); 
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
% Alpha part:

    % ADD Uo & Vo:
    %It's a little tricky: Scan through UO & VO, for each one, associate
    %the value to all data points in the X_input_mat within this grid cell:
    uo_sync = NaN(length(X_input_mat),1); vo_sync = uo_sync;
    for j=1:length(Ind_isdata)
        jj = find(Positions_in_vector==Ind_isdata(j));
        if ~isempty(jj)
            uo_sync(jj)= UO(j,i);
            vo_sync(jj)= VO(j,i);
        end
    end
     disp('Uo & Vo synced!');
     
    tic
    
%%    
    ALPHA(:,i+1) = Alpha_fit([X_input_mat uo_sync vo_sync],Y_input_mat,ALPHA(:,i),FFchoice);
    
%%
    disp('Alpha fit done!');
    toc
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Uo part
  if ~isequal(FFchoice,'UoSubstitute') && ~isequal(FFchoice,'AlphaCNoUw')  %-> UoSubstitute method does not evaluate Uo, otherwise Uo is evaluated
    tic
    [uo_new, vo_new] = U_OCN_fit(X_input_cell,Y_input_cell, ALPHA(:,i+1), [UO(:,i), VO(:,i)],FFchoice,i);
    UO(:,i+1) = uo_new;
    VO(:,i+1) = vo_new;
    disp('Uo fit done!');
    toc
  end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
 
end

%% Last sync of Uo to calculate rmse Ui_buoy - Ui_estimated
uo_sync = NaN(length(X_input_mat),1); vo_sync = uo_sync;
    for j=1:length(Ind_isdata)
        jj = find(Positions_in_vector==Ind_isdata(j));
        if ~isempty(jj)
            uo_sync(jj)= UO(j,end);
            vo_sync(jj)= VO(j,end);
        end
    end

[Ui_fit, Vi_fit, meanUb, meanUest, error] = calculate_rmse(X_input_mat,ALPHA(:,end), [uo_sync vo_sync], Y_input_mat,FFchoice);

% Calculate local alpha with the fitted Uw:


% Calculate mean speed and rmse for each year:
% YearlyRMSEmean = NaN(max(Year_mat)-min(Year_mat)+1,4);
% for j= min(Year_mat):max(Year_mat)
%     
%     indy = find(Year_mat==j);
%     
%     [Ui_fit, Vi_fit, meanUb, meanUest, error] = calculate_rmse(X_input_mat(indy,:),ALPHA(:,end), [uo_sync(indy) vo_sync(indy)], Y_input_mat(indy,:));
%     
%     YearlyRMSEmean(j+1-min(Year_mat),1)=j;
%     YearlyRMSEmean(j+1-min(Year_mat),2)=error;
%     YearlyRMSEmean(j+1-min(Year_mat),3)=meanUb;
%     YearlyRMSEmean(j+1-min(Year_mat),4)=meanUest;   
%     
%     disp(['done' int2str(j)])
% end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%% FUNCTIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Alpha_fit function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function vestimated = Alpha_fit(XDATA_FD,YDATA_FD,x0,FFchoice)

    %XDATA_FD = [Ua_ERA5 Va_ERA5 SIC Uo Vo]
    %YDATA_FD = [Ui Vi]
    
    % solver options:
    %options=optimset('disp','iter','LargeScale','off','TolFun',.001,'MaxIter',100000,'MaxFunEvals',100000);
    
    if isequal(FFchoice,'un')
        
         %x0 => initial guess
         lb = [0,-Inf,-Inf,0,-Inf];
         ub = [10,Inf,Inf, 2*pi,Inf];
        
        [vestimated,resnorm,residuals,exitflag,output] = lsqcurvefit(@cplxreal_alpha_it,x0,XDATA_FD,YDATA_FD,lb,ub);
        
    elseif isequal(FFchoice,'deux')
        
         %x0 => initial guess
         lb = [0,-Inf,-Inf,0];
         ub = [10,Inf,Inf, 2*pi];
        
        [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_alpha_it_UFD_UCI_SEP_Approach,x0,XDATA_FD,YDATA_FD,lb,ub);%,options);
        
    elseif isequal(FFchoice,'trois')
        
         lb = [0,-Inf,-Inf,0];
         ub = [10,Inf,Inf, 2*pi];
        
         [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_alpha_it_UFD_UCI_SEP_Approach_firstorder,x0,XDATA_FD,YDATA_FD,lb,ub);
        
    elseif isequal(FFchoice,'vanilla')
        
         lb = [0,-Inf,-Inf,0, -Inf];
         ub = [10,Inf,Inf, 2*pi, Inf];
        
         [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_alpha_it_vanilla,x0,XDATA_FD,YDATA_FD,lb,ub);
    
    elseif isequal(FFchoice,'thicknessXP')
        
         lb = [-Inf,-Inf,-Inf,0,-Inf];
         ub = [Inf,Inf,Inf, 2*pi,Inf];
        
         [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_alpha_it_thickness,x0,XDATA_FD,YDATA_FD,lb,ub);
        
    elseif isequal(FFchoice,'ComboAh')
        
         lb = [0,-Inf,-Inf,0,-Inf];
         ub = [10,Inf,Inf, 2*pi,Inf];
        
         [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_alpha_it_combo,x0,XDATA_FD,YDATA_FD,lb,ub);
        
    elseif isequal(FFchoice,'UoSubstitute')
        
         lb = [0,-Inf,-Inf,0,-Inf];
         ub = [10,Inf,Inf, 2*pi,Inf];
        
         [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_alpha_it_UoSubstitute,x0,XDATA_FD,YDATA_FD,lb,ub);  
        
    elseif isequal(FFchoice,'ThetaH')
        
         lb = [0,-Inf,-Inf,0,-Inf];
         ub = [10,Inf,Inf, 2*pi,Inf];
        
         [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_alpha_it_ThetaH,x0,XDATA_FD,YDATA_FD,lb,ub);
        
    elseif isequal(FFchoice,'AtanH')
        
         lb = [0,-Inf,-Inf,-Inf,0];
         ub = [10,Inf,Inf, Inf,2*pi];
        
         [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_alpha_it_AtanH,x0,XDATA_FD,YDATA_FD,lb,ub); 
        
    elseif isequal(FFchoice,'AlphaCNoUw')
        
         lb = [0,0,-Inf,-Inf,0];
         ub = [10,2+pi,Inf, Inf,2*pi];
        
         [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_alpha_it_AlphaCNoUw,x0,XDATA_FD,YDATA_FD,lb,ub);     
        
    end
    
    %Uncomment to display output:
    %vestimated,resnorm,exitflag,output.firstorderopt

end

function yout = cplxreal_alpha_it(v,xdata)

    % Separating U and V equations for the linear free drift approximation
    % parameterization test.

    %v(1): alpha_fd
    %v(2): alpha_ci
    %v(3): C
    %v(4): theta
  
    yout = zeros(length(xdata),2); % allocate yout

    % In the way of Hibbler: (alphafd - (alphafd-alphaci).*exp(-C(1-A))
    yout(:,1) = (v(1) - (v(1)-v(2)).*exp(-v(3).*(1-xdata(:,3)))).*(xdata(:,1).*cos(v(4)) - xdata(:,2).*sin(v(4))) + xdata(:,5);
    yout(:,2) = (v(1) - (v(1)-v(2)).*exp(-v(3).*(1-xdata(:,3)))).*(xdata(:,2).*cos(v(4)) + xdata(:,1).*sin(v(4))) + xdata(:,6);
    
    %yout(:,1) = v(1)*(1 - v(2).*exp(-v(3).*(1-xdata(:,3)))).*(xdata(:,1).*cos(v(4)) - xdata(:,2).*sin(v(4))) + xdata(:,5);
    %yout(:,2) = v(1)*(1 - v(2).*exp(-v(3).*(1-xdata(:,3)))).*(xdata(:,2).*cos(v(4)) + xdata(:,1).*sin(v(4))) + xdata(:,6);

end

function yout = cplxreal_alpha_it_UFD_UCI_SEP_Approach(v,xdata)

    % Separating U and V equations for the linear free drift approximation
    % parameterization test.

    %v(1): alpha_fd
    %v(2): beta
    %v(3): C
    %v(4): theta
  
    yout = zeros(length(xdata),2); % allocate yout

    %yout(:,1) = v(1).*sqrt(1- (v(2).*xdata(:,4))./(v(1)^2.*xdata(:,1).^2).*exp(-v(3).*(1-xdata(:,3)))).*(xdata(:,1).*cos(v(4)) - xdata(:,2).*sin(v(4))) + xdata(:,5);
    %yout(:,2) = v(1).*sqrt(1- (v(2).*xdata(:,4))./(v(1)^2.*xdata(:,2).^2).*exp(-v(3).*(1-xdata(:,3)))).*(xdata(:,2).*cos(v(4)) + xdata(:,1).*sin(v(4))) + xdata(:,6);
    
    %yout(:,1) = sqrt((v(1).*xdata(:,1)).^2 - (v(2).*xdata(:,4)).*exp(-v(3).*(1-xdata(:,3)))) + xdata(:,5);
    %yout(:,2) = sqrt((v(1).*xdata(:,2)).^2 - (v(2).*xdata(:,4)).*exp(-v(3).*(1-xdata(:,3)))) + xdata(:,6);
    
    % This version completely decouples u and v:
    yout(:,1) = sqrt((v(1).*xdata(:,1)).^2 - v(2).*xdata(:,4).*exp(-v(3).*(1-xdata(:,3))) ) + xdata(:,5);
    yout(:,2) = sqrt((v(1).*xdata(:,2)).^2 - v(2).*xdata(:,4).*exp(-v(3).*(1-xdata(:,3))) ) + xdata(:,6);
    
    
end

function yout = cplxreal_alpha_it_UFD_UCI_SEP_Approach_firstorder(v,xdata)

    % Separating U and V equations for the linear free drift approximation
    % parameterization test.

    %v(1): alpha_fd
    %v(2): beta
    %v(3): C
    %v(4): theta
  
    yout = zeros(length(xdata),2); % allocate yout

    %yout(:,1) = v(1).*(1- 1/2.*(v(2).*xdata(:,4))./(v(1).*abs(xdata(:,1)))).*exp(-v(3).*(1-xdata(:,3))).*(xdata(:,1).*cos(v(4)) - xdata(:,2).*sin(v(4))) + xdata(:,5);
    %yout(:,2) = v(1).*(1- 1/2.*(v(2).*xdata(:,4))./(v(1).*abs(xdata(:,2)))).*exp(-v(3).*(1-xdata(:,3))).*(xdata(:,2).*cos(v(4)) + xdata(:,1).*sin(v(4))) + xdata(:,6);
    
    % This version completely decouples u and v:
    %yout(:,1) = v(1).*xdata(:,1) - v(2).*xdata(:,4).*exp(-v(3).*(1-xdata(:,3)))./2 + xdata(:,5);
    %yout(:,2) = v(1).*xdata(:,2) - v(2).*xdata(:,4).*exp(-v(3).*(1-xdata(:,3)))./2 + xdata(:,6);
    
    % This version completely decouples u and v + searches for a turning
    % angle:
    yout(:,1) = v(1).*(xdata(:,1).*cos(v(4)) - xdata(:,2).*sin(v(4))) - v(2).*xdata(:,4).*exp(-v(3).*(1-xdata(:,3)))./2 + xdata(:,5);
    yout(:,2) = v(1).*(xdata(:,2).*cos(v(4)) + xdata(:,1).*sin(v(4))) - v(2).*xdata(:,4).*exp(-v(3).*(1-xdata(:,3)))./2 + xdata(:,6);

end

function yout = cplxreal_alpha_it_vanilla(v,xdata)

    % Simplest expression of Free drift

    %v(1): alpha_fd
    %v(4): theta
  
    yout = zeros(length(xdata),2); % allocate yout

    % angle:
    yout(:,1) = v(1).*(xdata(:,1).*cos(v(4)) - xdata(:,2).*sin(v(4))) + xdata(:,5);
    yout(:,2) = v(1).*(xdata(:,2).*cos(v(4)) + xdata(:,1).*sin(v(4))) + xdata(:,6);

end

function yout = cplxreal_alpha_it_thickness(v,xdata)

    % Separating U and V equations for the linear free drift approximation
    % parameterization test.

    %v(1): alpha_fd
    %v(2): beta
    %v(4): theta
  
    yout = zeros(length(xdata),2); % allocate yout

%     yout(:,1) = (v(1) - v(2).*xdata(:,4)).*(xdata(:,1).*cos(v(4)) - xdata(:,2).*sin(v(4))) + xdata(:,5);
%     yout(:,2) = (v(1) - v(2).*xdata(:,4)).*(xdata(:,2).*cos(v(4)) + xdata(:,1).*sin(v(4))) + xdata(:,6);
      yout(:,1) = v(1).*(1 - v(2).*xdata(:,4)).*(xdata(:,1).*cos(v(4)) - xdata(:,2).*sin(v(4))) + xdata(:,5);
      yout(:,2) = v(1).*(1 - v(2).*xdata(:,4)).*(xdata(:,2).*cos(v(4)) + xdata(:,1).*sin(v(4))) + xdata(:,6);

end

function yout = cplxreal_alpha_it_combo(v,xdata)

    % Separating U and V equations for the linear free drift approximation
    % parameterization test.

    %v(1): alpha_fd
    %v(2): alpha_ci
    %v(3): C
    %v(4): theta
    %v(5): beta
  
    yout = zeros(length(xdata),2); % allocate yout

    % In the way of Hibbler: (alphafd - (alphafd-alphaci).*exp(-C(1-A))
    %yout(:,1) = ((v(1) - v(5).*xdata(:,4)) - ((v(1) - v(5).*xdata(:,4))-v(2)).*exp(-v(3).*(1-xdata(:,3)))).*(xdata(:,1).*cos(v(4)) - xdata(:,2).*sin(v(4))) + xdata(:,5);
    %yout(:,2) = ((v(1) - v(5).*xdata(:,4)) - ((v(1) - v(5).*xdata(:,4))-v(2)).*exp(-v(3).*(1-xdata(:,3)))).*(xdata(:,2).*cos(v(4)) + xdata(:,1).*sin(v(4))) + xdata(:,6);
    
    yout(:,1) = (v(1) - (v(1) - v(2)).*xdata(:,4).*exp(-v(3).*(1-xdata(:,3)))).*(xdata(:,1).*cos(v(4)) - xdata(:,2).*sin(v(4))) + xdata(:,5);
    yout(:,2) = (v(1) - (v(1) - v(2)).*xdata(:,4).*exp(-v(3).*(1-xdata(:,3)))).*(xdata(:,2).*cos(v(4)) + xdata(:,1).*sin(v(4))) + xdata(:,6);

end

function yout = cplxreal_alpha_it_UoSubstitute(v,xdata)

    % Replacing Uo estimation by alpha_o.*Ui

    %v(1): alpha_fd
    %V(2): alpha_o
    %v(4): theta
  
    yout = zeros(length(xdata),2); % allocate yout

    yout(:,1) = v(1)./(1-v(2)).*(xdata(:,1).*cos(v(4)) - xdata(:,2).*sin(v(4)));
    yout(:,2) = v(1)./(1-v(2)).*(xdata(:,2).*cos(v(4)) + xdata(:,1).*sin(v(4)));

end

function yout = cplxreal_alpha_it_ThetaH(v,xdata)

    %v(1): alpha_fd
    %v(2): beta1
    %v(3): beta2
  
    yout = zeros(length(xdata),2); % allocate yout

    % angle:
     yout(:,1) = v(1).*(xdata(:,1).*cos(v(2) + v(3).*xdata(:,4)) - xdata(:,2).*sin(v(2)+ v(3).*xdata(:,4))) + xdata(:,5);
     yout(:,2) = v(1).*(xdata(:,2).*cos(v(2) + v(3).*xdata(:,4)) + xdata(:,1).*sin(v(2)+ v(3).*xdata(:,4))) + xdata(:,6);
end

function yout = cplxreal_alpha_it_AtanH(v,xdata)

    %4 parameters for shaping the atan(h) function + angle
    % beta1 + beta2*atan(beta3 + beta4*hi)

    %v(1): beta1
    %v(2): beta2
    %v(3): beta3
    %v(4): beta4
    %v(5): theta
  
    yout = zeros(length(xdata),2); % allocate yout

    %yout(:,1) = (v(1) + v(2).*atan(v(3) +v(4).*xdata(:,4))).*(xdata(:,1).*cos(v(5)) - xdata(:,2).*sin(v(5))) + xdata(:,5);
    %yout(:,2) = (v(1) + v(2).*atan(v(3) +v(4).*xdata(:,4))).*(xdata(:,2).*cos(v(5)) + xdata(:,1).*sin(v(5))) + xdata(:,6);
    
    %yout(:,1) = (v(1) + v(2).*atan(v(3) - xdata(:,4))).*(xdata(:,1).*cos(v(5)) - xdata(:,2).*sin(v(5))) + xdata(:,5);
    %yout(:,2) = (v(1) + v(2).*atan(v(3) - xdata(:,4))).*(xdata(:,2).*cos(v(5)) + xdata(:,1).*sin(v(5))) + xdata(:,6);
    
    yout(:,1) = (v(1) + v(2).*atan(v(3).*(-1.8 + xdata(:,4)))).*(xdata(:,1).*cos(v(5)) - xdata(:,2).*sin(v(5))) + xdata(:,5);
    yout(:,2) = (v(1) + v(2).*atan(v(3).*(-1.8 + xdata(:,4)))).*(xdata(:,2).*cos(v(5)) + xdata(:,1).*sin(v(5))) + xdata(:,6);

end

function yout = cplxreal_alpha_it_AlphaCNoUw(v,xdata)

    %2 parameters: beta1 * Ua *exp(-1i*thetaA)

    %v(1): beta1
    %v(2): theta
  
    yout = zeros(length(xdata),2); % allocate yout
    
    yout(:,1) = v(1).*(xdata(:,1).*cos(v(2)) - xdata(:,2).*sin(v(2)));
    yout(:,2) = v(1).*(xdata(:,2).*cos(v(2)) + xdata(:,1).*sin(v(2)));

end

%% Uo_fit function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [u, v] = U_OCN_fit(Xin,Yin, alpha,x0,FFchoice,iteration)
     f = waitbar(0,'Fitting Uo'); tic;
     u= NaN(length(Xin),1); v=u;
     for i=1:length(Xin)

         %Pick x & y data for that grid cell:
         al = [alpha; NaN(length(cell2mat(Xin(i,1:4)))-length(alpha),1)]; % append alpha parameters!
         x = [cell2mat(Xin(i,1:4)) al];
         y = cell2mat(Yin(i,1:2));

         %pick initial guess: [uo_guess; vo_guess]
         u_guess = [x0(i,1); x0(i,2)];

         %pass to lsqcurvefit
         lb = [-100,-100];
         ub = [100,100];

         if isequal(FFchoice,'un')

             [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_uo_it,u_guess,x,y,lb,ub);

         elseif isequal(FFchoice,'trois')

             [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_uo_it_UFD_UCI_SEP_Approach_firstorder,u_guess,x,y,lb,ub);

         elseif isequal(FFchoice,'vanilla')

             [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_uo_it_vanilla,u_guess,x,y,lb,ub);

         elseif isequal(FFchoice,'thicknessXP')

             [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_uo_it_thickness,u_guess,x,y,lb,ub);

          elseif isequal(FFchoice,'ComboAh')

             [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_uo_it_combo,u_guess,x,y,lb,ub); 
        
         elseif isequal(FFchoice,'ThetaH')

             [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_uo_it_ThetaH,u_guess,x,y,lb,ub); 
        
        elseif isequal(FFchoice,'AtanH')

             [vestimated,resnorm,residuals,exitflag,output] = ...
            lsqcurvefit(@cplxreal_uo_it_AtanH,u_guess,x,y,lb,ub);

         end
         
        u(i) = vestimated(1);
        v(i) = vestimated(2);
        %disp(['In Uo loop - #' num2str(i) ' Iteration ' num2str(iteration)]);
        waitbar(i/length(Xin),f,['Fitting ocean current. Loop ' num2str(iteration) ' ' int2str(i) '/' int2str(length(Xin)) ', TOC=' int2str(toc) 's']); 
     end
     close(f)
end

function yout = cplxreal_uo_it(v,xdata)

    %v(1): uo
    %v(2): vo
    
    %Alpha parameters:
    alpha_fd = xdata(1,end);
    alpha_ci = xdata(2,end);
    C        = xdata(3,end);
    theta    = xdata(4,end);
    
    yout = zeros(length(xdata),2); % allocate yout

    % In the way of Hibbler: (alphafd - (alphafd-alphaci).*exp(-C(1-A))
    yout(:,1) = (alpha_fd - (alpha_fd-alpha_ci).*exp(-C.*(1-xdata(:,3)))).*(xdata(:,1).*cos(theta) - xdata(:,2).*sin(theta)) + v(1);
    yout(:,2) = (alpha_fd - (alpha_fd-alpha_ci).*exp(-C.*(1-xdata(:,3)))).*(xdata(:,2).*cos(theta) + xdata(:,1).*sin(theta)) + v(2);
    %yout(:,1) = alpha_fd*(1 -alpha_ci.*exp(-C.*(1-xdata(:,3)))).*(xdata(:,1).*cos(theta) - xdata(:,2).*sin(theta)) + v(1);
    %yout(:,2) = alpha_fd*(1 -alpha_ci.*exp(-C.*(1-xdata(:,3)))).*(xdata(:,2).*cos(theta) + xdata(:,1).*sin(theta)) + v(2);

end

function yout = cplxreal_uo_it_UFD_UCI_SEP_Approach_firstorder(v,xdata)

    %v(1): uo
    %v(2): vo
    
    %Alpha parameters:
    alpha_fd = xdata(1,end);
    beta = xdata(2,end);
    C        = xdata(3,end);
    theta    = xdata(4,end);
    
    yout = zeros(length(xdata),2); % allocate yout
    
    %Case where U and V are completely decoupled
    %yout(:,1) = alpha_fd.*xdata(:,1) - beta.*xdata(:,4).*exp(-C.*(1-xdata(:,3)))./2 + v(1);
    %yout(:,2) = alpha_fd.*xdata(:,2) - beta.*xdata(:,4).*exp(-C.*(1-xdata(:,3)))./2 + v(2);
    
    yout(:,1) = alpha_fd.*(xdata(:,1).*cos(theta) - xdata(:,2).*sin(theta)) - beta.*xdata(:,4).*exp(-C.*(1-xdata(:,3)))./2 + v(1);
    yout(:,2) = alpha_fd.*(xdata(:,2).*cos(theta) + xdata(:,1).*sin(theta)) - beta.*xdata(:,4).*exp(-C.*(1-xdata(:,3)))./2 + v(2);

end

function yout = cplxreal_uo_it_vanilla(v,xdata)

    %Alpha parameters:
    alpha_fd = xdata(1,end);
    %alpha_ci = xdata(2,end);
    %C        = xdata(3,end);
    theta    = xdata(4,end);
    
    yout = zeros(length(xdata),2); % allocate yout

    yout(:,1) = alpha_fd.*(xdata(:,1).*cos(theta) - xdata(:,2).*sin(theta)) + v(1);
    yout(:,2) = alpha_fd.*(xdata(:,2).*cos(theta) + xdata(:,1).*sin(theta)) + v(2);

end

function yout = cplxreal_uo_it_thickness(v,xdata)

    %Alpha parameters:
    alpha_fd = xdata(1,end);
    beta = xdata(2,end);
    %C        = xdata(3,end);
    theta    = xdata(4,end);
    
    yout = zeros(length(xdata),2); % allocate yout

%     yout(:,1) = (alpha_fd - beta.*xdata(:,4)).*(xdata(:,1).*cos(theta) - xdata(:,2).*sin(theta)) + v(1);
%     yout(:,2) = (alpha_fd - beta.*xdata(:,4)).*(xdata(:,2).*cos(theta) + xdata(:,1).*sin(theta)) + v(2);
      yout(:,1) = alpha_fd.*(1 - beta.*xdata(:,4)).*(xdata(:,1).*cos(theta) - xdata(:,2).*sin(theta)) + v(1);
      yout(:,2) = alpha_fd.*(1 - beta.*xdata(:,4)).*(xdata(:,2).*cos(theta) + xdata(:,1).*sin(theta)) + v(2);


end

function yout = cplxreal_uo_it_combo(v,xdata)

    %v(1): uo
    %v(2): vo
    
    %Alpha parameters:
    alpha_fd = xdata(1,end);
    alpha_ci = xdata(2,end);
    C        = xdata(3,end);
    theta    = xdata(4,end);
    beta     = xdata(5,end);
    
    yout = zeros(length(xdata),2); % allocate yout

    % In the way of Hibbler: (alphafd - (alphafd-alphaci).*exp(-C(1-A))
    %yout(:,1) = ((alpha_fd - beta.*xdata(:,4)) - ((alpha_fd - beta.*xdata(:,4))-alpha_ci).*exp(-C.*(1-xdata(:,3)))).*(xdata(:,1).*cos(theta) - xdata(:,2).*sin(theta)) + v(1);
    %yout(:,2) = ((alpha_fd - beta.*xdata(:,4)) - ((alpha_fd - beta.*xdata(:,4))-alpha_ci).*exp(-C.*(1-xdata(:,3)))).*(xdata(:,2).*cos(theta) + xdata(:,1).*sin(theta)) + v(2);

    yout(:,1) = (alpha_fd  - (alpha_fd -alpha_ci).*xdata(:,4).*exp(-C.*(1-xdata(:,3)))).*(xdata(:,1).*cos(theta) - xdata(:,2).*sin(theta)) + v(1);
    yout(:,2) = (alpha_fd  - (alpha_fd -alpha_ci).*xdata(:,4).*exp(-C.*(1-xdata(:,3)))).*(xdata(:,2).*cos(theta) + xdata(:,1).*sin(theta)) + v(2);
end

function yout = cplxreal_uo_it_ThetaH(v,xdata)

    %Alpha parameters:
    alpha_fd = xdata(1,end);
    betaI    = xdata(2,end);
    betaII   = xdata(3,end);
    
    yout = zeros(length(xdata),2); % allocate yout

    yout(:,1) = alpha_fd.*(xdata(:,1).*cos(betaI + betaII.*xdata(:,4)) - xdata(:,2).*sin(betaI + betaII.*xdata(:,4))) + v(1);
    yout(:,2) = alpha_fd.*(xdata(:,2).*cos(betaI + betaII.*xdata(:,4)) + xdata(:,1).*sin(betaI + betaII.*xdata(:,4))) + v(2);

end

function yout = cplxreal_uo_it_AtanH(v,xdata)

    %Alpha parameters:
    betaI    = xdata(1,end);
    betaII   = xdata(2,end);
    betaIII  = xdata(3,end);
    betaIV   = xdata(4,end);
    theta    = xdata(5,end);
    
    yout = zeros(length(xdata),2); % allocate yout

    %yout(:,1) = (betaI + betaII.*atan(betaIII +betaIV.*xdata(:,4))).*(xdata(:,1).*cos(theta) - xdata(:,2).*sin(theta)) + v(1);
    %yout(:,2) = (betaI + betaII.*atan(betaIII +betaIV.*xdata(:,4))).*(xdata(:,2).*cos(theta) + xdata(:,1).*sin(theta)) + v(2);
    
    yout(:,1) = (betaI + betaII.*atan(betaIII - xdata(:,4))).*(xdata(:,1).*cos(theta) - xdata(:,2).*sin(theta)) + v(1);
    yout(:,2) = (betaI + betaII.*atan(betaIII - xdata(:,4))).*(xdata(:,2).*cos(theta) + xdata(:,1).*sin(theta)) + v(2);
    
    yout(:,1) = (betaI + betaII.*atan(betaIII.*(-1.8 + xdata(:,4)))).*(xdata(:,1).*cos(theta) - xdata(:,2).*sin(theta)) + v(1);
    yout(:,2) = (betaI + betaII.*atan(betaIII.*(-1.8 + xdata(:,4)))).*(xdata(:,2).*cos(theta) + xdata(:,1).*sin(theta)) + v(2);

end

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Synthetic dataset:
function [ui_sy,vi_sy] = synthetic_ui(xdata,alpha_fd,alpha_ci,C,thetaD,Uocn,Noise_mag)

    ui_sy = (alpha_fd - (alpha_fd-alpha_ci)*exp(-C*(1-xdata(:,3)))).*(xdata(:,1)*cosd(thetaD) - xdata(:,2)*sind(thetaD)) + Uocn(1);
    vi_sy = (alpha_fd - (alpha_fd-alpha_ci)*exp(-C*(1-xdata(:,3)))).*(xdata(:,2)*cosd(thetaD) + xdata(:,1)*sind(thetaD)) + Uocn(2);
    
    %Add some noise
    %-> randn returns random value normally distributed
    ui_sy = ui_sy + Noise_mag.*randn(length(ui_sy),1);
    vi_sy = vi_sy + Noise_mag.*randn(length(vi_sy),1);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Ui_fit, Vi_fit, meanUb, meanUest, error] = calculate_rmse(X_input_mat,ALPHA, Uocn, Ui_original,FFchoice)

if isequal(FFchoice,'un')
        
   Ui_fit = (ALPHA(1) - (ALPHA(1)-ALPHA(2)).*exp(-ALPHA(3).*(1-X_input_mat(:,3)))).*(X_input_mat(:,1)*cos(ALPHA(4)) - X_input_mat(:,2)*sin(ALPHA(4))) + Uocn(:,1);
   Vi_fit = (ALPHA(1) - (ALPHA(1)-ALPHA(2)).*exp(-ALPHA(3).*(1-X_input_mat(:,3)))).*(X_input_mat(:,2)*cos(ALPHA(4)) + X_input_mat(:,1)*sin(ALPHA(4))) + Uocn(:,2);
   
   %Ui_fit = ALPHA(1)*(1 - ALPHA(2).*exp(-ALPHA(3).*(1-X_input_mat(:,3)))).*(X_input_mat(:,1)*cos(ALPHA(4)) - X_input_mat(:,2)*sin(ALPHA(4))) + Uocn(:,1);
   %Vi_fit = ALPHA(1)*(1 - ALPHA(2).*exp(-ALPHA(3).*(1-X_input_mat(:,3)))).*(X_input_mat(:,2)*cos(ALPHA(4)) + X_input_mat(:,1)*sin(ALPHA(4))) + Uocn(:,2);
   
elseif isequal(FFchoice,'deux')
    
    
elseif isequal(FFchoice,'trois')
    
    %First messed up version
    %Ui_fit = ALPHA(1).*(1- 1/2.*(ALPHA(2).*X_input_mat(:,4))./(ALPHA(1).*abs(X_input_mat(:,1)))).*exp(-ALPHA(3).*(1-X_input_mat(:,3))).*(X_input_mat(:,1).*cos(ALPHA(4)) - X_input_mat(:,2).*sin(ALPHA(4))) + Uocn(:,1);
    %Vi_fit = ALPHA(1).*(1- 1/2.*(ALPHA(2).*X_input_mat(:,4))./(ALPHA(1).*abs(X_input_mat(:,2)))).*exp(-ALPHA(3).*(1-X_input_mat(:,3))).*(X_input_mat(:,2).*cos(ALPHA(4)) + X_input_mat(:,1).*sin(ALPHA(4))) + Uocn(:,2);
    
    %No turning angle
    %Ui_fit = ALPHA(1).*X_input_mat(:,1) - ALPHA(2).*X_input_mat(:,4).*exp(-ALPHA(3).*(1-X_input_mat(:,3)))./2 + Uocn(:,1);
    %Vi_fit = ALPHA(1).*X_input_mat(:,2) - ALPHA(2).*X_input_mat(:,4).*exp(-ALPHA(3).*(1-X_input_mat(:,3)))./2 + Uocn(:,2);
    
    %Including a turning angle
    Ui_fit = ALPHA(1).*(X_input_mat(:,1).*cos(ALPHA(4)) - X_input_mat(:,2).*sin(ALPHA(4))) - ALPHA(2).*X_input_mat(:,4).*exp(-ALPHA(3).*(1-X_input_mat(:,3)))./2 + Uocn(:,1);
    Vi_fit = ALPHA(1).*(X_input_mat(:,2).*cos(ALPHA(4)) + X_input_mat(:,1).*sin(ALPHA(4))) - ALPHA(2).*X_input_mat(:,4).*exp(-ALPHA(3).*(1-X_input_mat(:,3)))./2 + Uocn(:,2);

elseif isequal(FFchoice,'vanilla')
 
    Ui_fit = ALPHA(1).*(X_input_mat(:,1).*cos(ALPHA(4)) - X_input_mat(:,2).*sin(ALPHA(4))) + Uocn(:,1);
    Vi_fit = ALPHA(1).*(X_input_mat(:,2).*cos(ALPHA(4)) + X_input_mat(:,1).*sin(ALPHA(4))) + Uocn(:,2);
    
elseif isequal(FFchoice,'thicknessXP')
 
    Ui_fit = ALPHA(1).*(1 - ALPHA(2).*X_input_mat(:,4)).*(X_input_mat(:,1).*cos(ALPHA(4)) - X_input_mat(:,2).*sin(ALPHA(4))) + Uocn(:,1);
    Vi_fit = ALPHA(1).*(1 - ALPHA(2).*X_input_mat(:,4)).*(X_input_mat(:,2).*cos(ALPHA(4)) + X_input_mat(:,1).*sin(ALPHA(4))) + Uocn(:,2);
    
elseif isequal(FFchoice,'ComboAh')
        
   %Ui_fit = ((ALPHA(1) - ALPHA(5).*X_input_mat(:,4)) - ((ALPHA(1) - ALPHA(5).*X_input_mat(:,4))-ALPHA(2)).*exp(-ALPHA(3).*(1-X_input_mat(:,3)))).*(X_input_mat(:,1)*cos(ALPHA(4)) - X_input_mat(:,2)*sin(ALPHA(4))) + Uocn(:,1);
   %Vi_fit = ((ALPHA(1) - ALPHA(5).*X_input_mat(:,4)) - ((ALPHA(1) - ALPHA(5).*X_input_mat(:,4))-ALPHA(2)).*exp(-ALPHA(3).*(1-X_input_mat(:,3)))).*(X_input_mat(:,2)*cos(ALPHA(4)) + X_input_mat(:,1)*sin(ALPHA(4))) + Uocn(:,2);
    
   Ui_fit = (ALPHA(1) - (ALPHA(1) -ALPHA(2)).*X_input_mat(:,4).*exp(-ALPHA(3).*(1-X_input_mat(:,3)))).*(X_input_mat(:,1)*cos(ALPHA(4)) - X_input_mat(:,2)*sin(ALPHA(4))) + Uocn(:,1);
   Vi_fit = (ALPHA(1) - (ALPHA(1) -ALPHA(2)).*X_input_mat(:,4).*exp(-ALPHA(3).*(1-X_input_mat(:,3)))).*(X_input_mat(:,2)*cos(ALPHA(4)) + X_input_mat(:,1)*sin(ALPHA(4))) + Uocn(:,2);
   
elseif isequal(FFchoice,'UoSubstitute')
 
    Ui_fit = ALPHA(1)./(1 -ALPHA(2)).*(X_input_mat(:,1).*cos(ALPHA(4)) - X_input_mat(:,2).*sin(ALPHA(4)));
    Vi_fit = ALPHA(1)./(1 -ALPHA(2)).*(X_input_mat(:,2).*cos(ALPHA(4)) + X_input_mat(:,1).*sin(ALPHA(4)));
    
elseif isequal(FFchoice,'ThetaH')
 
    Ui_fit = ALPHA(1).*(X_input_mat(:,1).*cos(ALPHA(2)-ALPHA(3).*X_input_mat(:,4)) - X_input_mat(:,2).*sin(ALPHA(2)-ALPHA(3).*X_input_mat(:,4))) + Uocn(:,1);
    Vi_fit = ALPHA(1).*(X_input_mat(:,2).*cos(ALPHA(2)-ALPHA(3).*X_input_mat(:,4)) + X_input_mat(:,1).*sin(ALPHA(2)-ALPHA(3).*X_input_mat(:,4))) + Uocn(:,2);
    
elseif isequal(FFchoice,'AtanH')
 
    %Ui_fit = (ALPHA(1) + ALPHA(2).*atan(ALPHA(3) +ALPHA(4).*X_input_mat(:,4))).*(X_input_mat(:,1).*cos(ALPHA(5)) - X_input_mat(:,2).*sin(ALPHA(5))) + Uocn(:,1);
    %Vi_fit = (ALPHA(1) + ALPHA(2).*atan(ALPHA(3) +ALPHA(4).*X_input_mat(:,4))).*(X_input_mat(:,2).*cos(ALPHA(5)) + X_input_mat(:,1).*sin(ALPHA(5))) + Uocn(:,2); 
    
    %Ui_fit = (ALPHA(1) + ALPHA(2).*atan(ALPHA(3) - X_input_mat(:,4))).*(X_input_mat(:,1).*cos(ALPHA(5)) - X_input_mat(:,2).*sin(ALPHA(5))) + Uocn(:,1);
    %Vi_fit = (ALPHA(1) + ALPHA(2).*atan(ALPHA(3) - X_input_mat(:,4))).*(X_input_mat(:,2).*cos(ALPHA(5)) + X_input_mat(:,1).*sin(ALPHA(5))) + Uocn(:,2); 
    
    Ui_fit = (ALPHA(1) + ALPHA(2).*atan(ALPHA(3).*(-1.8 + X_input_mat(:,4)))).*(X_input_mat(:,1).*cos(ALPHA(5)) - X_input_mat(:,2).*sin(ALPHA(5))) + Uocn(:,1);
    Vi_fit = (ALPHA(1) + ALPHA(2).*atan(ALPHA(3).*(-1.8 + X_input_mat(:,4)))).*(X_input_mat(:,2).*cos(ALPHA(5)) + X_input_mat(:,1).*sin(ALPHA(5))) + Uocn(:,2); 
   
  elseif isequal(FFchoice,'AlphaCNoUw')
      
      Ui_fit = ALPHA(1).*(X_input_mat(:,1).*cos(ALPHA(2)) - X_input_mat(:,2).*sin(ALPHA(2))) ;
      Vi_fit = ALPHA(1).*(X_input_mat(:,2).*cos(ALPHA(2)) + X_input_mat(:,1).*sin(ALPHA(2))) ;
    
end
    
   meanUb = mean( sqrt(Ui_original(:,1).^2+Ui_original(:,2).^2) );
   meanUest = mean ( sqrt(Ui_fit.^2+Vi_fit.^2) );
   
   error = sqrt(mean( (sqrt(Ui_original(:,1).^2+Ui_original(:,2).^2) - sqrt(Ui_fit.^2+Vi_fit.^2)).^2 )) ;

end

%% Cell structure function

function [XDATA_cell,YDATA_cell,PPFw_cell,TimeCell,Datapoint_numbers] = MAKE_CELL_STRUCTURE(DATA,SaveCELLstruct,WindType)

    %Associate EASE grid x and y coordinates to matrix column indexing in
    %matlab:
    pos = round(DATA.EASE_x+1) + round(DATA.EASE_y+1-1).*361;

    YDATA_cell = cell(361*361,2); % u_buoy; v_buoy
    XDATA_cell = cell(361*361,4); % u_ERA5; v_ERA5; SIC; h_piomas
    PPFw_cell = cell(361*361,2); %u_ppf_w; v_ppf_w
    TimeCell = cell(361*361,1); % TimeCell
    
tic
f = waitbar(0,'Making Cell structure');

% Don't need to scan first and last columns of the EASE-grid - there's no
% data there. Filling them with NaN instead.
i_last = 361*361 - 361;

for j=1:361
        YDATA_cell{j,1} = NaN;
        YDATA_cell{j,2} = NaN;
        XDATA_cell{j,1} = NaN;
        XDATA_cell{j,2} = NaN;
        XDATA_cell{j,3} = NaN;
        XDATA_cell{j,4} = NaN;
        PPFw_cell{j,1} = NaN;
        PPFw_cell{j,2} = NaN;
        TimeCell{j,1} = NaN;
end

for j=i_last+1:361*361
        YDATA_cell{j,1} = NaN;
        YDATA_cell{j,2} = NaN;
        XDATA_cell{j,1} = NaN;
        XDATA_cell{j,2} = NaN;
        XDATA_cell{j,3} = NaN;
        XDATA_cell{j,4} = NaN;
        PPFw_cell{j,1} = NaN;
        PPFw_cell{j,2} = NaN;
        TimeCell{j,1} = NaN;
end
    
for i= 362:i_last % Loop on all EASE-grid elements + search window
    
    %Search for positions within a 3x3 search window around target grid
    %cell:
    
     %_ |_ i-1 |_ i+360
     %_ |_ i   |_ i+361
     %_ |_ i+1 |_ i+362
    
     ind = find(pos==i | pos==i-1 | pos==i+1 | ...
                pos==i+361 | pos==i+360 | pos==i+362 | ...
                pos==i-361 | pos==i-360 | pos==i-361);
    
    %associate values:       
           
    if numel(ind) > 10  %%%%%% HERE IS THE CRITERION I IMPOSE FOR THE MINIMUM NUMBER OF POINTS I CONSIDER FOR MAKING THE ESTIMATIONS LOCALLY!
        YDATA_cell{i,1} = DATA.u_buoy(ind);
        YDATA_cell{i,2} = DATA.v_buoy(ind);
        if isequal(WindType,'ERA5')
            XDATA_cell{i,1} = DATA.u_ERA5(ind);
            XDATA_cell{i,2} = DATA.v_ERA5(ind);
        else
            XDATA_cell{i,1} = DATA.u_ERAi(ind);
            XDATA_cell{i,2} = DATA.v_ERAi(ind);
        end
        XDATA_cell{i,3} = DATA.SIC(ind);
        XDATA_cell{i,4} = DATA.h_piomas(ind);
        PPFw_cell{i,1} = DATA.u_PPFv4Wind(ind);
        PPFw_cell{i,2} = DATA.v_PPFv4Wind(ind);
        TimeCell{i,1} = [DATA.year(ind) DATA.month(ind) DATA.day(ind) DATA.doy(ind)];
    else
        YDATA_cell{i,1} = NaN;
        YDATA_cell{i,2} = NaN;
        XDATA_cell{i,1} = NaN;
        XDATA_cell{i,2} = NaN;
        XDATA_cell{i,3} = NaN;
        XDATA_cell{i,4} = NaN;
        PPFw_cell{j,1} = NaN;
        PPFw_cell{j,2} = NaN;
        TimeCell{i,1} = NaN;
    end
    waitbar(i/(361*361),f,['Making Cell structure: ' int2str(i) '/' int2str(361*361) ', TOC=' int2str(toc) 's']); 

end

% Also number of data point for each index:
Datapoint_numbers = NaN(361*361,1);
for i= 1:361*361
    Datapoint_numbers(i) = sum(~isnan(XDATA_cell{i,1}));   
end

 if isequal(SaveCELLstruct,'yes')
   SVPath = '/Users/charlesbrunette/Dropbox/FREEDRIFT2020/Analysis/Freedrift/Data/';
   save([SVPath 'XDATA_YDATA_cells_A2021.mat'], 'XDATA_cell', 'YDATA_cell','PPFw_cell','Datapoint_numbers','TimeCell');
 end
end

%%
% UO_diff = UO - UO(:,end);
% itt = repmat([0:5]',1,12044);
% plot(itt,UO_diff'); hold on;
% ylabel('UO(k) - UO(end) [cm/s]');
% xlabel('k - iteration');
% title('Initial guess: 50 everywhere')
% set(gca, 'FontSize',15);