(sc-include "macros/macros")

(pre-include "gsl/gsl_blas.h")
(pre-include "gsl/gsl_cblas.h")
(pre-include "gsl/gsl_eigen.h")
(pre-include "extent-sampling.h")
(pre-include "nd-random.h")
(pre-include "oracles.h")
(pre-include "utils.h")

(pre-define ECCENTRICITY 2)
(pre-define DIMENSION 4)

(pre-define COVARIANCE_SAMPLES 1000)
(pre-define SAMPLES 10000)

;; We are generalizing ellipsoid sampling with estimation of
;; covariance matrix.
;; 
;; We need a fw(alpha) for a non-diagonal but symmetric A. With that,
;; we will know how to normalize the volume estimate. This new
;; fw(alpha) must involve some simple rotation to the eigen basis of
;; A.

(declare covariance-accumulator
         (type (struct (square gsl-matrix*)
                       (sum gsl-vector*)
                       (n int))))

(define (covariance-accumulator-alloc dimension) (covariance-accumulator int)
  (declare accumulator covariance-accumulator)
  (set (struct-get accumulator square) (gsl-matrix-alloc dimension dimension))
  (set (struct-get accumulator sum) (gsl-vector-alloc dimension))
  (set (struct-get accumulator n) 0)
  (return accumulator))

(define (covariance-accumulator-add accumulator sample) (void covariance-accumulator* gsl-vector*)
  (define square gsl-matrix* (: accumulator square))
  (cblas-dsyr CblasRowMajor CblasLower (: sample size) 1
              (: sample data) (: sample stride)
              (: square data) (: square tda))
  (gsl-vector-add (: accumulator sum) sample)
  (incr (: accumulator n)))

(define (covariance-accumulator-covariance accumulator result) (void covariance-accumulator gsl-matrix*)
  (let* ((dim int (: (struct-get accumulator sum) size))
         (n int (struct-get accumulator n))
         (sum gsl-vector* (struct-get accumulator sum))
         (square gsl-matrix* (struct-get accumulator square)))
    (gsl-matrix-memcpy result square)
    (gsl-matrix-scale result (/ 1.0 (- n 1)))
    (cblas-dsyr CblasRowMajor CblasLower dim (/ -1.0 n n)
                (: sum data) (: sum stride)
                (: result data) (: result tda))))

(define (covariance-accumulator-free accumulator) (void covariance-accumulator)
  (gsl-matrix-free (struct-get accumulator square))
  (gsl-vector-free (struct-get accumulator sum)))

(sc-define-syntax (with-covariance-accumulator var dimension body ...)
  (with-alloc var covariance-accumulator
              (covariance-accumulator-alloc dimension)
              covariance-accumulator-free
              body ...))

(define (matrix-vector-multiply A x y)
  (void (const gsl-matrix*) (const gsl-vector*) gsl-vector*)
  (cblas-dgemv CblasRowMajor CblasNoTrans (: A size1) (: A size2)
               1 (: A data) (: A tda)
               (: x data) (: x stride) 0 (: y data) (: y stride)))

;; (define (product-of-diagonal-entries mat)
;;   (double (const gsl-matrix*))
;;   (define result double 1)
;;   ;; TODO: Do not assume matrix is square.
;;   (for-i i (: mat size1)
;;     (set* result (gsl-matrix-get mat i i)))
;;   (return result))

(define (product-of-entries vec) (double (const gsl-vector*))
  (define result double 1)
  (for-i i (: vec size)
    (set* result (gsl-vector-get vec i)))
  (return result))

(sc-define-syntax (with-eigen-symmv workspace dimension body ...)
  (with-alloc workspace gsl-eigen-symmv-workspace*
              (gsl-eigen-symmv-alloc dimension)
              gsl-eigen-symmv-free
              body ...))

(define (eigen-symmetric A eval evec) (void gsl-matrix* gsl-vector* gsl-matrix*)
  (with-eigen-symmv workspace (: A size1)
    (gsl-eigen-symmv A eval evec workspace)))

(sc-define-syntax (invoke-extent-oracle extent-oracle r x)
  ((: extent-oracle oracle) r x (: extent-oracle params)))

(define (print-vector x) (void gsl-vector*)
  (for-i i (: x size)
    (printf "%g\t" (gsl-vector-get x i)))
  (printf "\n"))

(define (print-matrix A) (void gsl-matrix*)
  (for-i i (: A size1)
    (for-i j (: A size2)
      (printf "%g\t" (gsl-matrix-get A i j)))
    (printf "\n")))

(define (volume-by-ellipsoid extent-oracle rng dimension stats)
  (void (const extent-oracle-t*) (const gsl-rng*) (unsigned int) gsl-rstat-workspace*)
  (let* ((vn double (ln-volume-of-ball dimension)))
    (with-square-matrix transform dimension
      (with-covariance-accumulator accumulator dimension
        (with-vector x dimension
          (for-i i COVARIANCE-SAMPLES
            (random-direction-vector rng x)
            (gsl-vector-scale x (invoke-extent-oracle extent-oracle rng x))
            (covariance-accumulator-add (address-of accumulator) x)))
        (covariance-accumulator-covariance accumulator transform))
      (with-vector eval dimension
        (with-square-matrix evec dimension
          (eigen-symmetric transform eval evec)
          (print-vector eval)
          (printf "\n")
          (print-matrix evec))
        ;; (gsl-matrix-set-identity transform)
        ;; (gsl-matrix-set transform 0 0 (* 0.5 ECCENTRICITY))
        (let* ((ln-determinant double (log (product-of-entries eval))))
          (with-vector x dimension
            (with-vector y dimension
              (for-i i SAMPLES
                (random-direction-vector rng x)
                (matrix-vector-multiply transform x y)
                (let* ((norm double (gsl-blas-dnrm2 y)))
                  (gsl-vector-scale y (/ norm))
                  (gsl-rstat-add (exp (+ vn
                                         (* dimension (- (log (invoke-extent-oracle extent-oracle rng y))
                                                         (log norm)))
                                         ln-determinant))
                                 stats))))))))))

(define (main) (int)
  (declare params (struct-variable spheroid-params
                                   (eccentricity ECCENTRICITY)))
  (declare oracle (struct-variable extent-oracle-t
                                   (oracle spheroid-extent-oracle)
                                   (params (address-of params))))
  (printf "True volume: %g\n" (spheroid-true-volume DIMENSION (address-of params)))
  (with-rng rng
    (with-rstats stats
      (volume-by-ellipsoid (address-of oracle) rng DIMENSION stats)
      (printf "Estimated volume: %g\n" (gsl-rstat-mean stats))))
  (return 0))