unit Matrix;

// Copyright: Julian und Michel Schnell, Krefeld, Germany, mschnell@bschnell.de

interface
type
  TVector = array of Extended;
  TMatrix = array of TVector;

  TFunc = function(x: extended): extended;
  TFuncI = function(x: extended; i: Integer): extended;

function SolveLinearSystem(A: TMatrix): TVector;
// sloves Ax=y, with A a square martrix
// Parameters:
// A: Matrix with the vector y added "at the right side" to the original A
//    so A has a dimension of n, n+1
// Result: solution vector x

function SolveLinearSystems(A: TMatrix): TMatrix;
// sloves AX=Y, with A a square martrix, X and Y matrices with the same hight as A
// Parameters:
// A: Matrix with the matrix y added "at the right side" to the original A
//    so A has a dimension of n, n+m   (n = height of A, m = height of Y)
// Result: solution matrix X

function MatrixTrans (A: TMatrix) : TMatrix;
// transposes a matrix

function MatrixMulti (A, B: TMatrix) : TMatrix;
// multiplies matrices
// Parameters:
// A and B matrices, width of A = height of A
// Result: A*B

function MatrixVektorMulti (A: TMatrix; V:TVector ) : TVector;
// multiplies a matrix and a vector
// Parameters:
// A matrix, width of A = height of v
// Result: A*v

function MatrixConcat (A, B: TMatrix): TMatrix;
// Adds B to the right side of A
// Parameters:
// matrices A abd B with the same height

function MatrixIdent (n: Integer): TMatrix;
// returns an identity matrix I with dimension n

function MatrixInvert (A: TMatrix) : TMatrix;
// inverts a matrix
// Parameters:
// A a square matrix
// Result: A^-1

procedure MatrixBubbleSort(var A: TMatrix; m: Integer);
// sorts the lines of a matrix so that the column m is ascending
// (in place sort algorithm)

procedure DoSolveMatrix(A: TMatrix);
// mostly for internal use

implementation
uses
  Sysutils;


procedure DoSolveMatrix(A: TMatrix);
var
  i, j, k: Integer;
  Pivot: TVector;
  PivotLine: Integer;
  PivotElement, PivotElementAbs: extended;
  Multiplicator: Extended;
  n, m: Integer;
begin
  n := length(A);
  m := length(A[0]);
  for i := 0 to n - 1 do begin
// Due to the way Delphi and Free Pascal handle dynamic arrays
// exchanging matix lines is very cheap, as just two pointers are swapped.
// So we _always_ use the pivot element with the highest possible abs.
// Thus we don't need to compare a floating point to zero.
// We hope to improve stability this way.
    PivotLine := i;
    PivotElement := A[PivotLine, i];
    PivotElementAbs := Abs(PivotElement);
    for k := i + 1 to n - 1 do begin
      if Abs(A[k, i]) > PivotElementAbs then begin
        PivotLine := k;
        PivotElement := A[PivotLine, i];
        PivotElementAbs := Abs(PivotElement);
      end;
    end;
    if (PivotElementAbs = 0) then begin
        // Todo:
        // a decent way to detect an ill behaved
        // ("nearly singular") system
        raise EMathError.Create('System insolvable');
    end;
    if PivotLine <> i then begin
      Pivot:= A[PivotLine];       // reference counting could be avided by
      A[PivotLine] := A[i];       // doing a low level swap function
      A[i] := Pivot;              // that just swaps the pointers
    end;

    for j := i to n - 2 do begin
      Multiplicator := A[j + 1, i] / PivotElement;
      for k := i + 1 to m-1 do begin // The lower triangle elements are zero by definition.
                                     // They are not used any more
                                     // "  for k := i to n do begin "
                                     // would actually calculate the lower
                                     // triangle elements to be zero
        A[j + 1, k] := A[j + 1, k] - (Multiplicator * A[i, k]);
      end;
    end;
  end;


end;

function SolveLinearSystem(A: TMatrix): TVector;
// Beware:
// Due to the way Delphi and Free Pascal handle dynamic arrays
// A is destroyed even though it is not a var parameter
var
  i, k: Integer;
  Sum, Aii: Extended;
  n: Integer;
begin
  n:= length(A);
  if Length(A[0]) <> n + 1 then begin
    raise EMathError.Create('Matrix Dimension Error');
  end;

  DoSolveMatrix(A);

  SetLength(Result, n);
  for i := n - 1 downto 0 do begin
    try
      Sum := 0;
      Aii := A[i, i];
      for k := i + 1 to n - 1 do begin
//  first time ever, this loop is not taken at all -> Sum = 0
        Sum := Sum + Result[k] * A[i, k] / Aii;
      end;
      Result[i] := A[i, n] / Aii - Sum;
    except
      raise EMathError.Create('System insolvable');
    end;
  end;
end;

function SolveLinearSystems(A: TMatrix): TMatrix;
// Beware:
// Due to the way Delphi and Free Pascal handle dynamic arrays
// A is destroyed even though it is not a var parameter
var
  i, k: Integer;
  Sum, Aii: Extended;
  n, Results, r: Integer;
begin
  n:= length(A);
  Results := length(A[0]) - n;
  if Results <= 0 then begin
    raise EMathError.Create('Matrix Dimension Error');
  end;

  DoSolveMatrix(A);

  SetLength(Result, n, Results);
  for r := 0 to Results - 1 do begin
    for i := n - 1 downto 0 do begin
      try
        Sum := 0;
        Aii := A[i, i];
        for k := i + 1 to n - 1 do begin
  //  first time ever, this loop is not taken at all -> Sum = 0
          Sum := Sum + Result[k, r] * A[i, k] / Aii;
        end;
        Result[i, r] := A[i, n+r] / Aii - Sum;
      except
        raise EMathError.Create('System insolvable');
      end;
    end;
  end;
end;


function MatrixTrans (A: TMatrix) : TMatrix;
var
  i,j: integer;
  b,c: integer;
begin
  b:= length(A[low(A)]);
  c:= length(A);
  SetLength(Result, b, c);

  for i := low(A) to high(A) do begin
    for j := low(A[i]) to high(A[i]) do begin
       Result[j][i]:= A[i][j];
    end;
  end;
end;

function MatrixMulti (A, B: TMatrix) : TMatrix;
var
  i, j, k: integer;
  sum: extended;
begin
  if length(A[low(A)]) <> length(B) then begin
    raise EMathError.Create('Matrix Dimension Error');
    exit;
  end;
  i:= length(A);
  j:= length(B[low(B)]);
  SetLength(Result, i, j);

  for i := low(Result) to high(Result) do begin
    for j := low(Result[i]) to high(Result[i]) do begin
      sum:= 0;
      for k := low(B) to high(B) do begin
        sum:= sum + A[i][k]*B[k][j];
      end;
      Result[i][j]:= sum;
    end;
  end;
end;

function MatrixVektorMulti (A: TMatrix; V:TVector ) : TVector;
var
  i, j: integer;
  sum: extended;
begin
  if length(A[low(A)]) <> length(V) then begin
    raise EMathError.Create('Matrix Dimension Error');
    exit;
  end;
  SetLength(Result, length(A));
  for i := low(Result) to high(Result) do begin
    sum:= 0;
    for j := low(V) to high(V) do begin
      sum:= sum + A[i][j]*V[j];
    end;
    Result[i]:= sum;
  end;
end;

function MatrixConcat (A, B: TMatrix): TMatrix;
var
  n, i, j: Integer;
begin
  if length(A) <> length(B) then begin
    raise EMathError.Create('Matrix Dimension Error');
    exit;
  end;
  Result := NIL;
  i := length(A);
  j := length(A[0]) + length(B[0]);
  setlength(Result, i, j);
  for i  := low(A) to high(A) do begin
    n := 0;
    for j := low(A[0]) to high(A[0]) do begin
      Result[i,n] := A[i,j];
      inc(n);
    end;
    for j := low(B[0]) to high(B[0]) do begin
      Result[i,n] := B[i,j];
      inc(n);
    end;
  end;
end;

function MatrixIdent (n: Integer): TMatrix;
var
  i, j: Integer;
begin
  setlength(Result, n, n);
  for i  := 0 to n-1 do begin
    for j := 0 to n-1 do begin
      if i  = j then begin
        Result[i,j] := 1.0;
       end else begin
        Result[i,j] := 0.0;
      end;
    end;
  end;
end;

function MatrixInvert (A: TMatrix) : TMatrix;
// this is not a very efficient way to do the task, but it just uses what we have
var
  AI, A2: TMatrix;
begin
  AI := MatrixIdent(Length(A));
  A2 := MatrixConcat(A, AI);
  Result := SolveLinearSystems(A2);
end;

procedure MatrixBubbleSort(var A: TMatrix; m: Integer);
var
  i: integer;
  x: TVector;
  bool: Boolean;
begin
  repeat
  bool:= true;
    for i := Low(A) to High(A)-1 do begin
      if A[i][M] > A[i+1][M]  then begin
        x:= A[i];
        A[i]:= A[i+1];
        A[i+1]:= x;
        bool:= false;
      end;
    end;
  until (bool);
  setlength(x,0);
end;

end.