(* Question 4 : Implement SVD (this what took this assignment so long,
                i did not understand the NRiC code *)

(* Returns Householders matrix for A(nxm) for kth col 
   => all elements below A(l,k) are 0ed *)

Householder[A_,n_,m_,k_,l_] := 
  Block[{i,j,u,H},
	u =Map[#[[k]]&,A]; (* get column k *)
	u = u[[Range[l,n]]]; (* get col l..n *)
	u = Join[Table[0,{i,l-1}], u];
	u[[l]] = u[[l]] + N[Sqrt[u.u]];
	H = IdentityMatrix[n] - Table[u[[i]]*u[[j]]/(0.5*u.u),{i,n},{j,m}];
	Return[H];
	];
	

(* Structured for general n <= m *)
(* Bidiagonalization for SVD using householder's orthogonal
   transformations *)
Bidiagonalize[AA_] :=
  Block[{n = Length[AA], m = Length[AA[[1]]], A = AA, U1,V1,i,U,V},
	U = IdentityMatrix[n];
	V = IdentityMatrix[m];
	
	For[i=1, i < m, i++,
	    If[i < n,
	       Block[{},
		     U1 =Householder[A,n,m,i,i];
		     A = Dot[U1,A];
		     U = Dot[U1,U];]];
	    If[i < m-1,
	       Block[{},
		     V1 = Transpose[Householder[Transpose[A],m,n,i,i+1]];
		     A = Dot[A,V1];
		     V = Dot[V,V1];]];
            (* Print[A]; *)
	   ];
      Return[{U,V}];
   ];
(* Usage {U, V} = Bidiagonalize[M]; *)



Givens[n_,i_,j_,cos_,sin_] :=
  Block[{J = IdentityMatrix[n]},
	J[[i,i]] = cos;  J[[i,j]] = sin;
	J[[j,i]] = -sin; J[[j,j]] = cos;
	Return[J];];

Golub[BB_] :=
  Block[{n = Length[BB], B=BB, Ut,V,J,k,t,eigen,lambda,temp,x,y,cos,sin},
	t = IdentityMatrix[2];
	Ut = IdentityMatrix[n]; V = IdentityMatrix[n];
	t[[1,1]] = B[[n-1,n-1]]^2+B[[n-2,n-1]]^2;
	t[[1,2]] = B[[n-1,n-1]]*B[[n-1,n]];
	t[[2,1]] = t[[1,2]];
	t[[2,2]] = B[[n,n]]^2+B[[n-1,n]]^2;
	eigen = N[Eigenvalues[t]];
	lambda = eigen[[1]]; temp = eigen[[2]];
	If [Abs[temp-t[[2,2]]] < Abs[lambda-t[[2,2]]],
	    lambda = temp];
	x = N[B[[1,1]]^2-lambda];
	y = N[B[[1,1]]*B[[1,2]]];
	For[k=1, k < n, k++,
	    Block[{},
		  temp = Sqrt[x*x+y*y]; cos = N[x/temp]; sin = N[-y/temp];
		  J = Givens[n,k,k+1,cos,sin];
		  B = Dot[B,J]; V = Dot[V,J];
		  x = B[[k,k]]; y = B[[k+1,k]];
		  temp = Sqrt[x*x+y*y]; cos = N[x/temp]; sin = N[-y/temp];
		  J = Givens[n,k,k+1,cos,sin];
		  J = Transpose[J];
		  B = Dot[J,B]; Ut = Dot[J,Ut];
		  If[k < n-1,Block[{},x = B[[k,k+1]];y=B[[k,k+2]];]];
		  ];
	    ];
	Return[{B,Ut,V}];
	];

IsDiagonal[A_,delta_] :=
  Block[{i,j,n = Length[A], m = Length[A[[1]]]},
	For[i=1, i <= n, i++,
	    For[j=1, j<=m, j++,
		If[((i != j) && (Abs[A[[i,j]]] >= delta)),
		   Return[False]]]];
	Return[True]];

Delta = 0.00000001;

SVD[AA_] :=
  Block[{n = Length[AA], m = Length[AA[[1]]], A, U1,V1,i,Ut,V},
	{U1, V1} = Bidiagonalize[AA];
	A = Dot[U1,AA,V1];
(*	Print[MatrixForm[A]];Print["iterate"]; *)
	While[!(IsDiagonal[A,Delta]), 
	      Block[{},
		    {A,Ut,V} = Golub[A];
		    U1 = Dot[Ut,U1]; V1 = Dot[V1,V];
(*		    Print[MatrixForm[Chop[A,Delta]]];Print["iterate"];*)
                    ];
              ];
	Return[{Chop[A,Delta],Dot[Ut,U1],Dot[V1,V]}];
	];
(* A (diagonal) = Ut*AA*V , referring to notes Ut => U-transpose,
   V => V, A => Sigma(diagonal *)

SVDsolve[AA_,b_] :=
  Block[{A,Ut,V,Siginv,i},
	{A,Ut,V} = SVD[AA];
	Siginv = A;
	For [i=1, i <= Length[Siginv], i++,
	     If[(Abs[Siginv[[i,i]]] <= Delta), Siginv[[i,i]] = 0.0,
		Siginv[[i,i]] = 1.0/Siginv[[i,i]]]];
	Return[{A,Ut,V,Dot[V,Siginv,Ut,b]}];];
 
(*
 Question 5
{S,Ut,V,x} = SVDsolve[A,b];
MatrixForm[x]
MatrixForm[S]

 a)
   A = {{2,2,1},{1,1,1},{3,2,1}}; b = {{5},{-5},{0}};
   
Out[69]//MatrixForm= -5. (* Solution *)

                     15.

                     -15.

In[70]:= (* S = Sigma *)
Out[70]//MatrixForm= 5.04892     0           0

                     0           0.643104    0

                     0           0           -0.307979



 b)
   A = {{1,2,1},{2,3,1},{3,5,2}}; b = {{3}, {5},{1}};

IIn[140]:= (* Solution; In this case Least squares, since b does not
              span the col space of Ut-transpose= U *)

Out[140]//MatrixForm= 1.77778

                      0.222222

                      -1.55556

In[141]:= (* Sigma, Rank = 2 *)
Out[141]//MatrixForm= 7.60555    0          0

                      0          0.394449   0

                      0          0          0


n[132]:= MatrixForm[Ut]

Out[132]//MatrixForm= 0.3197      0.490799    0.810499

                      0.751304    -0.652521   0.0987837

                      0.57735     0.57735     -0.57735



 c)
   A = {{1,-1,0},{2,3,1},{3,5,2}}; b = {{2},{3},{5}};
Out[79]//MatrixForm= 1.5 (* Solution *)
 
                     -0.5

                     1.5

In[80]:= (* S = Sigma *)
Out[80]//MatrixForm= 7.21398     0           0

                     0           1.38507     0

                     0           0           -0.200162


*)

