from cozmo_fsm import *
import math
import time

initpose_x = 0
initpose_y = 0
r_avg = []


class Sender(StateNode):
    def start(self, event=None):
        value = [robot.pose.position.x, robot.pose.position.y]
        super().start(event)
        self.post_data(value)
 

class Receiver(StateNode):
    def start(self, event=None):
        global initpose_y
        global initpose_x
        global r_avg
        super().start(event)
        if isinstance(event, DataEvent):
            value = event.data
            r = math.sqrt(abs(value[0] - initpose_x)**2 + abs(value[1] - initpose_y)**2)
            initpose_y = value[1]
            initpose_x = value[0]
            r_avg.append(r)
            self.post_data(r)

class Average(StateNode):
     def start(self, event=None):
        global r_avg
        super().start(event)
        final_avg = sum(r_avg) / len(r_avg)
        print(final_avg)

class FindR(StateMachineProgram):
   $setup {
        loop: Iterate(10)
        loop =D=> Turn(90) =C=> Sender() =D=> Receiver() =Next=> loop
        loop =C=> Average()
      
          }