{-# LANGUAGE NoImplicitPrelude #-}
{-# LANGUAGE ScopedTypeVariables #-}

-- | QR factorization via Givens rotations.
--
module Linear.QR (
  eigenvalues,
  eigenvectors_symmetric,
  givens_rotator,
  qr )
where

import qualified Algebra.Ring as Ring ( C )
import qualified Algebra.Algebraic as Algebraic ( C )
import Control.Arrow ( first )
import Data.Vector.Fixed ( S, ifoldl )
import Data.Vector.Fixed.Cont ( Arity )
import NumericPrelude hiding ( (*) )

import Linear.Matrix (
  Col,
  Mat(..),
  (*),
  (!!!),
  construct,
  diagonal,
  identity_matrix,
  symmetric,
  transpose )


-- | Construct a givens rotation matrix that will operate on row @i@
--   and column @j@. This is done to create zeros in some column of
--   the target matrix. You must also supply that column's @i@th and
--   @j@th entries as arguments.
--
--   Examples (Watkins, p. 193):
--
--   >>> import Normed ( Normed(..) )
--   >>> import Linear.Vector ( Vec2, Vec3 )
--   >>> import Linear.Matrix ( Mat2, Mat3, fromList, frobenius_norm )
--   >>> import qualified Data.Vector.Fixed as V ( map )
--
--   >>> let m  = givens_rotator 0 1 1 1 :: Mat2 Double
--   >>> let m2 = fromList [[1, -1],[1, 1]] :: Mat2 Double
--   >>> m == (1 / (sqrt 2) :: Double) *> m2
--   True
--
--   >>> let m = fromList [[2,3],[5,7]] :: Mat2 Double
--   >>> let rot =  givens_rotator 0 1 2.0 5.0 :: Mat2 Double
--   >>> ((transpose rot) * m) !!! (1,0) < 1e-12
--   True
--   >>> let (Mat rows) = rot
--   >>> let (Mat cols) = transpose rot
--   >>> V.map norm rows :: Vec2 Double
--   fromList [1.0,1.0]
--   >>> V.map norm cols :: Vec2 Double
--   fromList [1.0,1.0]
--
--   >>> let m = fromList [[12,-51,4],[6,167,-68],[-4,24,-41]] :: Mat3 Double
--   >>> let rot = givens_rotator 1 2 6 (-4) :: Mat3 Double
--   >>> let ex_rot_r1 = [1,0,0] :: [Double]
--   >>> let ex_rot_r2 = [0,0.83205,-0.55470] :: [Double]
--   >>> let ex_rot_r3 = [0, 0.55470, 0.83205] :: [Double]
--   >>> let ex_rot = fromList [ex_rot_r1, ex_rot_r2, ex_rot_r3] :: Mat3 Double
--   >>> frobenius_norm ((transpose rot) - ex_rot) < 1e-4
--   True
--   >>> ((transpose rot) * m) !!! (2,0) == 0
--   True
--   >>> let (Mat rows) = rot
--   >>> let (Mat cols) = transpose rot
--   >>> V.map norm rows :: Vec3 Double
--   fromList [1.0,1.0,1.0]
--   >>> V.map norm cols :: Vec3 Double
--   fromList [1.0,1.0,1.0]
--
givens_rotator :: forall m a. (Eq a, Ring.C a, Algebraic.C a, Arity m)
               => Int -> Int -> a -> a -> Mat m m a
givens_rotator i j xi xj =
  construct f
  where
    xnorm = sqrt $ xi^2 + xj^2
    c = if xnorm == (fromInteger 0) then (fromInteger 1) else xi / xnorm
    s = if xnorm == (fromInteger 0) then (fromInteger 0) else xj / xnorm

    f :: Int -> Int -> a
    f y z
      | y == i && z == i = c
      | y == j && z == j = c
      | y == i && z == j = negate s
      | y == j && z == i = s
      | y == z = fromInteger 1
      | otherwise = fromInteger 0


-- | Compute the QR factorization of a matrix (using Givens
--   rotations). This is accomplished with two folds: the first one
--   traverses the columns of the matrix from left to right, and the
--   second traverses the entries of the column from top to
--   bottom.
--
--   The state that is passed through the fold is the current (q,r)
--   factorization. We keep the pair updated by multiplying @q@ and
--   @r@ by the new rotator (or its transpose).
--
--   We do not require that the diagonal elements of R are positive,
--   so our factorization is a little less unique than usual.
--
--   Examples:
--
--   >>> import Linear.Matrix
--
--   >>> let m = fromList [[1,2],[1,3]] :: Mat2 Double
--   >>> let (q,r) = qr m
--   >>> let c = (1 / (sqrt 2 :: Double))
--   >>> let ex_q = c *> (fromList [[1,-1],[1,1]] :: Mat2 Double)
--   >>> let ex_r = c *> (fromList [[2,5],[0,1]] :: Mat2 Double)
--   >>> frobenius_norm (q - ex_q) == 0
--   True
--   >>> frobenius_norm (r - ex_r) == 0
--   True
--   >>> let m' = q*r
--   >>> frobenius_norm (m - m') < 1e-10
--   True
--   >>> is_upper_triangular' 1e-10 r
--   True
--
--   >>> let m = fromList [[2,3],[5,7]] :: Mat2 Double
--   >>> let (q,r) = qr m
--   >>> frobenius_norm (m - (q*r)) < 1e-12
--   True
--   >>> is_upper_triangular' 1e-10 r
--   True
--
--   >>> let m = fromList [[12,-51,4],[6,167,-68],[-4,24,-41]] :: Mat3 Double
--   >>> let (q,r) = qr m
--   >>> frobenius_norm (m - (q*r)) < 1e-12
--   True
--   >>> is_upper_triangular' 1e-10 r
--   True
--
qr :: forall m n a. (Arity m, Arity n, Eq a, Algebraic.C a, Ring.C a)
   => Mat m n a -> (Mat m m a, Mat m n a)
qr matrix =
  ifoldl col_function initial_qr columns
  where
    Mat columns = transpose matrix
    initial_qr = (identity_matrix, matrix)

    -- | Process the column and spit out the current QR
    --   factorization. In the first column, we want to get rotators
    --   Q12, Q13, Q14,... In the second column, we want rotators Q23,
    --   Q24, Q25,...
    col_function (q,r) col_idx col =
      ifoldl (f col_idx) (q,r) col

    -- | Process the entries in a column, doing basically the same
    --   thing as col_function does. It updates the QR factorization,
    --   maybe, and returns the current one.
    f col_idx (q,r) idx _ -- ignore the current element
      | idx <= col_idx = (q,r) -- leave it alone
      | otherwise = (q*rotator, (transpose rotator)*r)
          where
            y = r !!! (idx, col_idx)
            rotator :: Mat m m a
            rotator = givens_rotator col_idx idx (r !!! (col_idx, col_idx)) y



-- | Determine the eigenvalues of the given @matrix@ using the
--   iterated QR algorithm (see Golub and Van Loan, \"Matrix
--   Computations\").
--
--   Warning: this may not converge if there are repeated eigenvalues
--   (in magnitude).
--
--   Examples:
--
--   >>> import Linear.Matrix ( Col2, Col3, Mat2, Mat3 )
--   >>> import Linear.Matrix ( frobenius_norm, fromList, identity_matrix )
--
--   >>> let m = fromList [[1,1],[-2,4]] :: Mat2 Double
--   >>> let actual = eigenvalues 1000 m
--   >>> let expected = fromList [[3],[2]] :: Col2 Double
--   >>> frobenius_norm (actual - expected) < 1e-12
--   True
--
--   >>> let m = identity_matrix :: Mat2 Double
--   >>> let actual = eigenvalues 10 m
--   >>> let expected = fromList [[1],[1]] :: Col2 Double
--   >>> frobenius_norm (actual - expected) < 1e-12
--   True
--
--   >>> let m = fromList [[0,1,0],[0,0,1],[1,-3,3]] :: Mat3 Double
--   >>> let actual = eigenvalues 1000 m
--   >>> let expected = fromList [[1],[1],[1]] :: Col3 Double
--   >>> frobenius_norm (actual - expected) < 1e-2
--   True
--
eigenvalues :: forall m a. (Arity m, Algebraic.C a, Eq a)
             => Int
             -> Mat (S m) (S m) a
             -> Col (S m) a
eigenvalues iterations matrix
  | iterations <  0 = error "negative iterations requested"
  | iterations == 0 = diagonal matrix
  | otherwise =
      diagonal (ut_approximation (iterations - 1))
      where
        ut_approximation :: Int -> Mat (S m) (S m) a
        ut_approximation 0 = matrix
        ut_approximation k = ut_next
          where
            ut_prev = ut_approximation (k-1)
            (qk,rk) = qr ut_prev
            ut_next = rk*qk



-- | Compute the eigenvalues and eigenvectors of a symmetric matrix
--   using an iterative QR algorithm. This is similar to what we do in
--   'eigenvalues' except we also return the product of all \"Q\"
--   matrices that we have generated. This turns out to me the matrix
--   of eigenvectors when the original matrix is symmetric. For
--   references see Goluv and Van Loan, \"Matrix Computations\", or
--   \"Calculation of Gauss Quadrature Rules\" by Golub and Welsch.
--
--   Warning: this may not converge if there are repeated eigenvalues
--   (in magnitude).
--
--   Examples:
--
--   >>> import Linear.Matrix ( Col2, Col3, Mat2, Mat3 )
--   >>> import Linear.Matrix ( column, frobenius_norm, fromList )
--   >>> import Linear.Matrix ( identity_matrix, vec3d )
--   >>> import Normed ( Normed(..) )
--
--   >>> let m = identity_matrix :: Mat3 Double
--   >>> let (vals, vecs) = eigenvectors_symmetric 100 m
--   >>> let expected_vals = fromList [[1],[1],[1]] :: Col3 Double
--   >>> let expected_vecs = m
--   >>> vals == expected_vals
--   True
--   >>> vecs == expected_vecs
--   True
--
--   >>> let m = fromList [[3,2,4],[2,0,2],[4,2,3]] :: Mat3 Double
--   >>> let (vals, vecs) = eigenvectors_symmetric 1000 m
--   >>> let expected_vals = fromList [[8],[-1],[-1]] :: Col3 Double
--   >>> let v0' = vec3d (2, 1, 2) :: Col3 Double
--   >>> let v0  = (1 / (norm v0') :: Double) *> v0'
--   >>> let v1' = vec3d (-1, 2, 0) :: Col3 Double
--   >>> let v1  = (1 / (norm v1') :: Double) *> v1'
--   >>> let v2' = vec3d (-4, -2, 5) :: Col3 Double
--   >>> let v2  = (1 / (norm v2') :: Double) *> v2'
--   >>> frobenius_norm ((column vecs 0) - v0) < 1e-12
--   True
--   >>> frobenius_norm ((column vecs 1) - v1) < 1e-12
--   True
--   >>> frobenius_norm ((column vecs 2) - v2) < 1e-12
--   True
--
eigenvectors_symmetric :: forall m a. (Arity m, Algebraic.C a, Eq a)
                       => Int
                       -> Mat (S m) (S m) a
                       -> (Col (S m) a, Mat (S m) (S m) a)
eigenvectors_symmetric iterations matrix
  | iterations <  0 = error "negative iterations requested"
  | iterations == 0 = (diagonal matrix, identity_matrix)
  | not $ symmetric matrix = error "argument is not symmetric"
  | otherwise =
      (values, vectors)
      where
        -- | We think of \"T\" as an approximation to an
        -- upper-triangular matrix from which we get our
        -- eigenvalues. The matrix \"P\" is the product of all
        -- previous \"Q\"s and its columns approximate the
        -- eigenvectors.
        tp_pair :: Int -> (Mat (S m) (S m) a, Mat (S m) (S m) a)
        tp_pair 0 = (matrix, identity_matrix)
        tp_pair k = (tk,pk)
                    where
                      (t_prev, p_prev) = tp_pair (k-1)
                      (qk,rk) = qr t_prev
                      pk = p_prev*qk
                      tk = rk*qk

        (values, vectors) = (first diagonal) (tp_pair iterations)