(* ::Package:: *)

(* :Name: BasicFunctionsForNumerics` *)

(* :Title: Functions for Numerical Calculations with Mathematica *)

(* :Author: Karel Houfek *)

(* :Summary:
 To be added
*)

(* :Package Version: 1.0 *)

(* :History:
 Version 1.0 by Karel Houfek, January 2013.
*)

(* :Keywords:
 to be added
*)

(* :Mathematica Version: 8.0 *)

(* :Limitation:
*)

(* :Discussion:
*)

BeginPackage["BasicFunctionsForTMF057`"];

MyAitkenDeltaSquareProcess::usage = "MyAitkenDeltaSquareProcess[x_] returns a new series from x which converges faster than x."
MyAitkenSteffensenFindRoot::usage = "MyAitkenSteffensenFindRoot[f_, x0_, n_] returns a root of the equation x = f[x], stops after n iterations."

MyBisectionFindRoot::usage = "MyBisectionFindRoot[g_, a_, b_, n_] returns a root of the equation 0 = g[x], stops after n iterations."
MyRegulaFalsiFindRoot::usage = "MyRegulaFalsiFindRoot[g_, a_, b_, n_] returns a root of the equation 0 = g[x], stops after n iterations."
MyNewtonFindRoot::usage = "MyNewtonFindRoot[g_, dg_, x0_, n_] returns a root of the equation 0 = g[x], stops after n iterations."
MyNewtonFindRootND::usage = "MyNewtonFindRootND[g_, J_, x0_, n_] returns a root of the set of equations 0 = g[x], J[x] is Jacobian, stops after n iterations."
MyNewtonFindRootWithConvergenceTest::usage = "MyNewtonFindRootWithConvergenceTest[g_, dg_, x0_, tol_, nmax_] returns a root of the equation 0 = g[x], stops after accuracy is tol or after nmax iterations."
MySecantFindRoot::usage = "MySecantFindRoot[g_, x0_, x1_, n_] returns a root of the equation 0 = g[x], stops after n iterations."
MyPadeFindRoot::usage = "MyPadeFindRoot[g_, x0_, x1_, x2_, n_] returns a root of the equation 0 = g[x], stops after n iterations."

MyAitkenNevilleInterpolation::usage = "MyAitkenNevilleInterpolation[xgrid_, fgrid_, x_] returns a value L_n[x] of Lagrange interpolating polynomial[x] through points (xgrid_i, fgrid_i)."

MyRectangularRule::usage = "MyRectangularRule[f_, a_, b_, n_, \[Lambda]_] returns an approximation of the integral of the function f[x] over <a,b> using the rectangular rule with n grid points x_i = a + h (i-1) + \[Lambda] h where h=(b-a)/n."
MyTrapezoidalRule::usage = "MyTrapezoidalRule[f_, a_, b_, n_] returns an approximation of the integral of the function f[x] over <a,b> using the rectangular rule with n + 1 grid points x_i = a + h i, i = 0,...,n  where h=(b-a)/n."
MyRombergIntegrate::usage = "MyRombergIntegrate[f_, a_, b_, k_] returns Romberg table of subsequent approximations of the integral of the function f[x] over <a,b>, the last row contains quadratures with h = (b-a)/2^k."
MyOrthogonalPolynomials::usage = "MyOrthogonalPolynomials[n_, a_, b_, v_] returns first n orthogonal MyPolynomials on interval <a,b> with weight function v[x]."
MyGaussianPointsAndWeights::usage = "MyGaussianPointsAndWeights[n_, a_, b_, v_] returns table of n points and weights for Gaussian quadrature on interval <a,b> with weight function v[x]."

MyGetLMMCoefficients::usage = "MyGetLMMCoefficients[method_, s_] returns \[Alpha]'s and \[Beta]'s of an s-step linear multistep method."
MyOrderOfAccuarcyOfLMM::usage = "OrderOfLMM[\[Alpha]_, \[Beta]_] determines order of accuracy of linear multistep method when \[Alpha]'s and \[Beta]'s are given."
MyODESolveUsingLMM::usage = "MyODESolveUsingLMM[{\[Alpha]_, \[Beta]_}, f_, t0_, vini_, k_, n_] solves numerically ODE du/dt = f[u,t] using a linear multistep method given by \[Alpha]'s and \[Beta]'s; returns the array {times, values}."

MyLUDecomposition::usage = "MyLUDecomposition[A_] does LU decomposition of a matrix A and the result returns as one matrix."
MyLUBackSubstitution::usage = "MyLUBackSubstitution[LU_, b_] solves Ax = b after LU decomposition of A."
MySolveUpperTriangular::usage = "MySolveUpperTriangular[U_, b_] solves Ux = b where U is an upper triangular matrix."
MyQRGramSchmidt::usage = "MyQRGramSchmidt[A_] returns QR decomposition of A as two matrices Q and R using basic Gram-Schmidt orthogonalization."
MyQRModifiedGramSchmidt::usage = "MyQRModifiedGramSchmidt[A_] returns QR decomposition of A as two matrices Q and R using basic modified Gram-Schmidt orthogonalization."
MyQRHouseholder::usage = "MyQRHouseholder[A_] returns QR decomposition of A as two matrices Q and R using Householder triangularization."
MyJacobiDiagonalization::usage = "MyJacobiDiagonalizationSimple[A_, niter_] calculates eigenvalues and eigenvectors of real symmetric matrix A using the Jacobi method with niter interations and the result returns as one vector of eigenvalues and one matrix of eigenvectors (in rows)."


Unprotect[
  MyPolynomial,
  MyAitkenDeltaSquareProcess,
  MyAitkenSteffensenFindRoot,
  MyBisectionFindRoot,
  MyRegulaFalsiFindRoot,
  MyNewtonFindRoot,
  MyNewtonFindRootND,
  MyNewtonFindRootWithConvergenceTest,
  MySecantFindRoot,
  MyPadeFindRoot,
  MyAitkenNevilleInterpolation,
  MyRectangularRule,
  MyTrapezoidalRule,
  MyRombergIntegrate,
  MyOrthogonalPolynomials,
  MyGaussianPointsAndWeights,
  MyGetLMMCoefficients,
  MyGetRKCoefficients,
  MyOrderOfAccuarcyOfLMM,
  MyODESolveUsingLMM,
  MyODESolveUsingBD,
  MyODESolveUsingRK,
  MyLUDecomposition,
  MyLUBackSubstitution,
  MySolveUpperTriangular,
  MyQRGramSchmidt,
  MyQRModifiedGramSchmidt,
  MyQRHouseholder,
  MyJacobiDiagonalization
];

Begin["`Private`"];

Clear[
  MyPolynomial,
  MyAitkenDeltaSquareProcess,
  MyAitkenSteffensenFindRoot,
  MyBisectionFindRoot,
  MyRegulaFalsiFindRoot,
  MyNewtonFindRoot,
  MyNewtonFindRootND,
  MyNewtonFindRootWithConvergenceTest,
  MySecantFindRoot,
  MyPadeFindRoot,
  MyAitkenNevilleInterpolation,
  MyRectangularRule,
  MyTrapezoidalRule,
  MyRombergIntegrate,
  MyOrthogonalPolynomials,
  MyGaussianPointsAndWeights,
  MyGetLMMCoefficients,
  MyGetRKCoefficients,
  MyOrderOfAccuarcyOfLMM,
  MyODESolveUsingLMM,
  MyODESolveUsingBD,
  MyODESolveUsingRK,
  MyLUDecomposition,
  MyLUBackSubstitution,
  MySolveUpperTriangular,
  MyQRGramSchmidt,
  MyQRModifiedGramSchmidt,
  MyQRHouseholder,
  MyJacobiDiagonalization
];

MyPolynomial[l_List, x_] := Sum[l[[i]]x^(i-1), {i, 1, Length[l]}]

MyAitkenDeltaSquareProcess[x_] :=
  Module[{Ax, n, denom},
  n = Length[x];
  Ax = ConstantArray[0.0, n-2];
  Do[
    denom = x[[i+2]]-2x[[i+1]]+x[[i]];
    If[denom == 0.0,
      If[i>1, Ax[[i]] = Ax[[i-1]], Ax[[i]] = 0.0],
      Ax[[i]] = x[[i]] - (x[[i+1]]-x[[i]])^2/denom
    ],
    {i, 1, n-2}
  ];
  Return[Ax]
];

MyAitkenSteffensenFindRoot[f_, x0_, n_] :=
  Module[{fx, ffx, x, denom},
  x = ConstantArray[0.0, n];
  x[[1]] = x0;
  Do[
    fx = f[x[[i-1]]]; ffx = f[fx];
    denom = ffx - 2 fx + x[[i-1]];
    If[denom == 0.0,
      x[[i]] = x[[i-1]],
      x[[i]] = x[[i-1]] - (fx - x[[i-1]])^2/denom
    ],
    {i, 2, n}
  ];
  Return[x]
];

MyBisectionFindRoot[f_, a_, b_, n_] :=
  Module[{aa, bb, ab, x},
  x = ConstantArray[0.0, n];
  If[f[a]f[b] > 0.0,
    Print["Warning: bisection method has wrong input, f[a]f[b]>0.0"],
    aa = a; bb = b;
    Do[
      ab = (aa + bb)/2;
      If[f[aa]f[ab] < 0.0,
        bb = ab,
        aa = ab
      ];
      x[[i]] = ab,
      {i, 1, n}
    ];
  ];
  Return[x]
];

MyNewtonFindRoot[f_, df_, x0_, n_] :=
  Module[{x},
  x = ConstantArray[0.0, n];
  x[[1]] = x0;
  Do[
    x[[i]] = x[[i-1]] - f[x[[i-1]]] / df[x[[i-1]]],
    {i, 2, n}
  ];
  Return[x]
];

MyNewtonFindRootND[f_, J_, x0_, n_] :=
  Module[{x},
  x = ConstantArray[0.0, n];
  x[[1]] = x0;
  Do[
    x[[i]] = x[[i-1]] - Inverse[J[x[[i-1]]]].f[x[[i-1]]],
    {i, 2, n}
  ];
  Return[x]
];

MyNewtonFindRootWithConvergenceTest[f_, df_, x0_, tol_, nmax_] :=
  Module[{x, xprev, n},
  x = x0;
  n = 0;
  Do[
    xprev = x; n++;
    x = xprev - f[xprev] / df[xprev];
    If[Abs[x-xprev] < tol, Break[]],
    {nmax}
  ];
  Return[{x, n}]
];

MySecantFindRoot[f_, x0_, x1_, n_] :=
  Module[{x, fx1, fx2, denom},
  x = ConstantArray[0.0, n];
  x[[1]] = x0; x[[2]] = x1;
  Do[
    fx1 = f[x[[i - 1]]];
    fx2 = f[x[[i - 2]]];
    denom = fx1 - fx2;
    If[denom == 0.0,
      x[[i]] = x[[i - 1]],
      x[[i]] = (x[[i - 2]] * fx1 - x[[i - 1]] * fx2) / denom
    ],
    {i, 3, n}
  ];
  Return[x]
];

MyPadeFindRoot[f_, x0_, x1_, x2_, n_] :=
  Module[{x, fx1, fx2, fx3, denom},
  x = ConstantArray[0.0, n];
  x[[1]] = x0; x[[2]] = x1; x[[3]] = x2;
  Do[
    fx1 = f[x[[i - 1]]]; If[fx1 == 0.0, x[[i]] = x[[i - 1]]; Continue[]];
    fx2 = f[x[[i - 2]]]; If[fx2 == 0.0, x[[i]] = x[[i - 2]]; Continue[]];
    fx3 = f[x[[i - 3]]]; If[fx3 == 0.0, x[[i]] = x[[i - 3]]; Continue[]];
    fx1 = (x[[i - 2]] - x[[i - 3]]) / fx1;
    fx2 = (x[[i - 3]] - x[[i - 1]]) / fx2;
    fx3 = (x[[i - 1]] - x[[i - 2]]) / fx3;
    denom = fx1 + fx2 + fx3;
    If[denom == 0.0,
      x[[i]] = (x[[i - 2]] + x[[i - 3]]) / 2.0,
      x[[i]] = (x[[i - 1]] * fx1 + x[[i - 2]] * fx2 + x[[i - 3]] * fx3) / denom
    ],
    {i, 4, n}
  ];
  Return[x]
];

MyAitkenNevilleInterpolation[xgrid_, fgrid_, x_] :=
  Module[{Px, n},
  n = Length[xgrid] - 1; (* degree of the final interpolating polynomial *)
  Px = fgrid;            (* values of subsequent interpolating polynamials at x, 
                            at the beginning just constants f[x_i], then replaced from the end *)
  Do[
    Do[
      Px[[i]] = ((x - xgrid[[i]]) * Px[[i - 1]] + (xgrid[[i - k]] - x) * Px[[i]]) / (xgrid[[i - k]] - xgrid[[i]]),
      {i, n + 1, k + 1, -1}
    ],
    {k, 1, n}
  ];
  Return[ Px[[n + 1]] ]
];

MyRectangularRule[f_, a_, b_, n_, \[Lambda]_] :=
  Module[{int, h, fx, r},
  h = N[(b - a)/n];
  fx = Table[f[a + h (r - 1)+\[Lambda] h],{r, 1, n}];
  int = h Total[fx];
  Return[int]
];

MyTrapezoidalRule[f_, a_, b_, n_] :=
  Module[{h, int, x, i},
  h = N[(b - a)/n];
  int = 0.0;
  x = a;
  Do[
    x = x + h;
    int = int + f[x],
    {i, 1, n - 1}
  ];
  int = int + 0.5 * (f[a] + f[b]);
  Return[h * int]
];

MyRombergIntegrate[f_, a_, b_, k_] :=
  Module[{int, h, fx, i, j, np, r, fourk},
  int = ConstantArray[0.0, {k + 1, k + 1}];
  h = (b - a)/2;
  np = 1;
  int[[1,1]] = h (f[b] + f[a]);
  Do[
    fx = Table[f[a + h (2 r - 1)],{r, 1, np}];
    int[[i,1]] = int[[i-1,1]] / 2 + h Total[fx];
    fourk = 4;
    Do[
      int[[i,j+1]] = (fourk int[[i,j]] - int[[i-1,j]]) / (fourk - 1);
      fourk = 4 fourk,
      {j, 1, i-1}
    ];
    np = 2 np;
    h = h / 2,
    {i, 2, k + 1}
  ];
  Return[int]
];

MyOrthogonalPolynomials[n_, a_, b_, v_, x_] :=
  Module[{p, \[Delta], \[Gamma]2, norm},
  p  = ConstantArray[1, n + 1];
  \[Delta]  = ConstantArray[0, n];
  \[Gamma]2 = ConstantArray[0, n];
  norm = ConstantArray[0, n];
  Do[
    norm[[i]] = Integrate[p[[i]]p[[i]]v[x], {x,a,b}];
    \[Delta][[i]] = Integrate[x p[[i]]p[[i]]v[x],{x,a,b}]/norm[[i]];
    If[i == 1, \[Gamma]2[[i]] = 0, \[Gamma]2[[i]] = norm[[i]]/norm[[i-1]]];
    p[[i+1]] = (x - \[Delta][[i]])p[[i]] - If[i == 1, 0, \[Gamma]2[[i]]p[[i-1]]]//Expand
   ,{i,1,n}
  ];
  Return[p]
];

MyGaussianPointsAndWeights[n_, a_, b_, v_] :=
  Module[{x, w, p, z, Vandermode, Moments},
  p = MyOrthogonalPolynomials[n+1, a, b, v, z];
  x = Sort[Re[z/.N[Solve[p[[n+1]] == 0, z],16]]];
  Vandermode = ConstantArray[1,{n,n}];
  Do[ Vandermode[[i,j]] = x[[j]]^(i-1),{j,1,n},{i,2,n}];
  Moments = ConstantArray[0,n];
  Do[ Moments[[i]] = Integrate[v[z]z^(i-1),{z,a,b}],{i,1,n}];
  w = LinearSolve[Vandermode,Moments];
  Return[{x, w}]
];

MyGetLMMCoefficients[method_, s_] :=
  Module[{\[Alpha], \[Beta], z, eqs, A, b, x},
  MyGetLMMCoefficients::method = "Unknown method `1`. Euler method returned.";
  \[Alpha] = ConstantArray[0, s+1];
  \[Beta] = ConstantArray[0, s+1];
  Switch[method,
    "Euler",
      \[Alpha] = {-1, 1};
      \[Beta] = {1, 0},
    "Backward Euler",
      \[Alpha] = {-1, 1};
      \[Beta] = {0, 1},
    "Trapezoid",
      \[Alpha] = {-1, 1};
      \[Beta] = {1/2, 1/2},
    "Midpoint",
      \[Alpha] = {-1, 0, 1};
      \[Beta] = {0, 2, 0},
    "Leap-frog",
      \[Alpha] = {-1, 0, 1};
      \[Beta] = {0, 2, 0},
    "Adams-Bashforth",
      \[Alpha][[s;;s+1]] = {-1, 1};
      \[Beta] = CoefficientList[Normal[Series[(z^s-z^(s-1))/Log[z],{z,1,s-1}]],z];
      AppendTo[\[Beta], 0],
    "Adams-Moulton",
      \[Alpha][[s;;s+1]] = {-1, 1};
      \[Beta] = CoefficientList[Normal[Series[(z^s-z^(s-1))/Log[z],{z,1,s}]],z],
    "Nystroem",
      \[Alpha][[s-1;;s+1]] = {-1, 0, 1};
      \[Beta] = CoefficientList[Normal[Series[(z^s-z^(s-2))/Log[z],{z,1,s-1}]],z];
      AppendTo[\[Beta], 0],
    "Milne-Simpson",
      \[Alpha][[s-1;;s+1]] = {-1, 0, 1};
      \[Beta] = CoefficientList[Normal[Series[(z^s-z^(s-2))/Log[z],{z,1,s}]],z],
    "BD", (* Backwards Differentiation *)
      \[Alpha] = CoefficientList[Normal[Series[z^s Log[z],{z,1,s}]],z];
      \[Beta][[s+1]] = 1 / \[Alpha][[s+1]];
      \[Alpha] = \[Beta][[s+1]] \[Alpha],
    "Linearly Unstable Explicit",
      \[Alpha] = {1, -2, 1};
      \[Beta] = {-1/2, 0, 1/2},
    "Most Accurate 2-step Explicit",
      \[Alpha] = {-5, 4, 1};
      \[Beta] = {2, 4, 0},
    _,
      Message[MyGetLMMCoefficients::method, method];
      \[Alpha] = {-1, 1};
      \[Beta] = {1, 0}
  ];
  Return[{\[Alpha], \[Beta]}]
];

MyGetRKCoefficients[method_, s_] :=
  Module[{c, A, b},
  MyGetRKCoefficients::method = "Unknown method `1`. Classical R-K method returned.";
  c = ConstantArray[0, s];
  A = ConstantArray[0, {s, s}];
  b = ConstantArray[0, s];
  Switch[method,
    "Classical",
      Switch[s,
        1,
          c = {0};
          A = {{0}};
          b = {1},
        2,
          c = {0, 1/2};
          A = {{0, 0},{1/2, 0}};
          b = {0, 1},
        4,
          c = {0, 1/2, 1/2, 1};
          A = {{0, 0, 0, 0},{1/2, 0, 0, 0},{0, 1/2, 0, 0},{0, 0, 1, 0}};
          b = {1/6, 1/3, 1/3, 1/6},
        _,
          c = {0, 1/2};
          A = {{0, 0},{1/2, 0}};
          b = {0, 1}
      ],
    "Cash-Karp",
      c = {0, 1/5, 3/10, 3/5, 1, 7/8};
      A = {{0, 0, 0, 0, 0, 0},
           {1/5, 0, 0, 0, 0, 0},
           {3/40, 9/40, 0, 0, 0, 0},
           {3/10, -9/10, 6/5, 0, 0, 0},
           {-11/54, 5/2, -70/27, 35/27, 0, 0},
           {1631/55296, 175/512, 575/13824, 44275/110592, 253/4096, 0}};
      Switch[s,
        4,
          b = {2825/27648, 0, 18575/48384, 13525/55296, 277/14336, 1/4},
        _, (* standard is this method of the order 5 *)
          b = {37/378, 0, 250/621, 125/594, 0, 512/1771}
      ],
    _,
      Message[MyGetRKCoefficients::method, method];
      {c, A, b} = MyGetRKCoefficients["Classical", s]
  ];
  Return[{c, A, b}]
];

MyOrderOfAccuarcyOfLMM[\[Alpha]_, \[Beta]_] :=
  Module[{L, \[Kappa], coef, i, order, c1},
  MyOrderOfAccuarcyOfLMM::lenarg = "Number of \[Alpha]'s (`1`) and \[Beta]'s (`2`) is different.";
  If[Length[\[Alpha]] == Length[\[Beta]],
    L = Normal[Series[MyPolynomial[\[Alpha], Exp[\[Kappa]]] - \[Kappa] MyPolynomial[\[Beta], Exp[\[Kappa]]],{\[Kappa], 0, 2 Length[\[Alpha]]}]];
    coef = CoefficientList[L, \[Kappa]];
    {order, c1} = Do[If[coef[[i]] != 0, Return[{i-2, coef[[i]]}]], {i, 1, Length[coef]}];
    Return[{order, c1}],
    Message[MyOrderOfAccuarcyOfLMM::lenarg, \[Alpha], \[Beta]];
    Return[0]
  ]
];

MyODESolveUsingLMM[{\[Alpha]_, \[Beta]_}, f_, t0_, vini_, k_, n_] :=
  Module[{a, b, s, t, v, i, j},
  a = N[\[Alpha]]; b = N[\[Beta]];
  s = Length[vini];
  t = N[t0 + k Range[0,n]];
  v = ConstantArray[0.0, n + 1];
  v[[1;;s]] = N[vini];
  If[\[Beta][[s+1]] == 0,
    (* if the method is explicit then a simple iteration will do *)
    Do[
      v[[i+1]] = k * Sum[b[[j]] * f[v[[i-s+j]], t[[i-s+j]]],{j,1,s}] - Sum[a[[j]] * v[[i-s+j]],{j,1,s}];
      v[[i+1]] = v[[i+1]] / a[[s+1]],
      {i, s, n}
    ],
    (* if the method is implicit then we have to solve a nonlinear equation *)
    Do[
      (* start with a guess from the previous step *)
      v[[i+1]] = v[[i]];
      (* and do a simple iteration a few times *)
      Do[
        v[[i+1]] = k * Sum[b[[j]] * f[v[[i-s+j]], t[[i-s+j]]],{j,1,s+1}] - Sum[a[[j]] * v[[i-s+j]],{j,1,s}];
        v[[i+1]] = v[[i+1]] / a[[s+1]],
        {p,1,5}
      ],
      {i, s, n}
    ]
  ];
  Return[Transpose[{t,v}]]
];

MyODESolveUsingBD[{\[Alpha]_, \[Beta]_}, A_, t0_, vini_, k_, n_] :=
  Module[{a, b, s, t, v, i, j, m, IkA, rhs},
  a = N[\[Alpha]]; b = N[\[Beta]];
  s = Length[vini];
  t = N[t0 + k Range[0,n]];
  v = ConstantArray[0.0, n + 1];
  v[[1;;s]] = N[vini];
  m = Length[v[[1]]];
  IkA = IdentityMatrix[m] - b[[s+1]] * k * A;
  Do[
    rhs = -Sum[a[[j]] * v[[i-s+j]],{j,1,s}];
    v[[i+1]] = LinearSolve[IkA, rhs],
    {i, s, n}
  ];
  Return[Transpose[{t,v}]]
];

MyODESolveUsingRK[{cs_, As_, bs_}, f_, t0_, vini_, k_, n_] :=
  Module[{c, A, b, r, t, v, y, i, j, l, nvar},
  c = N[cs]; A = N[As]; b = N[bs];
  r = Length[c];  (* number of intersteps *)
  t = N[t0 + k Range[0,n]];
  nvar = Length[vini];
  If[nvar == 0, 
    v = ConstantArray[0.0, n + 1];
    y = ConstantArray[0.0, r],
    v = ConstantArray[0.0, {n + 1, nvar}];
    y = ConstantArray[0.0, {r, nvar}]
  ];
  v[[1]] = N[vini];
  (* only explicit method is implemented yet *)
  Do[
     Do[
        y[[l]] = v[[i]] + k * Sum[A[[l]][[j]] * f[y[[j]], t[[i]] + c[[j]] * k],{j,1,r}],
        {l, 1, r}
     ];
     v[[i+1]] = v[[i]] + k * Sum[b[[j]] * f[y[[j]], t[[i]] + c[[j]] * k],{j,1,r}],
     {i, 1, n}
  ];
  Return[Transpose[{t,v}]]
];

MyLUDecomposition[A_] :=
  Module[{LU, m, i, j, k},
  m = Length[A];
  LU = A;
  Do[
    Do[
      LU[[j,k]] = LU[[j,k]] / LU[[k,k]];
      Do[
        LU[[j,i]] = LU[[j,i]] - LU[[j,k]] * LU[[k,i]],
        {i, k+1, m}
      ],
      {j, k+1, m}
    ],
    {k, 1, m-1}
  ];
  Return[LU]
];

MyLUBackSubstitution[LU_,b_] :=
  Module[{x, m, i, j, k},
  m = Length[LU];
  x = b;
  (* First solve Ly = b *)
  Do[
    Do[
      x[[i]] = x[[i]] - LU[[i,j]] * x[[j]],
      {j, 1, i-1}
    ],
    {i, 2, m}
  ];
  (* Then solve Ux = y *)
  Do[
    Do[
      x[[i]] = x[[i]] - LU[[i,j]] * x[[j]],
      {j, i+1, m}
    ];
    x[[i]] = x[[i]] / LU[[i,i]],
    {i, m, 1, -1}
  ];
  Return[x]
];

MySolveUpperTriangular[U_,b_] :=
  Module[{x, m, i, j, k},
  m = Length[U];
  x = b;
  (* Solve Ux = y *)
  Do[
    Do[
      x[[i]] = x[[i]] - U[[i,j]] * x[[j]],
      {j, i+1, m}
    ];
    x[[i]] = x[[i]] / U[[i,i]],
    {i, m, 1, -1}
  ];
  Return[x]
];

MyQRGramSchmidt[A_] :=
  Module[{Q, R, m, n, i, j},
  {m, n} = Dimensions[A];
  R = ConstantArray[0.0, {n, n}];
  Q = ConstantArray[0.0, {m, n}];
  Do[
    Q[[1;;m, j]] = A[[1;;m, j]];
    Do[
      R[[i, j]] = Dot[Conjugate[Q[[1;;m, i]]], A[[1;;m, j]]];
      Q[[1;;m, j]] -= R[[i, j]] * Q[[1;;m, i]],
      {i, 1, j-1}
    ];
    R[[j, j]] = Norm[Q[[1;;m, j]]];
    Q[[1;;m, j]] = Q[[1;;m, j]] / R[[j, j]],
    {j, 1, n}
  ];
  Return[{Q, R}]
];

MyQRModifiedGramSchmidt[A_] :=
  Module[{Q, R, m, n, i, j},
  {m, n} = Dimensions[A];
  R = ConstantArray[0.0, {n, n}];
  Q = A;
  Do[
    R[[i, i]] = Norm[Q[[1;;m, i]]];
    Q[[1;;m, i]] = Q[[1;;m, i]] / R[[i, i]];
    Do[
      R[[i, j]] = Dot[Conjugate[Q[[1;;m, i]]], Q[[1;;m, j]]];
      Q[[1;;m, j]] -= R[[i, j]] * Q[[1;;m, i]],
      {j, i+1, n}
    ],
    {i, 1, n}
  ];
  Return[{Q, R}]
];

MyQRHouseholder[A_] :=
  Module[{Q, R, v, \[Alpha], m, n, k, j},
  {m, n} = Dimensions[A];
  R = A;
  Q = IdentityMatrix[m];
  Do[
    v = R[[k;;m, k]];
    v += Sign[v[[1]]] * Norm[v] * UnitVector[m-k+1,1];
    v = 2.0 * v / Norm[v];
    Do[
      \[Alpha] = 0.5 * Dot[Conjugate[v], R[[k;;m, j]]];
      R[[k;;m, j]] -= \[Alpha] * v,
      {j, k, n}
    ];
    (* Calculate Q^\[Dagger] as Q^\[Dagger]I *)
    Do[
      \[Alpha] = 0.5 * Dot[Conjugate[v], Q[[k;;m, j]]];
      Q[[k;;m, j]] -= \[Alpha] * v,
      {j, 1, m}
    ],
    {k, 1, n}
  ];
  Do[
    If[R[[k, k]] < 0.0,
      R[[k, k;;n]] = -R[[k, k;;n]];
      Q[[k, 1;;m]] = -Q[[k, 1;;m]]
    ],
    {k, 1, n}
  ];
  Return[{Q, R}]
];

MyJacobiDiagonalization[A_, niter_] :=
(* Assuming A is real symmetric matrix *)
  Module[{EVal, EVec, SumOffDiag, m, \[Theta], t, s, c, \[Tau], i, p, q, r, x, y},
  m = Length[A];
  EVal = A;
  EVec = IdentityMatrix[m];
  SumOffDiag = ConstantArray[0.0, niter + 1];
  SumOffDiag[[1]] = Total[Abs[UpperTriangularize[EVal,1]]^2,2];
  Do[
    Do[
      Do[
        If[EVal[[p,q]] != 0.0,
          (* \[Theta] = cotg (2\[Phi]) = (c^2 - s^2)/2sc *)
          \[Theta] = (EVal[[q,q]] - EVal[[p,p]]) / (2 * EVal[[p,q]]);
          (* t = -\[Theta] + Sqrt[\[Theta]^2 + 1]; *)
          t = 1/(Abs[\[Theta]] + Sqrt[\[Theta]^2 + 1]); If[\[Theta] < 0.0, t = -t];
          c = 1 / Sqrt[1 + t^2]; s = t * c;
          \[Tau] = s / (1 + c);
          EVal[[p,p]] = EVal[[p,p]] - t * EVal[[p,q]];
          EVal[[q,q]] = EVal[[q,q]] + t * EVal[[p,q]];
          EVal[[p,q]] = 0.0;
          Do[
            x = EVal[[r,p]]; y = EVal[[r,q]];
            EVal[[r,p]] = x - s * (y + \[Tau] * x);
            EVal[[r,q]] = y + s * (x - \[Tau] * y),
          {r, 1, p-1}];
          Do[
            x = EVal[[p,r]]; y = EVal[[r,q]];
            EVal[[p,r]] = x - s * (y + \[Tau] * x);
            EVal[[r,q]] = y + s * (x - \[Tau] * y),
          {r, p+1, q-1}];
          Do[
            x = EVal[[p,r]]; y = EVal[[q,r]];
            EVal[[p,r]] = x - s * (y + \[Tau] * x);
            EVal[[q,r]] = y + s * (x - \[Tau] * y),
          {r, q+1, m}];
          Do[
            x = EVec[[r,p]]; y = EVec[[r,q]];
            EVec[[r,p]] = x - s * (y + \[Tau] * x);
            EVec[[r,q]] = y + s * (x - \[Tau] * y),
          {r, 1, m}]
        ],
        {q, p+1, m}
      ],
      {p, 1, m-1}
    ];
    SumOffDiag[[i+1]] = Total[Abs[UpperTriangularize[EVal,1]]^2,2],
    {i, 1, niter}
  ];
  Return[{Diagonal[EVal],EVec,SumOffDiag}]
];

End[ ];
 
(* End `Private` Context. *)

SetAttributes[{
  MyPolynomial,
  MyAitkenDeltaSquareProcess,
  MyAitkenSteffensenFindRoot,
  MyBisectionFindRoot,
  MyRegulaFalsiFindRoot,
  MyNewtonFindRoot,
  MyNewtonFindRootND,
  MyNewtonFindRootWithConvergenceTest,
  MySecantFindRoot,
  MyPadeFindRoot,
  MyAitkenNevilleInterpolation,
  MyRectangularRule,
  MyTrapezoidalRule,
  MyRombergIntegrate,
  MyOrthogonalPolynomials,
  MyGaussianPointsAndWeights,
  MyGetLMMCoefficients,
  MyGetRKCoefficients,
  MyOrderOfAccuarcyOfLMM,
  MyODESolveUsingLMM,
  MyODESolveUsingBD,
  MyODESolveUsingRK,
  MyLUDecomposition,
  MyLUBackSubstitution,
  MySolveUpperTriangular,
  MyQRGramSchmidt,
  MyQRModifiedGramSchmidt,
  MyQRHouseholder,
  MyJacobiDiagonalization
},
{ Protected, ReadProtected }
];

EndPackage[ ]; (* End package Context. *)

