"""
Motion Test Script

Script designed to generate GCODE for a given 3D path

Fluvio L Lobo Fenoglietto
"""

import  numpy   as  np

def gcode_gen( out_name, x, y, z, speed ):
    """
    Given a 3D path described by its coordinates in x, y, and z,
    the function generates a GCODE

    10/05/2018 - Added the speed input variable (same for all axes movement)
    """

    # Data-specific Variables and Edits ======================================= #
    array_len = len(x)                                                          # len(y) or len(z)
    
    printer_offset = [150.00, 150.00, 75.00]                                    # offset to center of printer
    x = x + printer_offset[0]
    y = y + printer_offset[1]
    z = z + printer_offset[2]
    
    x = x.round( decimals=2 )                                                   # truncate float to 2 decimals            
    y = y.round( decimals=2 )
    z = z.round( decimals=2 )
    
    
    ext = ".gcode"
    file_name = out_name + ext
    file = open( file_name, "w" )
    
    for i in range(0, array_len):                                               # write all the data from the data arrays

        if i == 0:

            # Terminal Debug ================================================== #
            print( '; G-Code generated by gcode_gen version 1.0' )
            print( '; Home axes...' )
            print( 'G28 ;' )                                                    # gcode command for homing all axes
            print( '; Initializing...' )
            print( ';  Applying Z Offset...' )
            print( 'G91 ; Relative position...' )                               # set relative position
            print( 'G1 Z50 ; Add Z Offset to avoid electronics...' )            # apply Z offset to avoid electronics
            print( '; Starting Path...\n' )
            print( 'G90 ; Absolute position...\n' )                             # set absolute position
            print( 'G1 X{} Y{} Z{} F{}'.format( x[i],y[i],z[i],speed ))
            # ================================================================= #

            # GCODE Generator ================================================= #
            file.write( '; G-Code generated by gcode_gen version 1.0\n' )
            file.write( '; Home axes...\n' )
            file.write( 'G28 ;\n' )                                             # gcode command for homing all axes
            file.write( '; Initializing...\n' )
            file.write( ';  Applying Z Offset...\n' )
            file.write( 'G91 ; Relative position...\n' )                        # set relative position
            file.write( 'G1 Z50 ; Add Z Offset to avoid electronics...\n' )     # apply Z offset to avoid electronics
            file.write( '; Starting Path...\n' )
            file.write( 'G90 ; Absolute position...\n' )                        # set absolute position
            file.write( 'G1 X{} Y{} Z{} F{} \n'.format( x[i],y[i],z[i],speed ))          # write to .gcode file...
            # ================================================================= #

        elif i > 0:

            # Terminal Debug ================================================== #
            print( 'G1 X{} Y{} Z{} F{}'.format( x[i],y[i],z[i],speed ))
            # ================================================================= #

            # GCODE Generator ================================================= #

            file.write( 'G1 X{} Y{} Z{} F{} \n'.format( x[i],y[i],z[i],speed ))           # write to .gcode file...
            # ================================================================= #

    file.close()

def gcode_gen_2( out_name, x, y, z, printer_offset, speed ):
    """
    gcode_gen_2
    Newest version of the gcode generator function

    Features:
    10/12/2018 - Converted printer offsets as an input to the function
    """

    # Data-specific Variables and Edits ======================================= #
    array_len = len(x)                                                          # len(y) or len(z)
    
    x = x + printer_offset[0]                                                   # offset to center of printer
    y = y + printer_offset[1]
    z = z + printer_offset[2]
    
    x = x.round( decimals=2 )                                                   # truncate float to 2 decimals            
    y = y.round( decimals=2 )
    z = z.round( decimals=2 )
    
    
    ext = ".gcode"
    file_name = out_name + ext
    file = open( file_name, "w" )
    
    for i in range(0, array_len):                                               # write all the data from the data arrays

        if i == 0:

            # Terminal Debug ================================================== #
            print( '; G-Code generated by gcode_gen version 1.0' )
            print( '; Home axes...' )
            print( 'G28 ;' )                                                    # gcode command for homing all axes
            print( '; Initializing...' )
            print( ';  Applying Z Offset...' )
            print( 'G91 ; Relative position...' )                               # set relative position
            print( 'G1 Z50 ; Add Z Offset to avoid electronics...' )            # apply Z offset to avoid electronics
            print( '; Starting Path...\n' )
            print( 'G90 ; Absolute position...\n' )                             # set absolute position
            print( 'G1 X{} Y{} Z{} F{}'.format( x[i],y[i],z[i],speed ))
            # ================================================================= #

            # GCODE Generator ================================================= #
            file.write( '; G-Code generated by gcode_gen version 1.0\n' )
            file.write( '; Home axes...\n' )
            file.write( 'G28 ;\n' )                                             # gcode command for homing all axes
            file.write( '; Initializing...\n' )
            file.write( ';  Applying Z Offset...\n' )
            file.write( 'G91 ; Relative position...\n' )                        # set relative position
            file.write( 'G1 Z50 ; Add Z Offset to avoid electronics...\n' )     # apply Z offset to avoid electronics
            file.write( '; Starting Path...\n' )
            file.write( 'G90 ; Absolute position...\n' )                        # set absolute position
            file.write( 'G1 X{} Y{} Z{} F{} \n'.format( x[i],y[i],z[i],speed ))          # write to .gcode file...
            # ================================================================= #

        elif i > 0:

            # Terminal Debug ================================================== #
            print( 'G1 X{} Y{} Z{} F{}'.format( x[i],y[i],z[i],speed ))
            # ================================================================= #

            # GCODE Generator ================================================= #

            file.write( 'G1 X{} Y{} Z{} F{} \n'.format( x[i],y[i],z[i],speed ))           # write to .gcode file...
            # ================================================================= #

    file.close()

def gcode_gen_cwalk( out_name, x, y, z, printer, speed, interval, mode ):
    """
    gcode_gen_rw
    Gcode Generator Function specific to a Random Walk, featuring;
        intervals   --> time intervals (in milliseconds) between motion
        mode        --> mode of operation;
                        --> mode = 0 --> normal mode
                        --> mode = 1 --> steppers are disable and re-enabled between intervals

    Features:
    10/12/2018 - Converted printer offsets as an input to the function
    """

    # Data-specific Variables and Edits ======================================= #
    array_len = len(x)                                                          # len(y) or len(z)
    
##    x = x + printer_offset[0]                                                   # offset to center of printer
##    y = y + printer_offset[1]
##    z = z + printer_offset[2]

    x0 = printer[0].round( decimals=2 )
    y0 = printer[1].round( decimals=2 )
    z0 = printer[2].round( decimals=2 )
    
    x = x.round( decimals=2 )                                                   # truncate float to 2 decimals            
    y = y.round( decimals=2 )
    z = z.round( decimals=2 )
    
    
    ext = ".gcode"
    file_name = out_name + ext
    file = open( file_name, "w" )
    
    for i in range(0, array_len):                                               # write all the data from the data arrays

        if i == 0:

            # Terminal Debug ================================================== #
            print( '; G-Code generated by gcode_gen version 1.0' )
            print( '; Home axes...' )
            print( 'G28 ;' )                                                    # gcode command for homing all axes
            print( '; Initializing...' )
            print( '; Moving to center...' )
            print( 'G90 ; Relative position...' )
            print( 'G1 X{} Y{} Z{} F{}'.format( x0,y0,z0,speed ))               # translation to the center of the printer
            print( '; Starting Path...\n' )
            print( 'G91 ; Relative position...\n' )                             # set absolute position
            print( 'G1 X{} Y{} Z{} F{}'.format( x[i],y[i],z[i],speed ))

            if ( mode == 0 ):
                print( 'G4 P{}'.format( interval ))                             # added interval or wait
            elif ( mode == 1 ):
                print( 'M18' )                                                  # disable motors
                print( 'G4 P{}'.format( interval ))                             # added interval or wait
                print( 'M17' )                                                  # enable motors
            
            # ================================================================= #

            # GCODE Generator ================================================= #
            file.write( '; G-Code generated by gcode_gen version 1.0\n' )
            file.write( '; Home axes...\n' )
            file.write( 'G28 ;\n' )                                             # gcode command for homing all axes
            file.write( '; Initializing...\n' )
            file.write( '; Moving to center...\n' )
            file.write( 'G90 ; Absolute position...\n' )
            file.write( 'G1 X{} Y{} Z{} F{} \n'.format( x0,y0,z0,speed ))       # translation to the center of the printer
            file.write( '; Starting Path...\n' )
            file.write( 'G91 ; Relative position...\n' )                        # set absolute position
            file.write( 'G1 X{} Y{} Z{} F{} \n'.format( x[i],y[i],z[i],speed )) # write to .gcode file...

            if ( mode == 0 ):
                file.write( 'G4 P{} \n'.format( interval ))                     # added interval or wait
            elif ( mode == 1 ):
                file.write( 'M18 \n' )                                          # disable motors
                file.write( 'G4 P{} \n'.format( interval ))                     # added interval or wait
                file.write( 'M17 \n' )                                          # enable motors
            # ================================================================= #

        elif i > 0:

            # Terminal Debug ================================================== #
            print( 'G1 X{} Y{} Z{} F{}'.format( x[i],y[i],z[i],speed ))
            if ( mode == 0 ):
                print( 'G4 P{}'.format( interval ))                             # added interval or wait
            elif ( mode == 1 ):
                print( 'M18' )                                                  # disable motors
                print( 'G4 P{}'.format( interval ))                             # added interval or wait
                print( 'M17' )                                                  # enable motors
            # ================================================================= #

            # GCODE Generator ================================================= #

            file.write( 'G1 X{} Y{} Z{} F{} \n'.format( x[i],y[i],z[i],speed )) # write to .gcode file...
            if ( mode == 0 ):
                file.write( 'G4 P{} \n'.format( interval ))                     # added interval or wait
            elif ( mode == 1 ):
                file.write( 'M18 \n' )                                          # disable motors
                file.write( 'G4 P{} \n'.format( interval ))                     # added interval or wait
                file.write( 'M17 \n' )                                          # enable motors
            # ================================================================= #

    file.close()

"""
References:
- https://www.simplify3d.com/support/articles/3d-printing-gcode-tutorial/
"""
