# Implementation of the module {gcode_read}.
# Last edited on 2021-03-12 21:11:22 by jstolfi

import gcode_read
import contour
import block
import move
import path
import contact

import math as m

#################### READ.
def read_gcode(inputName, nSlice):
  C = list() # Contour.
  R = list() # Filling.
  Z = 0 
  
  listPoints = list()

  cSlice = False
  isRaster = False
  newList = False
  
  with open('./tests/in/' + inputName + '.gcode', 'r', encoding = "ISO-8859-1") as f:
    for line in f:
      if '(Layer' in line:
        layer = line.replace('(Layer', '')
        layer = int(layer.replace(')', ''))

        if layer == nSlice:
          cSlice = True
        elif layer > nSlice:
          break

      elif cSlice:
        if 'Z' in line:
          line = line.replace('  ', ' ')
          Z = line.split(' ')
          Z = float(Z[1].replace('Z', ''))

        if '(Offset ' in line:
          isRaster = False
          
          if 'Offset 1)' not in line: 
            newList = True 
        
        elif '(Raster ' in line:
          if isRaster == False:
            if len(listPoints) > 0:
              C.append(listPoints)
            listPoints = list()
            isRaster = True

          if 'Raster 1)' not in line: 
            newList = True 

        elif line[0:3] == 'G1 ' and 'X' in line and 'Y' in line:
          # Verifica se um novo raster foi iniciado.
          if newList == True:
            if not isRaster:
              if len(listPoints) > 0:
                C.append(listPoints)
            else: 
              if len(listPoints) > 0:
                R.append(listPoints)

            listPoints = list()
            newList = False

          XY = line.replace('  ', ' ').split(' ')
                
          for i in XY:
            if 'X' in i:
              x = float(i.replace('X',''))
            elif 'Y' in i:
              y = float(i.replace('Y',''))
          
          listPoints.append((x, y))

  if len(listPoints) > 0:
    R.append(listPoints)
  
  return Z, C, R
  # ----------------------------------------------------------------------

#################### RASTER LINES + SIDES.
def check_side(r1, r2):
  wd = move.width(r1['move'])

  y1 = r1['p'][1]
  y2 = r2['p'][1]
        
  if is_equal(y1 + wd, y2, 0.005) or is_equal(y2 + wd, y1, 0.005):
    p1 = r1['p'][0]
    q1 = r1['q'][0] 

    p2 = r2['p'][0]
    q2 = r2['q'][0]

    if min(p2, q2) < p1 and p1 < max(p2, q2): 
      return y1, y2
    elif min(p2, q2) < q1 and q1 < max(p2, q2): 
      return y1, y2
    elif min(p1, q1) < p2 and p2 < max(p1, q1): 
      return y1, y2
    elif min(p1, q1) < q2 and q2 < max(p1, q1): 
      return y1, y2
    elif is_equal(p1, p2, 0.005):
      return y1, y2
    elif is_equal(p1, q2, 0.005):
      return y1, y2
    elif is_equal(q1, p2, 0.005):
      return y1, y2
    elif is_equal(q1, q2, 0.005):
      return y1, y2
  
  return None, None
  # ----------------------------------------------------------------------
  
def find_sides(R):
  S = []

  for index1 in range(len(R)):
    r1 = R[index1]
    if r1['isRaster']:
      for index2 in range(index1 + 1, len(R)):
        r2 = R[index2]
        if r2['isRaster']:
            y1, y2 = check_side(r1, r2)

            if y1 != None and y2 != None:
              if r1['group'] != r2['group']:
                S.append([r1['sid'], r2['sid'], None, None])

              if y2 > y1:
                R[index1]['links'][1].append(r2['sid'])
                R[index2]['links'][0].append(r1['sid']) 
              elif y1 > y2:
                R[index1]['links'][0].append(r2['sid']) 
                R[index2]['links'][1].append(r1['sid']) 

  return S
  # ----------------------------------------------------------------------

def check_points_line(p, q, mp, islice, angle):  
  r_move = move.make(p, q, mp)

  if (islice % 2) == 0:
    p_x = rotate_x(p[0], p[1], angle)
    p_y = rotate_y(p[0], p[1], angle)

    q_x = rotate_x(q[0], q[1], angle)
    q_y = rotate_y(q[0], q[1], angle)
  
  else:
    p_x = p[0]
    p_y = p[1]

    q_x = q[0]
    q_y = q[1]
  
  if is_equal(p_y, q_y, 0.001) and (p_y != q_y): 
    if(p_y > q_y):
      q_y = p_y
    else:
      p_y = q_y

  p = (p_x, p_y)
  q = (q_x, q_y)

  return p, q, r_move

def create_raster_lines(islice, RP, mp_trace_raster, angle):
  R = []
  NB = len(RP) - 1
  
  for r in RP:
    p = None

    for q in r:    
      q_aux = q
      if p != None and not is_same_point(p, q, 0.00001):
        p, q, r_move = check_points_line(p, q, mp_trace_raster, islice, angle)
        line = {'sid': len(R), 'p': p, 'q': q, 'move': r_move, 'group': RP.index(r), 'isRaster': False, 'links': [[], []]}

        if is_equal(p[1], q[1], 0.001):
          line['isRaster'] = True
        
        R.append(line)

      p = q_aux
  
  S = find_sides(R)
  
  return R, S, NB
  # ----------------------------------------------------------------------
  
#################### SPLIT + LIMIT.
def sort_group(R):
  groups = []

  for r in R:
    if r!= None and r['group'] not in groups:
      groups.append(r['group'])

  for r in R:
    if r != None:
      r['group'] = groups.index(r['group'])

  return
  # ----------------------------------------------------------------------
  
def delete_link(R, link):
  points = []
  
  for l in link:
    p = move.pini(R[l]['move'])
    q = move.pfin(R[l]['move'])

    if p not in points:
      points.append(p)

    if q not in points:
      points.append(q)

    R[l] = None

  return points
  # ----------------------------------------------------------------------
  
def split_block(R, group_max, S):
  TO = group_max + 1
  
  for i in range(0, TO):
    swap_later = False
    r_prev = None
    raster_link = []
    group = i

    for r in R:
      if r != None and r['group'] == i:
        if r['isRaster']:
          if swap_later == True:
            swap_later = False
            group_max += 1
            group = group_max
            raster_link = delete_link(R, raster_link)
            S.append((r_prev, r['sid'], raster_link, None))

          input_lines = len(r['links'][0])
          output_lines = len(r['links'][1])
          
          if input_lines > 1 or output_lines > 1:
            if input_lines > 1:
              group_max += 1
              group = group_max
              raster_link = delete_link(R, raster_link)
              S.append((r_prev, r['sid'], raster_link, None))
            
            if output_lines > 1:
              swap_later = True

          raster_link = []
          r_prev = r['sid']

        else:
          raster_link.append(r['sid'])

        r['group'] = group

  sort_group(R)
  return group_max
  # ----------------------------------------------------------------------
  
def limit_block(R, group_max, S, limit_max):
  TO = group_max + 1

  for i in range(0, TO):
    group = i
    r_prev = None
    raster_link = []
    lines = 0

    for r in R:
      if r != None and r['group'] == i:
        if r['isRaster']:
          if lines < limit_max:
            lines = lines + 1

          else:
            lines = 1
            group_max += 1
            group = group_max
            raster_link = delete_link(R, raster_link)
            S.append((r_prev, r['sid'], raster_link, None))
          
          raster_link = []
          r_prev = r['sid']

        else:
          raster_link.append(r['sid'])
        
        r['group'] = group

  sort_group(R)
  return group_max
  # ----------------------------------------------------------------------

#################### BLOCKS + CONTACTS. 
def create_contact(S, R, BS, mp_trace, mp_jump, angle, scan_line):
  CS = []
  angle = -angle

  for s in S:
    r0 = R[s[0]]
    p0 = r0['p'][0]
    q0 = r0['q'][0]
    y0 = r0['p'][1]
    wd0 = move.width(r0['move'])
    mv0 = r0['move']
    bc0 = BS[r0['group']]

    r1 = R[s[1]]
    p1 = r1['p'][0]
    q1 = r1['q'][0]
    y1 = r1['p'][1]
    wd1 = move.width(r1['move'])
    mv1 = r1['move']
    bc1 = BS[r1['group']]

    xlo = max(min(p0, q0), min(p1, q1))
    xhi = min(max(p0, q0), max(p1, q1))
    ylo = (y0 + y1 + (wd0 - wd1)/2)/2
    yhi = (y0 + y1 + (wd0 - wd1)/2)/2

    if scan_line:
      rl0 = None
      rl1 = None
    else:
      rl0 = s[2]
      if rl0 != None:
        rl0 = path.from_points(rl0, mp_trace, mp_jump)

      rl1 = s[3]
      if rl1 != None:
        rl1 = path.from_points(rl1, mp_trace, mp_jump)

    end0 = move.endpoints(mv0)
    end1 = move.endpoints(mv1)

    if not is_equal(end0[0][1], end0[1][1], 0.05) or not is_equal(end1[0][1], end1[1][1], 0.05):
      xlo_rotate = rotate_x(xlo, ylo, angle)
      ylo_rotate = rotate_y(xlo, ylo, angle)
      xlo = xlo_rotate
      ylo = ylo_rotate

      xhi_rotate = rotate_x(xhi, yhi, angle)
      yhi_rotate = rotate_y(xhi, yhi, angle)
      xhi = xhi_rotate
      yhi = yhi_rotate

    ct = contact.make((xlo, ylo), (xhi, yhi), mv0, mv1, bc0, bc1, rl0, rl1)
    CS.append(ct)

    block.add_ct(bc0, ct)
    block.add_ct(bc1, ct)

  return CS
  # ----------------------------------------------------------------------
  
def create_blocks(R, S, CTRS, group_max, mp_trace, mp_jump, angle, scan_line):
  BS = []
  CS = []
  
  for index_block in range(group_max + 1):
    move_path = []

    for r in R:
      if r != None:
        if r['group'] == index_block:
          move_path.append(r['move'])

    if len(move_path) > 0:
      ph = path.from_moves(move_path)
      bc = block.from_paths([(ph, 0), path.rev(ph)])
      BS.append(bc)

  if len(CTRS) > 0:
    contour.find_contour(CTRS, BS)
  
  if len(S) > 0:
    CS = create_contact(S, R, BS, mp_trace, mp_jump, angle, scan_line)

  return BS, CS
  # ----------------------------------------------------------------------

####################
def is_same_point(A, B, tolerance):
  if (abs(A[0] - B[0]) < tolerance) and (abs(A[1] - B[1]) < tolerance):
    return True 
  return False

def is_equal(A, B, tolerance):
  if (abs(A - B) < tolerance):
    return True 
  return False


def rotate_x(x, y, angle):
  return ((x*m.cos(-angle) - y*m.sin(-angle))*100)/100.0

def rotate_y(x, y, angle):
  return ((x*m.sin(-angle) + y*m.cos(-angle))*100)/100.0