import os
import sys

import math as m

import pandas as pd

from pyx import *
from PIL import Image, ImageDraw, ImageFont

colors = [
      color.rgb(255 / 255.0, 0   / 255.0, 102 / 255.0),
      color.rgb(0   / 255.0, 102 / 255.0, 255 / 255.0),
      color.rgb(51  / 255.0, 204 / 255.0, 0   / 255.0),
      color.rgb(179 / 255.0, 0   / 255.0, 179 / 255.0),
      color.rgb(255 / 255.0, 100 / 255.0, 0   / 255.0), 
      color.rgb(84  / 255.0, 113 / 255.0, 152 / 255.0), 
      color.rgb(228 / 255.0, 0   / 255.0, 0   / 255.0), 
      color.rgb(117 / 255.0, 178 / 255.0, 112 / 255.0), 
      color.rgb(197 / 255.0, 61  / 255.0, 61  / 255.0),
      color.rgb(0   / 255.0, 194 / 255.0, 186 / 255.0), 
      color.rgb(248 / 255.0, 194 / 255.0, 96  / 255.0), 
      color.rgb(0   / 255.0, 135 / 255.0, 68  / 255.0), 
      color.rgb(248 / 255.0, 84  / 255.0, 121 / 255.0),
      color.rgb(0   / 255.0, 140 / 255.0, 166 / 255.0), 
      color.rgb(244 / 255.0, 127 / 255.0, 123 / 255.0), 
      color.rgb(157 / 255.0, 214 / 255.0, 78  / 255.0), 
      color.rgb(150 / 255.0, 95  / 255.0, 173 / 255.0),
      color.rgb(54  / 255.0, 171 / 255.0, 181 / 255.0), 
      color.rgb(254 / 255.0, 94  / 255.0, 81  / 255.0), 
      color.rgb(92  / 255.0, 184 / 255.0, 92  / 255.0), 
      color.rgb(158 / 255.0, 61  / 255.0, 100 / 255.0),
      color.rgb(0   / 255.0, 174 / 255.0, 219 / 255.0), 
      color.rgb(190 / 255.0, 41  / 255.0, 236 / 255.0), 
      color.rgb(0   / 255.0, 177 / 255.0, 89  / 255.0)
   ]

COLUMNS_NAMES = [
    'NOME',
    'NUMERO DE BLOCOS',
    'TEMPO DE EXTRUSAO',
    'TEMPO DE REPOSICIONAMENTO',
    'TEMPO TOTAL',
    'MAIOR RESFRIAMENTO'
   ]

def isEqual (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

def rotate_file (filename, R):
    if '90' in filename:
        for r in R:
            p = r['p']
            q = r['q']

            x = rotate_x(p[0], p[1], 1.5708)
            y = rotate_y(p[0], p[1], 1.5708)

            r['p'] = (x, y)
            
            x = rotate_x(q[0], q[1], 1.5708)
            y = rotate_y(q[0], q[1], 1.5708)
            
            r['q'] = (x, y)
        
def accelerationDistance(speed, acceleration):
    return (speed*speed)/(2*acceleration)

def accelerationTime(speed, acceleration):
    return speed/acceleration

def calculate_distance (p, q):
    if p == None or q == None:
        return 0
        
    dx = p[0] - q[0]
    dy = p[1] - q[1]
    return m.sqrt (dx*dx + dy*dy)

def extrude_time (distance, parameters):
    maxSpeedDistance = distance - 2*parameters['filling_dist']
    
    if maxSpeedDistance > 0:
        return 2*parameters['filling_time'] + maxSpeedDistance/parameters['filling_speed']

    else:
        return m.sqrt(distance/parameters['acceleration'])

def air_time (distance, parameters):
    maxSpeedDistance = distance - 2*parameters['travel_dist']
    
    if maxSpeedDistance > 0:
        return 2*parameters['travel_time'] + maxSpeedDistance/parameters['travel_speed'] + 2 * parameters['retraction']

    else:
        return m.sqrt(distance/parameters['acceleration']) + 2 * parameters['retraction']

def extrude_time_passage (p, q, M, parameters):
    dx = p[0] - q[0]
    dy = p[1] - q[1]
    d = m.sqrt (dx*dx + dy*dy)

    gx = p[0] - M[0]
    gy = p[1] - M[1]
    g = m.sqrt (gx*gx + gy*gy)
    
    if d >= 2 * parameters['filling_dist']:
        if g >= parameters['filling_dist'] and g <= d - parameters['filling_dist']:  
           return parameters['filling_time'] + (g - parameters['filling_dist'])/parameters['filling_speed']

        else:   
           return m.sqrt((calculate_distance(p, M))/parameters['acceleration'])

    else:   
        return m.sqrt((calculate_distance(p, M))/parameters['acceleration'])

def calculate_time_raster(R, startPoint):
    e = 0
    a = 0
    t = 0
    g = 0

    rPrev = dict()
    rPrev['q'] = startPoint
    rPrev['group'] = -1
    aPrev = 0

    for r in R:
        e += r['e']

        if rPrev != None:
            if rPrev['group'] != r['group']:
                aPrev = air_time(calculate_distance(rPrev['q'], r['p']), parameters)
                a += aPrev
            else:
                aPrev = 0
        
        r['Tini'] = t + aPrev
        r['Tend'] = r['Tini'] + r['e']
        t = r['Tend']
        
        rPrev = r

        if r['group'] > g: # Encontra o número de blocos.
            g = r['group']

    return e, a, t, g+1

def calculate_worst_cooling(R):
    worst = 0
    r1 = None
    r2 = None

    for raster1 in R:
        for side1 in raster1['sides']:
            raster2 = R[side1['id']]
            side2 = R[side1['id']]['sides'][side1['sid']]
            cooling = (raster2['Tini'] + side2['tm']) - (raster1['Tini'] + side1['tm'])
                
            if cooling > worst:
                worst = cooling
                r1 = raster1
                r2 = raster2
    return worst, r1, r2

def init_config():
    parameters = dict()
    parameters['acceleration'] = 3000
    parameters['filling_speed'] = 40
    parameters['filling_dist'] = accelerationDistance(parameters['filling_speed'], parameters['acceleration'])
    parameters['filling_time'] = accelerationTime(parameters['filling_speed'], parameters['acceleration'])
    parameters['travel_speed'] = 100
    parameters['travel_dist'] = accelerationDistance(parameters['travel_speed'], parameters['acceleration'])
    parameters['travel_time'] = accelerationTime(parameters['travel_speed'], parameters['acceleration'])
    parameters['width'] = 0.4 # + 0 # width + gap
    parameters['retraction'] = 2/40 

    parameters['sides'] = True
    parameters['max_cool'] = True
    parameters['png'] = True

    return parameters

def read_gcode(filename):
    R = list()
    
    listPoints = list()
    newList = False
    isRaster = False
    startPoint = (0,0)

    with open(filename, 'r', encoding="ISO-8859-1") as f:
        for line in f:
            if '(Raster ' in line:
                isRaster = True
                if 'Raster 0)' not in line: 
                    newList = True 

            elif isRaster and line[0:3] == 'G1 ' and 'X' in line and 'Y' in line:
                if newList == True:
                    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',''))
                
                if startPoint != None:
                    listPoints.append((x,y))
                else:
                    startPoint = (x, y)
   
    R.append(listPoints)
    return R, startPoint

def create_lines(R):
    size = 0

    for indexRaster in R:
        size = size + len(indexRaster) - 1
    S = [None]*size

    size = 0

    for indexRaster in range(len(R)):
        p = None
        for indexPoint in range(len(R[indexRaster])):
            q = R[indexRaster][indexPoint]
            
            if p != None:
                isRaster = False
                if isEqual(p[1], q[1], 0.01):
                    isRaster = True
                S[size] = {
                    'id': size,
                    'p':  p,
                    'q': q,
                    'e': extrude_time(calculate_distance(p, q), parameters),
                    'isRaster': isRaster,
                    'group': indexRaster, # ID of the group the stroke belongs to.
                    'Tini': None, # Time when the stroke started to be extruded.
                    'Tend': None, # Time when the stroke finished being extruded.
                    'sides': list()
                }
                size = size + 1
            p = q

    return S

def find_sides(S, parameters):
    for indexRaster1 in range(len(S)):
        if S[indexRaster1]['isRaster']:
            for indexRaster2 in range(indexRaster1 + 1, len(S)):
                if S[indexRaster2]['isRaster']:
                    isSide = False
                    raster1 = S[indexRaster1]
                    raster2 = S[indexRaster2]
                    
                    y1 = raster1['p'][1]
                    y2 = raster2['p'][1]

                    if isEqual(y1, y2 + parameters['width'], 0.05) or isEqual(y2, y1 + parameters['width'], 0.05):
                        isSide = True
                        p1 = raster1['p']
                        q1 = raster1['q']
                        p2 = raster2['p']
                        q2 = raster2['q']
                    else:
                        isSide = False

                    if isSide:
                        if p1[0] > p2[0] and p1[0] < q2[0]:
                            isSide = True
                        elif p1[0] > q2[0] and p1[0] < p2[0]:
                            isSide = True
                        elif q1[0] > p2[0] and q1[0] < q2[0]:
                            isSide = True
                        elif q1[0] > q2[0] and q1[0] < p2[0]:
                            isSide = True
                        elif p2[0] > p1[0] and p2[0] < q1[0]:
                            isSide = True
                        elif p2[0] > q1[0] and p2[0] < p1[0]:
                            isSide = True
                        elif q2[0] > p1[0] and q2[0] < q1[0]:
                            isSide = True
                        elif q2[0] > q1[0] and q2[0] < p1[0]:
                            isSide = True
                        elif isEqual(p1[0], p2[0], 0.05):
                            isSide = True
                        elif isEqual(p1[0], q2[0], 0.05):
                            isSide = True
                        elif isEqual(q1[0], p2[0], 0.05):
                            isSide = True
                        elif isEqual(q1[0], q2[0], 0.05):
                            isSide = True
                        else:
                            isSide = False

                    if isSide:
                        x1_min = min(p1[0], q1[0])
                        x1_max = max(p1[0], q1[0])

                        x2_min = min(p2[0], q2[0])
                        x2_max = max(p2[0], q2[0])

                        x0 = max(x1_min, x2_min)
                        x1 = min(x1_max, x2_max)

                        xm = (x0 + x1)/2

                        side1 = {
                            'sid': len(raster2['sides']),
                            'id': raster2['id'],
                            'xm': xm,
                            'ym': (y1 + y2)/2,
                            'xl': abs(x0 - x1),
                            'tm': extrude_time_passage(p1, q1, (xm, y1), parameters)
                        }

                        side2 = {
                            'sid': len(raster1['sides']),
                            'id': raster1['id'],
                            'xm': xm,
                            'ym': (y1 + y2)/2,
                            'xl': abs(x0 - x1),
                            'tm': extrude_time_passage (p2, q2, (xm, y2), parameters)
                        }

                        S[indexRaster1]['sides'].append(side1)
                        S[indexRaster2]['sides'].append(side2)

    return

def plot_slice (R, r1, r2, startPoint, filename):
    c = canvas.canvas()
    style.linewidth(0.1 * unit.w_cm)
    line_width = 0.08 * unit.w_cm

    #rotate_file (filename, R)

    airJumps = plot_raster(startPoint, R, r1, r2, c, line_width)

    plor_airJumps(airJumps, c, line_width)

    c.fill(path.circle(startPoint[0], startPoint[1], 0.2), [color.rgb.red])
    c.fill(path.circle(R[0]['p'][0], R[0]['p'][1], 0.2), [color.rgb.black])

    c.writeEPSfile(filename.replace('.gcode','') + '.eps')
    im = Image.open(filename.replace('.gcode','') +  '.eps')
    fig = im.convert('RGBA')
    fig.save(filename.replace('.gcode','') + '.png', lossless = True)
    im.close()
    os.remove(filename.replace('.gcode','') +  '.eps')
    return

def plot_raster (startPoint, R, r1, r2, c, line_width):
    airJumps = list()
    airJumps.append((startPoint, R[0]['p']))

    rPrev = None
    check = False

    for r in R:
        p = r['p']
        q = r['q']
        
        if (r['id'] == r1 or r['id'] == r2) and parameters['max_cool']:
            c.stroke(path.line(p[0], p[1], q[0], q[1]), [style.linewidth(2 * line_width), style.linecap.round, color.rgb.red])
            check = not check
        elif check:
            c.stroke(path.line(p[0], p[1], q[0], q[1]), [style.linewidth(2 * line_width), style.linecap.round, color.rgb.black])
        else:
            c.stroke(path.line(p[0], p[1], q[0], q[1]), [style.linewidth(2 * line_width), style.linecap.round, colors[r['group'] % len(colors)]])

        if rPrev != None:
            if rPrev['group'] != r['group']:
                airJumps.append((rPrev['q'], p))

        if parameters['sides']:
            plot_sides(r['id'], r['sides'], c, line_width) 
        
        rPrev = r

    return airJumps

def plot_sides (sid, sides, c, line_width):
    for s in sides:
        xm = s['xm']
        ym = s['ym']
        xl = s['xl']
        x0 = xm - xl/2
        x1 = xm + xl/2
        if sid < s['id']:
            c.stroke(path.line(x0, ym, x1, ym), [style.linewidth(0.1 * unit.w_cm), color.rgb.black])
    return

def plor_airJumps(airJumps, c, line_width):
    for jump in airJumps:
        p = jump[0]
        q = jump[1]
        c.stroke(path.line(p[0], p[1], q[0], q[1]), [style.linewidth(1 * line_width), style.linestyle.dashed, color.rgb.black])
    return

############################# 
parameters = init_config()
summary = list()

if parameters['png']:
    if not os.path.exists("./PNG"):
        os.makedirs("./PNG")

if not os.path.exists("./CSV"):
    os.makedirs("./CSV")

for folderResults in os.listdir("./"):
    if not '.py' in folderResults and not 'CSV' in folderResults and not 'PNG' in folderResults:
        for file in os.listdir("./" + folderResults + '/'):
            if parameters['png']:
                if not os.path.exists("./PNG/" + folderResults):
                    os.makedirs("./PNG/" + folderResults)

            if file.endswith(".gcode"):
                inputFile = "./" + folderResults + '/' + file            
                outputFile = "./PNG/" + folderResults + '/' + file
                
                rasterPoints, startPoint = read_gcode (inputFile)
                R = create_lines(rasterPoints)
                find_sides(R, parameters)

                e, a, t, g = calculate_time_raster(R, startPoint)
                w, r1, r2 = calculate_worst_cooling(R)
                
                summary.append([file.replace('.gcode', ''), str(g).replace('.', ','), str(e).replace('.', ','), str(a).replace('.', ','), str(t).replace('.', ','), str(w).replace('.', ',')])

                plot_slice (R, r1['id'], r2['id'], startPoint, outputFile)
        
        df = pd.DataFrame(summary, columns = COLUMNS_NAMES)
        df.to_csv ("./CSV/" + folderResults + '.csv', index = False, header = True, sep = ';')