#!/usr/bin/env python
import math

from geometry_msgs.msg import Point
from constants import SIMULATION_TIME_IN_SECONDS, TRAJECTORY


def create_trajectory():
    if TRAJECTORY == 'linear':
        return LinearTrajectory(0.05, 0.01, 0.05, 0.01)
    elif TRAJECTORY == 'circular':
        return CircularTrajectory(2.0, 120)
    elif TRAJECTORY == 'squared':
        return SquaredTrajectory(2.0, 0.01, 0.01)


class NegativeTimeException(Exception):
    pass

class Trajectory:
    def __init__(self):
        self.position = Point()

    def get_position_at(self, t):
        pass


class LinearTrajectory(Trajectory):
    def __init__(self, v_x, x_0=0, v_y=0, y_0=0):
        Trajectory.__init__(self)
        self.v_x = v_x
        self.v_y = v_y
        self.x_0 = x_0
        self.y_0 = y_0

    def get_position_at(self, t):
        if t < 0:
            raise NegativeTimeException()


        self.position.x = self.v_x * t + self.x_0
        self.position.y = self.v_y * t + self.y_0

        return self.position

class CircularTrajectory(Trajectory):
    def __init__(self, radius, period, x_0=0, y_0=0):
        Trajectory.__init__(self)
        self.radius = radius
        self. period = period
        self.x_0 = x_0
        self.y_0 = y_0

    def get_position_at(self, t):
        self.position.x = self.radius * math.sin(2 * math.pi * t / self.period) + self.x_0
        self.position.y = -self.radius * math.cos(2 * math.pi * t / self.period) + self.y_0

        return self.position


class SquaredTrajectory(Trajectory):
    def __init__(self, side, x_0=0, y_0=0):
        Trajectory.__init__(self)
        self.side = side
        self.v = 4.0 * side / SIMULATION_TIME_IN_SECONDS
        self.x_0 = x_0
        self.y_0 = y_0

    def get_position_at(self, t):
        if 0 <= t < SIMULATION_TIME_IN_SECONDS / 4:
            self.position.x = self.v * t + self.x_0
            self.position.y = self.y_0
        elif SIMULATION_TIME_IN_SECONDS / 4 <= t < SIMULATION_TIME_IN_SECONDS / 2:
            self.position.x = self.x_0 + self.side
            self.position.y = self.v * (t - SIMULATION_TIME_IN_SECONDS / 4) + self.y_0
        elif SIMULATION_TIME_IN_SECONDS / 2 <= t < 3 * SIMULATION_TIME_IN_SECONDS / 4:
            self.position.x = -self.v * (t - SIMULATION_TIME_IN_SECONDS / 2) + self.x_0 + self.side
            self.position.y = self.y_0 + self.side
        else:
            self.position.x = self.x_0
            self.position.y = -self.v * (t - 3 * SIMULATION_TIME_IN_SECONDS / 4) + self.y_0 + self.side

        return self.position

@@ -317,11 +317,34 @@
 .0, 
-120
+SIMULATION_TIME_IN_SECONDS
 )%0A  
@@ -412,16 +412,44 @@
 ory(2.0,
+ SIMULATION_TIME_IN_SECONDS,
  0.01, 0
@@ -622,20 +622,67 @@
         
-pass
+if t %3C 0:%0A            raise NegativeTimeException()
 %0A%0A%0Aclass
@@ -691,32 +691,40 @@
 inearTrajectory(
+object, 
 Trajectory):%0A   
@@ -943,60 +943,56 @@
     
-if t %3C 0:%0A            raise NegativeTimeException()%0A
+super(LinearTrajectory, self).get_position_at(t)
 %0A%0A  
@@ -1137,32 +1137,40 @@
 cularTrajectory(
+object, 
 Trajectory):%0A   
@@ -1386,32 +1386,91 @@
 on_at(self, t):%0A
+        super(CircularTrajectory, self).get_position_at(t)%0A
         self.pos
@@ -1692,16 +1692,24 @@
 jectory(
+object, 
 Trajecto
@@ -1741,16 +1741,24 @@
 f, side,
+ period,
  x_0=0, 
@@ -1841,51 +1841,60 @@
 elf.
-v = 4.0 * side / SIMULATION_TIME_IN_SECONDS
+period = period%0A        self.v = 4.0 * side / period
 %0A   
@@ -1983,46 +1983,90 @@
     
-if 0 %3C= t %3C SIMULATION_TIME_IN_SECONDS
+super(SquaredTrajectory, self).get_position_at(t)%0A%0A        if 0 %3C= t %3C self.period
  / 4
@@ -2175,72 +2175,42 @@
 lif 
-SIMULATION_TIME_IN_SECONDS / 4 %3C= t %3C SIMULATION_TIME_IN_SECONDS
+self.period / 4 %3C= t %3C self.period
  / 2
@@ -2310,34 +2310,19 @@
 t - 
-SIMULATION_TIME_IN_SECONDS
+self.period
  / 4
@@ -2351,76 +2351,46 @@
 lif 
-SIMULATION_TIME_IN_SECONDS / 2 %3C= t %3C 3 * SIMULATION_TIME_IN_SECONDS
+self.period / 2 %3C= t %3C 3 * self.period
  / 4
@@ -2440,34 +2440,19 @@
 t - 
-SIMULATION_TIME_IN_SECONDS
+self.period
  / 2
@@ -2633,34 +2633,19 @@
 3 * 
-SIMULATION_TIME_IN_SECONDS
+self.period
  / 4

Implement get position in Trajectory class