#!/usr/bin/python

# $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/test/end_2_end/test_single_TANservice_connection.py 408 2018-02-02 11:47:43Z depa $

import sys
import logging
import argparse

import Nice
import NiceApplication
import NiceProperty 
import Basda
from BasdaService import Worker
import BasdaMoccaWheel as moccaW
import BasdaMoccaX as moccaX
import BasdaMoccaXY as moccaXY
import NiceLog

import time

def printAndFlush(message):
    print message
    sys.stdout.flush()

class devWorker(Worker):
    def __init__(self, idx, strEp):
        NiceLog.U8_LOG("hello devWorker")
        self.device = moccaW.Wheel.connectWithEndpoint(idx, strEp)
        if self.device.isReachable():
            # svcname = "APO_W"
            # Worker.__init__(self, svcname)
            NiceApplication.Application.distribute(self.config("."), False)
            self.config("CMD.MOVEABSOLUTE.EXEC").leaf().applyBridge(Nice.PropertyBridgeMemFnCompletion.create(self, "callback"))
        # self.device.config().node("CMD.MOVEABSOLUTE.EXEC").leaf().applyBridge(Nice.PropertyBridgeMemFnCompletion.create(self, "callback"))

    def callback(self, _c):
        NiceLog.U8_LOG("hello callback %s" % _c)
        print "hello callback {}".format(_c)

# class cbFun(moccaW.CompletionBridgeMemberFunction):
#     def __init__(self):
#         moccaW.CompletionBridgeMemberFunction.__init__(self)

#     """@InvocableFromCPlusPlus"""
#     def execute(self):
#         # whatToGet = "STATE"
#         whatToGet = "WHEELS.APO_W.SVC.CMD.MOVEABSOLUTE.EXEC"
#         printAndFlush(">> CB: new state: " + Nice.get(whatToGet)["State"])

#     def onUpdate(self, s):
#         printAndFlush("String property update: '%s'" % s)


def main():
    defaultIP = '127.0.0.1'
    defaultPort = 10300;
    defaultServerName = 'TEST_CTRL'
    defaultServiceName = 'TEST_WHEEL' # 'TEST_APO_W'
    defaultResolution = 2048
    defaultDuration = 1
    defaultStartPosition = 1000

    parser = argparse.ArgumentParser(prog='%(prog)s',
                                     description='Moves a motor along a define path')
    parser.add_argument('-i', '--ip',
                        default=defaultIP,
                        help='The IP address of the Server. default: ' + defaultIP)

    parser.add_argument('-p', '--port',
                        default=defaultPort,
                        help='The port of the Server. default: ' + str(defaultPort),
                        type=int)

    parser.add_argument('-svrn', '--servername',
                        default=defaultServerName,
                        help='The name of the Server. default: ' + defaultServerName)

    parser.add_argument('-svcn', '--servicename',
                        default=defaultServiceName,
                        help='The name of the Service. default: ' + defaultServiceName)

    parser.add_argument('-r', '--resolution',
                        default=defaultResolution,
                        help='The motor resolution in steps/rev. default: ' + str(defaultResolution),
                        type=int)

    parser.add_argument('-d', '--duration',
                        default=defaultDuration,
                        help='The trajectory duration in minutes. default: ' + str(defaultDuration),
                        type=float)

    parser.add_argument('-sp', '--startposition',
                        default=defaultStartPosition,
                        help='The trajectory start position in steps. default: ' + str(defaultStartPosition),
                        type=int)

    args = vars(parser.parse_args())

    ep = "[NAME=" + args['servername'] + ",HOSTNAME=" + args['ip'] + ",PORT=" + str(args['port']) + ",PATH=" + args['servicename'] + ".SVC]"
    print "create application with " + ep

    logging.basicConfig(level= logging.DEBUG)

    try:
        print "Server name {}".format(args['servername'])
        wheel = Mocca.Wheel.connectWithEndpoint(args['servername'], ep)

        logging.debug("service name: %s" % wheel.readServiceName())
        assert wheel.isReachable()

        logging.debug("Startup position = {}".format(args['startposition']))
        wheel.moveAbsolute(args['startposition'])
    except SystemError:
        print "ops"
    # except RuntimeError:
    #     print "runtime error"

# _idx is used to identify and discriminate between endpoints
def TAN_connect(_serverName, _hostIP, _port, _serviceName, _idx, _devType):
    ep = "[NAME=" + _serverName + ",HOSTNAME=" + _hostIP + ",PORT=" + str(_port) + ",PATH=" + _serviceName + ".SVC]"

    print "> Create application with " + ep

    logging.basicConfig(level= logging.DEBUG)

    try:
        print "> Server name {}".format(_serverName)
        if _devType == "wheel":
            device = moccaW.Wheel.connectWithEndpoint(_idx, ep)
            # w = devWorker(_idx, ep)
            # assert w.device.isReachable()
        elif _devType == "xy":
            device = moccaXY.XY.connectWithEndpoint(_idx, ep)
        elif _devType == "x":
            device = moccaX.X.connectWithEndpoint(_idx, ep)

        # logging.debug("service name: %s" % device.readServiceName())
        assert device.isReachable()

        if (device.isReachable()):
            print "> Connection to {} succeeded!".format(device.readServiceName())
        else:
            print "> Connection to {} failed!".format(_serviceName)
            raise RuntimeError(_serviceName + " is not reachable!")
        # logging.debug("Startup position = {}".format(args['startposition']))

    except SystemError as error:
        print "TANconnect to {} failed for : {}".format(_serviceName, error)
    except AssertionError as error:
        print "TANconnect to {} failed for : {}".format(_serviceName, error)
    # except RuntimeError:
    #     print "runtime error"

    range = [0,0]
    range[0] = Nice.Application.config().node(_serviceName+"")

    return device

# moccaDevice is in BasdaMoccaWheel BasdaMoccaX BasdaMoccaXY
def TAN_service_move_absolute(_moccaDevice, _steps):
    # service.moveAbsolute()
    # commandFun = cbFun()
    commandStr = "WHEELS.{}.CMD.MOVEABSOLUTE.EXEC".format(_moccaDevice.readServiceName())
    
    # commandStr = "WHEELS.{}.CMD".format(_moccaDevice.readServiceName())
    # moccaW.installCompletionBridgeMemberFunction(commandStr, commandFun)

    if (_moccaDevice.isReachable()):
        print "> Move {} to absolute position {} [steps]\n".format(_moccaDevice.readServiceName(), _steps)
        # moccaW.execute(commandStr, "", {"State": "EXEC"}, 10.0)
        #_moccaDevice.device.config().node("CMD.MOVEABSOLUTE.EXEC").leaf().applyBridge(Nice.PropertyBridgeMemFnCompletion.create(_moccaDevice, "callback"))
        _moccaDevice.moveAbsolute(_steps)


def TAN_service_move_home(_moccaDevice):
    commandStr = "WHEELS.{}.CMD.MOVEABSOLUTE.EXEC".format(_moccaDevice.readServiceName())
    if (_moccaDevice.isReachable()):
        print "> Move {} to home\n".format(_moccaDevice.readServiceName(), _steps)
        _moccaDevice.moveHove()

        

if __name__ == "__main__":
    main()
