###################################################################################################
#  Deployment specific global definitions - executed after startup.py
###################################################################################################
import traceback
from ch.psi.pshell.serial import TcpDevice
from ch.psi.pshell.modbus import ModbusTCP


run("setup/Layout")


###################################################################################################
#  Scripted devices and pseudo-devices
###################################################################################################

for script in ["devices/RobotSC", "devices/Wago", "devices/BarcodeReader", "devices/LaserDistance",  \
               "devices/LedCtrl", "devices/HexiPosi"]:
    try:
        run(script)
    except:
        print  >> sys.stderr, traceback.format_exc()    

add_device(img.getContrast(), force = True)
add_device(img.getCamera(), force = True)


###################################################################################################
#  Utility modules
###################################################################################################

run("motion/tools")
run("motion/mount")
run("motion/unmount")
run("imgproc/Utils")
run("tools/Math")

def system_check(robot_move=True):
    if not air_pressure_ok.read():
        raise Exception("Air pressure is not ok")
    if not n2_pressure_ok.read():
        raise Exception("N2 pressure is not ok")
    if robot_move:
        if not feedback_local_safety.read():
            raise Exception("Local safety not released")
        if not feedback_psys_safety.read():
            raise Exception("Psys safety not released")
        if not guiding_tool_park().read():
            raise Exception("Guiding tool not parked")
        

###################################################################################################
#  Device initialization
###################################################################################################

try:
    release_local_safety.write(False)
    release_psys_safety.write(False)
except:
   print  >> sys.stderr, traceback.format_exc()    

try:
    hexiposi.polling=500    
except:
   print  >> sys.stderr, traceback.format_exc()    

try:
    robot.setPolling(DEFAULT_ROBOT_POLLING)          
    robot.set_tool(TOOL_CALIBRATION)
    robot.set_motors_enabled(True)
    robot.set_joint_motors_enabled(True)
except:
   print  >> sys.stderr, traceback.format_exc()    

try:
    import ch.psi.pshell.device.Camera as Camera
    #img.camera.setColorMode(Camera.ColorMode.Mono)
    #img.camera.setDataType(Camera.DataType.UInt8)
    img.camera.setGrabMode(Camera.GrabMode.Continuous)
    img.camera.setTriggerMode(Camera.TriggerMode.Fixed_Rate)
    img.camera.setExposure(50.00)
    img.camera.setAcquirePeriod(200.00)
    img.camera.setGain(0.0)
    #img.camera.setROI(200, 0,1200,1200)
    """
    img.camera.setROI(300, 200,1000,1000)    
    img.config.rotation=17
    img.config.rotationCrop=True
    img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight = 50,50,900,900
    """
    img.camera.setROI(int(get_setting("roi_x")), int(get_setting("roi_y")), int(get_setting("roi_w")), int(get_setting("roi_h")))  
    
    img.camera.stop()
    img.camera.start()
except:
   print  >> sys.stderr, traceback.format_exc()    



###################################################################################################
#  Global variables
###################################################################################################


context = get_context()



update()

print "Initialization complete"