class Controller:
    def __init__(self):
        pass

    def __call__(self, gtm, *args, **kwargs):
        return None


# We control position, but the output of the controller is a set speed.
# Therefore, we have proportional, differential and second differential terms.
class PIDController(Controller):
    err_x, err_y, err_z = 0., 0., 0.
    derr_x, derr_y, derr_z = 0., 0., 0.
    d2err_x, d2err_y, d2err_z = 0., 0., 0.

    def __init__(self, p, i, d):
        super().__init__()
        self.p, self.i, self.d = p, i, d

    def __call__(self, gtm, *args, **kwargs):
        err_x_new, err_y_new, err_z_new = gtm.delta_steps
        derr_x_new, derr_y_new, derr_z_new = err_x_new - self.err_x, err_y_new - self.err_y, err_z_new - self.err_z
        d2err_x_new, d2err_y_new, d2err_z_new = derr_x_new - self.derr_x, derr_y_new - self.derr_y, derr_z_new - self.derr_z

        self.err_x, self.err_y, self.err_z = err_x_new, err_y_new, err_z_new
        self.derr_x, self.derr_y, self.derr_z = derr_x_new, derr_y_new, derr_z_new
        self.d2err_x, self.d2err_y, self.d2err_z = d2err_x_new, d2err_y_new, d2err_z_new

        speed_cmd_x = self.p * self.derr_x + self.i * self.err_x + self.d * self.d2err_x
        speed_cmd_y = self.p * self.derr_y + self.i * self.err_y + self.d * self.d2err_y
        speed_cmd_z = self.p * self.derr_z + self.i * self.err_z + self.d * self.d2err_z

        # It is the responsability of the coefficients to make sure this results in the desired feedrate
        gtm.x_throttle = gtm.target_feedrate / abs(speed_cmd_x)
        gtm.x_direction = 1 if (speed_cmd_x > 0) else 0

        gtm.y_throttle = gtm.target_feedrate / abs(speed_cmd_y)
        gtm.y_direction = 1 if (speed_cmd_y > 0) else 0

        gtm.z_throttle = gtm.target_feedrate / abs(speed_cmd_z)
        gtm.z_direction = 1 if (speed_cmd_z > 0) else 0

        if gtm.x_throttle > 65536:
            gtm.x_throttle = 0

        if gtm.y_throttle > 65536:
            gtm.y_throttle = 0

        if gtm.z_throttle > 65536:
            gtm.z_throttle = 0


class DummyController(Controller):
    def __init__(self):
        super().__init__()

    def __call__(self, gtm, *args, **kwargs):
        print("Call Me")

				feed = self.feedrate
				dx, dy, dz = self.delta_units
				feed_x = feed * dx/(dx**2 + dy**2 + dz**2)
				feed_y = feed * dy/(dx**2 + dy**2 + dz**2)
				feed_z = feed * dz/(dz**2 + dy**2 + dz**2)

        gtm.x_direction = 1 if gtm.delta_steps[0] > 0 else 0
        gtm.y_direction = 1 if gtm.delta_steps[1] > 0 else 0
        gtm.z_direction = 1 if gtm.delta_steps[2] > 0 else 0

				gtm.throttle_x = floor(gtm.FAST_XY_FEEDRATE/feed_x)
				gtm.throttle_y = floor(gtm.FAST_XY_FEEDRATE/feed_y)
				gtm.throttle_z = floor(gtm.FAST_Z_FEEDRATE/feed_z)

				gtm.throttle_x = 0 if gtm.throttle_x >= 65536 else gtm.throttle_x
				gtm.throttle_y = 0 if gtm.throttle_y >= 65536 else gtm.throttle_y
				gtm.throttle_z = 0 if gtm.throttle_z >= 65536 else gtm.throttle_z

				# If we are close enough, we're good
				return (gtm.ctrl_step*(feed*1000./60) >= 3.*(dx**2+dy**2+dz**2)**.5):