MODULE LR4x4 EXPORTS LR4x4, LR4x4Extras;
(*
  Hand-optimized implementation OF the "LR3x3" interface. 

  Created by J. Stolfi with contributions by Marcos C. Carrard
  and R. L. W. Liesenfeld. *)

IMPORT LR4Rep, LR4x4Rep, LR4Extras, LRMxN, Wr, Thread;

TYPE RowT = ARRAY [0..N-1] OF ElemT;

PROCEDURE Null(): T =
  BEGIN
    RETURN T{RowT{0.0d0, ..}, ..}
  END Null;
  
PROCEDURE Identity(): T =
  BEGIN
    RETURN T{
      RowT{1.0d0, 0.0d0, 0.0d0, 0.0d0},
      RowT{0.0d0, 1.0d0, 0.0d0, 0.0d0},
      RowT{0.0d0, 0.0d0, 1.0d0, 0.0d0},
      RowT{0.0d0, 0.0d0, 0.0d0, 1.0d0}
    }
  END Identity;
    
PROCEDURE ToReal(READONLY m: T): RealT =
  VAR rm: RealT;
  BEGIN
    FOR i := 0 TO N-1 DO
      FOR j := 0 TO N-1 DO
        rm[i,j] := FLOAT(m[i,j], REAL)
      END
    END;
    RETURN rm
  END ToReal;
  
PROCEDURE ToLongReal(READONLY m: T): LongRealT =
  BEGIN
    RETURN m
  END ToLongReal;
    
PROCEDURE FromReal(READONLY c: RealT): T =
  VAR m: T;
  BEGIN
    FOR i := 0 TO N-1 DO
      FOR j := 0 TO N-1 DO
        m[i,j] := FLOAT(c[i,j], ElemT)
      END
    END;
    RETURN m
  END FromReal;

PROCEDURE FromLongReal(READONLY c: LongRealT): T =
  BEGIN
    RETURN c
  END FromLongReal;

PROCEDURE Get(READONLY m: T; i, j: [0..N-1]): LONGREAL =
  BEGIN
    RETURN m[i, j]
  END Get;
    
PROCEDURE Set(VAR m: T; i, j: [0..N-1]; x: LONGREAL) =
  BEGIN
    m[i, j] := x
  END Set;

PROCEDURE MapRow(READONLY a: LR4Rep.T; READONLY m: T): LR4Rep.T =
  VAR r: LR4Rep.T;
  BEGIN
    WITH
      a0 = a[0], a1 = a[1], a2 = a[2], a3 = a[3]
    DO
      WITH  m00 = m[0,0], m10 = m[1,0], m20 = m[2,0], m30 = m[3,0]  DO
        r[0] := a0*m00 + a1*m10 + a2*m20 + a3*m30
      END;
      WITH  m01 = m[0,1], m11 = m[1,1], m21 = m[2,1], m31 = m[3,1]  DO
        r[1] := a0*m01 + a1*m11 + a2*m21 + a3*m31
      END;
      WITH  m02 = m[0,2], m12 = m[1,2], m22 = m[2,2], m32 = m[3,2]  DO
        r[2] := a0*m02 + a1*m12 + a2*m22 + a3*m32
      END;
      WITH  m03 = m[0,3], m13 = m[1,3], m23 = m[2,3], m33 = m[3,3]  DO
        r[3] := a0*m03 + a1*m13 + a2*m23 + a3*m33
      END
    END;
    RETURN r
  END MapRow;

PROCEDURE MapCol(READONLY m: T; READONLY a: LR4Rep.T): LR4Rep.T =
  VAR r: LR4Rep.T;
  BEGIN
    WITH
      a0 = a[0],
      a1 = a[1],
      a2 = a[2],
      a3 = a[3]
    DO
      WITH  m00 = m[0,0], m01 = m[0,1], m02 = m[0,2], m03 = m[0,3]  DO
        r[0] := m00*a0 + m01*a1 + m02*a2 + m03*a3
      END;
      WITH  m10 = m[1,0], m11 = m[1,1], m12 = m[1,2], m13 = m[1,3]  DO
        r[1] := m10*a0 + m11*a1 + m12*a2 + m13*a3
      END;
      WITH  m20 = m[2,0], m21 = m[2,1], m22 = m[2,2], m23 = m[2,3]  DO
        r[2] := m20*a0 + m21*a1 + m22*a2 + m23*a3
      END;
      WITH  m30 = m[3,0], m31 = m[3,1], m32 = m[3,2], m33 = m[3,3]  DO
        r[3] := m30*a0 + m31*a1 + m32*a2 + m33*a3
      END
    END;
    RETURN r
  END MapCol;

PROCEDURE Mul(READONLY m, n: T): T =
  VAR r: T;
  BEGIN
    FOR i := 0 TO N-1 DO
      r[i] := MapRow(m[i], n)
    END;
    RETURN r
  END Mul;

PROCEDURE Adj(READONLY m: T): T =
  BEGIN
    WITH
      m00 = m[0,0], m01 = m[0,1], m02 = m[0,2], m03 = m[0,3],
      m10 = m[1,0], m11 = m[1,1], m12 = m[1,2], m13 = m[1,3],
      m20 = m[2,0], m21 = m[2,1], m22 = m[2,2], m23 = m[2,3],
      m30 = m[3,0], m31 = m[3,1], m32 = m[3,2], m33 = m[3,3],
      
      m01_01 = m00 * m11 - m01 * m10,
      m01_02 = m00 * m12 - m02 * m10,
      m01_12 = m01 * m12 - m02 * m11,
      m01_03 = m00 * m13 - m03 * m10,
      m01_13 = m01 * m13 - m03 * m11,
      m01_23 = m02 * m13 - m03 * m12,

      m23_01 = m20 * m31 - m21 * m30,
      m23_02 = m20 * m32 - m22 * m30,
      m23_12 = m21 * m32 - m22 * m31,
      m23_03 = m20 * m33 - m23 * m30,
      m23_13 = m21 * m33 - m23 * m31,
      m23_23 = m22 * m33 - m23 * m32,
      
      m012_012 = m01_01 * m22 - m01_02 * m21 + m01_12 * m20,
      m012_013 = m01_01 * m23 - m01_03 * m21 + m01_13 * m20,
      m012_023 = m01_02 * m23 - m01_03 * m22 + m01_23 * m20,
      m012_123 = m01_12 * m23 - m01_13 * m22 + m01_23 * m21,
      
      m013_012 = m01_01 * m32 - m01_02 * m31 + m01_12 * m30,
      m013_013 = m01_01 * m33 - m01_03 * m31 + m01_13 * m30,
      m013_023 = m01_02 * m33 - m01_03 * m32 + m01_23 * m30,
      m013_123 = m01_12 * m33 - m01_13 * m32 + m01_23 * m31,
      
      m023_012 = m00 * m23_12 - m01 * m23_02 + m02 * m23_01,
      m023_013 = m00 * m23_13 - m01 * m23_03 + m03 * m23_01,
      m023_023 = m00 * m23_23 - m02 * m23_03 + m03 * m23_02,
      m023_123 = m01 * m23_23 - m02 * m23_13 + m03 * m23_12,
      
      m123_012 = m10 * m23_12 - m11 * m23_02 + m12 * m23_01,
      m123_013 = m10 * m23_13 - m11 * m23_03 + m13 * m23_01,
      m123_023 = m10 * m23_23 - m12 * m23_03 + m13 * m23_02,
      m123_123 = m11 * m23_23 - m12 * m23_13 + m13 * m23_12
      
    DO
      RETURN T{
        LR4Rep.T{+ m123_123, - m023_123, + m013_123, - m012_123},
        LR4Rep.T{- m123_023, + m023_023, - m013_023, + m012_023},
        LR4Rep.T{+ m123_013, - m023_013, + m013_013, - m012_013},
        LR4Rep.T{- m123_012, + m023_012, - m013_012, + m012_012}
      }
    END
  END Adj;

PROCEDURE Inv(READONLY m: T): T =
  BEGIN
    WITH
      m00 = m[0,0], m01 = m[0,1], m02 = m[0,2], m03 = m[0,3],
      m10 = m[1,0], m11 = m[1,1], m12 = m[1,2], m13 = m[1,3],
      m20 = m[2,0], m21 = m[2,1], m22 = m[2,2], m23 = m[2,3],
      m30 = m[3,0], m31 = m[3,1], m32 = m[3,2], m33 = m[3,3],
      
      m01_01 = m00 * m11 - m01 * m10,
      m01_02 = m00 * m12 - m02 * m10,
      m01_12 = m01 * m12 - m02 * m11,
      m01_03 = m00 * m13 - m03 * m10,
      m01_13 = m01 * m13 - m03 * m11,
      m01_23 = m02 * m13 - m03 * m12,

      m23_01 = m20 * m31 - m21 * m30,
      m23_02 = m20 * m32 - m22 * m30,
      m23_12 = m21 * m32 - m22 * m31,
      m23_03 = m20 * m33 - m23 * m30,
      m23_13 = m21 * m33 - m23 * m31,
      m23_23 = m22 * m33 - m23 * m32,
      
      m012_012 = m01_01 * m22 - m01_02 * m21 + m01_12 * m20,
      m012_013 = m01_01 * m23 - m01_03 * m21 + m01_13 * m20,
      m012_023 = m01_02 * m23 - m01_03 * m22 + m01_23 * m20,
      m012_123 = m01_12 * m23 - m01_13 * m22 + m01_23 * m21,
      
      m013_012 = m01_01 * m32 - m01_02 * m31 + m01_12 * m30,
      m013_013 = m01_01 * m33 - m01_03 * m31 + m01_13 * m30,
      m013_023 = m01_02 * m33 - m01_03 * m32 + m01_23 * m30,
      m013_123 = m01_12 * m33 - m01_13 * m32 + m01_23 * m31,
      
      m023_012 = m00 * m23_12 - m01 * m23_02 + m02 * m23_01,
      m023_013 = m00 * m23_13 - m01 * m23_03 + m03 * m23_01,
      m023_023 = m00 * m23_23 - m02 * m23_03 + m03 * m23_02,
      m023_123 = m01 * m23_23 - m02 * m23_13 + m03 * m23_12,
      
      m123_012 = m10 * m23_12 - m11 * m23_02 + m12 * m23_01,
      m123_013 = m10 * m23_13 - m11 * m23_03 + m13 * m23_01,
      m123_023 = m10 * m23_23 - m12 * m23_03 + m13 * m23_02,
      m123_123 = m11 * m23_23 - m12 * m23_13 + m13 * m23_12,
      
      d = m012_012 * m33 - m012_013 * m32 + m012_023 * m31 - m012_123 * m30
      
    DO
      RETURN T{
        LR4Rep.T{+ m123_123/d, - m023_123/d, + m013_123/d, - m012_123/d},
        LR4Rep.T{- m123_023/d, + m023_023/d, - m013_023/d, + m012_023/d},
        LR4Rep.T{+ m123_013/d, - m023_013/d, + m013_013/d, - m012_013/d},
        LR4Rep.T{- m123_012/d, + m023_012/d, - m013_012/d, + m012_012/d}
      }
    END
  END Inv;

PROCEDURE Det(READONLY m: T): LONGREAL =
  BEGIN
    RETURN LR4Extras.Det(m[0], m[1], m[2], m[3])
  END Det;  
  
PROCEDURE Print(
    wr: Wr.T; 
    READONLY m: T; 
    olp := "( "; osep := "\n  "; orp := " )";
    ilp := "("; isep := " "; irp := ")";
    fmt: PROCEDURE (x: ElemT): TEXT := LR4x4Rep.DefaultElemFmt;
    align: BOOLEAN := TRUE;
  ) RAISES {Wr.Failure, Thread.Alerted} =
  BEGIN
    LRMxN.Print(wr, m, olp, osep, orp, ilp, isep, irp, fmt, align)
  END Print;

PROCEDURE ToText(READONLY m: T): TEXT =
  BEGIN
    RETURN LRMxN.ToText(m)
  END ToText;

BEGIN 
END LR4x4.