MODULE R2x2 EXPORTS R2x2, R2x2Extras;
(*
  Hand-optimized implementation of the interfaces "R2x2" and "R2x2Extras". 

  Created by J. Stolfi. *)
  
IMPORT R2Rep, R2x2Rep, RMxN, Wr, Thread, Math;

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

PROCEDURE Null(): T =
  BEGIN
    RETURN T{RowT{0.0, ..}, ..}
  END Null;
  
PROCEDURE Identity(): T =
  BEGIN
    RETURN T{
      RowT{1.0, 0.0},
      RowT{0.0, 1.0}
    }
  END Identity;
    
PROCEDURE ToReal(READONLY m: T): RealT =
  BEGIN
    RETURN m
  END ToReal;
  
PROCEDURE ToLongReal(READONLY m: T): LongRealT =
  VAR lm: LongRealT;
  BEGIN
    lm[0,0] := FLOAT(m[0,0], LONGREAL);
    lm[0,1] := FLOAT(m[0,1], LONGREAL);
    lm[1,0] := FLOAT(m[1,0], LONGREAL);
    lm[1,1] := FLOAT(m[1,1], LONGREAL);
    RETURN lm
  END ToLongReal;
    
PROCEDURE FromReal(READONLY c: RealT): T =
  BEGIN
    RETURN c
  END FromReal;

PROCEDURE FromLongReal(READONLY c: LongRealT): T =
  VAR m: T;
  BEGIN
    m[0,0] := FLOAT(c[0,0], ElemT);
    m[0,1] := FLOAT(c[0,1], ElemT);
    m[1,0] := FLOAT(c[1,0], ElemT);
    m[1,1] := FLOAT(c[1,1], ElemT);
    RETURN m
  END FromLongReal;

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

PROCEDURE MapRow(READONLY a: R2Rep.T; READONLY m: T): R2Rep.T =
  VAR r: R2Rep.T;
  BEGIN
    WITH
      a0 = FLOAT(a[0], LONGREAL), 
      a1 = FLOAT(a[1], LONGREAL)
    DO
      WITH  
        m00 = FLOAT(m[0,0], LONGREAL), 
        m10 = FLOAT(m[1,0], LONGREAL) 
      DO
        r[0] := FLOAT(a0*m00 + a1*m10)
      END;
      WITH 
        m01 = FLOAT(m[0,1], LONGREAL),
        m11 = FLOAT(m[1,1], LONGREAL)
      DO
        r[1] := FLOAT(a0*m01 + a1*m11)
      END;
    END;
    RETURN r
  END MapRow;

PROCEDURE MapCol(READONLY m: T; READONLY a: R2Rep.T): R2Rep.T =
  VAR r: R2Rep.T;
  BEGIN
    WITH
      a0 = FLOAT(a[0], LONGREAL),
      a1 = FLOAT(a[1], LONGREAL)
    DO
      WITH 
        m00 = FLOAT(m[0,0], LONGREAL), 
        m01 = FLOAT(m[0,1], LONGREAL)
      DO
        r[0] := FLOAT(m00*a0 + m01*a1)
      END;
      WITH  
        m10 = FLOAT(m[1,0], LONGREAL), 
        m11 = FLOAT(m[1,1], LONGREAL)
      DO
        r[1] := FLOAT(m10*a0 + m11*a1)
      END;
    END;
    RETURN r
  END MapCol;

PROCEDURE Mul(READONLY m, n: T): T =
  BEGIN
    WITH
      m00 = FLOAT(m[0,0], LONGREAL), m01 = FLOAT(m[0,1], LONGREAL),
      m10 = FLOAT(m[1,0], LONGREAL), m11 = FLOAT(m[1,1], LONGREAL),
                                                 
      n00 = FLOAT(n[0,0], LONGREAL), n01 = FLOAT(n[0,1], LONGREAL),
      n10 = FLOAT(n[1,0], LONGREAL), n11 = FLOAT(n[1,1], LONGREAL)
    DO
      RETURN T{
        RowT{FLOAT(m00*n00 + m01*n10), FLOAT(m00*n01 + m01*n11)},
        RowT{FLOAT(m10*n00 + m11*n10), FLOAT(m10*n01 + m11*n11)}
      }
    END
  END Mul;
  
PROCEDURE Adj(READONLY m: T): T =
  BEGIN
    RETURN T{
      RowT{+m[1,1], -m[0,1]},
      RowT{-m[1,0], +m[0,0]}
    }
  END Adj;

PROCEDURE Inv(READONLY m: T): T =
  BEGIN
    WITH
      m00 = FLOAT(m[0,0], LONGREAL),
      m01 = FLOAT(m[0,1], LONGREAL),
      
      m10 = FLOAT(m[1,0], LONGREAL),
      m11 = FLOAT(m[1,1], LONGREAL),
      
      d = m00 * m11 - m01 * m10
    DO
      RETURN T{
        RowT{FLOAT(+m11/d), FLOAT(-m01/d)},
        RowT{FLOAT(-m10/d), FLOAT(+m00/d)}
      }
    END;
  END Inv;

PROCEDURE Det(READONLY m: T): LONGREAL =
  BEGIN
    WITH
      m00 = FLOAT(m[0,0], LONGREAL),
      m01 = FLOAT(m[0,1], LONGREAL),
      
      m10 = FLOAT(m[1,0], LONGREAL),
      m11 = FLOAT(m[1,1], LONGREAL),
      
      d = + m00 * m11 - m01 * m10
    DO
      RETURN d
    END;
  END Det;  

PROCEDURE Print(
    wr: Wr.T; 
    READONLY m: T; 
    olp := "( "; osep := "\n  "; orp := " )";
    ilp := "("; isep := " "; irp := ")";
    fmt: PROCEDURE (x: ElemT): TEXT := R2x2Rep.DefaultElemFmt;
    align: BOOLEAN := TRUE;
  ) RAISES {Wr.Failure, Thread.Alerted} =
  BEGIN
    RMxN.Print(wr, m, olp, osep, orp, ilp, isep, irp, fmt, align)
  END Print;

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

(* R2x2Extras *)

PROCEDURE Rotation(
    angle: LONGREAL; 
    scale: LONGREAL := 1.0d0;
  ): T =
  BEGIN
    WITH 
      ct = scale * Math.cos(angle),
      st = scale * Math.sin(angle)
    DO
      RETURN T{
          RowT{+FLOAT(ct), +FLOAT(st)},
          RowT{-FLOAT(st), +FLOAT(ct)}
        }
    END
  END Rotation;

BEGIN 
END R2x2.