# $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/test/end_2_end/standa_test.py 451 2018-02-15 10:09:57Z depa $

import Nice
import Basda
import BasdaMoccaWheel as moccaW
import BasdaMoccaX as moccaX
import BasdaMoccaXY as moccaXY
import test_single_TANservice_connection as TANcon

import numpy as np

class bcolors:
    HEADER = '\033[95m'
    OKBLUE = '\033[94m'
    OKGREEN = '\033[92m'
    WARNING = '\033[93m'
    FAIL = '\033[91m'
    ENDC = '\033[0m'
    BOLD = '\033[1m'
    UNDERLINE = '\033[4m'

if __name__ == "__main__":
    hosts = {
        "host0": ["192.168.1.51",   0], # test MoCon in office
        "host1": ["192.168.103.50", 0], # MoCon1 Stepper motors: 2 ADC, 1 rotator 
        "host2": ["192.168.103.51", 1], # MoCon2 DC motors (deployers + actuators)
        "host3": ["192.168.103.52", 1]  # MoCon3 DC motors (wheels + shutter)
    }
    portMotion  = 50200
    portTrack   = 50300
    portCalUnit = 50400

    dictServiceNamePortsDUMMY = {
        "host0": [("TEST_MOTION", "WHEELS.APO_W", portTrack, "wheel"),
                  ("TEST_MOTION", "WHEELS.ND_W" , portTrack, "wheel")],
        
        "host1": [("TRACK_CTRL-DUMMY", "ADC-DUMMY.PRISMS-DUMMY", portTrack, "xy"),
                  ("TRACK_CTRL-DUMMY", "DROT-DUMMY.DEROTATOR-DUMMY.1", portTrack, "wheel"), # these 2 will not work
                  ("TRACK_CTRL-DUMMY", "DROT-DUMMY.DEROTATOR-DUMMY.2", portTrack, "wheel")],# service names have to be better checked
        
        "host2": [("MOTION_CTRL-DUMMY"  , "BEAM-DUMMY.INBEAM_DEP-DUMMY" , portMotion, "x"),
                  ("MOTION_CTRL-DUMMY"  , "BEAM-DUMMY.INBEAM_TT-DUMMY"  , portMotion, "xy"), # the service receives as input a Nice::Point
                  ("MOTION_CTRL-DUMMY"  , "BEAM-DUMMY.LENS_DEP-DUMMY"   , portMotion, "x"),
                  ("CALUNIT_CTRL-DUMMY" , "CAL_MIRROR_DEP-DUMMY"  , portCalUnit, "x"),
                  ("CALUNIT_CTRL-DUMMY" , "FIBER_DEP-DUMMY"       , portCalUnit, "x")],
        
        "host3": [("MOTION_CTRL-DUMMY", "WHEELS-DUMMY.APO_W-DUMMY", portMotion, "wheel"),
                  ("MOTION_CTRL-DUMMY", "WHEELS-DUMMY.ND_W-DUMMY", portMotion, "wheel"),
                  ("MOTION_CTRL-DUMMY", "WHEELS-DUMMY.CORO_W-DUMMY", portMotion, "wheel"),
                  ("MOTION_CTRL-DUMMY", "WHEELS-DUMMY.LYOT_W-DUMMY", portMotion, "wheel"),
                  ("MOTION_CTRL-DUMMY", "WHEELS-DUMMY.SCI1_W-DUMMY", portMotion, "wheel"),
                  ("MOTION_CTRL-DUMMY", "WHEELS-DUMMY.SCI2_W-DUMMY", portMotion, "wheel"),
                  ("MOTION_CTRL-DUMMY", "WHEELS-DUMMY.DB_W-DUMMY", portMotion, "wheel")]#,
        # ("MOTION_CTRL-DUMMY", "BEAM-DUMMY.SHUTTER-DUMMY", portMotion, "x")] 
     }
    
    isDummy = True
    n = 50
    for key in hosts.keys():
        hostIP = hosts[key][0]
        onlineFlag = hosts[key][1]
        if  (onlineFlag == 1):
            if isDummy:
                dictJolly = dictServiceNamePortsDUMMY
            else:
                dictJolly = dictServiceNamePorts
            for svc in dictJolly[key]:
                if (svc[1] == "BEAM-DUMMY.LENS_DEP-DUMMY"):
                    pos = np.zeros(n)
                    serverName = svc[0]
                    hostPort = svc[2]
                    serviceName = "{}.{}".format(serverName,svc[1])
                    serverName = serverName.lower()
                    serverName = serverName.replace("-dummy", "-svr-DUMMY")
                    devType = svc[3]
                    device = TANcon.TAN_connect(serverName, hostIP, hostPort, serviceName, svc[1], devType)
                    steps = [0, 3605244]*(n/2)

                    if (device.isAtLimit):
                        print("{}{}{} is at limit! Homing".format(bcolors.WARNING, device.readServiceName(), bcolors.ENDC))
                        device.moveToHome()

                    for i in range(n):
                        TANcon.TAN_service_move_absolute(device, steps[i])
                        pos[i] = device.getPosition()
                        print ("> {} position: {}".format(device.readServiceName(), pos[i]))

                    device.moveToHome()
                    pos.tofile("standa_lens_dep-test.txt", sep="\t", format="%s")
