;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;
;;; LUSH Lisp Universal Shell
;;;   Copyright (C) 2002 Leon Bottou, Yann Le Cun, AT&T Corp, NECI.
;;; Includes parts of TL3:
;;;   Copyright (C) 1987-1999 Leon Bottou and Neuristique.
;;; Includes selected parts of SN3.2:
;;;   Copyright (C) 1991-2001 AT&T Corp.
;;;
;;; This program is free software; you can redistribute it and/or modify
;;; it under the terms of the GNU General Public License as published by
;;; the Free Software Foundation; either version 2 of the License, or
;;; (at your option) any later version.
;;;
;;; This program is distributed in the hope that it will be useful,
;;; but WITHOUT ANY WARRANTY; without even the implied warranty of
;;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
;;; GNU General Public License for more details.
;;;
;;; You should have received a copy of the GNU General Public License
;;; along with this program; if not, write to the Free Software
;;; Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111, USA
;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; $Id: fimage.lsh,v 1.5 2006/02/27 15:43:47 laseray Exp $
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

#? *** Greyscale Images of floats
;; basic image processing on greyscale images
;; where each pixel is a float.

(libload "idx-macros")
(libload "libimage/image-transform")

#? (fim-subsample <in> <nlin> <ncol>)
;; subsample image <in> by integer ratios <nlin> 
;; vertically and <ncol> horizontally (with averaging).
(de fim-subsample (in nlin ncol)
    ((-idx2- (-flt-)) in)
    ((-int-) nlin ncol)
    (let* ((h (idx-dim in 0))
           (w (idx-dim in 1))
           (nh (int (/ h nlin)))
           (nw (int (/ w ncol)))
           (inp (narrow (narrow in 0 (* nlin nh) 0) 1 (* ncol nw) 0))
           (out (float-matrix-nc nh nw)))
      #{{
      flt *_idx2loopc1, *pin;
      flt *_idx2loopc2, *pout;
      int i, _imax = ($out)->dim[0];
      int j, _jmax = ($out)->dim[1];
      int _imat1_m0 = ($inp)->mod[0];
      int _imat1_m1 = ($inp)->mod[1];
      int _imat2_m0 = ($out)->mod[0];
      int _imat2_m1 = ($out)->mod[1];
      int pin_incr = $ncol * _imat1_m1;
      float norm = 1.0/($ncol * $nlin);
      register float acc0;
      register int k,l;
      register float *pinptr;
      register int pinptr_incr = _imat1_m0 - $ncol* _imat1_m1;
      _idx2loopc1 = IDX_PTR (($inp), flt);
      _idx2loopc2 = IDX_PTR (($out), flt);
      for (i = 0; i < _imax; i++) {
        pin = _idx2loopc1;
        pout = _idx2loopc2;
        for (j = 0; j < _jmax; j++) {
          acc0 =0.0;
          pinptr = pin;
          for (k=0; k<$nlin; k++) {
            for (l=0; l<$ncol; l++) { 
              acc0 += pinptr[0];
              pinptr += _imat1_m1;
              }
            pinptr += pinptr_incr;
            }
          pout[0] = acc0*norm;
          pin += pin_incr;
          pout += _imat2_m1;
	  }
	  _idx2loopc1 += _imat1_m0 * $nlin;
	  _idx2loopc2 += _imat2_m0;
	}
        } #}
  out))))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
#? (fim-interpolate-bilin <background> <pin> <indimi> <indimj> <inmodi> <inmodj> <ppi> <ppj>)
;;.VP
;; (-gptr- pin):
;; (-int-  background indimi indimj inmodi inmodj ppi ppj):
;;
;; RETURNS: (-float-)
;; SIDE EFFECTS: None
;; AUTHOR: Y. LeCun
;; COMPILABLE: Yes
;; DESCRIPTION:
;; returns a bilinearly interpolated pixel value for coordinate
;; <ppi> <ppj>. The image data is pointed to by <pin>, with 
;; <indimi> <indimj> <inmodi> <inmodj> being the dimensions and modulos.
;; This function clips automatically if <ppi> <ppj> are outside of the
;; bounds by assuming the outside values are equal to <background>.
;; pixel values are floats, while coordinates are 32 bit fixed point
;; with 16 bit integer part and 16 bit fractional part.
;; The function does not use floating point arithmetics.
(de fim-interpolate-bilin (background pin indimi indimj inmodi inmodj ppi ppj)
  ((-gptr- "float *") pin)
  ((-float-) background)
  ((-int-)  indimi indimj inmodi inmodj ppi ppj)
  (let ((rez 0.0))
    ((-float-) rez)
    #{{
         int li0, lj0;
	 register int li1, lj1;
	 flt deltai, ndeltai;
	 flt deltaj, ndeltaj;
	 register flt *pin00;
	 register flt v00, v01, v10, v11;
	 li0 = $ppi >> 16;
	 li1 = li0+1;
	 deltai = 0.0000152587890625*(flt)($ppi & 0x0000ffff);
	 ndeltai = 1.0 - deltai;
	 lj0 = $ppj  >> 16;
	 lj1 = lj0+1;
	 deltaj = 0.0000152587890625*(flt)($ppj & 0x0000ffff);
	 ndeltaj = 1.0 - deltaj;
	 pin00 = (flt*)($pin)+$inmodi*li0+$inmodj*lj0;
         if ((li1>0)&&(li1<$indimi)) {
	     if ((lj1>0)&&(lj1<$indimj)) { 
		 v00 = *(pin00); 
		 v01 = *(pin00+$inmodj); 
		 v11 = *(pin00+$inmodi+$inmodj); 
		 v10 = *(pin00+$inmodi);
		 v00 = *(pin00); 
	     } else if (lj1==0) { 
		 v00 = $background;
		 v01 = *(pin00+$inmodj); 
		 v11 = *(pin00+$inmodi+$inmodj);
		 v10 = $background;
	     } else if (lj1==$indimj) {
		 v00 = *(pin00); 
		 v01 = $background;
		 v11 = $background;
		 v10 = *(pin00+$inmodi); 
	     } else {
		 v00 = $background;
		 v01 = $background;
		 v11 = $background;
		 v10 = $background;
	     }
	 } else if (li1==0) {
	     if ((lj1>0)&&(lj1<$indimj)) { 
		 v00 = $background;
		 v01 = $background;
		 v11 = *(pin00+$inmodi+$inmodj); 
		 v10 = *(pin00+$inmodi);
	     } else if (lj1==0) { 
		 v00 = $background;
		 v01 = $background;
		 v11 = *(pin00+$inmodi+$inmodj);
		 v10 = $background;
	     } else if (lj1==$indimj) {
		 v00 = $background;
		 v01 = $background;
		 v11 = $background;
		 v10 = *(pin00+$inmodi); 
	     } else {
		 v00 = $background;
		 v01 = $background;
		 v11 = $background;
		 v10 = $background;
	     }
	 } else if (li1==$indimi) {
	     if ((lj1>0)&&(lj1<$indimj)) { 
		 v00 = *(pin00); 
		 v01 = *(pin00+$inmodj); 
		 v11 = $background;
		 v10 = $background;
	     } else if (lj1==0) { 
		 v00 = $background;
		 v01 = *(pin00+$inmodj); 
		 v11 = $background;
		 v10 = $background;
	     } else if (lj1==$indimj) {
		 v00 = *(pin00); 
		 v01 = $background;
		 v11 = $background;
		 v10 = $background;
	     } else {
		 v00 = $background;
		 v01 = $background;
		 v11 = $background;
		 v10 = $background;
	     }
         } else {
	     v00 = $background;
	     v01 = $background;
	     v11 = $background;
	     v10 = $background;
	 }
         $rez = (ndeltaj * (( v10*deltai + v00*ndeltai )) +
                 deltaj  * (( v11*deltai + v01*ndeltai )));
     }

  #} rez))


#? (fim-warp <in> <out> <background> <pi> <pj>)
;;.VP
;; ((-float-) background):
;; ((-idx2- (-float-)) in out):
;; ((-idx2- (-int-)) pi pj):
;;
;; RETURNS: Null
;; SIDE EFFECTS: <out> 
;; AUTHOR: Y. LeCun
;; COMPILABLE: Yes
;; DESCRIPTION:
;; Warps an image using <pi> and <pj> as tabulated coordinate transforms.
;; <in> and <out> are idx2 of floats. <background> is the value assumed outside
;; of the input image. <pi> and <pj> are tabulated coordinates which can
;; be filled up using compute-bilin-transform or similar functions.
;; Pixel values are antialiased using bilinear interpolation.
(de fim-warp (in out background pi pj)
    ((-float-) background)
    ((-idx2- (-float-)) in out)
    ((-idx2- (-int-)) pi pj)

    (let ((ni (idx-dim in 0))
          (nj (idx-dim in 1))
          (pin (gptr (cinline "IDX_PTR(%s, float)" in)))
          (indimi (idx-dim in 0))
          (indimj (idx-dim in 1))
          (inmodi (idx-modulo in 0))
          (inmodj (idx-modulo in 1)))
      ((-gptr- "float *") pin)
      ((-int-)  indimi indimj inmodi inmodj)
      (idx-bloop ((lout out) (lpi pi) (lpj pj))
        (idx-bloop ((llout lout) (llpi lpi) (llpj lpj))
          (let ((ppi (llpi)) (ppj (llpj)))
            ((-int-) ppi ppj)
            (llout (fim-interpolate-bilin background pin indimi indimj inmodi inmodj ppi ppj))))))
    ())

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; fast version, not interpolation.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;


#? (fim-warp-fast <in> <out> <background> <pi> <pj>)
;;.VP
;; ((-int-) background):
;; ((-idx2- (-float-)) in out):
;; ((-idx2- (-int-)) pi pj):
;;
;; RETURNS: Null
;; SIDE EFFECTS: <out>
;; AUTHOR: Y. LeCun
;; COMPILABLE: Yes
;; DESCRIPTION:
;; Warps an image using <pi> and <pj> as tabulated coordinate transforms.
;; <in> and <out> are idx2 of floats. <background> is the value assumed outside
;; of the input image. <pi> and <pj> are tabulated coordinates which can
;; be filled up using compute-bilin-transform or similar functions.
;; This is essentially identical to warp-fimage, except no antialiasing
;; is performed (it goes a lot faster, but is not nearly as nice).
(de fim-warp-fast (in out background pi pj)
    ((-float-) background)
    ((-idx2- (-float-)) in out)
    ((-idx2- (-int-)) pi pj)

    (unprotect
      (let ((ni (idx-dim in 0))
            (nj (idx-dim in 1))
            (pin (gptr (cinline "IDX_PTR(%s, float)" in)))
            (indimi (idx-dim in 0))
            (indimj (idx-dim in 1))
            (inmodi (idx-modulo in 0))
            (inmodj (idx-modulo in 1)))
        ((-gptr- "float *") pin)
        ((-int-)  indimi indimj inmodi inmodj)
        (idx-bloop ((lout out) (lpi pi) (lpj pj))
          (idx-bloop ((llout lout) (llpi lpi) (llpj lpj))
            (let ((ppi (llpi)) (ppj (llpj)))
              ((-int-) ppi ppj)
  	    (let ((resu 0))
  	      ((-float-) resu)
                #{{
  		     register int li, lj;
  		     li = ($ppi+0x7f) >> 16;
  		     lj = ($ppj+0x7f) >> 16;
  		     if ((li>=0)&&(li<$indimi)&&(lj>=0)&&(lj<$indimj)) {
  			 $resu = *((float*)($pin)+$inmodi*li+$inmodj*lj);
		     } else {
			 $resu = $background;
		     }
  		   }
                 #} 
  	         (llout resu)))))))
      ())

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; top-level function


#? (fim-warp-quad <in> <out> <background> <mode> <x1> <y1> <x2> <y2> <x3> <y3> <x4> <y4> <p1> <q1> <p3> <q3>)
;;.VP
;; ((-flt-) x1 y1 x2 y2 x3 y3 x4 y4 p1 q1 p3 q3):
;; ((-float-) background)
;; ((-int-) mode):
;; ((-idx2- (-float-)) in out):
;;
;; RETURNS: Null
;; SIDE EFFECTS: <out>
;; AUTHOR: Y. LeCun
;; COMPILABLE: Yes
;; DESCRIPTION:
;; transforms fimage <in> (idx2 of float) mapping quadrilateral <x1> <y1> <x2>
;; <y2> <x3> <y3> <x4> (points are numbered clockwise starting from upper left)
;; to rectangle whose upper left and lower right points are <p1><q1>, <p3><q3>.
;; result is put in <out> (idx2 of float).  Clipping is automatically performed.
;; pixels outside of the bounds of <in> are assumed to have the value
;; <background>.  <mode>=0: no antialiasing, <mode>=1: antialiasing with
;; bilinear interpolation (2 times slower).  execution time on sparc 10 is about
;; 5 ms in mode 0 and 10 ms in mode 1 for a 32x32 target image.
(de fim-warp-quad (in out background mode x1 y1 x2 y2 x3 y3 x4 y4 p1 q1 p3 q3)
    ((-flt-) x1 y1 x2 y2 x3 y3 x4 y4 p1 q1 p3 q3)
    ((-float-) background)
    ((-int-) mode)
    ((-idx2- (-float-)) in out)

    (let* ((outi (idx-dim out 0))
	   (outj (idx-dim out 1))
	   (dispi (int-matrix-nc outi outj))
	   (dispj (int-matrix-nc outi outj)))
      (compute-bilin-transform dispi dispj x1 y1 x2 y2 x3 y3 x4 y4 p1 q1 p3 q3)
      (if (= 0 mode)
	  (fim-warp-fast in out background dispi dispj)
	(fim-warp in out background dispi dispj)))
    ())

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(dhc-make () 
	  fim-subsample 
	  fim-interpolate-bilin fim-warp fim-warp-fast fim-warp-quad)

