function [und_img,cornersW,X_coord,Y_coord]=undistort(dis_img,rt,prec,Corners_x,Corners_y,ocam_model,focale,shiftX,shiftY,new_width,new_height,rectify,use_angular_norm,dorotate90)
%%prec=40;
%%rt=lookup(prec,ocam_model,focale);
  
  if (nargin < 10) new_width=ocam_model.width; new_height=ocam_model.height;
  end
  sizert=length(rt);
  und_img=zeros(new_height, new_width);
  xc_und=floor(new_height/2);
  yc_und=floor(new_width/2);
  %%dis_img=I_1;%%aa(:,:,:,1);
  if (nargin < 12) rectify=eye(3);
    disp('no rectify');
  end

  if (nargin < 13) use_angular_norm=0;
  end
  if (nargin < 14) dorotate90=0;
  end


  xc=ocam_model.xc;
  yc=ocam_model.yc;
  oc=ocam_model.c;
  od=ocam_model.d;
  oe=ocam_model.e;

  a=double(dis_img);

  rotatemat=eye(3);

  %%
  if (dorotate90)
    rotatemat=[[0 -1 new_height];[1 0 0]; [0 0 1]];
    xc_und=floor(new_width/2);
    yc_und=floor(new_height/2);
  end
  %%

  for i=1:size(und_img,1)  %%  var i,x = dimension verticale
    for j=1:size(und_img,2)  %% var j,y = dimension horizontale
      

      jnorm=j;
      inorm=i;



      %%y_p=y_p*atan(abs(y_p/focale))/tan(y_p/focale);

      if (use_angular_norm)
        if (dorotate90)

          icenter=size(und_img,1)/2;
          alpha=atan(abs( (i-icenter)/focale));
          if (alpha>0.001) 
            inorm=(i-icenter)*tan(alpha)/alpha+icenter;
          end
        else
          jcenter=size(und_img,2)/2;
          alpha=atan(abs( (j-jcenter)/focale));
          if (alpha>0.001) 
            jnorm=(j-jcenter)*tan(alpha)/alpha+jcenter;
          end

        end

      end

      %%%  Ci dessous permutation i & j pour rectify matrix puis repermutation
      p=rectify*rotatemat*[jnorm ; inorm ; 1];
      xr=p(2)/p(3);
      yr=p(1)/p(3);


      x_p= xr - xc_und;
      y_p= yr - yc_und;
      

      rho_und= ( sqrt( x_p^2 + y_p^2 ) );
      if ( round(rho_und*prec) <=sizert-1 & round(rho_und*prec)~=0)
        ind= rt( round(rho_und*prec) );
        mx=  ( (x_p / rho_und)*ind ) ;
        my=  ( (y_p / rho_und)*ind ) ;

        %% modification from Kaori
        x0=mx*oc+my*od+ xc + shiftY;
        y0=mx*oe+my + yc + shiftX;

%        x0=mx*oc+my*od+ xc;
%        y0=mx*oe+my + yc;

        X_coord(i,j)=x0-1;
        Y_coord(i,j)=y0-1;
        u=floor(y0);
        v=floor(x0);
        
        dx=y0-u; dy=x0-v;
        a1=(1-dy)*(1-dx);
        a2=(1-dy)*dx;
        a3=dy*(1-dx);
        a4=dy*dx;

        %%und_img(i,j,:)=a(v,u,:)*a1 + a(v,u+1,:)*a2 + a(v+1,u,:)*a3 + a(v+1,u+1,:)*a4;
        if ((u>0) & (u<ocam_model.width) & (v>0) & (v<ocam_model.height)) und_img(i,j)=a(v,u)*a1 + a(v,u+1)*a2 + a(v+1,u)*a3 + a(v+1,u+1)*a4;
        else
          X_coord(i,j)=0;
          Y_coord(i,j)=0;
        end
        
      else
        X_coord(i,j)=0;
        Y_coord(i,j)=0;

      end
      
    end
  end

  und_img=uint8(und_img);
  %%figure; imshow(uint8(und_img),[0 255]);
  %%figure; imshow(uint8(a),[0 255]);%%dis_img);



  cornersW=cam2world([Corners_x Corners_y]',ocam_model);

  cornersW(1,:)=-cornersW(1,:).*focale./cornersW(3,:);
  cornersW(2,:)=-cornersW(2,:).*focale./cornersW(3,:);
  cornersW(3,:)=ones(1,size(cornersW(3,:),2));


  cornersW(1,:)=cornersW(1,:)+xc_und;
  cornersW(2,:)=cornersW(2,:)+yc_und;


  %%hold on
  %%for ind=1:size(cornersW,2)
  %%     plot(cornersW(2,ind),cornersW(1,ind),'+r');
  %%end
  %%hold off


