import behavior
import time
import math

class Behavior(behavior.RobotBehavior):
    def __init__(self, name, robotList, mag_diff, freq_cutoff, detecting_period, queue):
        super(Behavior, self).__init__(name, robotList)
        self._period = detecting_period
        self._queue = queue
        self._freq_cutoff = freq_cutoff;
        self._RC = 1/(2 * math.pi * self._freq_cutoff)
        self._alpha = self._period / (self._RC + self._period)
        self._acc_x = 0
        self._acc_y = 0
        self._mag_diff2 = mag_diff * mag_diff
        print "_alpha", self._alpha
        print "magnitude", mag_diff

    def lowpass(self, alpha, old, new):
        return alpha * new + (1.0 - alpha) * old

    def difference(self, old, new):
        return old - new

    def collision_detection(self, acc_x, acc_y):
        dx = self.difference(self._acc_x, acc_x)
        dy = self.difference(self._acc_y, acc_y)
        mag2 = dx*dx + dy*dy
        if (mag2 > self._mag_diff2):
            return math.sqrt(mag2)
        else:
            return 0

    def behavior_loop(self):
        print self._name, "behavior_loop() starts!"
        while (not self._bQuit):
            for robot in self._robotList:
                #print self._name, "behavior_loop(): time = ", time.time(), "sec"
                acc_x = robot.get_acceleration(0)/100.0
                acc_y = robot.get_acceleration(1)/100.0
                #prox_r = -3 * robot.get_signal()
                acc_x = self.lowpass(self._alpha, self._acc_x, acc_x)
                acc_y = self.lowpass(self._alpha, self._acc_y, acc_y)
                mag = self.collision_detection(acc_x, acc_y)
                self._acc_x = acc_x
                self._acc_y = acc_y
                if (mag > 0):
                    print "collision detected!", mag
                    elem = {}
                    elem[0] = self._acc_x
                    elem[1] = self._acc_y
                    #print elem[0], elem[1]
                    self._queue.put(elem)
            time.sleep(self._period)
        print self._name, "behavior_loop() finished!"