###################################################################################################
#  Deployment specific global definitions - executed after startup.py
###################################################################################################
import traceback
from ch.psi.pshell.serial import TcpDevice
from ch.psi.pshell.modbus import ModbusTCP
import ch.psi.mxsc.Controller as Controller
import ch.psi.pshell.core.Nameable as Nameable
import ch.psi.utils.Chrono as Chrono
import ch.psi.mxsc.Controller as Controller



run("setup/Layout")



###################################################################################################
# Configuration
###################################################################################################

IMAGING_ENABLED_PREFERENCE = "imaging_enabled"
PUCK_TYPES_PREFERENCE = "puck_types"
BARCODE_READER_SCAN_PUCKS = "barcode_reader_scan_pucks"

def is_imaging_enabled():
    setting = get_setting(IMAGING_ENABLED_PREFERENCE)
    return not (str(setting).lower() == 'false')

def set_imaging_enabled(value):
    set_setting(IMAGING_ENABLED_PREFERENCE, (True if value else False) )

def assert_imaging_enabled():
    if is_imaging_enabled() == False:
        raise Exception ("Imaging is disabled")

#"unipuck", "minispine" or "mixed"
def set_puck_types(value):
    set_setting(PUCK_TYPES_PREFERENCE, True if value else False )
    
def get_puck_types():
    setting = get_setting(PUCK_TYPES_PREFERENCE)
    if setting == "unipuck" or  setting == "minispine":
        return setting
    return "mixed"


def is_barcode_reader_scan_pucks():
    setting = get_setting(BARCODE_READER_SCAN_PUCKS)
    return False if setting is None else setting.lower() == "true" 

def set_barcode_reader_scan_pucks(value):
    set_setting(BARCODE_READER_SCAN_PUCKS, True if value else False )

def reset_mounted_sample_position():
 set_setting("mounted_sample_position", None)  

#In order to apply current config
set_imaging_enabled(is_imaging_enabled())    
set_puck_types(get_puck_types())
set_barcode_reader_scan_pucks(is_barcode_reader_scan_pucks())

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

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

if is_imaging_enabled():
    add_device(img.getContrast(), force = True)
    add_device(img.getCamera(), force = True)


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

run("data/samples")
run("data/pucks")
run("motion/tools")
run("motion/mount")
run("motion/unmount")
run("motion/get_dewar")
run("motion/put_dewar")
run("motion/get_gonio")
run("motion/put_gonio")
run("motion/move_dewar")
run("motion/move_gonio")
run("motion/move_heater")
run("motion/move_home")
run("motion/move_park")
run("motion/move_cold")
run("motion/move_scanner")
run("motion/move_aux")
run("motion/get_aux")
run("motion/put_aux")
run("motion/dry")
run("motion/trash")
run("motion/homing_hexiposi")
run("motion/calibrate_tool")
run("motion/scan_pin")
run("motion/robot_recover")
run("motion/recover")
run("tools/Math")
if is_imaging_enabled():
    run("imgproc/Utils")

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")
        auto = not is_manual_mode()
        if auto:
            if not feedback_psys_safety.read():
                raise Exception("Psys safety not released")
        if not guiding_tool_park.read():
            raise Exception("Guiding tool not parked")
        
def get_puck_elect_detection(segment, puck):
    return str(Controller.getInstance().getPuck(str(segment).upper() + str(puck)).detection)

def get_puck_img_detection(segment, puck):
    return str(Controller.getInstance().getPuck(str(segment).upper() + str(puck)).imageDetection)

def assert_puck_detected(segment, puck):
    if (segment == AUX_SEGMENT) and (puck == 1):
        return
    if get_puck_elect_detection(segment, puck) != "Present":
        raise Exception ("Puck " + str(segment).upper() + str(puck) + " not present")


def start_puck_detection():
    run("tools/RestartPuckDetection")

def check_puck_detection():
    return run("tools/CheckPuckDetection")

def stop_puck_detection():
    run("tools/StopPuckDetection")



def get_detected_pucks():
    ret = []
    for i in range(30): 
        p = BasePlate.getPucks()[i]
        if (str(p.getDetection()) == "Present"): 
            ret.append(str(p.getName()))
    return ret

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

try:
    set_heater(False)
    set_air_stream(False)
except:
   print  >> sys.stderr, traceback.format_exc() 


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_DEFAULT)
    robot.set_frame(FRAME_DEFAULT)
    robot.set_motors_enabled(True)
    robot.set_joint_motors_enabled(True)
except:
   print  >> sys.stderr, traceback.format_exc()    

if is_imaging_enabled():
  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(25.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()    


#gripper_cam.paused = True
###################################################################################################
#  Device monitoring
###################################################################################################

DEWAR_LEVEL_RT = 5.0
is_room_temperature = False

def is_room_temp():
    return is_room_temperature


class DewarLevelListener (DeviceListener):
    def onValueChanged(self, device, value, former):
        global is_room_temperature
        if value is not None:
            is_room_temperature =  value <= DEWAR_LEVEL_RT
dewar_level_listener = DewarLevelListener()

for l in dewar_level.listeners:
    #if isinstance(l, DewarLevelListener):    #Class changes...
    if Nameable.getShortClassName(l.getClass()) == "DewarLevelListener":
        dewar_level.removeListener(l)
        
dewar_level.addListener(dewar_level_listener)
dewar_level_listener.onValueChanged(dewar_level, dewar_level.take(), None)




class HexiposiListener (DeviceListener):
    def onValueChanging(self, device, value, former):
        robot.assert_cleared()
        
hexiposi_listener = HexiposiListener()
hexiposi.addListener(hexiposi_listener)

###################################################################################################
#  Global variables & application state
###################################################################################################


context = get_context()

cover_detection_debug = False


def is_puck_loading():
    return robot.state == State.Ready and robot.take()["pos"] == 'pPark' and \
    feedback_psys_safety.take() == False and \
    not guiding_tool_park.read()

def set_pin_offset(val):
    if abs(val) >5:
        raise Exception("Invalid pin offset: " + str(val)) 
    try:
        set_setting("pin_offset",float(val))
    except:
        log("Error setting pin offset: " + str(sys.exc_info()[1]), False)

def get_pin_offset():
    try:
        ret =  float(get_setting("pin_offset"))
        if abs(ret) >5:
            raise Exception("Invalid configured pin offset: " + str(ret))         
        return ret
    except:
        log("Error getting pin offset: " + str(sys.exc_info()[1]), False)
        return 0.0


def set_pin_angle_offset(val):
    if (abs(val) > 180.0) or (abs(val) < -180.0):
        raise Exception("Invalid pin angle offset: " + str(val)) 
    try:
        set_setting("pin_angle_offset",float(val))
    except:
        log("Error setting pin angle offset: " + str(sys.exc_info()[1]), False)

def get_pin_angle_offset():
    try:
        ret =  float(get_setting("pin_angle_offset"))
        if (abs(ret) > 180.0) or (abs(ret) < -180.0):
            raise Exception("Invalid configured pin angle offset: " + str(ret))         
        return ret
    except:
        log("Error getting pin angle offset: " + str(sys.exc_info()[1]), False)
        return 0.0

def is_force_dry():
    try:
        dry_mount_counter = int(get_setting("dry_mount_counter"))
    except:
        dry_mount_counter = 0
        
    try:  
        dry_timespan = time.time() - float( get_setting("dry_timestamp"))
    except:
        dry_timespan = 3600

    try:
        force_dry_mount_count = int(get_setting("force_dry_mount_count"))
        if force_dry_mount_count>0 and dry_mount_counter>=force_dry_mount_count:
            return True
    except:
        pass

    try:
        force_dry_timeout = float(get_setting("force_dry_timeout"))
        if force_dry_timeout>0 and dry_timespan>=force_dry_timeout:
            return True
    except:
        pass    
    return False    
     

update()
add_device(Controller.getInstance().basePlate, True)
restore_samples_info()
    

print "Initialization complete"