# Copyright 2015-2019 - 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 a Universal Robot with 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
# ----------------------------------------------------

DEFAULT_HEADER_SCRIPT = """
  #--------------------------
  # Add any default subprograms here
  # For example, to drive a gripper as a program call:
  # def Gripper_Open():
  #   ...
  # end
  #
  # Example to drive a spray gun:
  def SprayOn(value):
    # use the value as an output:
    DO_SPRAY = 5
    if value == 0:
      set_standard_digital_out(DO_SPRAY, False)
    else:
      set_standard_digital_out(DO_SPRAY, True)
    end
  end

  # Example to drive an extruder:
  def Extruder(value):
    # use the value as an output:
    if value < 0:
      # stop extruder
    else:
      # start extruder
    end
  end
  
  # Example to move an external axis
  def MoveAxis(value):
    # use the value as an output:
    DO_AXIS_1 = 1
    DI_AXIS_1 = 1
    if value <= 0:
      set_standard_digital_out(DO_AXIS_1, False)
      
      # Wait for digital input to change state
      #while (get_standard_digital_in(DI_AXIS_1) != False):
      #  sync()
      #end
    else:
      set_standard_digital_out(DO_AXIS_1, True)
      
      # Wait for digital input to change state
      #while (get_standard_digital_in(DI_AXIS_1) != True):
      #  sync()
      #end
    end
  end
  #--------------------------
"""

#SCRIPT_URP = '''<URProgram name="%s">
#  <children>
#    <MainProgram runOnlyOnce="false" motionType="MoveJ" speed="1.0471975511965976" acceleration="1.3962634015954636" useActiveTCP="false">
#      <children>
#        <Script type="File">
#          <cachedContents>%s
#</cachedContents>
#          <file resolves-to="file">%s</file>
#        </Script>
#      </children>
#    </MainProgram>
#  </children>
#</URProgram>'''

#SCRIPT_URP = '''<URProgram createdIn="3.0.0" lastSavedIn="3.0.0" name="%s" directory="/" installation="default">
#  <children>
#    <MainProgram runOnlyOnce="true" motionType="MoveJ" speed="1.0471975511965976" acceleration="1.3962634015954636" useActiveTCP="false">
#      <children>
#        <Script type="File">
#          <cachedContents>%s
#</cachedContents>
#          <file resolves-to="file">%s</file>
#        </Script>
#      </children>
#    </MainProgram>
#  </children>
#</URProgram>'''

#<URProgram createdIn="3.4.3.361" lastSavedIn="3.4.3.361" name="%s" directory="." installation="default">
SCRIPT_URP = '''<URProgram createdIn="3.0.0" lastSavedIn="3.0.0" name="%s" directory="." installation="default">
  <children>
    <MainProgram runOnlyOnce="true" motionType="MoveJ" speed="1.0471975511965976" acceleration="1.3962634015954636" useActiveTCP="false">
      <children>
        <Script type="File">
          <cachedContents>%s
</cachedContents>
          <file>%s</file>
        </Script>
      </children>
    </MainProgram>
  </children>
</URProgram>'''

def get_safe_name(progname):
    """Get a safe program name"""
    for c in r'-[]/\;,><&*:%=+@!#^|?^':
        progname = progname.replace(c,'')
    if len(progname) <= 0:
        progname = 'Program'
    if progname[0].isdigit():
        progname = 'P' + progname    
    return progname

# ----------------------------------------------------
# Import RoboDK tools
from robodk import *

# ----------------------------------------------------
import socket
import struct
# UR information for real time control and monitoring
# Byte shifts for the real time packet:
UR_GET_RUNTIME_MODE = 132*8-4

RUNTIME_CANCELLED = 0
RUNTIME_READY = 1
RUNTIME_BUSY = 2

RUNTIME_MODE_MSG = []
RUNTIME_MODE_MSG.append("Operation cancelled") #0
RUNTIME_MODE_MSG.append("Ready") #1
RUNTIME_MODE_MSG.append("Running") #2 # Running or Jogging

# Get packet size according to the byte array
def UR_packet_size(buf):
    if len(buf) < 4:
        return 0
    return struct.unpack_from("!i", buf, 0)[0]
   
# Check if a packet is complete
def UR_packet_check(buf):
    msg_sz = UR_packet_size(buf)
    if len(buf) < msg_sz:
        print("Incorrect packet size %i vs %i" % (msg_sz, len(buf)))
        return False
    
    return True

# Get specific information from a packet
def UR_packet_value(buf, offset, nval=6):
    if len(buf) < offset+nval:
        print("Not available offset (maybe older Polyscope version?): %i - %i" % (len(buf), offset))
        return None
    format = '!'
    for i in range(nval):
        format+='d'
    return list(struct.unpack_from(format, buf, offset))    #return list(struct.unpack_from("!dddddd", buf, offset))


ROBOT_PROGRAM_ERROR = -1
ROBOT_NOT_CONNECTED = 0
ROBOT_OK = 1

def GetErrorMsg(rec_bytes):
    idx_error = -1
    try:
        idx_error = rec_bytes.index(b'error')
    except:
        return None
    
    if idx_error >= 0:
        idx_error_end = min(idx_error + 20, len(rec_bytes))
        try:
            idx_error_end = rec_bytes.index(b'\0',idx_error)
        except:
            return "Unknown error"
    return rec_bytes[idx_error:idx_error_end].decode("utf-8")

def UR_SendProgramRobot(robot_ip, data):
    print("POPUP: Connecting to robot...")
    sys.stdout.flush()
    robot_socket = socket.create_connection((robot_ip, 30002))
    print("POPUP: Sending program..")
    sys.stdout.flush()
    robot_socket.send(data)
    print("POPUP: Sending program...")
    sys.stdout.flush()            
    pause(1)
    received = robot_socket.recv(4096)
    robot_socket.close()
                    
    if received:
        #print("POPUP: Program running")
        #print(str(received))
        sys.stdout.flush()
        error_msg = GetErrorMsg(received)        
        if error_msg:            
            print("POPUP: Robot response: <strong>" + error_msg + "</strong>")
            sys.stdout.flush()
            pause(5)
            return ROBOT_PROGRAM_ERROR
        else:
            print("POPUP: Program sent. The program should be running on the robot.")
            sys.stdout.flush()
            return ROBOT_OK
    else:
        print("POPUP: Robot connection problems...")
        sys.stdout.flush()
        pause(2)
        return ROBOT_NOT_CONNECTED        

# Monitor thread to retrieve information from the robot
def UR_Wait_Ready(robot_ip, percent_cmpl):
    RUNTIME_MODE_LAST = -1
    while True:
        print("Connecting to robot %s:%i" % (robot_ip, 30003))
        rt_socket = socket.create_connection((robot_ip, 30003))
        print("Connected")
        buf = b''
        while True:
            more = rt_socket.recv(4096)
            if more:
                buf = buf + more
                if UR_packet_check(buf):
                    packet_len = UR_packet_size(buf) 
                    packet, buf = buf[:packet_len], buf[packet_len:]
                    RUNTIME_MODE = round(UR_packet_value(packet, UR_GET_RUNTIME_MODE, 1)[0])
                    if RUNTIME_MODE_LAST != RUNTIME_MODE:
                        RUNTIME_MODE_LAST = RUNTIME_MODE
                        if RUNTIME_MODE < len(RUNTIME_MODE_MSG):
                            print("POPUP: Robot " + RUNTIME_MODE_MSG[RUNTIME_MODE] + " (transfer in progress, %.1f%% completed)" % percent_cmpl)
                            sys.stdout.flush()
                        else:
                            print("POPUP: Robot Status Unknown (%.i)" % RUNTIME_MODE + " (transfer %.1f%% completed)" % percent_cmpl)
                            sys.stdout.flush()
                            
                        if RUNTIME_MODE == RUNTIME_READY:
                            rt_socket.close()
                            return True
                        
        rt_socket.close()
        return False

def pose_2_ur(pose):
    """Calculate the p[x,y,z,rx,ry,rz] position for a pose target"""
    NUMERIC_TOLERANCE = 1e-8;
    def saturate_1(value):
        return min(max(value,-1.0),1.0)
        
    angle = acos(  saturate_1((pose[0,0]+pose[1,1]+pose[2,2]-1)/2)   )    
    rxyz = [pose[2,1]-pose[1,2], pose[0,2]-pose[2,0], pose[1,0]-pose[0,1]]
    if angle < NUMERIC_TOLERANCE:
        rxyz = [0,0,0]
    else:
        sin_angle = sin(angle)
        if abs(sin_angle) < NUMERIC_TOLERANCE:
            d3 = [pose[0,0],pose[1,1],pose[2,2]]
            mx = max(d3)
            mx_id = d3.index(mx)
            if mx_id == 0:
                rxyz = [pose[0,0]+1, pose[1,0]  , pose[2,0]  ]
            elif mx_id == 1:
                rxyz = [pose[0,1]  , pose[1,1]+1, pose[2,1]  ]
            else:
                rxyz = [pose[0,2]  , pose[1,2]  , pose[2,2]+1]
            
            rxyz = mult3(rxyz, angle/(sqrt(max(0,2*(1+mx)))))            
        else:            
            rxyz = normalize3(rxyz)
            rxyz = mult3(rxyz, angle)
    return [pose[0,3], pose[1,3], pose[2,3], rxyz[0], rxyz[1], rxyz[2]]

def pose_2_str(pose):
    """Prints a pose target"""
    [x,y,z,w,p,r] = pose_2_ur(pose)
    MM_2_M = 0.001
    return ('p[%.6f, %.6f, %.6f, %.6f, %.6f, %.6f]' % (x*MM_2_M,y*MM_2_M,z*MM_2_M,w,p,r))
    
def angles_2_str(angles):
    """Prints a joint target"""
    njoints = len(angles)
    d2r = pi/180.0
    if njoints == 6:
        return ('[%.6f, %.6f, %.6f, %.6f, %.6f, %.6f]' % (angles[0]*d2r, angles[1]*d2r, angles[2]*d2r, angles[3]*d2r, angles[4]*d2r, angles[5]*d2r))
    else:
        return 'this post only supports 6 joints'

def circle_radius(p0,p1,p2):
    a = norm(subs3(p0,p1))
    b = norm(subs3(p1,p2))
    c = norm(subs3(p2,p0))
    radius = a*b*c/sqrt(pow(a*a+b*b+c*c,2)-2*(pow(a,4)+pow(b,4)+pow(c,4)))            
    return radius
    
#def distance_p1_p02(p0,p1,p2):
#    v01 = subs3(p1, p0)
#    v02 = subs3(p2, p0)
#    return dot(v02,v01)/dot(v02,v02)
        
        
from Universal_Robots import RobotPost as MainPost
        
# ----------------------------------------------------    
# Object class that handles the robot instructions/syntax
class RobotPost(MainPost):
    # Set to True to use MoveP, set to False to use MoveL
    USE_MOVEP = True    
    
    # If True, it will attempt to upload using SFTP. It requires PYSFTP (pip install pysftp. Important: It requires Visual Studio Community C++ 10.0)
    UPLOAD_SFTP = False     
    
    # default speed for linear moves in m/s
    SPEED_MS       = 0.250  
    
    # default speed for joint moves in rad/s
    SPEED_RADS     = 0.75   
    
    # default acceleration for lineaer moves in m/ss
    ACCEL_MSS      = 1.2    
    
    # default acceleration for joint moves in rad/ss
    ACCEL_RADSS    = 1.2    
    
    # default blend radius in meters (corners smoothing)
    BLEND_RADIUS_M = 0.001  
    
    # 5000    # Maximum number of lines per program. If the number of lines is exceeded, the program will be executed step by step by RoboDK
    MAX_LINES_X_PROG = 1e9  
    
    # minimum circle radius to output (in mm). It does not take into account the Blend radius
    MOVEC_MIN_RADIUS = 1    
    
    # maximum circle radius to output (in mm). It does not take into account the Blend radius
    MOVEC_MAX_RADIUS = 10000  

# -------------------------------------------------
# ------------ 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('Universal Robotics', 'Generic UR robot')

    robot.ProgStart("Program")
    robot.RunMessage("Program generated by RoboDK", 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.setSpeed(100) # set speed to 100 mm/s
    robot.setAcceleration(3000) # set speed to 3000 mm/ss    
    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)
    for line in robot.PROG:
        print(line)
    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()