'''
Created on 07.06.2014

@author: Thomas
'''
import threading
import time

class MessageHandler(object):
    '''
    classdocs
    '''


    def __init__(self, ServoHandler):
        '''
        Constructor
        '''
        self.ServoHandler = ServoHandler
        self.Mover = MoveJoint()
        self.Mover.start()
        
        self.Dummy = False
    
    def push(self, data):
        self.DirectPush = False
        if self.DirectPush:
            self.ServoHandler.push(data)
        else:
            self.Messages = data.split(';')
            self.Servos = self.ServoHandler.getJoints()
            self.Joint = ""
            self.Angle = 0.0
            self.Message = ""
            for Message in self.Messages:
                self.Parts = Message.split()
                if self.Parts[0] == "setJoint":
                    if self.Parts[1] in self.Servos:
                        try:
                            self.Joint = self.Parts[1]
                            self.Angle = float(self.Parts[2])
                            self.ServoHandler.setAngle(self.Joint, self.Angle)
                        except:
                            pass
                        self.Angle = 0.0
                        self.Joint = ""
                elif self.Parts[0] == "moveJoint":
                    self.Dummy = not self.Dummy
                    self.Mover.SetUpdate(self.Dummy)
            pass
    
    def Exit(self):
        self.Mover.Exit()
        self.Mover.join()
    
class MoveJoint(threading.Thread):
    def __init__(self):
        threading.Thread.__init__(self)
        self.running = True
        self.doUpdate = False
    
    def run(self):
        while self.running:
            if self.doUpdate:
                self.startTime = time.time()
                self.UpdateJoints()
                self.endTime = time.time()-self.startTime
                time.sleep(0.1-self.endTime)
        
    def Exit(self):
        self.running = False
        
    def SetUpdate(self, doUpdate):
        self.doUpdate = doUpdate
        
    def UpdateJoints(self):
        print self.startTime

@@ -305,16 +305,33 @@
 veJoint(
+self.ServoHandler
 )%0D%0A     
@@ -1382,35 +1382,870 @@
     
+if 
 self.
-Dummy = not self.Dummy
+Parts%5B1%5D == %22set%22:%0D%0A                        if self.Parts%5B2%5D in self.Servos:%0D%0A                            try:%0D%0A                                self.Mover.SetMovement(self.Parts%5B2%5D, %0D%0A                                                       float(self.Parts%5B3%5D), %0D%0A                                                       float(self.Parts%5B4%5D), %0D%0A                                                       bool(self.Parts%5B5%5D), %0D%0A                                                       bool(self.Parts%5B6%5D))%0D%0A                            except:%0D%0A                                pass%0D%0A                    elif self.Parts%5B1%5D == %22init%22:%0D%0A                        try:%0D%0A                            self.Mover.InitMovement()%0D%0A                        except:%0D%0A                            pass%0D%0A                    elif self.Parts%5B1%5D == %22move%22:%0D%0A                        try:
 %0D%0A  
@@ -2254,32 +2254,40 @@
                 
+        
 self.Mover.SetUp
@@ -2295,19 +2295,285 @@
 ate(
-self.Dummy)
+True)%0D%0A                        except:%0D%0A                            pass%0D%0A                    elif self.Parts%5B1%5D == %22stop%22:%0D%0A                        try:%0D%0A                            self.Mover.SetUpdate(False)%0D%0A                        except:%0D%0A                            pass
 %0D%0A  
@@ -2728,24 +2728,38 @@
 _init__(self
+, ServoHandler
 ):%0D%0A        
@@ -2843,32 +2843,238 @@
 Update = False%0D%0A
+        %0D%0A        self.ServoHandler = ServoHandler%0D%0A        self.Servos = self.ServoHandler.getJoints()%0D%0A        self.Movements = dict.fromkeys(self.Servos, %5B0, 0, False, False%5D) #Start, End, Repeat, Move%0D%0A
     %0D%0A    def ru
@@ -3316,11 +3316,11 @@
 eep(
-0.1
+1.0
 -sel
@@ -3474,32 +3474,643 @@
 date%0D%0A        %0D%0A
+    def SetMovement(self, Joint, Start, End, Repeat, Move):%0D%0A        if not self.doUpdate:%0D%0A            try:%0D%0A                self.Movements%5BJoint%5D%5B0%5D = Start%0D%0A                self.Movements%5BJoint%5D%5B1%5D = End%0D%0A                self.Movements%5BJoint%5D%5B2%5D = Repeat%0D%0A                self.Movements%5BJoint%5D%5B3%5D = Move%0D%0A            except:%0D%0A                pass%0D%0A    %0D%0A    def InitMovement(self):%0D%0A        if not self.doUpdate:%0D%0A            for servo, movement in self.Servos, self.Movements:%0D%0A                if movement%5B3%5D:%0D%0A                    self.ServoHandler.setAngle(servo, movement%5B0%5D)%0D%0A                %0D%0A        %0D%0A
     def UpdateJo
@@ -4136,24 +4136,54 @@
   print self.startTime%0D%0A
+        print self.Movements%0D%0A

Test implementation of movement functions.