;;; Level two optimizations for double matrices

;;; Copyright (C) 2009 Joern Inge Vestgaarden
;;;
;;; 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.,
;;; 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

(in-package :lisplab) 

(defmethod circ-shift ((A matrix-base-dge) shift)
  (let* ((rows (rows A))
	 (cols (cols A))
	 (A-store (vector-store A))
	 (B (mcreate A))
	 (B-store (vector-store B))
	 (dr (first shift))
	 (dc (second shift)))
    (declare (type type-blas-store A-store B-store)
	     (type type-blas-idx rows cols)
	     (fixnum dr dc))
    (dotimes (i rows)
      (dotimes (j cols)
	(setf (aref B-store (column-major-idx 
			     (mod (+ i dr) rows) 
			     (mod (+ j dc) cols)
			     rows))
	      (aref A-store (column-major-idx i j rows)))))
      B))

(defmethod pad-shift ((A matrix-base-dge) shift &optional (value 0d0))
  (let* ((rows (rows A))
	 (cols (cols A))
	 (A-store (vector-store A))
	 (B (mcreate A value))
	 (B-store (vector-store B))
	 (dr (first shift))
	 (dc (second shift)))
    (declare (type type-blas-store A-store B-store)
	     (type type-blas-idx rows cols)
	     (fixnum dr dc))
    (loop for i from (max 0 dr) below  (min rows (+ rows dr)) do
	 (loop for j from (max 0 dc) below (min cols (+ cols dc)) do
	      (setf (aref B-store 
			  (column-major-idx i j rows))
		    (aref A-store 
			  (column-major-idx (- i dr) 
					    (- j dc) 
					    rows)))))
      B))

;;;; The column operations

(defmethod col-smul! ((A matrix-dge) i num)
  (let* ((num (coerce num 'double-float))
	 (A-store (vector-store A))
	 (r (rows A))
	 (start (* r i))
	 (end (* r (1+ i))))
    (declare (type type-blas-store A-store)
	     (type type-blas-idx start end))
    (loop for k from start below end do
	 (setf (aref A-store k)
	       (* (aref A-store k) num))))
  A)

(defmethod col-swap! ((A matrix-dge) i j)
  (let* ((A-store (vector-store A))
	 (r (rows A))
	 (tmp 0d0)
	 (ii (* i r))
	 (jj (* j r)))
    (declare (type type-blas-store A-store)
	     (type type-blas-idx r ii jj)
	     (type double-float tmp))
    (dotimes (k r)
      (setf tmp (aref A-store ii)
	    (aref A-store ii) (aref A-store jj)
	    (aref A-store jj) tmp)
      (incf ii)
      (incf jj))
    A))

(defmethod col-sum ((A matrix-dge) i)
  (let* ((A-store (vector-store A))
	 (r (rows A))
	 (start (* r i))
	 (end (* r (1+ i)))
	 (sum 0d0))
    (declare (type type-blas-store A-store)
	     (type type-blas-idx i r start end)
	     (type double-float sum))
    (loop for k from start below end do
	 (incf sum (aref A-store k)))
    sum))

(defmethod col-col-mul-sum ((A matrix-dge) i
			    (B matrix-dge) j)
  (let ((A-store (vector-store A))
	(B-store (vector-store B))
	(r (rows A))
	(sum 0d0))
    (declare (type type-blas-store A-store B-store)
	     (type type-blas-idx i j r)
	     (type double-float sum))
    (dotimes (k r)
      (incf sum (* (aref A-store (column-major-idx k i r))
		   (aref B-store (column-major-idx k j r)))))
    sum))