% function [flat_img]=orth(I,phi,)
% % Fisheye image calibration
% % I: Input fisheye image
% % phi: Field of view

clc;
clear all;
I = imread('test.jpg');
phi =  1.037;


win = [5 5];

x = I(:,:,1);                %red
y = I(:,:,2);                %green
z = I(:,:,3);                %blue

R = medfilt2(x,win); 
G = medfilt2(y,win); 
B = medfilt2(z,win); 

I = cat(3,R,G,B); 

s = size(I);
r_max = max(s(1),s(2))/2;
x0 = floor((s(2)+1)/2);   % the center of the image
y0 = floor((s(1)+1)/2);

ss(1) = 2*y0*r_max*tan((phi)*((x0^2+y0^2)^0.5)/r_max)/phi/(x0^2+y0^2)^0.5;
ss(2) = 2*x0*r_max*tan((phi)*((x0^2+y0^2)^0.5)/r_max)/phi/(x0^2+y0^2)^0.5;
ss(1) = floor(ss(1));
ss(2) = floor(ss(2));
ss(3) = s(3);

xx0 = floor((ss(2)+1)/2);
yy0 = floor((ss(1)+1)/2);
% phi = phi/2;
II = zeros(ss);

for i=1:ss(1)
    for j=1:ss(2)
        x = j-xx0;
        y = i-yy0;
        
        cft = r_max*atan(phi*((x^2+y^2)^0.5)/r_max)/(phi);
        
        yy = y*cft/(x^2+y^2)^0.5;
        xx = x*cft/(x^2+y^2)^0.5;
        xx = uint32(xx+x0);
        yy = uint32(yy+y0);
        
%         if xx <= 1.0e-8
%             xx = 1;
%         end
%         if xx > s(2)
%             xx = s(2);
%         end
%         if yy <= 1.0e-8
%             yy = 1;
%         end
%         if yy > s(1)
%             yy = s(1);
%         end
        
        if xx >= 1 && xx <= s(2) && yy >= 1 && yy <= s(1)
            
            if xx/double(uint16(xx)) == 1 && yy/double(uint16(yy)) == 1
                RR(i,j) = double(R(yy,xx));
                GG(i,j) = double(G(yy,xx));
                BB(i,j) = double(B(yy,xx));
            else 
                xx_new = double(floor(xx));
                yy_new = double(floor(yy));
            
                R_1 = double(R(xx_new+1,yy_new))*(xx-xx_new)+double(R(xx_new,yy_new))*(xx_new+1-xx);
                R_2 = double(R(xx_new+1,yy_new+1))*(xx-xx_new)+double(R(xx_new,yy_new+1))*(xx_new+1-xx);
                RR(i,j) = uint8(R_1*(yy_new+1-yy)+R_2*(yy-yy_new));
            
                G_1 = double(G(xx_new+1,yy_new))*(xx-xx_new)+double(G(xx_new,yy_new))*(xx_new+1-xx);
                G_2 = double(G(xx_new+1,yy_new+1))*(xx-xx_new)+double(G(xx_new,yy_new+1))*(xx_new+1-xx);
                GG(i,j) = uint8(G_1*(yy_new+1-yy)+G_2*(yy-yy_new));        
            
                B_1 = double(B(xx_new+1,yy_new))*(xx-xx_new)+double(B(xx_new,yy_new))*(xx_new+1-xx);
                B_2 = double(B(xx_new+1,yy_new+1))*(xx-xx_new)+double(B(xx_new,yy_new+1))*(xx_new+1-xx);
                BB(i,j) = uint8(B_1*(yy_new+1-yy)+B_2*(yy-yy_new));
            end 
        end 
        
        
        
        
    end
end
       
II(:,:,1) = RR;
II(:,:,2) = GG;
II(:,:,3) = BB;
II = uint8(II); 
figure,
subplot(1,2,1),imshow(I);
subplot(1,2,2),imshow(II);



