import hotpath
import path
import block
import example_path
import input_data
import hacks
import job_parms
import rn
import pyx
import sys
from math import sqrt, sin, cos, floor, ceil, inf, nan, pi

import move
import move_parms
import contact
import gcode
import os
import time

def plot_solution(Q, name, c_box):
  # Plots the oriened path {Q} to files called "hotpath_TST_{name}.{ext}",
  # where {ext} is "eps", "jpg", "png", etc.
  c = pyx.canvas.canvas()
  
  # Find the bounding box:
  pbox = c_box[0]
  sz_max = c_box[1]

  wd_frame = 0.003*sz_max
  hacks.plot_frame(c, pyx.color.rgb.white, 0.02, None, pbox, -0.75*wd_frame)

  wd_grid = 0.002*sz_max
  hacks.plot_grid(c, pyx.color.rgb(0.800, 0.750, 0.600), 0.03, None, pbox, +5*wd_grid, 1,1)

  pyx.unit.set(uscale=1.00, wscale=1.00, vscale=1.00)
 
  ctrace =  "Colors"#pyx.color.rgb(0.050, 0.850, 0.000) # Color of nominal trace area.
  waxes = 0.05*parms['solid_raster_width']; 
  axes = False
  dots = False
  arrows = False
  matter = True
  path.plot_standard(c, Q, (0,0), None, ctrace, waxes, axes, dots, arrows, matter)
  hacks.write_plot(c, name)
  #gcode.write(open("tests/out/hotpath_TST_sol_" + name + '.gcode', 'w+'), Q, 0, parms)
  return 
  # ----------------------------------------------------------------------

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

def create_stroke (sid, p, q, rbit, group, mp):
  if rbit == 0:
    r_move = move.make(p, q, mp)
  else:
    r_move = move.make(q, p, mp)

  return {
      'sid': sid, 
      'p': [p, q],
      'move': r_move,
      'rbit': rbit,
      'group': group,
      'links': [[].copy(),[].copy()]
  }

def add_side (R1, R2, rasterLink0, rasterLink1):
  y1 = R1['p'][0][1]
  y2 = R2['p'][0][1]

  if y2 > y1:
    R1['links'][1].append([R2['sid'], rasterLink0, rasterLink1]) 
    R2['links'][0].append([R1['sid'], rasterLink0, rasterLink1]) 

  elif y1 > y2:
    R1['links'][0].append([R2['sid'], rasterLink0, rasterLink1]) 
    R2['links'][1].append([R1['sid'], rasterLink0, rasterLink1]) 

  return

def read_rasterLink (link):
  if 'None' in link:
    rasterLink = None
      
  else:
    aux = link.split(';')
    rasterLink = list()

    for index in range(1, len(aux)):
      p = aux[index].split('&')
      rasterLink.append((float(p[0]), float(p[1])))

  return rasterLink

def read_file (name, mp_trace):
  R = []
  S = []
  groupMax = 0

  with open('./tests/in/' + name + '.txt', 'r', encoding = "ISO-8859-1") as f:
    for line in f:
      if line[0] == 'N':
        R = [None]*int(line[1:])
      
      elif line[0] == 'R':
        r = line[1:].split(",")
        p_x = float(r[1])
        p_y = float(r[2])

        q_x = float(r[3])
        q_y = float(r[4])

        if p_y != q_y: 
          if(p_y > q_y):
            q_y = p_y
          else:
            p_y = q_y

        if p_x > q_x: 
          x_aux = q_x
          q_x = p_x
          p_x = x_aux

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

        if groupMax < int(r[6]):
          groupMax = int(r[6])

        R[int(r[0])] = create_stroke(int(r[0]), p, q, int(r[5]), int(r[6]), mp_trace)
      
      elif line[0] == 'L':
        l = line[1:].split(",")

        rasterLink0 = read_rasterLink(l[2])
        rasterLink1 = read_rasterLink(l[3])

        add_side(R[int(l[0].replace('L', ''))], R[int(l[1].replace('L', ''))], rasterLink0, rasterLink1)
        S.append((int(l[0].replace('L', '')), int(l[1].replace('L', ''))))

  return R, S, groupMax

def split_blocks(R, groupMax): 
  totalOriginal = groupMax + 1

  for i in range(0, totalOriginal):
    swap_later = False
    group = i
  
    for r in R:
      if swap_later == True:
        swap_later = False
        group = groupMax + 1

      if r['group'] == i:
        input_lines = 0
        output_lines = 0

        if len(r['links'][0]) > 1 or len(r['links'][1]) > 1:
          input_lines = len(r['links'][0])
          output_lines = len(r['links'][1])

          if input_lines > 1:
            group = groupMax + 1

          if output_lines > 1:
            swap_later = True

        r['group'] = group
        
        if r['group'] > groupMax:
          groupMax = r['group']

  return groupMax

def limit_blocks(R, groupMax, maxLimit):
  for i in range(0, groupMax + 1):
    group = i
    lines = 0

    for r in R:
      if r['group'] == i:
        if lines < maxLimit:
          lines = lines + 1
        else:
          lines = 1
          groupMax += 1
          group = groupMax

        r['group'] = group

  return groupMax

def delete_rasterLink(R):
  for index1 in range(len(R)):
    for edge in range(2):
      for indexLink in range(len(R[index1]['links'][edge])):
        index2 = R[index1]['links'][edge][indexLink][0]
        
        if R[index1]['group'] != R[index2]['group']:
          R[index1]['links'][edge][indexLink][1] = None
          R[index1]['links'][edge][indexLink][2] = None

          for indexLink2 in range(len(R[index2]['links'][1-edge])):
            if R[index2]['links'][1-edge][indexLink2][0] == index1:
              R[index2]['links'][1-edge][indexLink2][1] = None
              R[index2]['links'][1-edge][indexLink2][2] = None

  return

def raster_raster_contact(bc0, bc1, parms):
  # The {X} range of the contact will be {[xlo _ xhi]}
  xlo = -inf 
  xhi = +inf
  mv = [None,None]    # Top raster traces at top of {bc0} and bottom of {bc1}.
  for k in range(2):
    bck = (bc0, bc1)[k][0]
    # Pick some choice of the block, {bc0}  or {bc1}
    ophk = block.choice(bck, 0)
    omvk = path.elem(ophk, (bc0, bc1)[k][1])
    
    # Check if they are both traces of the same width
    wdk = move.width(omvk)
    assert wdk > 0 # Must be trace not jump.
    # Save the unoriented move into {mv[k]}:
    mv[k], drk = move.unpack(omvk)
    # Find the {X} span and intersect with current {[xlo _ xhi]}
    pk = move.pini(mv[k])
    qk = move.pfin(mv[k])
    assert pk[1] == qk[1] # Trace must be horizontal.
    xlok = min(pk[0], qk[0])
    xhik = max(pk[0], qk[0])
    xlo = max(xlo, xlok)
    xhi = min(xhi, xhik)

  # Check coordinates:
  y0 = move.pini(mv[0])[1]; wd0 = move.width(mv[0])
  y1 = move.pini(mv[1])[1]; wd1 = move.width(mv[1])

  assert abs((y0 + wd0/2) - (y1 - wd1/2)) < 0.01*(wd0+wd1), "traces are not adjacent"
  assert xlo < xhi, "{X} randges do not overlap"
  
  # Create the contact:
  ymd = (y0 + y1 + (wd0 - wd1)/2)/2
  ct = contact.make((xlo,ymd), (xhi, ymd), mv[0], mv[1], bc0, bc1)
  return ct

def create_input (name, wdf, test_parms):
  mp_trace = move_parms.make(wdf, 3000, 40, 0)
  mp_jump = move_parms.make(0, 3000, 100, 0.00)
  
  R, S, groupMax = read_file(name, mp_trace)
  
  if test_parms['splitLines']:
    groupMax = split_blocks(R, groupMax)
  if test_parms['limitLines']:
    groupMax = limit_blocks(R, groupMax, test_parms['nMaxLines'])
  if test_parms['splitLines'] or test_parms['limitLines']:
    delete_rasterLink(R)

  BS = []
  CS = []

  for g in range(groupMax + 1):
    move_path = list()
    qEnd = None

    for r in R:
      if r['group'] == g:
        if len(move_path) > 0:
          rasterLink = None

          for link in r['links'][0]:
            if r['group'] == R[link[0]]['group']:
              if link[1 + r['rbit']] != None:
                if isEqual(link[1 + r['rbit']][0], r['p'][r['rbit']], 0.05) or isEqual(link[1 + r['rbit']][-1], r['p'][r['rbit']], 0.05):
                  if isEqual(link[1 + r['rbit']][0], qEnd, 0.05) or isEqual(link[1 + r['rbit']][-1], qEnd, 0.05):
                    rasterLink = link[1 + r['rbit']]
          
          if rasterLink != None:
            if isEqual(rasterLink[0], qEnd, 0.05) and isEqual(rasterLink[-1], r['p'][r['rbit']], 0.05):
              rasterLink[0] = qEnd
              rasterLink[-1] = r['p'][r['rbit']]
                
            elif isEqual(rasterLink[-1], qEnd, 0.05) and isEqual(rasterLink[0], r['p'][r['rbit']], 0.05):
              rasterLink[0] = r['p'][r['rbit']]
              rasterLink[-1] = qEnd
              rasterLink.reverse()

            for index in range(len(rasterLink)-1):
              p = rasterLink[index]
              q = rasterLink[index+1]
              move_path.append(move.make(p, q, mp_trace))

        move_path.append(r['move'])
        qEnd = r['p'][1-r['rbit']]
    
    move_path_rev = list()
    qEnd = None

    for indexR in range(len(R) - 1, -1, -1):
      r = R[indexR]
      if r['group'] == g:
        if len(move_path_rev) > 0:
          rasterLink = None

          for link in r['links'][1]:
            if r['group'] == R[link[0]]['group']:
              if link[1 + r['rbit']] != None:
                if isEqual(link[1 + r['rbit']][0], r['p'][r['rbit']], 0.05) or isEqual(link[1 + r['rbit']][-1], r['p'][r['rbit']], 0.05):
                  if isEqual(link[1 + r['rbit']][0], qEnd, 0.05) or isEqual(link[1 + r['rbit']][-1], qEnd, 0.05):
                    rasterLink = link[1 + r['rbit']]
          
          if rasterLink != None:
            if isEqual(rasterLink[0], qEnd, 0.05) and isEqual(rasterLink[-1], r['p'][r['rbit']], 0.05):
              rasterLink[0] = qEnd
              rasterLink[-1] = r['p'][r['rbit']]
                
            elif isEqual(rasterLink[-1], qEnd, 0.05) and isEqual(rasterLink[0], r['p'][r['rbit']], 0.05):
              rasterLink[0] = r['p'][r['rbit']]
              rasterLink[-1] = qEnd
              rasterLink.reverse()

            for index in range(len(rasterLink)-1):
              p = rasterLink[index]
              q = rasterLink[index+1]
              move_path_rev.append(move.make(p, q, mp_trace))

          else:
            move_path_rev.append(move.make(qEnd, r['p'][r['rbit']], mp_jump))
        
        move_path_rev.append(r['move'])
        qEnd = r['p'][1-r['rbit']]

    if len(move_path) > 0:
      if len(move_path) == 2:
        ph = path.from_moves(move_path)
        bc = block.from_paths([ph, path.rev(ph)])
      else:
        ph = path.from_moves(move_path)
        ph_rev = path.from_moves(move_path_rev)
        bc = block.from_paths([ph, path.rev(ph), ph_rev, path.rev(ph_rev)])
      
      block.set_used(bc, False)
      BS.append(bc)
  
  for s in S:
    r0 = R[s[0]]
    r1 = R[s[1]]

    if r0['group'] != r1['group']:
      p0 = r0['p'][r0['rbit']]
      q0 = r0['p'][1-r0['rbit']]
      bc0 = None

      p1 = r1['p'][r1['rbit']]
      q1 = r1['p'][1-r1['rbit']]
      bc1 = None

      for bc in BS:
        ophk = block.choice(bc, 0)
        n = path.nelems(ophk)
        for k in range(n):
          omvk = path.elem(ophk, k)
          mvk, drk = move.unpack(omvk)
          if mvk.endpts == (p0, q0): 
            bc0 = [bc, k]
          if mvk.endpts == (p1, q1): 
            bc1 = [bc, k]

      if bc0 != None and bc1 != None:
        if bc0[1] > bc1[1]:
          ct = raster_raster_contact(bc0, bc1, parms)
        else:
          ct = raster_raster_contact(bc0, bc1, parms)
        CS.append(ct)
  
  return BS, CS, len(R)

################################
def create_folder(inputName, test_parms):
  outFolder = './tests/out/' + inputName + '__DELTA' + str(test_parms['Delta'])

  if test_parms['splitLines']:
    outFolder += "_split"

  if test_parms['limitLines']:
    outFolder += "_linesLimit#" + str(test_parms['nMaxLines'])

  if not os.path.exists(outFolder):
    os.makedirs(outFolder)
  
  outFolder += "/"

  if test_parms['debug']:
    if not os.path.exists(outFolder + '/debug'):
      os.makedirs(outFolder + '/debug')
    else:
      for filencalls in os.listdir(outFolder + '/debug'):
        if "ncall" in filencalls:
          os.remove(outFolder + '/debug/' + filencalls)
  elif os.path.exists(outFolder + '/debug'):
      for filencalls in os.listdir(outFolder + '/debug'):
        if "ncall" in filencalls:
          os.remove(outFolder + '/debug/' + filencalls)

  outFile = open(outFolder + "/" + inputName + ".txt", 'w')

  return outFolder, outFile

def print_input_details(name, lenBS, lenCS, lenR, o, test_parms, wr):
  wr.write("Name: %s\n" % name)
  wr.write("# lines: %d\n" % lenR)
  wr.write("# blocks: %d\n" % lenBS)
  wr.write("# contacts: %d\n" % lenCS)
  for d in test_parms:
    wr.write(d + ': ' + str(test_parms[d]) + "\n")
  wr.write('start_point: ' + str(o) + "\n")
  wr.write("----------------------------------------------------------------------\n")
  return

################################
parms = job_parms.typical()
test_parms = {
    'Delta': float(sys.argv[2]),
    'maxcalls': int(sys.argv[3]),
    'use_rasterlinks': True,
    'splitLines': True,
    'limitLines': True,
    'nMaxLines': int(sys.argv[4]),
    'debug': False
  }

if sys.argv[5] == 'True':
  test_parms['debug'] = True
inputName = sys.argv[1]

outFolder, outFile = create_folder(inputName, test_parms)
sys.stdout = outFile

wdf = parms['solid_raster_width']

o = (0,0)
BS, CS, lenR = create_input(inputName, wdf, test_parms)
print_input_details(inputName, len(BS), len(CS), lenR, o, test_parms, sys.stdout)
job_parms.write(sys.stdout, None, parms, None)
sys.stdout.write("----------------------------------------------------------------------\n")

c_box = input_data.plot(outFolder + inputName, BS, CS, o, wdf)  
c_box.append(outFolder + '/debug/' + inputName + '_ncall')
os.remove(outFolder + inputName + '_ip00.eps')
os.remove(outFolder + inputName + '_ip01.eps')
if os.path.exists(outFolder + inputName + '_ip02.eps'):
  os.remove(outFolder + inputName + '_ip02.eps')
  os.remove(outFolder + inputName + '_ip03.eps')

start_time = time.time()
Q = hotpath.best_path(o, BS, CS, test_parms['Delta'], test_parms['maxcalls'], parms, test_parms, c_box, sys.stdout)
end_time = time.time()

sys.stdout.write("----------------------------------------------------------------------\n")
sys.stdout.write('TIME: ' + str(end_time - start_time))

if Q != None:
  plot_solution(Q, outFolder + inputName + "_sol", c_box) 
  os.remove( outFolder + inputName + "_sol" + '.eps')

outFile.close()