MODULE SMPoint;

IMPORT MPInt,I3,I4,I6,HI3,HLR3,LR3,Rd,Wr,Fmt,Math,Lex,Scan,Thread,FloatMode;

PROCEDURE IsApointNull(READONLY p: AT) : BOOLEAN =
  BEGIN
    RETURN I3.IsAllZero(p.a)
  END IsApointNull;

PROCEDURE IsBpointNull(READONLY p: BT) : BOOLEAN =
  BEGIN
    RETURN I3.IsAllZero(p.b)
  END IsBpointNull;
  
PROCEDURE IsCpointNull(READONLY p: CT) : BOOLEAN =
  BEGIN
    RETURN HI3.IsInvalidLine(LineFromCpoint(p))
  END IsCpointNull;

(* ------------------------------------------------------------ *)
(* "CToA" is an internal procedure used to define the canonical *)
(* representation of a point. It checks whether the C-point     *)
(* "p" is in "A", that is, if "\Cob - \Co" is a perfect square  *)
(* ------------------------------------------------------------ *)
PROCEDURE CToA(READONLY p: CT; VAR ap: AT) : BOOLEAN =
  VAR r: MPInt.T;
  BEGIN
    IF IsCpointNull(p) THEN 
      RETURN FALSE
    ELSE 
      WITH
        u   = I3.T{p.c[0],p.c[1],p.c[3]},
        v   = I3.T{p.c[2],p.c[4],p.c[5]},
        C0  = I3.Dot(u,u),
        C0b = I3.Dot(v,v),
        d   = MPInt.Sub(C0b,C0)
      DO
        ap:=APNull;
        IF MPInt.GetSign(d) < 0 THEN 
          RETURN FALSE
        ELSE
          WITH rd = MPInt.Sqrt(d,r) DO
            IF MPInt.IsZero(r) THEN  (* d is a perfect square *)
              ap.a[0]:=MPInt.Sub(MPInt.Mult(p.c[5],rd),
                                 MPInt.Add(MPInt.Mult(p.c[3],p.c[4]),
                                           MPInt.Mult(p.c[1],p.c[2])));
              ap.a[1]:=MPInt.Sub(MPInt.Sub(MPInt.Mult(p.c[0],p.c[2]),
                                           MPInt.Mult(p.c[3],p.c[5])),
                                 MPInt.Mult(p.c[4],rd));
              ap.a[2]:=MPInt.Add(MPInt.Add(MPInt.Mult(p.c[0],p.c[4]),
                                           MPInt.Mult(p.c[1],p.c[5])),
                                 MPInt.Mult(p.c[2],rd));
              RETURN TRUE;
            ELSE
              RETURN FALSE 
            END
          END
        END  
      END
    END
  END CToA;

PROCEDURE Stab(READONLY p: CT) : CT = 
  VAR ap : AT;
  BEGIN
    IF CToA(p,ap) THEN
      WITH 
        hap = ApointToHomogPoint(ap),
        l = HI3.LineFromTwoPoints(HI3.Origin,hap) 
      DO
        RETURN CpointFromLine(l)
      END 
    ELSE
      RETURN p
    END
  END Stab;

PROCEDURE IsApoint(READONLY p: CT; VAR ap: AT) : BOOLEAN =
  (* assuming that "p" is in its canonical representation, then 
     "p" is an A-point iff its stabing line "l" doesn't pass through 
     the origin, so (l_0,l_1,l_3) \not= (0,0,0) or "l" passes through 
     the origin but "p" is not a B-point. If "p" is an A-point, returns 
     its homogeneous coordinates. *)
  BEGIN
    IF IsCpointNull(p) THEN 
      RETURN FALSE
    ELSE 
      WITH 
        u   = I3.T{p.c[0],p.c[1],p.c[3]},
        u2  = I3.Dot(u,u)
      DO
        IF MPInt.IsZero(u2) THEN 
          RETURN CToA(p,ap)
        ELSE 
          ap:=APNull;
          RETURN FALSE 
        END
      END
    END
  END IsApoint;

PROCEDURE IsCpoint(READONLY l: HI3.Line): BOOLEAN = 
  BEGIN
    WITH
      t   = l.k,
      u   = I3.T{t[0],t[1],t[3]},
      v   = I3.T{t[2],t[4],t[5]},
      C0  = I3.Dot(u,u),
      C0b = I3.Dot(v,v),
      d   = MPInt.Sub(C0b,C0),
      s   = MPInt.GetSign(d)
    DO
      RETURN (s >= 0) 
    END
  END IsCpoint;

PROCEDURE AToB(READONLY p: AT): BT =
  BEGIN
    RETURN BT{b:=p.a} 
  END AToB;
  
PROCEDURE AToC(READONLY p: AT) : CT =
  BEGIN
    RETURN BToC(AToB(p))
  END AToC;  
      
PROCEDURE AToS(READONLY p: AT) : S2 =
  BEGIN
    RETURN BToS(AToB(p))
  END AToS;

PROCEDURE BToC(READONLY p: BT) : CT =
  BEGIN
    WITH 
      u = I3.Reduce(I3.T{p.b[2],MPInt.Neg(p.b[1]),p.b[0]}),
      q = CT{c:=I6.T{MPInt.DecDig[0],MPInt.DecDig[0],u[0],
                        MPInt.DecDig[0],u[1],u[2]}}  
    DO
      RETURN Stab(q)
    END
  END BToC;  

PROCEDURE BToS(READONLY p: BT) : S2 =
  BEGIN
    WITH 
      w = I3.Norm(p.b),
      x = MPInt.Float(p.b[0])/w,
      y = MPInt.Float(p.b[1])/w,
      z = MPInt.Float(p.b[2])/w
    DO 
      RETURN S2{s:=LR3.T{x,y,z}}
    END   
  END BToS;
  
PROCEDURE CToS(READONLY p: CT) : S2 =
  BEGIN
    WITH
      l01 = MPInt.Float(p.c[0]),
      l02 = MPInt.Float(p.c[1]),
      l12 = MPInt.Float(p.c[2]),
      l03 = MPInt.Float(p.c[3]),
      l13 = MPInt.Float(p.c[4]),
      l23 = MPInt.Float(p.c[5]),
      u   = LR3.T{l01,l02,l03},
      v   = LR3.T{l12,l13,l23},
      C0  = LR3.Dot(u,u),
      C0b = LR3.Dot(v,v),
      rd  = Math.sqrt(C0b-C0),
      x   = (-(l02*l12+l03*l13)+l23*rd)/C0b,
      y   = ( (l01*l12-l03*l23)-l13*rd)/C0b,
      z   = ( (l01*l13+l02*l23)+l12*rd)/C0b
    DO
      RETURN S2{s:=LR3.T{x,y,z}}
    END 
  END CToS;

PROCEDURE SToA(READONLY p: S2; eps: LONGREAL:=1.0d-6) : AT =
  BEGIN
    WITH 
      s = SStereo(p),
      q = Q2{q:=I3.FromLongReal(s.r,eps)}
    DO 
      RETURN InvAStereo(q)
    END
  END SToA;

PROCEDURE SToB(READONLY p: S2; eps: LONGREAL:=1.0d-6) : BT =
  BEGIN
    WITH
      pb= I3.FromLongReal(p.s,eps)
    DO
      RETURN BT{b:=I3.T{pb[0],pb[1],pb[2]}}
    END 
  END SToB;

PROCEDURE SToC(READONLY p: S2; READONLY eps: LONGREAL:=1.0d-6) : CT =
  BEGIN
    WITH 
      q = SToB(p,eps)
    DO 
      RETURN BToC(q)
    END 
  END SToC;

PROCEDURE AStereo(READONLY p: AT) : Q2 =
  BEGIN
    WITH 
      two = MPInt.DecDig[2],
      w = MPInt.Trunc(I3.Norm(p.a)), 
      (* "w" is integer since "p" is in "A"*)
      wq= MPInt.Add(w,p.a[2]),
      xq= MPInt.Mult(two,p.a[0]),
      yq= MPInt.Mult(two,p.a[1])
    DO
      RETURN Q2{q:=I3.T{wq,xq,yq}}
    END
  END AStereo;

PROCEDURE BStereo(READONLY p: BT) : T2 =
  BEGIN
    WITH 
      q = BToS(p)
    DO 
      RETURN SStereo(q)
    END  
  END BStereo;

PROCEDURE CStereo(READONLY p: CT) : T2 =
  BEGIN
    WITH 
      q = CToS(p)
    DO
      RETURN SStereo(q)
    END
  END CStereo;
  
PROCEDURE SStereo(READONLY p: S2) : T2 =
  BEGIN
    WITH 
      w = LR3.Norm(p.s),
      wr= w+p.s[2],
      xr= 2.0d0*p.s[0],
      yr= 2.0d0*p.s[1]
    DO
      RETURN T2{r:=LR3.T{wr,xr,yr}}
    END
  END SStereo;

PROCEDURE InvAStereo(READONLY q: Q2) : AT =
  BEGIN
    WITH
      four=MPInt.DecDig[4],
      w2= MPInt.Mult(q.q[0],q.q[0]),
      x2= MPInt.Mult(q.q[1],q.q[1]),
      y2= MPInt.Mult(q.q[2],q.q[2]),
      xa= MPInt.Mult(four,MPInt.Mult(q.q[0],q.q[1])),
      ya= MPInt.Mult(four,MPInt.Mult(q.q[0],q.q[2])),
      za= MPInt.Sub(MPInt.Sub(MPInt.Mult(four,w2),x2),y2)
    DO
      RETURN AT{a:=I3.T{xa,ya,za}}
    END
  END InvAStereo;

PROCEDURE InvBStereo(READONLY q: T2; READONLY eps: LONGREAL:=1.0d-6) : BT =
  BEGIN 
    WITH
      p = InvSStereo(q)
    DO
      RETURN SToB(p,eps)
    END 
  END InvBStereo;

PROCEDURE InvCStereo(READONLY q: T2; READONLY eps: LONGREAL:=1.0d-6) : CT =
  BEGIN 
    WITH
      p = InvSStereo(q)
    DO
      RETURN SToC(p,eps)
    END 
  END InvCStereo;
  
PROCEDURE InvSStereo(READONLY q: T2) : S2 =
  BEGIN 
    WITH
      xs = 4.0d0*q.r[0]*q.r[1],
      ys = 4.0d0*q.r[0]*q.r[2],
      zs = 4.0d0*q.r[0]*q.r[0]-q.r[1]*q.r[1]-q.r[2]*q.r[2]
    DO
      RETURN S2{s:=LR3.T{xs,ys,zs}}
    END
  END InvSStereo;

PROCEDURE HomogenizeSPoint(READONLY p: S2) : HLR3.Point =
   VAR hp: HLR3.Point;
  BEGIN
    hp.c[0]:=1.0d0;
    FOR i:=1 TO 3 DO 
      hp.c[i] := p.s[i-1];
    END;
    RETURN hp
  END HomogenizeSPoint;

PROCEDURE ApointToHomogPoint(READONLY p: AT): HI3.Point = 
  VAR r: MPInt.T;
  BEGIN
    WITH
      w = MPInt.Sqrt(I3.Dot(p.a,p.a),r)
    DO
      RETURN HI3.Point{c:=I4.T{w,p.a[0],p.a[1],p.a[2]}}
    END
  END ApointToHomogPoint;

PROCEDURE BpointToHomogPoint(READONLY p: BT; 
                             READONLY eps: LONGREAL:=1.0d-6) : HI3.Point =
  BEGIN
    RETURN(ApointToHomogPoint(SToA(BToS(p),eps)))
  END BpointToHomogPoint;

PROCEDURE CpointToHomogPoint(READONLY p: CT; 
                             READONLY eps: LONGREAL:=1.0d-6) : HI3.Point =
  BEGIN
    RETURN(ApointToHomogPoint(SToA(CToS(p),eps)))
  END CpointToHomogPoint;

PROCEDURE ApointFromHomogPoint(READONLY p: HI3.Point; 
                               READONLY eps: LONGREAL:=1.0d-6) : AT = 
  BEGIN
    WITH
      x  = MPInt.Float(MPInt.Div(p.c[1],p.c[0])),
      y  = MPInt.Float(MPInt.Div(p.c[2],p.c[0])),
      z  = MPInt.Float(MPInt.Div(p.c[3],p.c[0])),
      ps = S2{s:=LR3.T{x,y,z}}
    DO
      RETURN(SToA(ps,eps))
    END 
  END ApointFromHomogPoint;

PROCEDURE BpointFromHomogPoint(READONLY p: HI3.Point; 
                               READONLY eps: LONGREAL:=1.0d-6) : BT = 
  BEGIN
    WITH
      x  = MPInt.Float(MPInt.Div(p.c[1],p.c[0])),
      y  = MPInt.Float(MPInt.Div(p.c[2],p.c[0])),
      z  = MPInt.Float(MPInt.Div(p.c[3],p.c[0])),
      ps = S2{s:=LR3.T{x,y,z}}
    DO
      RETURN(SToB(ps,eps))
    END 
  END BpointFromHomogPoint;

PROCEDURE CpointFromHomogPoint(READONLY p: HI3.Point; 
                               READONLY eps: LONGREAL:=1.0d-6) : CT = 
  BEGIN
    WITH
      x  = MPInt.Float(MPInt.Div(p.c[1],p.c[0])),
      y  = MPInt.Float(MPInt.Div(p.c[2],p.c[0])),
      z  = MPInt.Float(MPInt.Div(p.c[3],p.c[0])),
      ps = S2{s:=LR3.T{x,y,z}}
    DO
      RETURN(SToC(ps,eps))
    END 
  END CpointFromHomogPoint;

PROCEDURE LineFromCpoint(READONLY p: CT) : HI3.Line = 
  BEGIN
    RETURN HI3.Line{k:=p.c}
  END LineFromCpoint;

PROCEDURE CpointFromLine(READONLY r: HI3.Line) : CT =
  BEGIN
    RETURN CT{c:=r.k}
  END CpointFromLine;

PROCEDURE ReadApoint(READONLY rd : Rd.T) : AT RAISES{Rd.Failure, Thread.Alerted, Lex.Error} =  
  BEGIN
    RETURN AT{a:=I3.Read(rd,"[","]")}
  END ReadApoint;
  
PROCEDURE ReadBpoint(READONLY rd: Rd.T) : BT RAISES{Rd.Failure, Thread.Alerted, Lex.Error} =
  BEGIN
    RETURN BT{b:=I3.Read(rd,"[","]")}
  END ReadBpoint;

PROCEDURE ReadBpoint2(READONLY rd: Rd.T) : BT RAISES{Rd.Failure, Thread.Alerted, Lex.Error} =
  BEGIN
    RETURN BT{b:=I3.Read(rd,"(",")")}
  END ReadBpoint2;

PROCEDURE ReadCpoint(READONLY rd: Rd.T) : CT RAISES{Rd.Failure, Thread.Alerted, Lex.Error} =
  BEGIN
    WITH p = CT{c:=I6.Read(rd,"<<",">>")}  DO 
      RETURN Stab(p)
    END
  END ReadCpoint;

PROCEDURE ReadSpoint(READONLY rd: Rd.T) : S2 RAISES{Rd.Failure, Thread.Alerted, FloatMode.Trap, Lex.Error} =
  CONST cs = SET OF CHAR {'0' .. '9','-','.'};
        blank = SET OF CHAR{' ','\t','\n','\r','\013','\f','(',',',')'};
  VAR p: S2;
  BEGIN
    FOR i:=0 TO 2 DO 
      Lex.Skip(rd,blank);
      p.s[i]:=Scan.LongReal(Lex.Scan(rd,cs));
    END;
    Lex.Match(rd,")");
    RETURN p
  END ReadSpoint;

PROCEDURE PrintApoint (wr: Wr.T; READONLY p: AT) RAISES{Thread.Alerted, Wr.Failure} =
  BEGIN
    I3.Print(wr,p.a,10,"[",",","]")      
  END PrintApoint;

PROCEDURE PrintBpoint (wr: Wr.T; READONLY p: BT) RAISES{Thread.Alerted, Wr.Failure} =
  BEGIN
    I3.Print(wr,p.b,10,"[",",","]")      
  END PrintBpoint;

PROCEDURE PrintCpoint (wr: Wr.T; READONLY p: CT) RAISES{Thread.Alerted, Wr.Failure} =
  BEGIN
    I6.Print(wr,p.c,10,"<<",",",">>")      
  END PrintCpoint;

PROCEDURE PrintSpoint (wr: Wr.T; READONLY p: S2; ndig: CARDINAL:=15) RAISES{Thread.Alerted, Wr.Failure} =
  BEGIN
    Wr.PutText(wr,"(");
    FOR i:=0 TO 1 DO 
      Wr.PutText(wr,Fmt.LongReal(p.s[i],Fmt.Style.Auto,ndig)&", ");
    END;
    Wr.PutText(wr,Fmt.LongReal(p.s[2],Fmt.Style.Auto,ndig)&")");
  END PrintSpoint;
  
BEGIN
  APNull := AT{a:=I3.All(MPInt.DecDig[0])};
  CPNull := CT{c:=I6.All(MPInt.DecDig[0])};
END SMPoint.