# Copyright 2015 - RoboDK Inc. - https://robodk.com/
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

# ----------------------------------------------------
# This file is a POST PROCESSOR for Robot Offline Programming to generate programs
# for KEBA's KAIRO controllers using RoboDK
#
# To edit/test this POST PROCESSOR script file:
# Select "Program"->"Add/Edit Post Processor", then select your post or create a new one.
# You can edit this file using any text editor or Python editor. Using a Python editor allows to quickly evaluate a sample program at the end of this file.
# Python should be automatically installed with RoboDK
#
# You can also edit the POST PROCESSOR manually:
#    1- Open the *.py file with Python IDLE (right click -> Edit with IDLE)
#    2- Make the necessary changes
#    3- Run the file to open Python Shell: Run -> Run module (F5 by default)
#    4- The "test_post()" function is called automatically
# Alternatively, you can edit this file using a text editor and run it with Python
#
# To use a POST PROCESSOR file you must place the *.py file in "C:/RoboDK/Posts/"
# To select one POST PROCESSOR for your robot in RoboDK you must follow these steps:
#    1- Open the robot panel (double click a robot)
#    2- Select "Parameters"
#    3- Select "Unlock advanced options"
#    4- Select your post as the file name in the "Robot brand" box
#
# To delete an existing POST PROCESSOR script, simply delete this file (.py file)
#
# ----------------------------------------------------
# More information about RoboDK Post Processors and Offline Programming here:
#     https://robodk.com/help#PostProcessor
#     https://robodk.com/doc/en/PythonAPI/postprocessor.html
# ----------------------------------------------------

# ----------------------------------------------------
# Description
# The KEBA KAIRO post processor generates .tip files tailored for KEBA KAIRO instruction set.
#
#
# Supported Controllers
#
# ----------------------------------------------------
# Special command for RoboDK before post processing: Make sure the export target names setting is enabled (creates the _TargetName variable for each movement)
RDK_COMMANDS = {"ProgMoveExtAxesPoses": "1","ProgMoveNames": "1"}


# ----------------------------------------------------
# Import RoboDK tools
from robodk import *
#Custom ftp uppload as robot requires sftp and making a folder

def UploadFileFTP(file_path_name, server_ip, remote_path, usernameIn, passwordIn,fIsMainProg):
    """Upload a file to a robot through FTP"""
    filepath = getFileDir(file_path_name)
    filename = getBaseName(file_path_name)
    if '/' in server_ip:
        server_ip = server_ip.split('/')[0]
    import robolink
    import warnings
    warnings.filterwarnings('ignore')
    robolink.import_install('pysftp')
    import pysftp
    import os
    import sys
    print("POPUP: <p>Connecting to <strong>%s</strong> using user name <strong>%s</strong> and password ****</p><p>Please wait...</p>" % (server_ip, usernameIn))
    sys.stdout.flush()
    try:
        cnoptsIn = pysftp.CnOpts()
        cnoptsIn.hostkeys = None
        os.chdir(os.path.dirname(os.path.abspath(__file__)))
        if os.path.isfile('../api/service_key.pem'):
            pemFilePath = '../api/service_key.pem'
            myFTP = pysftp.Connection(server_ip,username='service',private_key=pemFilePath,cnopts=cnoptsIn)
        else:
            myFTP = pysftp.Connection(server_ip,username=usernameIn,password=passwordIn,cnopts=cnoptsIn)
    except:
        error_str = sys.exc_info()[1]
        print("POPUP: <font color=\"red\">Connection to %s failed: <p>%s</p></font>" % (server_ip,error_str))
        sys.stdout.flush()
        pause(4)
        return False

    if fIsMainProg == True:
        remote_path = remote_path + '/' + os.path.splitext(filename)[0] + '.tt'
        remote_path_prog = remote_path + '/' + filename
        print("POPUP: Connected. Deleting remote file %s..." % remote_path_prog)
        sys.stdout.flush()
        print("POPUP: Connected. Uploading program to %s..." % server_ip)
        sys.stdout.flush()
        if myFTP.exists(remote_path) is False:
            myFTP.mkdir(remote_path)

    try:
        myFTP.cwd(remote_path)
    except:
        error_str = sys.exc_info()[1]
        print("POPUP: <font color=\"red\">Remote path not found or can't be created: %s</font>" % (remote_path))
        sys.stdout.flush()
        pause(4)
        contin = mbox("Remote path\n%s\nnot found or can't create folder.\n\nChange path and permissions and retry." % remote_path)
        return False
        
    def uploadThis(localfile, filename):
        print('  Sending file: %s' % localfile)
        print("POPUP: Sending file: %s" % filename)
        sys.stdout.flush()
        fh = open(localfile, 'rb')
        myFTP.put(localfile)
        fh.close()

    uploadThis(file_path_name, filename)
    myFTP.close()
    print("POPUP: File trasfer completed: <font color=\"blue\">%s</font>" % remote_path)
    sys.stdout.flush()
    return True

def UploadFTP(program, robot_ip, remote_path, ftp_user, ftp_pass, pause_sec = 2):
    """Upload a program or a list of programs to the robot through FTP provided the connection parameters"""
    # Iterate through program list if it is a list of files
    import os
    if isinstance(program, list):
        if len(program) == 0:
            print('POPUP: Nothing to transfer')
            sys.stdout.flush()
            pause(pause_sec)
            return
        
        fIsMainProg = True
        mainProgRemotePath = ""
        for prog in program:
            import os
            if os.path.isfile(prog):
                print('Sending program file %s...' % prog)
                if fIsMainProg == True:
                    UploadFileFTP(prog, robot_ip, remote_path, ftp_user, ftp_pass,fIsMainProg)
                    filename = getBaseName(prog)
                    remote_path = remote_path + '/' + os.path.splitext(filename)[0] + '.tt/'
                    fIsMainProg = False
                else:
                    UploadFileFTP(prog, robot_ip, remote_path, ftp_user, ftp_pass,fIsMainProg)
            else:
                print('Sending program folder %s...' % prog)
                UploadDirFTP(prog, robot_ip, remote_path, ftp_user, ftp_pass)  
                UploadFTP(prog, robot_ip, remote_path, ftp_user, ftp_pass, 0)  
                
        
        print("POPUP: <font color=\"blue\">Done: %i files and folders successfully transferred</font>" % len(program))
        sys.stdout.flush()
        pause(pause_sec)
        print("POPUP: Done")
        sys.stdout.flush()
        return

    
    pause(pause_sec)
    print("POPUP: Done")
    sys.stdout.flush()

# ----------------------------------------------------
def getExternalAxes(joints,jointFlags):
    result = []
    for i in range(len(joints)):
        if (jointFlags[i] != 'R'):
            result.append(joints[i])
    return result

def CARTPOS_2_str(pose,externalAxes):
    """Prints a pose target"""
    [x,y,z,r,p,w] = Pose_2_Adept(pose)
    data = ['aux1','aux2','aux3','aux4','aux5','aux6','aux7','aux8']
    result = ('x := %.3f, y := %.3f, z := %.3f, a := %.3f, b := %.3f, c := %.3f, ' % (x,y,z,r,p,w))
    for i in range(len(externalAxes)):
        result += "%s := %.4f, " % (data[i],externalAxes[i]) 

    result = result[:-2]
    return result


def RefFrame_2_str(pose):
    """Prints a pose target"""
    [x,y,z,r,p,w] = Pose_2_Adept(pose)
    return ('baseRefSys := MAP(World), x := %.3f, y := %.3f, z := %.3f, a := %.3f, b := %.3f, c := %.3f' % (x,y,z,r,p,w))

def tool_2_str(pose):
    """Prints a pose target"""
    [x,y,z,r,p,w] = Pose_2_KUKA(pose)
    return ('x := %.3f, y := %.3f, z := %.3f, a := %.3f, b := %.3f, c:= %.3f, guard := (vector := ((active := TRUE, type := BEGINTRANSFORMATION), (), (), (), (), (), (), (), (), (), (), (), (), (), (), (), (), (), (), (), (), (), (), ()))' % (x,y,z,r,p,w))
    
def angles_2_str(angles,AXES_TYPE):
    """Prints a joint target"""
    str = ''
    data = ['a1','a2','a3','a4','a5','a6','a7','a8','aux1','aux2','aux3','aux4','aux5','aux6','aux7','aux8']
    externalCount = 0
    if 'T' in AXES_TYPE:
        externalAxesPositions = []
        for i in range(8):
            if (i) >= len(angles):
                break
            elif AXES_TYPE[i] == 'R':
                str = str + ('%s := %.4f, ' % (data[i], angles[i]))
            else:
                externalAxesPositions.append(angles[i])
        for i in range(len(externalAxesPositions)):
            str = str + ('%s := %.4f, ' % (data[i+8], externalAxesPositions[i]))

    else:
        for i in range(len(angles)):
                str = str + ('%s := %.4f, ' % (data[i], angles[i]))

    str = str[:-2]
    return str
    
# FilterName is also available in robofileio since 2023-11-16
def FilterName(namefilter, safechar='P', reserved_names=None, max_len=16, space_to_underscore=False, invalid_chars=r' .-[]/\;,><&*:%=+@!#^|?^'):
    """
    Get a safe program or variable name that can be used for robot programming.
    Removes invalid characters ( .-[]/\;,><&*:%=+@!#^|?), remove non-english characters, etc.

    :param namefilter: The name to filter
    :type namefilter: str
    :param safechar: Safe character to start a name with, in case the first character is a digit. Defaults to 'P'
    :type safechar: str, optional
    :param reserved_names: List of reserved names. A number is appended at the end if it already exists. The new name is added to the list. Defaults to None
    :type reserved_names: list of str, optional
    :param max_len: Maximum length of the name (number of characters), -1 means no maximum. Defaults to -1
    :type max_len: int, optional
    :param space_to_underscore: Replace whitespaces with underscores
    :type space_to_underscore: bool, optional
    :param invalid_chars: string containing all invalid character to remove. Defaults to r' .-[]/\;,><&*:%=+@!#^|?^'
    :type invalid_chars: str, optional

    :return: The filtered name
    :rtype: str
    """
    if space_to_underscore and not '_' in invalid_chars:
        namefilter = namefilter.replace(' ', '_')

    # Remove non accepted characters
    for c in invalid_chars:
        namefilter = namefilter.replace(c, '')

    # Remove non english characters
    namefilter = ''.join((c for c in namefilter if 0 < ord(c) < 127))

    # Make sure we have a non empty string
    if len(namefilter) <= 0:
        namefilter = safechar

    # Make sure we don't start with a number
    if namefilter[0].isdigit():
        namefilter = safechar + namefilter

    # Some controllers have a limit of characters
    if max_len > 0:
        namefilter = namefilter[:max_len]

    # Make sure we are not using a reserved name
    if reserved_names:
        reserved_names_lower = [n.lower() for n in reserved_names]

        cnt = 1  # Count current instance as 1, subsequent is 2
        while namefilter.lower() in reserved_names_lower:
            if len(namefilter) == max_len:
                namefilter = namefilter[:-1]
            if cnt > 1:
                if max_len > 0:
                    namefilter = namefilter[:min(max_len - len(str(cnt)), len(namefilter))]
                namefilter = namefilter + "%i" % cnt
            cnt += 1

        # Add the name to reserved names
        if type(reserved_names) is list:
            reserved_names.append(namefilter)
        else:
            reserved_names.add(namefilter)

    return namefilter

# ----------------------------------------------------    
# Object class that handles the robot instructions/syntax
class RobotPost(object):
    """Robot post object"""
    PROG_EXT = 'tip'        # set the program extension
    PROG_EXT_VAR = 'tid'
    count_var_ptp = 0
    count_var_lin = 0
    
    # ----------------------------------------------------
    # other variables
    ROBOT_POST = ''
    ROBOT_NAME = ''
    PROG_FILES = []
    PROG = ''
    LOG = ''
    nAxes = 6
    do_array = []
    di_array = []


    #robotState
    REF_FRAME = eye()
    PoseTrackOffset = eye()
    PoseTurntableOffset = eye()
    _PoseTrack = None
    _PoseTurntable = None
    AXES_TYPE = ['R', 'R', 'R', 'R', 'R', 'R']



    def __init__(self, robotpost=None, robotname=None, robot_axes = 6, **kwargs):
        self.ROBOT_POST = robotpost
        self.ROBOT_NAME = robotname
        self.PROG = ''
        self.PROG_VAR = ''
        self.PROJECT_DATA = []

        self.varNames = []
        self.fHasRail = False

        self.LOG = ''
        self.nAxes = robot_axes
        for k,v in kwargs.items():
            if k == 'lines_x_prog':
                self.MAX_LINES_X_PROG = v  
            elif k == 'axes_type':
                self.AXES_TYPE = v  
            elif k == 'pose_turntable':
                self.PoseTurntableOffset = v
            elif k == 'pose_rail':
                self.fHasRail = True
                self.PoseTrackOffset = v   

        
    def ProgStart(self, progname):
        progname = FilterName(progname)
        self.addline('//KEBA')
        self.addline('//0.0HZ')
        self.addline('//100.0%')
        self.addline('//PROG NAME : %s' % progname)
        self.addline_var('//PROG NAME : %s' % progname)
        self.addline_var('EROP_OL : OVLABS := (posDist := 0, vConst := TRUE)')
        self.addline_var('EROP_D0 : DYNAMIC := (velAxis := 100, accAxis := 100, decAxis := 100, jerkAxis := 100, vel := 2000, acc := 8000, dec := 8000, jerk := 80000, velOri := 360, accOri := 720, decOri := 720, jerkOri := 7200)')

        
    def ProgFinish(self, progname):
        progname = FilterName(progname)
        self.addline('//ENDPROG')
        self.PROJECT_DATA.append([progname,self.PROG,self.PROG_VAR,self.varNames,self.REF_FRAME,self._PoseTrack,self._PoseTurntable])
        self.PROG = ''
        self.PROG_VAR = ''
        self.varNames = []

        
    def ProgSave(self, folder, progname, ask_user = False, show_result = False):
        progname = FilterName(progname)
        #Iterate through all programs
        for i in range(len(self.PROJECT_DATA)):
            progname = self.PROJECT_DATA[i][0]            
            #Write out current program
            prognametip = progname + '.' + self.PROG_EXT
            if ask_user or not DirExists(folder):
                filesave = getSaveFile(folder, prognametip, 'Save program as...')
                if filesave is not None:
                    filesave = filesave.name
                else:
                    return
            else:
                filesave = folder + '/' + prognametip
            fid = open(filesave, "w")
            fid.write(self.PROJECT_DATA[i][1])
            fid.close()
            print('SAVED: %s\n' % filesave) # tell RoboDK the path of the saved file
            self.PROG_FILES.append(filesave)
            
            # variables file:
            filesave_var = filesave[:-3] + self.PROG_EXT_VAR
            self.PROG_FILES.append(filesave_var)
            fid = open(filesave_var, "w")
            fid.write(self.PROJECT_DATA[i][2])
            fid.close()  
            
            
            # open file with default application
            if show_result:
                if type(show_result) is str:
                    # Open file with provided application
                    import subprocess
                    p = subprocess.Popen([show_result, filesave])
                    p = subprocess.Popen([show_result, filesave_var])
                    
                elif type(show_result) is list:
                    import subprocess
                    a=0
                #   p = subprocess.Popen(show_result + [filesave]) 
                #   p = subprocess.Popen([show_result, filesave_var])  
                else:
                    # open file with default application
                    import os
                    a=0
                # os.startfile(filesave)
                # os.startfile(filesave_var)
                if len(self.LOG) > 0:
                    mbox('Program generation LOG:\n\n' + self.LOG)
            
            #import subprocess
            #show_result = 'C:/RoboDK/Other/VSCodium/VSCodium.exe'
            #p = subprocess.Popen([show_result, filesave])
            #p = subprocess.Popen([show_result, filesave_var])
        
        
    def ProgSendRobot(self, robot_ip, remote_path, ftp_user, ftp_pass):
        """Send a program to the robot using the provided parameters. This method is executed right after ProgSave if we selected the option "Send Program to Robot".
        The connection parameters must be provided in the robot connection menu of RoboDK"""
        UploadFTP(self.PROG_FILES, robot_ip, remote_path, ftp_user, ftp_pass)
       
        
    def MoveJ(self, pose, joints, conf_RLF=None):
        """Add a joint movement"""
        varname = 'ap%i' % self.count_var_ptp
        if hasattr(self,"_TargetName") and self._TargetName is not None:
            varname = FilterName(self._TargetName)
            delattr(self,"_TargetName")

        self.addline('PTP(%s, EROP_D0, EROP_OL)' % varname)
        if varname not in self.varNames:
            self.varNames.append(varname)
            self.addline_var('%s : AXISPOS := (%s)' % (varname, angles_2_str(joints,self.AXES_TYPE)))
        self.count_var_ptp = self.count_var_ptp + 1
        
    def MoveL(self, pose, joints, conf_RLF=None):
        """Add a linear movement"""
        if pose is None:
            msg = "Linear movement using joint targets is not supported. Change the target type to cartesian or use a joint movement."
            self.addlog(msg)
            self.RunMessage(msg, True)
            return

        poseKeba = pose
        if self.fHasRail == True:        
            if self._PoseTrack is None:
                # We have turntable only
                poseKeba = self.PoseTurntableOffset * self._PoseTurntable * self.REF_FRAME * pose

            elif self._PoseTurntable is None:
                # we have a track only
                poseKeba = self.REF_FRAME.inv() * invH(self._PoseTrack * self.PoseTrackOffset) * transl(joints[-1],0,0) * self.REF_FRAME * pose
            else:
                # We have rail and turntable
                poseKeba = invH(self._PoseTrack * self.PoseTrackOffset) * self.PoseTurntableOffset * self._PoseTurntable * self.REF_FRAME * pose


        varname = 'pos%i' % self.count_var_lin
        if hasattr(self,"_TargetName") and self._TargetName is not None:
            varname = FilterName(self._TargetName)
            delattr(self,"_TargetName")


        self.addline('Lin(%s, EROP_D0, EROP_OL)' % varname)
        if varname not in self.varNames:
            self.varNames.append(varname)
            self.addline_var('%s : CARTPOS := (%s)' % (varname, CARTPOS_2_str(poseKeba,getExternalAxes(joints,self.AXES_TYPE))))

        self.count_var_lin = self.count_var_lin + 1
        
    def MoveC(self, pose1, joints1, pose2, joints2, conf_RLF_1=None, conf_RLF_2=None):
        """Add a circular movement"""
        if pose1 is None or pose2 is None:
            msg = "Circular movement using joint targets is not supported. Change the target type to cartesian or use a joint movement."
            self.addlog(msg)
            self.RunMessage(msg, True)
            return
        
        varname1 = 'pos%i' % self.count_var_lin
        self.count_var_lin = self.count_var_lin + 1
        varname2 = 'pos%i' % self.count_var_lin
        self.count_var_lin = self.count_var_lin + 1        
        self.addline('Circ(%s, %s, EROP_D0, EROP_OL)' % (varname1, varname2))
        self.addline_var('%s : CARTPOS := (%s)' % (varname1, CARTPOS_2_str(pose1,getExternalAxes(joints1,self.AXES_TYPE))))
        self.addline_var('%s : CARTPOS := (%s)' % (varname2, CARTPOS_2_str(pose2,getExternalAxes(joints2,self.AXES_TYPE))))
        
    def setFrame(self, pose, frame_id=None, frame_name=None):
        """Change the robot reference frame"""
        self.REF_FRAME = pose
        if frame_name is None:
            frame_name = 'crsWorkPiece'
        else:
            frame_name = FilterName(frame_name)
            
        self.addline('RefSys(%s)' % frame_name)
        if frame_name not in self.varNames:
            self.varNames.append(frame_name)
            self.addline_var('%s : CARTREFSYS := (%s)' % (frame_name, RefFrame_2_str(pose)))

        #crsWorkPiece : CARTREFSYS := (x:=100, y:=-200, z:=1200)
        #RefSys(crsWorkPiece)
        
    def setTool(self, pose, tool_id=None, tool_name=None):
        """Change the robot TCP"""
        #t1 : TOOL := (x:=1, y:=2, z:=120, a:=0.3, b:=60, c:=0.11)
        #Tool(t1)
        if tool_name is None:
            tool_name = 't2'
        else:
            tool_name = FilterName(tool_name)
           
        self.addline('Tool(%s)' % tool_name)

        if tool_name not in self.varNames:
            self.varNames.append(tool_name)
            self.addline_var('%s : TOOL := (%s)' % (tool_name, tool_2_str(pose)))

    def Pause(self, time_ms):
        """Pause the robot program"""
        if time_ms < 0:
            self.addline('WaitIsFinished()')
            self.addline('Stop()')            
        else:
            self.addline('WaitTime(%.3f)' % (time_ms))
    
    def setSpeed(self, speed_mms):
        """Changes the robot speed (in mm/s)"""
        self.addline('EROP_D0.vel := %.3f' % speed_mms)
    
    def setAcceleration(self, accel_mmss):
        """Changes the robot acceleration (in mm/s2)"""

        self.addline('EROP_D0.acc := %.3f' % accel_mmss)
        self.addline('EROP_D0.dec := %.3f' % accel_mmss)        
    
    def setSpeedJoints(self, speed_degs):
        """Changes the robot joint speed (in deg/s)"""
        """Must be in %"""
        if speed_degs >=550:
            speed_degs = 100
        else:
            speed_degs = (100/550)*speed_degs

        self.addline('EROP_D0.velAxis := %.3f' % speed_degs)
    
    def setAccelerationJoints(self, accel_degss):
        """Changes the robot joint acceleration (in % of max deg/s2)"""
        """Must be in %"""
        if accel_degss >= 450:
            accel_degss = 100
        else:
            accel_degss = (100/450)*accel_degss

        self.addline('EROP_D0.accAxis := %.3f' % accel_degss)
        self.addline('EROP_D0.decAxis := %.3f' % accel_degss)

       
        
    def setZoneData(self, zone_mm):
        """Changes the zone data approach (makes the movement more smooth)"""
        self.addline('EROP_OL.posDist := %s' % (zone_mm))
        
        
    def setDO(self, io_var, io_value):
        """Set a Digital Output"""
        if type(io_var) != str:  # set default variable name if io_var is a number
            io_var = '%s' % str(io_var)        
        if type(io_value) != str: # set default variable value if io_value is a number            
            if io_value > 0:
                io_value = 'TRUE'
            else:
                io_value = 'FALSE'
        
        # at this point, io_var and io_value must be string values
        self.addline('Out%s.Set(%s)' % (io_var, io_value))
        
        if io_var not in self.do_array:
            self.do_array.append(io_var)
            self.addline_var('Out%s : BOOLSIGOUT := (signal := MAP(IoDOut[%s]))' % (io_var, io_var))
        
    def setAO(self, io_var, io_value):
        """Set an Analog Output"""
        self.setDO(io_var, io_value)
        
    def waitDI(self, io_var, io_value, timeout_ms=-1):
        """Waits for an input io_var to attain a given value io_value. Optionally, a timeout can be provided."""
        if type(io_var) != str:  # set default variable name if io_var is a number
            io_var = '%s' % str(io_var)        
        if type(io_value) != str: # set default variable value if io_value is a number            
            if io_value > 0:
                io_value = 'TRUE'
            else:
                io_value = 'FALSE'

        msg = "Wait I/O instruction not implemented currently"
        self.addlog(msg)
        self.RunMessage(msg, True)

        # at this point, io_var and io_value must be string values
        #if timeout_ms < 0:
        #    self.addline('WaitBool(%s, %s)' % (io_var, io_value))
        #else:
        #    self.addline('WaitBool(%s, %s, %i)' % (io_var, io_value, int(timeout_ms))) 

        #self.addline_var('In%s : BOOLSIGIN := (signal := MAP(IoDIn[%s]) ' % (io_var, io_var)) 
        
    def RunCode(self, code, is_function_call = False):
        """Adds code or a function call"""
        if is_function_call:
            code = FilterName(code, max_len=-1)
            self.addline('CALL ' + code)
        else:
           self.addline(code)
        
    def RunMessage(self, message, iscomment = False):
        """Display a message in the robot controller screen (teach pendant)"""
        if iscomment:
            self.addline('// ' + message)
        else:
            self.addline('Info("%s")' % message)
        
# ------------------ private ----------------------                
    def addline(self, newline):
        """Add a program line"""
        self.PROG = self.PROG + newline + '\n'
    
    def addline_var(self, newline):
        """Add a program line"""
        self.PROG_VAR = self.PROG_VAR + newline + '\n'
    
        
    def addlog(self, newline):
        """Add a log message"""
        self.LOG = self.LOG + newline + '\n'

# -------------------------------------------------
# ------------ For testing purposes ---------------   
def Pose(xyzrpw):
    [x,y,z,r,p,w] = xyzrpw
    a = r*math.pi/180
    b = p*math.pi/180
    c = w*math.pi/180
    ca = math.cos(a)
    sa = math.sin(a)
    cb = math.cos(b)
    sb = math.sin(b)
    cc = math.cos(c)
    sc = math.sin(c)
    return Mat([[cb*ca, ca*sc*sb - cc*sa, sc*sa + cc*ca*sb, x],[cb*sa, cc*ca + sc*sb*sa, cc*sb*sa - ca*sc, y],[-sb, cb*sc, cc*cb, z],[0,0,0,1]])

def test_post():
    """Test the post with a basic program"""

    robot = RobotPost()

    robot.ProgStart("Program")
    robot.RunMessage("Program generated by RoboDK using a custom post processor", True)
    robot.setFrame(Pose([807.766544, -963.699898, 41.478944, 0, 0, 0]))
    robot.setTool(Pose([62.5, -108.253175, 100, -60, 90, 0]))
    robot.MoveJ(Pose([200, 200, 500, 180, 0, 180]), [-46.18419, -6.77518, -20.54925, 71.38674, 49.58727, -302.54752] )
    robot.MoveL(Pose([200, 250, 348.734575, 180, 0, -150]), [-41.62707, -8.89064, -30.01809, 60.62329, 49.66749, -258.98418] )
    robot.MoveL(Pose([200, 200, 262.132034, 180, 0, -150]), [-43.73892, -3.91728, -35.77935, 58.57566, 54.11615, -253.81122] )
    robot.RunMessage("Setting air valve 1 on")
    robot.RunCode("TCP_On", True)
    robot.Pause(1000)
    robot.MoveL(Pose([200, 250, 348.734575, 180, 0, -150]), [-41.62707, -8.89064, -30.01809, 60.62329, 49.66749, -258.98418] )
    robot.MoveL(Pose([250, 300, 278.023897, 180, 0, -150]), [-37.52588, -6.32628, -34.59693, 53.52525, 49.24426, -251.44677] )
    robot.MoveL(Pose([250, 250, 191.421356, 180, 0, -150]), [-39.75778, -1.04537, -40.37883, 52.09118, 54.15317, -246.94403] )
    robot.RunMessage("Setting air valve off")
    robot.RunCode("TCP_Off", True)
    robot.Pause(1000)
    robot.MoveL(Pose([250, 300, 278.023897, 180, 0, -150]), [-37.52588, -6.32628, -34.59693, 53.52525, 49.24426, -251.44677] )
    robot.MoveL(Pose([250, 200, 278.023897, 180, 0, -150]), [-41.85389, -1.95619, -34.89154, 57.43912, 52.34162, -253.73403] )
    robot.MoveL(Pose([250, 150, 191.421356, 180, 0, -150]), [-43.82111, 3.29703, -40.29493, 56.02402, 56.61169, -249.23532] )
    robot.ProgFinish("Program")
    # robot.ProgSave(".","Program",True)
    print(robot.PROG)
    if len(robot.LOG) > 0:
        mbox('Program generation LOG:\n\n' + robot.LOG)

    input("Press Enter to close...")

if __name__ == "__main__":
    """Function to call when the module is executed by itself: test"""
    test_post()