%begin
!Rubik: an exercise in 3d->3d and 3d->2d transformations and more
!J. Butler mods 1987:  1) Now uses level:graphinc.imp
!                      2) Drawing visibility can be turned off

%include "level1:graphinc.imp"

%integerfn int(%real r)
  %result = intpt(0.5+r) %unless r<0
  %result = -intpt(0.5-r)
%end

%realfn minus(%real r)
  %result=-r
%end

%record(*)%name nil
%recordformat r3(%real x,y,z)
%recordformat r2(%real x,y)
%recordformat r3r3(%real xx,yx,zx,xy,yy,zy,xz,yz,zz)
%recordformat r3r2(%real xx,yx,xy,yy,xz,yz)

%routine transform(%record(r3)%name in,out,%record(r3r3)%name f)
! 3d -> 3d transformation       out = f(in)
%real x,y,z
  x = f_xx*in_x + f_xy*in_y + f_xz*in_z
  y = f_yx*in_x + f_yy*in_y + f_yz*in_z
  z = f_zx*in_x + f_zy*in_y + f_zz*in_z
  out_x = x; out_y = y; out_z = z
%end

%routine project(%record(r3)%name in,%record(r2)%name out,
  %record(r3r2)%name f)
! 3d -> 2d transformation   out = f(in)
  out_x = f_xx*in_x + f_xy*in_y + f_xz*in_z
  out_y = f_yx*in_x + f_yy*in_y + f_yz*in_z
%end

%routine p3(%record(r3)%name p,%real x,y,z)
  p_x=x; p_y=y; p_z=z
%end

%routine p2(%record(r2)%name p,%real x,y)
  p_x=x; p_y=y
%end

%routine form r3r3 matrix(%record(r3r3)%name m,%record(r3)%name i,j,k)
  m_xx = i_x; m_yx = i_y; m_zx = i_z
  m_xy = j_x; m_yy = j_y; m_zy = j_z
  m_xz = k_x; m_yz = k_y; m_zz = k_z
%end

%routine form r3r2 matrix(%record(r3r2)%name m,%record(r2)%name i,j,k)
  m_xx = i_x; m_yx = i_y
  m_xy = j_x; m_yy = j_y
  m_xz = k_x; m_yz = k_y
%end

%recordformat squaref(%record(squaref)%name next,dnext,%integer colour,
                      %record(r3)middle,corner1,corner2)
%recordformat piecef(%record(piecef)%name next,%record(squaref)%namec
                     firstsquare,%record(r3)middle)
%recordformat pref(%record(piecef)%name ref)

%record(piecef)%array piece(-13:13)
%record(squaref)%array square(1:54)
%record(pref)%array cube(-13:13)

%record(r3)pi,pj,pk,mi,mj,mk; !(pos and neg)unit vectors spanning r3
%record(r3r3)pr,pl,pu,pd,pf,pb; !Rotation matrices
%record(r3r2)proj
%record(r3)observer
%record(r2)iproj,jproj,kproj
%integer ihf, drv

%routine realign point(%record(r3)%name p)
! Eliminate rounding errors incurred by repeated transformations:
! We assume point P should be at a point in r3 which
! is on a 0.05 unit grid.
  p_x = int(p_x*20.0)/20.0
  p_y = int(p_y*20.0)/20.0
  p_z = int(p_z*20.0)/20.0
%end

%routine realign cube
%record(piecef)%name p
%record(r3)%name m
%record(squaref)%name s
%integer i,j,k,x,y,z
  cube(i)_ref == nil %for i = -13,1,13
  %for i=-1,1,1 %cycle
  %for j=-1,1,1 %cycle
  %for k=-1,1,1 %cycle
    p == piece(i+j*3+k*9); m == p_middle
    realign point(m)
    x = int(m_x); y = int(m_y); z = int(m_z)
    cube(x+y*3+z*9)_ref == p
    s == p_firstsquare
    %whilenot addr(s)=0 %cycle
      realign point(s_middle)
      realign point(s_corner1)
      realign point(s_corner2)
      s == s_next
    %repeat
  %repeat
  %repeat
  %repeat
%end

%record(piecef)%map cubelet(%integer i,j,k)
  %result == cube(i+j*3+k*9)_ref
%end

%record(r3r3)%name rot
%record(piecef)%name face

%routine form face(%integer f)
%integer i,j,k,i1,j1,k1,i2,j2,k2
%record(piecef)%name c
  i1=-1; j1=-1; k1=-1; i2=1; j2=1; k2=1
  face == nil; rot == nil
  f=f&95
  %if f='L' %start
    j2=-1; rot == pl
  %finishelseif f='R' %start
    j1=1;  rot == pr
  %finishelseif f='F' %start
    i1=1;  rot == pf
  %finishelseif f='B' %start
    i2=-1; rot == pb
  %finishelseif f='U' %start
    k1=1;  rot == pu
  %finishelseif f='D' %start
    k2=-1; rot == pd
  %finishelsereturn
  %for i = i1,1,i2 %cycle
    %for j =j1,1,j2 %cycle
      %for k=k1,1,k2 %cycle
        c == cubelet(i,j,k); c_next == face; face == c
      %repeat
    %repeat
  %repeat
%end

%record(squaref)%map newsquare(%record(squaref)%name link)
%record(squaref)%name this
%owninteger sq=0
  sq = sq+1; this == square(sq)
  this = 0; this_next == link; %result==this
%end

%routine showpoint(%record(r3)%name r)
  printsymbol('('); print(r_x,0,1); printsymbol('|')
  print(r_y,0,1); printsymbol('|'); print(r_z,0,1); printsymbol(')')
%end

%routine show everything

  %routine showsquares(%record(squaref)%name s)
  %conststring(7)%array cname(0:7)=%C
  "black  ", "red    ", "green  ", "yellow ",
  "blue   ", "magenta", "cyan   ", "white  "
    %whilenot addr(s)=0 %cycle
      printstring(cname(s_colour)); space; showpoint(s_middle)
      space; showpoint(s_corner1); space; showpoint(s_corner2)
      newline; s == s_next
    %repeat
  %end

%integer i,j,k
%integer x,y,z
%record(piecef)%name p

  %for i=-1,1,1 %cycle
  %for j=-1,1,1 %cycle
  %for k=-1,1,1 %cycle
    p == piece(i+j*3+k*9)
    printstring("Piece "); write(i,1); write(j,1); write(k,1)
    printstring(" is at "); showpoint(p_middle); newline
    showsquares(p_firstsquare)
  %repeat
  %repeat
  %repeat
  %for i=-1,1,1 %cycle
  %for j=-1,1,1 %cycle
  %for k=-1,1,1 %cycle
    %for x=-1,1,1 %cycle
    %for y=-1,1,1 %cycle
    %for z=-1,1,1 %cycle
      %if i#x %and j#y %and k#z %start
      %if addr(cubelet(i,j,k))=addr(cubelet(x,y,z)) %start
        printstring("Cubelets "); write(i,1); write(j,1); write(k,1)
        printstring(" and ");     write(x,1); write(y,1); write(z,1)
        printstring(" refer to the same piece."); newline
      %finish
      %finish
    %repeat
    %repeat
    %repeat
  %repeat
  %repeat
  %repeat
%end

%routine display cube
%record(squaref)%name s
%constinteger maxint=(-1)>>1,minint=\maxint
%owninteger left=0,right=1023,bot=0,top=1023
%integer i,j,k
%record(r3)c3,c4
%record(r2)p1,p2,p3,p4
%recordformat i2(%integer x,y)
%record(i2)q1,q2,q3,q4

  %routine mirror(%record(r3)%name point,centre,image)
    image_x = centre_x*2-point_x
    image_y = centre_y*2-point_y
    image_z = centre_z*2-point_z
  %end

  %routine scale(%record(r2)%name i,%record(i2)%name j)
  %constinteger fx=60,fy=60,mx=344,my=256
  %integer x,y
    x = int(i_x*fx+mx); j_x = x
    left = x %if x<left; right=x %if x>right
    y = int(i_y*fy+my); j_y = y
    bot = y %if y<bot; top = y %if y>top
  %end

  %routine add squares(%record(squaref)%name f,%integer x,y,z)
  %record(r3)%name r
  %integer a,b,c
    %whilenot addr(f)=0 %cycle
      r == f_middle
      a = int(r_x*2); b = int(r_y*2); c = int(r_z*2)
      %if (x=0 %or x=a) %and (y=0 %or y=b) %and (z=0 %or z=c) %start
        f_dnext == s %and s == f
      %finish
      f == f_next
    %repeat
  %end

  s == nil
  %for j=-1,1,1 %cycle
    add squares(cubelet(1,j,k)_firstsquare,3,0,0) %for k=-1,1,1
  %repeat
  %for i=-1,1,1 %cycle
    add squares(cubelet(i,1,k)_firstsquare,0,3,0) %for k=-1,1,1
  %repeat
  %for i=-1,1,1 %cycle
    add squares(cubelet(i,j,1)_firstsquare,0,0,3) %for j=-1,1,1
  %repeat
  %for j=-1,1,1 %cycle
    add squares(cubelet(-1,j,k)_firstsquare,-3,0,0) %for k=-1,1,1
  %repeat
  %for i=-1,1,1 %cycle
    add squares(cubelet(i,-1,k)_firstsquare,0,-3,0) %for k=-1,1,1
  %repeat
  %for i=-1,1,1 %cycle
    add squares(cubelet(i,j,-1)_firstsquare,0,0,-3) %for j=-1,1,1
  %repeat

!ihf is the invisible half frame.   0= top half displayed, 1=bottom half.
!We draw into the invisible half then flip them over.
  half clear(ihf)
  left = maxint; bot = maxint; right = minint; top = minint
  %whilenot addr(s)=0 %cycle
    colour(s_colour)
    mirror(s_corner1,s_middle,c3); mirror(s_corner2,s_middle,c4)
    project(s_corner1,p1,proj); scale(p1,q1)
    project(s_corner2,p2,proj); scale(p2,q2)
    project(c3,p3,proj); scale(p3,q3)
    project(c4,p4,proj); scale(p4,q4)
    triangle(q1_x,q1_y+512*ihf,q2_x,q2_y+512*ihf,q3_x,q3_y+512*ihf)
    triangle(q1_x,q1_y+512*ihf,q4_x,q4_y+512*ihf,q3_x,q3_y+512*ihf)
    s == s_dnext
  %repeat
  offset(0, 512*ihf)
  ihf=1-ihf %if drv=0
%end

%routine initialise faces(%integer c1,c2)
%integer j,k

%routine do(%record(squaref)%name c,%real x,y,z,%integer col)
%constreal co=0.45; !corner offset
  %while c_colour#0 %cycle
    c == c_next
    %if addr(c)=0 %start
      printstring("** Off squares list **"); newline
      show everything; %stop
    %finish
  %repeat
  c_colour = col
  p3(c_middle,x,y,z)
  p3(c_corner1,x,y-co,z+co)
  p3(c_corner2,x,y+co,z+co)
%end

  %for j = -1,1,1 %cycle
    %for k = -1,1,1 %cycle
      do(cubelet(1,j,k)_firstsquare,1.5,j,k,c1)
      do(cubelet(-1,j,k)_firstsquare,minus(1.5),j,k,c2)
    %repeat
  %repeat
%end

%routine rotate piece(%record(piecef)%name p)
%record(squaref)%name s
  transform(p_middle,p_middle,rot)
  s == p_firstsquare
  %whilenot addr(s)=0 %cycle
    transform(s_middle,s_middle,rot)
    transform(s_corner1,s_corner1,rot)
    transform(s_corner2,s_corner2,rot)
    s == s_next
  %repeat
%end

%routine rotate face
%record(piecef)%name p
  p == face
  %whilenot addr(p)=0 %cycle
    rotate piece(p); p == p_next
  %repeat
  realign cube
%end

%routine rotate cube
%integer i,j,k
  %for i=-1,1,1 %cycle
  %for j=-1,1,1 %cycle
  %for k=-1,1,1 %cycle
    rotate piece(piece(i+j*3+k*9))
  %repeat
  %repeat
  %repeat
  realign cube
%end

%integer i,j,k,l
%record(squaref)%name head
%record(piecef)%name p
%real temp

nil == record(0)
clear; offset(0,0); ihf=0; drv=1

! Set up spanning vectors

temp = minus(1.0)
p3(pi,1.0,0,0); p3(mi,temp,0,0)
p3(pj,0,1.0,0); p3(mj,0,temp,0)
p3(pk,0,0,1.0); p3(mk,0,0,temp)

p2(kproj,0,1.0); p2(jproj,1.0,0)
temp = minus(0.4); p2(iproj,temp,temp)

p3(observer,3.0,3.0,3.0)

! Set up rotation matrices

form r3r3 matrix(pr,pk,pj,mi)
form r3r3 matrix(pl,mk,pj,pi)
form r3r3 matrix(pf,pi,mk,pj)
form r3r3 matrix(pb,pi,pk,mj)
form r3r3 matrix(pu,mj,pi,pk)
form r3r3 matrix(pd,pj,mi,pk)

form r3r2 matrix(proj,iproj,jproj,kproj)

! Set up cube array, giving each cubelet the right number of squares

%for i=-1,1,1 %cycle
%for j=-1,1,1 %cycle
%for k=-1,1,1 %cycle
  l = i+j*3+k*9; p == piece(l); p = 0
  p3(p_middle,i,j,k)
  cube(l)_ref == p; head == nil
  head == newsquare(head) %for l = 1,1,|i|+|j|+|k|
  p_firstsquare == head
%repeat
%repeat
%repeat

! Now we must initialise the squares.  We do this by initialising one
! face only, then rotating the whole cube round so that it presents a
! different face to the initialising tool.

initialise faces(red,magenta)
rot == pu; rotate cube
initialise faces(white,yellow)
rot == pr; rotate cube
initialise faces(blue,green)

display cube
newline; printstring("Moves are R r L l U u D d F f B b"); newline
printstring("space or RETURN redraws without moving"); newline
printstring("v toggles drawing visibility"); newline
printstring("! after move to rotate whole cube rather than face"); newline
prompt("Move:")
%cycle
  readsymbol(i)
  %if i=' ' %or i=nl %start
    display cube
  %elseif i='v'
    ihf=drv; drv = 1-drv
  %else
    rot == nil
    form face(i); %continueif addr(rot)=0
    %if nextsymbol='!' %start
      rotate cube
      rotate cube %and rotate cube %if i&32=0
    %else
      rotate face
      rotate face %and rotate face %if i&32=0
    %finish
  %finish
%repeat

%endofprogram