function [co_sar, co_inc] = Coreg(sar_filenames)
% Coregisters sar and inc files. sar and inc are cell arrays, same length.
% First entry in each array is the "fixed" image for coregistration.
% Reccomend that this is the best image with the most visible features.

% sar1 = read_isis(sar1);
% inc1 = read_isis(inc1);
% sar2 = read_isis(sar2);
% inc2 = read_isis(inc2);

n = numel(sar_filenames);

inc_filenames = strrep (sar_filenames, 'BIS', 'BIE');
lat_filenames = strrep (sar_filenames, 'BIS', 'BIN'); % replace BISQH with BINQ - LAT
lon_filenames = strrep (sar_filenames, 'BIS', 'BIT'); % replace BISQH with BITQ - LON


sar = cell(size(sar_filenames)); %setup empty arrays
inc = sar;
lat = sar;
lon = sar;

for ii=1:n
  disp(ii)
  sar{ii} = importdata(sar_filenames{ii});
  inc{ii} = importdata(inc_filenames{ii});
  lat{ii} = importdata(lat_filenames{ii});
  lon{ii} = importdata(lon_filenames{ii});
end


%Crop each image to contain vaguely the region of interest. Crop using the
%SAR, but the others will be edited also. 

for i = 1:n
    figure 
    [sar{i}, rect] = imcrop(sar{i});
    inc{i} = imcrop(inc{i}, rect);
    lat{i} = imcrop(lat{i}, rect);
    lon{i} = imcrop(lon{i}, rect);

    close 
end


% figure
% [sar1_crop,rect1] = imcrop(sar1);
% inc1_crop = imcrop(inc1, rect1);
% 
% [sar2_crop,rect2] = imcrop(sar2);
% inc2_crop = imcrop(inc2, rect2);
co_sar = zeros(size(sar{1},1),size(sar{1},2),n);
co_inc = co_sar;
co_sar(:,:,1) = sar{1};
co_inc(:,:,1) = inc{1};

%% NEW SECTION: PRE-COREG USING LAT/LON INFO

figure; % create a new figure
imshow (co_sar (:,:,1)); % display the image
[x, y] = getpts; % choose points with the mouse
init_tie_pts = round([x y]); % store the points in an n x 2 array, y x because of imshow
num_tie_pts = size(init_tie_pts,1);
close; % close the figure

%Now find the lat/lon of each of those points in the fixed image. 
% Convert the row and column indices into linear indices
I = sub2ind (size (lat {1}), init_tie_pts (:, 2), init_tie_pts (:, 1));
% Index into the lat and lon arrays using the linear indices
lat_coords = lat {1} (I);
lon_coords = lon {1} (I);
% Combine the lat and lon coordinates into an n x 2 array
init_tie_coords = [lat_coords, lon_coords];

%Now, for each other swath, find the closest tie point for each of the input
%points

for i = 2:n %for each image except the master
    for j = 1:num_tie_pts %for each tie point
     dist = sqrt((lat{i} - init_tie_coords(j, 1)).^2 + (lon{i}-init_tie_coords(j,2)).^2);

     [temp_x, temp_y] = find(dist==min(dist(:)), 1);
     init_tie_pts(j, :, i) = [temp_y, temp_x]; %swap because xy vs indexing is strange <-- dont forget!

    end
end

%% Select Control Points and warp the images

for i = 2:n 

%Load the cropped moving image and fixed image from above
movingImg = sar{i};
fixedImg = co_sar(:,:,1);

%first, get rid of the predicted tiepoints that fall outside of the moving
%image. Those are not useful. 
movingPoints = init_tie_pts(:, :, i);
fixedPoints = init_tie_pts(:, :, 1);
isBorder = ((movingPoints(:,1) == 1) | (movingPoints(:,1) == size(movingImg, 1)));
isBorder = isBorder | ((movingPoints(:,2) == 1) | (movingPoints(:,2) == size(movingImg, 2)));
movingPoints(isBorder, :) = [];
fixedPoints(isBorder,:) = [];


%Use the cpselect tool and warping tool
figure
[mp,fp] = cpselect(movingImg, fixedImg, movingPoints, fixedPoints, 'Wait',true);
t = fitgeotrans(mp,fp,'projective');
Rfixed = imref2d(size(co_sar(:,:,1)));
co_sar(:,:,i) = imwarp(sar{i},t,'OutputView',Rfixed, 'FillValues', NaN);
co_inc(:,:,i) = imwarp(inc{i},t,'OutputView',Rfixed, 'FillValues', NaN);
close
end

Plotsite(co_sar, co_inc);

% figure;
% ax = zeros([2*n,1]);
% for i = 1:n
% indsar = sub2ind([2,n],1,i);
% subplot(n,2,indsar);
% ax(indsar) = gca;
% imagesc(co_sar(:,:,i),[0,1]);colormap(gray);
% 
% indinc = sub2ind([2,n],2,i);
% subplot(n,2,indinc);
% imagesc(co_inc(:,:,i));colormap(gray);
% ax(indinc) = gca;
% end

end

