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

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

IMPORT LR4Rep, LRN, Math, Wr, Thread, Random, GaussRandom;

PROCEDURE Zero(): T =
  BEGIN 
    RETURN T{0.0d0, ..}
  END Zero;
    
PROCEDURE All(x: LONGREAL): T =
  BEGIN
    RETURN T{x, ..}
  END All;

PROCEDURE Axis(i: [0..N-1]): T =
  VAR r: T := T{0.0d0, ..};
  BEGIN
    r[i] := 1.0d0;
    RETURN r
  END Axis;

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

PROCEDURE Add(READONLY a, b: T): T =
  BEGIN
    RETURN T{a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3]}
  END Add;

PROCEDURE Sub(READONLY a, b: T): T =
  BEGIN
    RETURN T{a[0] - b[0], a[1] - b[1], a[2] - b[2], a[3] - b[3]}
  END Sub;

PROCEDURE Neg(READONLY a: T): T =
  BEGIN
    RETURN T{- a[0], - a[1], - a[2], - a[3]}
  END Neg;

PROCEDURE Scale(s: LONGREAL; READONLY a: T): T = 
  BEGIN
    RETURN T{s * a[0], s * a[1], s * a[2], s * a[3]}
  END Scale;

PROCEDURE Weigh(READONLY w: LongRealT; READONLY a: T): T =
  BEGIN
    RETURN T{a[0] * w[0], a[1] * w[1], a[2] * w[2], a[3] * w[3]}
  END Weigh;

PROCEDURE Mix(s: LONGREAL; READONLY a: T; t: LONGREAL; READONLY b: T): T = 
  BEGIN
    RETURN T{
      s * a[0] + t * b[0],
      s * a[1] + t * b[1],
      s * a[2] + t * b[2],
      s * a[3] + t * b[3]
    }
  END Mix;

PROCEDURE Norm(READONLY a: T): LONGREAL =
  BEGIN
    WITH
      a0 = a[0],
      a1 = a[1],
      a2 = a[2],
      a3 = a[3],
      (* Normalize to avoid spurious overflow: *)
      (* This is probably faster than three calls to "Math.hypot" *)
      m = MAX(MAX(MAX(ABS(a0), ABS(a1)), MAX(ABS(a2), ABS(a3))), 1.0d0),
      na0 = a0/m,
      na1 = a1/m,
      na2 = a2/m,
      na3 = a3/m,
      norm = m * Math.sqrt(na0*na0 + na1*na1 + na2*na2 + na3*na3)
    DO
      RETURN norm
    END
  END Norm;

PROCEDURE NormSqr(READONLY a: T): LONGREAL =
  BEGIN
    WITH
      a0 = a[0],
      a1 = a[1],
      a2 = a[2],
      a3 = a[3],
      normSqr = a0*a0 + a1*a1 + a2*a2 + a3*a3
    DO
      RETURN normSqr
    END
  END NormSqr;

PROCEDURE Dist(READONLY a, b: T): LONGREAL =
  BEGIN
    WITH
      t0 = a[0] - b[0],
      t1 = a[1] - b[1],
      t2 = a[2] - b[2],
      t3 = a[3] - b[3],
      (* Normalize to avoid spurious overflow: *)
      (* This is probably faster than three calls to "Math.hypot" *)
      m = MAX(MAX(MAX(ABS(t0), ABS(t1)), MAX(ABS(t2), ABS(t3))), 1.0d0),
      nt0 = t0/m,
      nt1 = t1/m,
      nt2 = t2/m,
      nt3 = t3/m,
      dist = m * Math.sqrt(nt0*nt0 + nt1*nt1 + nt2*nt2 + nt3*nt3)
    DO
      RETURN dist
    END
  END Dist;
    
PROCEDURE DistSqr(READONLY a, b: T): LONGREAL =
  BEGIN
    WITH
      t0 = a[0] - b[0],
      t1 = a[1] - b[1],
      t2 = a[2] - b[2],
      t3 = a[3] - b[3],
      distSqr = t0*t0 + t1*t1 + t2*t2 + t3*t3
    DO
      RETURN distSqr
    END
  END DistSqr;

PROCEDURE Dir(READONLY a: T): T =
  BEGIN
    WITH
      a0 = a[0],
      a1 = a[1],
      a2 = a[2],
      a3 = a[3],
      (* Normalize to avoid spurious overflow: *)
      (* This is probably faster than three calls to "Math.hypot" *)
      m = MAX(MAX(MAX(ABS(a0), ABS(a1)), MAX(ABS(a2), ABS(a3))), 1.0d0),
      na0 = a0/m,
      na1 = a1/m,
      na2 = a2/m,
      na3 = a3/m,
      norm = Math.sqrt(na0*na0 + na1*na1 + na2*na2 + na3*na3)
    DO
      RETURN T{na0/norm, na1/norm, na2/norm, na3/norm}
    END
  END Dir; 

PROCEDURE LInfNorm(READONLY a: T): LONGREAL =
  BEGIN
    WITH 
      norm = MAX(MAX(ABS(a[0]), ABS(a[1])), MAX(ABS(a[2]), ABS(a[3])))
    DO
      RETURN norm
    END
  END LInfNorm;  

PROCEDURE LInfDist(READONLY a, b: T): LONGREAL =
  BEGIN
    WITH 
      dist = MAX(
        MAX(ABS(a[0]-b[0]), ABS(a[1]-b[1])), 
        MAX(ABS(a[2]-b[2]), ABS(a[3]-b[3]))
      )
    DO
      RETURN dist
    END
  END LInfDist;  

PROCEDURE LInfDir(READONLY a: T): T =
  BEGIN
    WITH 
      norm = MAX(MAX(ABS(a[0]), ABS(a[1])), MAX(ABS(a[2]), ABS(a[3])))
    DO
      RETURN T{a[0]/norm, a[1]/norm, a[2]/norm, a[3]/norm}
    END
  END LInfDir;  

PROCEDURE Dot(READONLY a, b: T): LONGREAL =
  BEGIN
    WITH
      a0 = a[0],
      a1 = a[1],
      a2 = a[2],
      a3 = a[3],

      b0 = b[0],
      b1 = b[1],
      b2 = b[2],
      b3 = b[3],

      dot = a0*b0 + a1*b1 + a2*b2 + a3*b3
    DO
      RETURN dot
    END
  END Dot;

PROCEDURE Cos(READONLY a, b: T): LONGREAL =
  BEGIN
    RETURN LRN.Cos(a, b)
  END Cos;

PROCEDURE Sin(READONLY a, b: T): LONGREAL =
  BEGIN 
    RETURN LRN.Sin(a, b)
  END Sin;

PROCEDURE Det(READONLY a, b, c, d: T): LONGREAL =
  BEGIN
    WITH
      a_0 = a[0],
      a_1 = a[1],
      a_2 = a[2],
      a_3 = a[3],
      
      b_0 = b[0],
      b_1 = b[1],
      b_2 = b[2],
      b_3 = b[3],
      
      c_0 = c[0],
      c_1 = c[1],
      c_2 = c[2],
      c_3 = c[3],
      
      d_0 = d[0],
      d_1 = d[1],
      d_2 = d[2],
      d_3 = d[3],
      
      ab_01 = a_0 * b_1 - a_1 * b_0,
      ab_02 = a_0 * b_2 - a_2 * b_0,
      ab_12 = a_1 * b_2 - a_2 * b_1,
      ab_03 = a_0 * b_3 - a_3 * b_0,
      ab_13 = a_1 * b_3 - a_3 * b_1,
      ab_23 = a_2 * b_3 - a_3 * b_2,

      cd_01 = c_0 * d_1 - c_1 * d_0,
      cd_02 = c_0 * d_2 - c_2 * d_0,
      cd_12 = c_1 * d_2 - c_2 * d_1,
      cd_03 = c_0 * d_3 - c_3 * d_0,
      cd_13 = c_1 * d_3 - c_3 * d_1,
      cd_23 = c_2 * d_3 - c_3 * d_2,
      
      t = + ab_01 * cd_23 - ab_02 * cd_13 + ab_12 * cd_03
          + ab_03 * cd_12 - ab_13 * cd_02 + ab_23 * cd_01
    DO
      RETURN t
    END
  END Det;  
  
PROCEDURE Cross(READONLY a, b, c: T): T =
  BEGIN
    WITH
      a0 = a[0],
      a1 = a[1],
      a2 = a[2],
      a3 = a[3],

      b0 = b[0],
      b1 = b[1],
      b2 = b[2],
      b3 = b[3],

      c0 = c[0],
      c1 = c[1],
      c2 = c[2],
      c3 = c[3],

      ab_01 = a0*b1 - a1*b0,
      ab_02 = a0*b2 - a2*b0,
      ab_12 = a1*b2 - a2*b1,
      ab_03 = a0*b3 - a3*b0,
      ab_13 = a1*b3 - a3*b1,
      ab_23 = a2*b3 - a3*b2,
      
      abc_012 = ab_01 * c2 - ab_02 * c1 + ab_12 * c0,
      abc_013 = ab_01 * c3 - ab_03 * c1 + ab_13 * c0,
      abc_023 = ab_02 * c3 - ab_03 * c2 + ab_23 * c0,
      abc_123 = ab_12 * c3 - ab_13 * c2 + ab_23 * c1
    DO
      RETURN T{
        - abc_123,
        + abc_023,
        - abc_013,
        + abc_012
      }
    END;
  END Cross;

PROCEDURE Project(READONLY a, u: T): T =
  BEGIN
    WITH
      a0 = a[0],
      a1 = a[1],
      a2 = a[2],
      a3 = a[3],

      u0 = u[0],
      u1 = u[1],
      u2 = u[2],
      u3 = u[3],

      sau = a0*u0 + a1*u1 + a2*u2 + a3*u3
    DO
      IF sau = 0.0d0 THEN
        RETURN T{0.0d0, ..}
      ELSE
        WITH
          suu = u0*u0 + u1*u1 + u2*u2 + u3*u3,
          c = sau/suu
        DO
          RETURN T{c*u0, c*u1, c*u2, c*u3}
        END
      END
    END;
  END Project;
  
PROCEDURE Orthize(READONLY a, u: T): T =
  BEGIN
    WITH
      u0 = u[0],
      u1 = u[1],
      u2 = u[2],
      u3 = u[3],

      suu = u0*u0 + u1*u1 + u2*u2 + u3*u3
    DO
      IF suu = 0.0d0 THEN
        RETURN a
      ELSE
        WITH
          a0 = a[0],
          a1 = a[1],
          a2 = a[2],
          a3 = a[3],

          sau = a0*u0 + a1*u1 + a2*u2 + a3*u3
        DO
          IF sau = 0.0d0 THEN
            RETURN a
          ELSE
            WITH
              c = sau/suu
            DO
              RETURN T{a0-c*u0, a1-c*u1, a2-c*u2, a3-c*u3}
            END
          END
        END
      END
    END;
  END Orthize;
  
PROCEDURE ToReal(READONLY a: T): RealT =
  BEGIN
    RETURN RealT{
      FLOAT(a[0], REAL),
      FLOAT(a[1], REAL),
      FLOAT(a[2], REAL),
      FLOAT(a[3], REAL)
    }
  END ToReal;
  
PROCEDURE FromReal(READONLY a: RealT): T =
  BEGIN
    RETURN T{
      FLOAT(a[0], LONGREAL),
      FLOAT(a[1], LONGREAL),
      FLOAT(a[2], LONGREAL),
      FLOAT(a[3], LONGREAL)
    }
  END FromReal;

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

PROCEDURE URandom(rnd: Random.T; min, max: LONGREAL): T =
  VAR r: T;
  BEGIN
    FOR i := 0 TO N-1 DO
      r[i] := rnd.longreal(min, max)
    END;
    RETURN r
  END URandom;
    
PROCEDURE NRandom(rnd: Random.T; avg, dev: LONGREAL): T =
  VAR r: T;
  BEGIN
    FOR i := 0 TO N-1 DO
      r[i] := avg + dev * GaussRandom.LongReal(rnd)
    END;
    RETURN r
  END NRandom;

PROCEDURE Print(
    wr: Wr.T;
    READONLY a: T; 
    lp := "("; sep := " "; rp := ")";
    fmt: PROCEDURE (x: ElemT): TEXT := LR4Rep.DefaultElemFmt;
  ) RAISES {Wr.Failure, Thread.Alerted} =
  BEGIN
    LRN.Print(wr, a, lp, sep, rp, fmt)
  END Print;

PROCEDURE ToText(READONLY a: T): TEXT =
  BEGIN
    RETURN LRN.ToText(a)
  END ToText;

BEGIN 
END LR4.