-- ParametricMapImpl.mesa
-- Last changed by John Warnock
-- Last changed by Maureen Stone January 31, 1984 4:27:26 pm PST
-- Last changed by Rick Beach, November 30, 1982 2:24 am

DIRECTORY
    ParametricMap,
    GraphicsOps USING[UserToDevice],
    CGVector USING [Add,Min,Max,Sub,In],
    CGCubic USING[Bezier,Split,Flat],
    RealFns USING [SqRt],
    Graphics,
    GraphicsBasic USING [Vec];

ParametricMapImpl: PROGRAM IMPORTS GraphicsOps,RealFns,CGVector,CGCubic EXPORTS ParametricMap= 
{OPEN ParametricMap,GraphicsBasic,CGVector,CGCubic,Graphics,GraphicsOps;

dc:Context;
cpx,cpy:REAL;
map:PROC[u:Vec] RETURNS [t:Vec];

SetMap:PUBLIC PROC[f:PROC[u:Vec] RETURNS [t:Vec]]=
{CacheInit[];
map_f;};

MapMoveTo:PUBLIC PROC[self: Graphics.Context, p:Vec] RETURNS[tp:Vec]={
dc _ self;
cpx_p.x;
cpy_p.y;
RETURN[map[p]];
};

MapCurveTo:PUBLIC PROC[p1,p2,p3:Vec,point:PROC[v:Vec]]={
bz:Bezier;
bz_Bezier[Vec[cpx,cpy],p1,p2,p3];
MapCSplit[bz,point];
cpx_p3.x;
cpy_p3.y;
RETURN;
};

MapLineTo: PUBLIC PROC[p:Vec,point:PROC[v:Vec]]={
MapLSplit[Vec[cpx,cpy],p,point];
cpx_p.x;
cpy_p.y;
RETURN;
};

MapCSplit:PROC [bz:Bezier,point:PROC[v:Vec]]=
{mbz:Bezier;
v0,v1,v2,v3,v3p:Vec;
v0_UToD[map[bz.b0]];
v1_UToD[map[bz.b1]];
v2_UToD[map[bz.b2]];
v3p_map[bz.b3];
v3_UToD[v3p];
mbz_Bezier[v0,v1,v2,v3];
IF CGCubic.Flat[mbz,1.0] -- was 0.5 originally
   THEN {point[v3p]; RETURN;}
    ELSE {bz1,bz2:Bezier;
            [bz1,bz2]_Split[bz];
            MapCSplit[bz1,point]; 
            MapCSplit[bz2,point]; 
            RETURN;};
 };

MapLSplit:PROC [p1,p2:Vec,point:PROC[v:Vec]]=
{v1,v2,v2p,v3,v3p,pi:Vec;
pi_Vec[(p1.x+p2.x)*.5,(p1.y+p2.y)*.5];
v1_UToD[map[p1]];
v2p_map[pi];
v2_UToD[v2p];
v3p_map[p2];
v3_UToD[v3p];
IF Flat[v1,v2,v3,1.0] -- was 0.5 originally
    THEN {point[v2p]; point[v3p]; RETURN;}
    ELSE {MapLSplit[p1,pi,point]; MapLSplit[pi,p2,point];RETURN;};
 };

UToD:PROC[v:Vec]RETURNS[tv:Vec]=
{x,y:REAL;
[x,y]_UserToDevice[dc,v.x,v.y,FALSE];
RETURN[Vec[x,y]];
};

GetTforArc:PUBLIC PROC [bz:LONG POINTER TO Bezier,arc:REAL] RETURNS [success:BOOLEAN,t:REAL]=
{exact:BOOLEAN;
bzs:Bezier;
t0,t1,arcl:REAL;
IF ~validCache THEN InitCache[bz];
[exact,t0,t1,arcl]_FindCSec[arc];
IF exact THEN RETURN [TRUE,t0];
bzs_CubicPiece[bz,t0,t1];
[success,t,]_ArcSplit[@bzs,0.0,0.0,arc-arcl,1,0];
t_t0+t*(t1-t0);
RETURN[success,t];
};

ArcLength:PUBLIC PROC [bz:LONG POINTER TO Bezier] RETURNS [arc:REAL]=
{IF ~validCache THEN InitCache[bz];
RETURN[vcache[0]];
};

FindCSec:PROC[arc:REAL] RETURNS [exact:BOOLEAN,t0,t1,tarc:REAL]=
 	{cnt,level:CARDINAL;
 	t:REAL;
 	IF arc =vcache[0] THEN RETURN[TRUE,1,1,arc];
 	tarc_0;
 	t0_0;
 	t1_1.0;
 	level_0;
	cnt_1;
 	UNTIL level = maxlevel DO
 	 	t_(t0+t1)/2;
 	 	SELECT vcache[cnt] FROM
  			< arc =>{t0_t0+Exp2[level];
  					  tarc_vcache[cnt];
  				      cnt_cnt*2+1;};
  			= arc =>RETURN[TRUE,t,t,vcache[cnt]];
  			> arc =>{t1_t1-Exp2[level];
  				      cnt_cnt*2;};
	ENDCASE;
 	level_level+1;
  	ENDLOOP;
 	RETURN[FALSE,t0,t1,tarc];
 	};
 

InitCache:PUBLIC PROC [bz:LONG POINTER TO Bezier]=
{vcache[0]_ArcAdd[bz,0.0,1,0];
validCache _ TRUE;
};

ArcAdd:PUBLIC PROC [bz:LONG POINTER TO Bezier,sum:REAL,cnt,level:CARDINAL] RETURNS [arc:REAL]=
{IF CGCubic.Flat[bz^,0.1] AND level >= maxlevel
	THEN {tarc:REAL;
			tarc_Mag[Sub[bz.b3,bz.b0]]+sum;
			RETURN[tarc];}
	ELSE {tarc:REAL;
    		bz1,bz2:Bezier;
            [bz1,bz2]_Split[bz^];
            tarc_ArcAdd[@bz1,sum,cnt*2,level+1]; 
            IF level < maxlevel THEN vcache[cnt]_tarc;
			tarc_ArcAdd[@bz2,tarc,cnt*2+1,level+1]; 
            RETURN[tarc];};
 };


ArcSplit:PROC [bz:LONG POINTER TO Bezier,t,sum,arc:REAL,cnt,level:CARDINAL] RETURNS [success:BOOLEAN,dt,arcl:REAL]=
{ IF CGCubic.Flat[bz^,0.1]
	THEN {tt:REAL;
			da:REAL;
 			darc:REAL_Mag[Sub[bz.b3,bz.b0]];
			IF (da_darc+sum) < arc 
   				THEN {RETURN[FALSE,t+Exp2[level],da];}
				ELSE {tt_PSolve[bz,t,sum,arc,darc,level];
						RETURN[TRUE,tt,arc];};}
	ELSE {sccs:BOOLEAN;
    		tt,tarc:REAL;
    		bz1,bz2:Bezier;
            [bz1,bz2]_Split[bz^];
            [sccs,tt,tarc]_ArcSplit[@bz1,t,sum,arc,cnt*2,level+1]; 
            IF sccs THEN {RETURN [sccs,tt,tarc]; }
            ELSE {[sccs,tt,tarc]_ArcSplit[@bz2,t+Exp2[level],tarc,arc,cnt*2+1,level+1]; 
            RETURN[sccs,tt,tarc];};};
 };


PSolve:PROC[bz:LONG POINTER TO Bezier,t,sum,arc,darc:REAL,level:CARDINAL] RETURNS[dt:REAL]=
{HalfBezier:TYPE=RECORD[c0,c1,c2,c3:REAL];
lcnt:CARDINAL;
eps,fd,fchange,fdel,fcmax,qt,dx,dy,tarc,delarc,fnew,fold:REAL;
dz,cz:HalfBezier;
eps_0.0001;
dx_bz.b3.x-bz.b0.x;
dy_bz.b3.y-bz.b0.y;
delarc_(arc-sum);
IF ABS[dy] < ABS[dx] 
	THEN {tarc_delarc*dx/darc;
			cz_HalfBezier[0,bz.b1.x-bz.b0.x,bz.b2.x-bz.b0.x,bz.b3.x-bz.b0.x]}
	ELSE  {tarc_delarc*dy/darc;
			cz_HalfBezier[0,bz.b1.y-bz.b0.y,bz.b2.y-bz.b0.y,bz.b3.y-bz.b0.y];};
  -- d0 _ c0;
  -- d1 _ 3(c1-c0);
  -- d2 _ 3(c0-2c1+c2); [ = 3(c2-c1) - d1 ]
  -- d3 _ -c0 +3(c1-c2) + c3; [ = c3 - (c0 + 3(c2-c1)) ]
 qt_3*(cz.c2-cz.c1);
 dz.c0 _ cz.c0;
 dz.c1 _ 3*(cz.c1-cz.c0);
 dz.c2 _ qt-dz.c1;
 dz.c3 _ cz.c3-(cz.c0+qt);
fnew_1;
fold_0;
lcnt_0;
fcmax_1.0;
UNTIL ABS[fnew-fold] < eps OR lcnt = 10 DO
fold_fnew;
fd_(3*dz.c3*fold+2*dz.c2)*fold+dz.c1;
IF fd = 0 THEN EXIT;
fchange_-(((dz.c3*fold+dz.c2)*fold+dz.c1)*fold+dz.c0-tarc)/fd;
fdel_IF ABS[fchange] > fcmax 
	THEN 
	IF fchange < 0 THEN -fcmax ELSE fcmax 
	ELSE fchange;
fnew_fold+fdel;
fcmax_fcmax/2;
lcnt_lcnt+1;
ENDLOOP;
dt_2*Exp2[level]*fnew+t;};
	
Flat:PROC[v1,v2,v3:Vec,epsilon:REAL] RETURNS [b:BOOLEAN]=
{OPEN CGVector;
  dx,dy: REAL;
  d1,d,bl,bh: Vec;
  oh: Vec=[0.5,0.5];
  bh _ Add[Max[v1,v3],oh];
  bl _ Sub[Min[v1,v3],oh];
  IF ~In[v2,bl,bh] THEN RETURN[FALSE];
  d1 _ Sub[v2,v1];
  d _ Sub[v3,v1];
  dx _ ABS[d.x]; dy _ ABS[d.y];
  IF dx+dy < 1 THEN RETURN[TRUE];
  IF dy < dx THEN {
    dydx: REAL _ d.y/d.x;
    RETURN[ABS[d1.y-d1.x*dydx]<epsilon] }
  ELSE {
    dxdy: REAL _ d.x/d.y;
    RETURN[ABS[d1.x-d1.y*dxdy]<epsilon] };
};

--CubicPiece computes the cubic section between t0 and t1 for the cubic:bz

CubicPiece:PUBLIC PROC [bz:LONG POINTER TO Bezier,t0,t1:REAL]RETURNS[pbz:Bezier]=
{nt:REAL;
 tbz:Bezier;
 a,b,c,ab,bc,p: Vec;
  TMid: PROC[u,v:Vec,t:REAL] RETURNS[Vec] = INLINE {
  RETURN[[u.x+(v.x-u.x)*t, u.y+(v.y-u.y)*t]]
  };
  a _ TMid[bz.b0,bz.b1,t0];
  b _ TMid[bz.b1,bz.b2,t0];
  c _ TMid[bz.b2,bz.b3,t0];
  ab _ TMid[a,b,t0];
  bc _ TMid[b,c,t0];
  p _ TMid[ab,bc,t0];
 tbz_Bezier[p,bc,c,bz.b3];
 nt_(t1-t0)/(1.0-t0);
 a _ TMid[tbz.b0,tbz.b1,nt];
  b _ TMid[tbz.b1,tbz.b2,nt];
  c _ TMid[tbz.b2,tbz.b3,nt];
  ab _ TMid[a,b,nt];
  bc _ TMid[b,c,nt];
  p _ TMid[ab,bc,nt];
 RETURN[Bezier[tbz.b0,a,ab,p]];
};

Mag:PROC[v:Vec]RETURNS [REAL]=INLINE
	{RETURN[RealFns.SqRt[v.x*v.x+v.y*v.y]];};
	
GetDirCos: PUBLIC PROC[t:REAL,bz: LONG POINTER TO Bezier] RETURNS[pos,dc:Vec] = {
  a,b,c,ab,bc,p: Vec;
  TMid: PROC[u,v: Vec] RETURNS[Vec] = INLINE {
  RETURN[[u.x+(v.x-u.x)*t, u.y+(v.y-u.y)*t]]
  };
  a _ TMid[bz.b0,bz.b1];
  b _ TMid[bz.b1,bz.b2];
  c _ TMid[bz.b2,bz.b3];
  ab _ TMid[a,b];
  bc _ TMid[b,c];
  p _ TMid[ab,bc];
  RETURN[p,Norm[Sub[bc,ab]]];
  };

Norm:PROC[v:Vec]RETURNS [Vec]=INLINE
	{m:REAL_RealFns.SqRt[v.x*v.x+v.y*v.y];
	RETURN[Vec[v.x/m,v.y/m]];};

Exp2:PROC[e:CARDINAL]RETURNS [REAL]=
{RETURN[Etab[e]];};

CacheInit:PROC=
 {i:CARDINAL;
 FOR i IN [0..maxcnt] DO
 vcache[i]_-1.0;
 ENDLOOP;
 validCache _FALSE};
 
Etab:ARRAY [0..16] OF REAL=
[.5,1/4.0,1/8.0,1/16.0,1/32.0,1/64.0,1/128.0,1/256.0,1/512.0,1/1024.0,1/2048.0,1/4096.0,1/(4096.0*2),1/(4096.0*4),1/(4096.0*8),1/(4096.0*16),1/(4096.0*32)];

maxlevel:CARDINAL=7;
maxcnt:CARDINAL=128;
validCache: BOOLEAN _ FALSE;
vcache:ARRAY[0..maxcnt] OF REAL;
CacheInit[];

}.