#! /usr/bin/python3
# Last edited on 2025-10-18 12:53:23 by stolfi

import sys, re
from math import sqrt, hypot, log, exp, sin, cos, inf, nan, pi, floor, ceil, isfinite, isnan
from sys import stdout as out, stderr as err

def main():
  # Reads a list of simple object specs. Turns it into a geomview model.
  # A simple object can be a point, a tetrahedron, or a box (a
  # hexahedron with quadrangular faces).  See the description of
  # the input format in {view_point_cloud.sh}.
  #
  # Ignores blank lines and '#'-comments.
  
  usage = f"{sys.argv[0]} {{data_file}} {{geom_file}}"
  st = init_state() # Program state.
  st['data_file'] = sys.argv[1]
  st['rd'] = sys.stdin  if st['data_file'] == "-" else open(st['data_file'], "r")
  st['geom_file'] = sys.argv[2]
  st['wr'] = sys.stdout if st['geom_file'] == "-" else open(st['geom_file'], "w")
  # General parameters (should come from command line...):
  st['clip_lo'] = (-2, -2, -2) # Low coords for clipping.
  st['clip_hi'] = (+2, +2, +2) # High coords for clipping.
  write_preamble(st)
  main_loop(st)
  write_postamble(st)
  show_stats(st)
  return
  # ----------------------------------------------------------------------
  
def init_state():
  st = dict()
  st['line'] = 0
  st['ct'] = dict()
  st['ct']['line'] = 0
  st['ct']['point'] = 0
  st['ct']['segment'] = 0
  st['ct']['tetra'] = 0
  st['ct']['box'] = 0
  st['nread'] = 0
  # Data of object currently being parsed:
  st['obj'] = None
  return st
  # ----------------------------------------------------------------------

def main_loop(st):
  # We temporarily save the definition of each object in {obj}.
  # Once the object is complete we discard points out of bounds
  # clip edges, discard those entirely out, and finally 
  # output the skeleton item for geomview.
  
  rd = st['rd']
  while True:
    line = rd.readline()
    # The {readline} returns "" only at end-of-file:
    if line == "": rd.close(); return 
    st['ct']['line'] += 1
    st['line'] = line
    # Remove '#'-comments:
    line = re.sub(r"[#].*$", "", line)
    # Remove leading and trailing blanks and newline:
    line = line.strip()
    if line == "":
      pass
    elif re.match(r"[-][a-zA-Z]", line) and st['obj'] != None:
      data_error(st, f"incomplete object {st['obj']['kind']}")
    elif re.match(r"-point\b", line):
      start_object(st, "-point")
      line = re.sub(r"^-point\b", "", line)
      parse_object_point(st, line)
      output_object(st, st['obj'])
      st['obj'] = None
    elif re.match(r"[-](segment|tetra|box)\b", line):
      start_object(st, line)
    elif re.match(r" *[-+]?[0-9]", line):
      # Point coordinate line:
      if st['obj'] == None:
        data_error(st, "spurious coords line")
      else:
        parse_object_point(st, line)
        if obj_is_complete(st['obj']):
          output_object(st, st['obj'])
          st['obj'] = None
    else:
      data_error(st, "invalid line format")
  assert False # Never reaches here.
  # ----------------------------------------------------------------------    
    
def show_stats(st):
  ct = st['ct']
  err.write("counts:\n")
  for key, val in ct.items():
    err.write(f"  %-7s %7d\n" % (key, val))
  return
  # ----------------------------------------------------------------------

def start_object(st, line):  
  fld = line.split()
  nf = len(fld)
  if nf != 1 and nf != 4: 
    data_error(st, "bad object header")
  if not re.fullmatch(r"[-](point|segment|tetra|box)", fld[0]): 
    data_error(st, "unknown object kind")
  kind = fld[0][1:]
  if nf == 1:
    R = 1.0; G = 0.9; B = 0.2
  else:
    R = float(fld[1]); G = float(fld[2]); B = float(fld[3])
  st['ct'][kind] += 1
  st['obj'] = new_object(kind,R,G,B)
  return
  # ----------------------------------------------------------------------

def new_object(kind,R,G,B):
  obj = dict()
  obj['kind'] = kind
  obj['ecolor'] = (R,G,B) # Color of edges of object.
  obj['points'] = [ ]     # Key points of object, including coord, radii, colors.           
  return obj
  # ----------------------------------------------------------------------
        
def parse_object_point(st, line):
  # Parses a line with three coordinates of a point and a radius,
  # and optionally a point color. Saves that point data in the current object.
  fld = line.split()
  nf = len(fld)
  if nf != 4 and nf != 7: 
    data_error(st, "invalid object point format")
  x = float(fld[0]); y = float(fld[1]); z = float(fld[2])
  rad = float(fld[3])
  if nf == 4:
    R = None; G = None; B = None
  elif nf == 7:
    R = float(fld[4]); G = float(fld[5]); B = float(fld[6])
  obj = st['obj']
  if obj == None: prog_error("no object")
  obj['points'].append(((x,y,z), rad, (R,G,B)))
  return
  # ----------------------------------------------------------------------

def obj_is_complete(obj):
  kind = obj['kind']
  np = len(obj['points'])
  if kind == 'point'   and np == 1: return True
  if kind == 'segment' and np == 2: return True
  if kind == 'tetra'   and np == 4: return True
  if kind == 'box'     and np == 8: return True
  return False
  # ----------------------------------------------------------------------

def output_object(st, obj):
  kind = obj['kind']
  points = obj['points']
  np = len(points)
  # err.write(f"output_object {kind = } {np = } ...\n")
  skel = new_skel(obj['ecolor'])
  for iv in range(np):
    skel_add_obj_point(st, skel, points[iv])
  skel_add_obj_edges(st, skel, obj);
  output_skel(st, skel)
  return
  # ----------------------------------------------------------------------
  
def new_skel(ecolor):
  skel = dict()
  skel['ecolor'] = ecolor
  skel['vertices'] = [ ] # Endpoints of edges of skeleton.
  skel['edges'] = [ ]    # Edges of skeleton, as pairs of vertex indices.
  return skel
  # ----------------------------------------------------------------------
  
def skel_add_obj_edges(st, skel, obj):
  kind = obj['kind']
  if kind == 'point':   pass
  if kind == 'segment': skel_add_edges_segment(st, skel)
  if kind == 'tetra':   skel_add_edges_tetra(st, skel)
  if kind == 'box':     skel_add_edges_box(st, skel)
  return
  # ----------------------------------------------------------------------

def skel_add_obj_point(st, skel, point):
  # Adds to {skel} a vertex at {point} which must be a triple 
  # {((cx,cy,cz),rad,(R,G,B))}. If the point is not in the clip box,
  # its radius and color are set as {None} instead of {rad} and {(R,G,B)}.

  ctr, rad, color = point
  if not point_is_visible(point[0], st['clip_lo'], st['clip_hi']):
    rad = None; color = None
  skel['vertices'].append((ctr, rad, color))
  return
  # ----------------------------------------------------------------------
  
def point_is_visible(p, clip_lo, clip_hi):
  for k in range(3):
    if p[k] <= clip_lo[k] or p[k] >= clip_hi[k]: return False
  return True
  # ----------------------------------------------------------------------

def skel_add_edges_segment(st, skel):
  nv = len(skel['vertices'])
  if nv != 2: prog_error(f"bad segment skeleton {nv = }")
  skel_add_edge(st, skel, 0, 1)
  return
  # ----------------------------------------------------------------------

def skel_add_edges_tetra(st, skel):
  nv = len(skel['vertices'])
  if nv != 4: prog_error(f"bad tetra skeleton {nv = }")
  for iv0 in range(4):
    for iv1 in range(iv0+1,4):
      skel_add_edge(st, skel, iv0, iv1)
  return
  # ----------------------------------------------------------------------

def skel_add_edges_box(st, skel):
  # err.write(f"adding box edges ...\n")
  nv = len(skel['vertices'])
  if nv != 8: prog_error(f"bad box skeleton {nv = }")
  for axis in range(3):
    # err.write(f"{axis = }\n")
    for u in range(2): 
      for v in range(2):
        ua = u if axis > 0 else 2*u
        va = 2*v if axis > 1 else 4*v
        iv0 = ua + va
        iv1 = ua + 2**axis + va
        skel_add_edge(st, skel, iv0, iv1)
  return
  # ----------------------------------------------------------------------

def skel_add_edge(st, skel, iv0, iv1):
  # Adds to {skel} an edge between vertices with indices {iv0} and {iv1}.
  # If either of the vertices is invisible, clips the edge to the visible box,
  # adding any new endpoints to {skel}, and adds an edge with those endpoints instead.
  
  vertices = skel['vertices']
  nv = len(vertices)
  err.write(f"adding edge {iv0} -- {iv1} ...")
  iv0, iv1 = clip_edge(st, skel, iv0, iv1)
  err.write(f" clipped to {iv0} -- {iv1}\n")
  if iv0 != None and iv1 != None:
    skel['edges'].append((iv0, iv1))
  elif iv0 != None or iv1 != None:
    prog_error(f"inconsitent clip_edge {iv0 = } {iv1 = }")
  return
  # ----------------------------------------------------------------------

def clip_edge(st, skel, iv0, iv1):
  # Clips the edge to the clip box.
  # May append new vertices to {skel['vertices']}
  # If edge is totally invisible, returns {None,None}
  vertices = skel['vertices']
  nv = len(vertices)
  if iv0 < 0 or iv0 >= nv: prog_error(f"invalid {iv0 = }")
  if iv1 < 0 or iv1 >= nv: prog_error(f"invalid {iv1 = }")
  if iv0 == iv1: prog_error(f"endpoints must be distinct {iv0 = } {iv1 = }")
  p0 = vertices[iv0][0]; q0 = p0
  p1 = vertices[iv1][0]; q1 = p1
  # err.write(f"clip_edge: {p0 = } {p1 = }\n")
  clip_lo = st['clip_lo']
  q0, q1 = clip_edge_with_plane(q0, q1, +1, +0, +0, -clip_lo[0])
  q0, q1 = clip_edge_with_plane(q0, q1, +0, +1, +0, -clip_lo[1])
  q0, q1 = clip_edge_with_plane(q0, q1, +0, +0, +1, -clip_lo[2])
  clip_hi = st['clip_hi']
  q0, q1 = clip_edge_with_plane(q0, q1, -1, +0, -0, +clip_hi[0])
  q0, q1 = clip_edge_with_plane(q0, q1, -0, -1, -0, +clip_hi[1])
  q0, q1 = clip_edge_with_plane(q0, q1, -0, -0, -1, +clip_hi[2])
  if q0 == None or q1 == None:
    # Edge totally outside clip box.
    iv0 = None; iv1 = None
  elif q0 != None and q1 != None:
    # If the edge was trimmed, we should output a small sphere to close
    # off the trimmed edge. But in geomview the edges are "zero-width"
    # lines.
    if q0 != p0: 
      # Origin end was trimmed:
      iv0 = len(vertices)
      vertices.append((q0,None,None))
    if q1 != p1: 
      # Origin end was trimmed:
      iv1 = len(vertices)
      vertices.append((q1,None,None))
  else:
    prog_error("inconsistent clip_edge_with_plane result")
  return iv0, iv1
  # ----------------------------------------------------------------------
  
def clip_edge_with_plane(q0, q1, Ax, Ay, Az, D):
  if q0 == None and q1 == None: return None, None
  s0 = Ax*q0[0] + Ay*q0[1] + Az*q0[2] + D
  s1 = Ax*q1[0] + Ay*q1[1] + Az*q1[2] + D
  if s0 <= 0 and s1 <= 0:
    # Edge is wholly in non-positive halfspace:
    return None, None
  elif s0 >= 0 and s1 >= 0:
    # Edge is wholly in non-negative halfspace:
    return q0, q1
  else:
    # Edge crosses the plane:
    r0 = s0/(s0 - s1)
    r1 = 1 - r0
    u = (r1*q0[0] + r0*q1[0],  r1*q0[1] + r0*q1[1],  r1*q0[2] + r0*q1[2])  
    if s0 < 0:
      # Trim the {q0} end:
      return u, q1
    else:
      # Trim the {q1} end:
      return q0, u
  assert False
  # ----------------------------------------------------------------------

def output_skel(st, skel):
  vertices = skel['vertices']
  edges = skel['edges']
  nv = len(vertices)
  for iv in range(nv):
    ctr, rad, color = vertices[iv]
    if rad != None:
      if color == None: prog_error("missing vertex color")
      output_sphere(st, ctr, rad, color)
  skel_cleanup_unused_vertices(st, vertices, edges)
  output_rods(st, skel['ecolor'], vertices, edges)
  return
  # ----------------------------------------------------------------------

def output_sphere(st, ctr, rad, color):
  # err.write(f"output_sphere: {ctr =} {rad = } {color = }\n")
  wr = st['wr']
  wr.write("    { \n")
  R, G, B = color
  wr.write("      appearance { material { diffuse %5.3f %5.3f %5.3f } }\n" % (R, G, B))
  cx, cy, cz = ctr
  wr.write("      SPHERE %7.5f  %7.4f %7.4f %7.4f\n" % (rad, cx, cy, cz))
  wr.write("    }\n")
  return
  # ----------------------------------------------------------------------
  
def skel_cleanup_unused_vertices(st, vertices, edges):
  # Replaces unused vertics of {skel} by the center of the clip box,
  # to prevent them from messing up the view.
  nv = len(vertices)
  ne = len(edges)
  used = [ False ]*nv
  for ie in range(ne):
    iv0, iv1 = edges[ie]
    used[iv0] = True; used[iv1] = True
  clip_lo = st['clip_lo']
  clip_hi = st['clip_hi']
  for iv in range(nv):
    if not used[iv]:
      vertices[iv] = (tuple( (clip_lo[k] + clip_hi[k]) for k in range(3) ), None, None)
  return
  # ----------------------------------------------------------------------

def output_rods(st, ecolor, vertices, edges):
  # Outputs the edges of an object as cylinders. Assumes that the
  # SPHEREs at the ends have been outputted separately.
  nv = len(vertices)
  ne = len(edges)  
  wr = st['wr']
  wr.write("    {\n")
  output_edge_appearance(st, ecolor)
  wr.write("      SKEL\n")
  wr.write("        %d %d\n" % (nv, ne)) 
  # Define the coords of the vertices:
  for iv in range(nv):
    ctr, rad, color = vertices[iv]
    wr.write("        %7.4f %7.4f %7.4f\n" % ctr)
  # Ouput the edeges as 2-vertex polyines:
  for ie in range(ne):
    iv0, iv1 = edges[ie]
    wr.write("        2 %d %d\n" % (iv0, iv1));
  wr.write("    }\n")
  return
  # ----------------------------------------------------------------------

def output_edge_appearance(st, ecolor):
  wr = st['wr']
  wr.write("      appearance {\n")
  wr.write("        material {\n")
  wr.write("          ka 0.5 ambient 0.5 1.0 1.0\n")
  wr.write("          kd 0.5 diffuse 0.5 1.0 1.0\n")
  wr.write("          ks 0.0\n")
  wr.write("          edgecolor %5.3f %5.3f %5.3f \n" % ecolor)
  wr.write("        }\n")
  wr.write("      }\n")
  return
  # ----------------------------------------------------------------------
  
def write_preamble(st):
  wr = st['wr']
  wr.write("{\n")
  wr.write("  LIST\n")
  wr.write("  { appearance {\n")
  wr.write("      lighting { ambient 1 1 1 }\n")
  wr.write("      material { ka 0.7 kd 0.3 ks 0.0 }\n")
  wr.write("    }\n")
  return
  # ----------------------------------------------------------------------
  
def write_postamble(st):
  wr = st['wr']
  wr.write("  }\n")
  wr.write("}\n")
  return
  # ----------------------------------------------------------------------

def data_error(st, msg):
  err.write("%s:%d: ** %s\n" % (st['data_file'], st['ct']['line'], msg))
  err.write("  [[%s]]\n" % st['line'])
  assert False
  # ----------------------------------------------------------------------

def prog_error(msg):
  err.write("** PROG ERROR: %s\n" % msg)
  assert False
  # ----------------------------------------------------------------------

main()