#!/usr/bin/env python

from hand_yarp_client import Hand
from time import sleep
import yarp as y

def server_yarp():
    
    #init yarp ports for comm
    y.Network.init()
    portlistener_in = y.BufferedPortBottle()
    portlistener_in.open("/listener/in")

    while(True):
        bottle_in = portlistener_in.read()
        command = bottle_in.get(0).asString()
        if command == "getdata":
            finger = bottle_in.get(1).asInt()
            enabled = bottle_in.get(2).asDouble()
            pos = []
            pos[0] = bottle_in.get(3).asDouble()
            pos[1] = bottle_in.get(4).asDouble()
            pos[2] = bottle_in.get(5).asDouble()
            vel = []
            vel[0] = bottle_in.get(6).asDouble()
            vel[1] = bottle_in.get(7).asDouble()
            vel[2] = bottle_in.get(8).asDouble()
            tor = []
            tor[0] = bottle_in.get(9).asDouble()
            tor[1] = bottle_in.get(10).asDouble()
            tor[2] = bottle_in.get(11).asDouble()
            print "Finger:", finger
            print "Enabled:", enabled
            print "Position:", pos
            print "Velocity:", vel
            print "Torque:", tor
        
